From e56382a55c7b25d1abdabccaf3e66edb9f9484e7 Mon Sep 17 00:00:00 2001 From: Sai Vemprala Date: Wed, 9 Dec 2020 17:34:50 -0800 Subject: [PATCH 1/6] Event simulator in Python --- .../eventcamera_sim/event_simulator.py | 162 ++++++++++++++++++ .../eventcamera_sim/test_event_sim.py | 88 ++++++++++ 2 files changed, 250 insertions(+) create mode 100644 PythonClient/eventcamera_sim/event_simulator.py create mode 100644 PythonClient/eventcamera_sim/test_event_sim.py diff --git a/PythonClient/eventcamera_sim/event_simulator.py b/PythonClient/eventcamera_sim/event_simulator.py new file mode 100644 index 0000000000..4a0f6baef4 --- /dev/null +++ b/PythonClient/eventcamera_sim/event_simulator.py @@ -0,0 +1,162 @@ +from numba.np.ufunc import parallel +import numpy as np +from types import SimpleNamespace +from numba import njit, prange, set_num_threads + +EVENT_TYPE = np.dtype( + [("timestamp", "f8"), ("x", "u2"), ("y", "u2"), ("polarity", "b")], align=True +) + +TOL = 0.5 +MINIMUM_CONTRAST_THRESHOLD = 0.01 + +CONFIG = SimpleNamespace( + **{ + "contrast_thresholds": (0.01, 0.01), + "sigma_contrast_thresholds": (0.0, 0.0), + "refractory_period_ns": 1000, + "max_events_per_frame": 200000, + } +) + + +@njit(parallel=True) +def esim( + x_end, + current_image, + previous_image, + delta_time, + crossings, + last_time, + output_events, + spikes, + refractory_period_ns, + max_events_per_frame, + n_pix_row, +): + count = 0 + maxSpikes = int(delta_time / (refractory_period_ns * 1e-3)) + for x in prange(x_end): + itdt = np.log(current_image[x]) + it = np.log(previous_image[x]) + deltaL = itdt - it + + if np.abs(deltaL) < TOL: + continue + + pol = np.sign(deltaL) + + crossUpdate = pol * TOL + crossings[x] = np.log(crossings[x]) + crossUpdate + + lb = crossings[x] - it + ub = crossings[x] - itdt + + posCheck = lb > 0 and (pol == 1) and ub < 0 + negCheck = lb < 0 and (pol == -1) and ub > 0 + + spikeNums = (itdt - crossings[x]) / TOL + crossCheck = posCheck + negCheck + spikeNums = np.abs(int(spikeNums * crossCheck)) + + crossings[x] = itdt - crossUpdate + if spikeNums > 0: + spikes[x] = pol + + spikeNums = maxSpikes if spikeNums > maxSpikes else spikeNums + + current_time = last_time + for i in range(spikeNums): + output_events[count].x = x // n_pix_row + output_events[count].y = x % n_pix_row + output_events[count].timestamp = np.round(current_time * 1e-6, 6) + output_events[count].polarity = pol > 0 + + count += 1 + current_time += (delta_time) / spikeNums + + if count == max_events_per_frame: + return count + + return count + + +class EventSimulator: + def __init__(self, H, W, first_image=None, first_time=None, config=CONFIG): + self.H = H + self.W = W + self.config = config + self.last_image = None + if first_image is not None: + assert first_time is not None + self.init(first_image, first_time) + + self.npix = H * W + + def init(self, first_image, first_time): + print("Initialized event camera simulator with sensor size:", first_image.shape) + + self.resolution = first_image.shape # The resolution of the image + + # We ignore the 2D nature of the problem as it is not relevant here + # It makes multi-core processing more straightforward + first_image = first_image.reshape(-1) + + # Allocations + self.last_image = first_image.copy() + self.current_image = first_image.copy() + + self.last_time = first_time + + self.output_events = np.zeros( + (self.config.max_events_per_frame), dtype=EVENT_TYPE + ) + self.event_count = 0 + self.spikes = np.zeros((self.npix)) + + # One of the trick here for good performance is to avoid allocating + # memory here because we will be calling it very often + def image_callback(self, new_image, new_time): + if self.last_image is None: + self.init(new_image, new_time) + return None, None + + assert new_time > 0 + assert new_image.shape == self.resolution + new_image = new_image.reshape(-1) # Free operation + + # Copy is faster than reallocating memory + np.copyto(self.current_image, new_image) + + delta_time = new_time - self.last_time + + config = self.config + self.output_events = np.zeros( + (self.config.max_events_per_frame), dtype=EVENT_TYPE + ) + self.spikes = np.zeros((self.npix)) + + self.crossings = self.last_image.copy() + self.event_count = esim( + self.current_image.size, + self.current_image, + self.last_image, + delta_time, + self.crossings, + self.last_time, + self.output_events, + self.spikes, + config.refractory_period_ns, + config.max_events_per_frame, + self.W, + ) + + np.copyto(self.last_image, self.current_image) + self.last_time = new_time + + result = self.output_events[: self.event_count] + result.sort(order=["timestamp"], axis=0) + + return self.spikes, result + + # TODO sort events or insert them into a heap diff --git a/PythonClient/eventcamera_sim/test_event_sim.py b/PythonClient/eventcamera_sim/test_event_sim.py new file mode 100644 index 0000000000..124d9105f8 --- /dev/null +++ b/PythonClient/eventcamera_sim/test_event_sim.py @@ -0,0 +1,88 @@ +import numpy as np +import airsim +import time +import cv2 +import matplotlib.pyplot as plt + +from event_simulator import * + + +class AirSimEventGen: + def __init__(self, W, H, debug=False): + self.ev_sim = EventSimulator(W, H) + self.W = W + self.H = H + + self.image_request = airsim.ImageRequest( + "0", airsim.ImageType.Scene, False, False + ) + + self.client = airsim.VehicleClient() + self.client.confirmConnection() + self.init = True + self.start_ts = None + + self.rgb_image_shape = [H, W, 3] + self.debug = debug + + if debug: + self.fig, self.ax = plt.subplots(1, 1) + + def visualize_events(self, event_img): + event_img = self.convert_event_img_rgb(event_img) + self.ax.cla() + self.ax.imshow(event_img, cmap="viridis") + plt.draw() + plt.pause(0.01) + + def convert_event_img_rgb(self, image): + image = image.reshape(self.H, self.W) + out = np.zeros((self.H, self.W, 3), dtype=np.uint8) + out[:, :, 0] = np.clip(image, 0, 1) * 255 + out[:, :, 2] = np.clip(image, -1, 0) * -255 + + return out + + +if __name__ == "__main__": + event_generator = AirSimEventGen(256, 144, debug=True) + i = 0 + start_time = 0 + t_start = time.time() + + while True: + image_request = airsim.ImageRequest("0", airsim.ImageType.Scene, False, False) + + response = event_generator.client.simGetImages([event_generator.image_request]) + while response[0].height == 0 or response[0].width == 0: + response = event_generator.client.simGetImages( + [event_generator.image_request] + ) + + ts = time.time_ns() + + if event_generator.init: + event_generator.start_ts = ts + event_generator.init = False + + img = np.reshape( + np.fromstring(response[0].image_data_uint8, dtype=np.uint8), + event_generator.rgb_image_shape, + ) + + img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY).astype(np.float32) + img = cv2.add(img, 0.001) + + ts = time.time_ns() + ts_delta = (ts - event_generator.start_ts) * 1e-3 + start = time.time() + event_img, events = event_generator.ev_sim.image_callback(img, ts_delta) + print(f"Time: {time.time() - start}") + bytestream = [] + + if events is not None and events.shape[0] > 0: + bytestream = events.tolist() + print(len(bytestream)) + + event_generator.visualize_events(event_img) + From 4eac25e5efccd874cea4605edc4af9c9e07604aa Mon Sep 17 00:00:00 2001 From: Sai Vemprala Date: Wed, 9 Dec 2020 17:35:57 -0800 Subject: [PATCH 2/6] Minor changes --- PythonClient/eventcamera_sim/event_simulator.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/PythonClient/eventcamera_sim/event_simulator.py b/PythonClient/eventcamera_sim/event_simulator.py index 4a0f6baef4..81cf88d56f 100644 --- a/PythonClient/eventcamera_sim/event_simulator.py +++ b/PythonClient/eventcamera_sim/event_simulator.py @@ -114,8 +114,6 @@ def init(self, first_image, first_time): self.event_count = 0 self.spikes = np.zeros((self.npix)) - # One of the trick here for good performance is to avoid allocating - # memory here because we will be calling it very often def image_callback(self, new_image, new_time): if self.last_image is None: self.init(new_image, new_time) @@ -125,7 +123,6 @@ def image_callback(self, new_image, new_time): assert new_image.shape == self.resolution new_image = new_image.reshape(-1) # Free operation - # Copy is faster than reallocating memory np.copyto(self.current_image, new_image) delta_time = new_time - self.last_time @@ -158,5 +155,3 @@ def image_callback(self, new_image, new_time): result.sort(order=["timestamp"], axis=0) return self.spikes, result - - # TODO sort events or insert them into a heap From 2d61b5036f327b94c3707a0324d4aec1ef081cae Mon Sep 17 00:00:00 2001 From: Sai Vemprala Date: Mon, 14 Dec 2020 18:45:13 -0800 Subject: [PATCH 3/6] Some additions --- .../eventcamera_sim/test_event_sim.py | 25 +++++++++++++++---- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/PythonClient/eventcamera_sim/test_event_sim.py b/PythonClient/eventcamera_sim/test_event_sim.py index 124d9105f8..5ae99e25e3 100644 --- a/PythonClient/eventcamera_sim/test_event_sim.py +++ b/PythonClient/eventcamera_sim/test_event_sim.py @@ -3,9 +3,15 @@ import time import cv2 import matplotlib.pyplot as plt - +import argparse +import sys, signal from event_simulator import * +parser = argparse.ArgumentParser(description="Simulate event data from AirSim") +parser.add_argument("--debug", action="store_true") +parser.add_argument("--height", type=int, default=144) +parser.add_argument("--width", type=int, default=256) + class AirSimEventGen: def __init__(self, W, H, debug=False): @@ -44,12 +50,21 @@ def convert_event_img_rgb(self, image): return out +def _stop_event_gen(signal, frame): + print("\nCtrl+C received. Stopping event sim...") + sys.exit(0) + + if __name__ == "__main__": - event_generator = AirSimEventGen(256, 144, debug=True) + args = parser.parse_args() + + event_generator = AirSimEventGen(args.W, args.H, debug=args.debug) i = 0 start_time = 0 t_start = time.time() + signal.signal(signal.SIGINT, event_generator._stop_gen) + while True: image_request = airsim.ImageRequest("0", airsim.ImageType.Scene, False, False) @@ -82,7 +97,7 @@ def convert_event_img_rgb(self, image): if events is not None and events.shape[0] > 0: bytestream = events.tolist() - print(len(bytestream)) - - event_generator.visualize_events(event_img) + if event_generator.debug: + print(len(bytestream)) + event_generator.visualize_events(event_img) From 76fde3a76108896c6bfbb313808a25aacc823d0a Mon Sep 17 00:00:00 2001 From: Sai Vemprala Date: Mon, 14 Dec 2020 19:55:00 -0800 Subject: [PATCH 4/6] Pickle dump of events --- .../eventcamera_sim/event_simulator.py | 2 +- .../eventcamera_sim/test_event_sim.py | 31 ++++++++++++------- 2 files changed, 20 insertions(+), 13 deletions(-) diff --git a/PythonClient/eventcamera_sim/event_simulator.py b/PythonClient/eventcamera_sim/event_simulator.py index 81cf88d56f..10619524cf 100644 --- a/PythonClient/eventcamera_sim/event_simulator.py +++ b/PythonClient/eventcamera_sim/event_simulator.py @@ -70,7 +70,7 @@ def esim( output_events[count].x = x // n_pix_row output_events[count].y = x % n_pix_row output_events[count].timestamp = np.round(current_time * 1e-6, 6) - output_events[count].polarity = pol > 0 + output_events[count].polarity = 1 if pol > 0 else -1 count += 1 current_time += (delta_time) / spikeNums diff --git a/PythonClient/eventcamera_sim/test_event_sim.py b/PythonClient/eventcamera_sim/test_event_sim.py index 5ae99e25e3..c19bc04bf9 100644 --- a/PythonClient/eventcamera_sim/test_event_sim.py +++ b/PythonClient/eventcamera_sim/test_event_sim.py @@ -5,10 +5,13 @@ import matplotlib.pyplot as plt import argparse import sys, signal +import pandas as pd +import pickle from event_simulator import * parser = argparse.ArgumentParser(description="Simulate event data from AirSim") -parser.add_argument("--debug", action="store_true") +parser.add_argument("--debug", action="store_false") +parser.add_argument("--save", action="store_false") parser.add_argument("--height", type=int, default=144) parser.add_argument("--width", type=int, default=256) @@ -31,6 +34,9 @@ def __init__(self, W, H, debug=False): self.rgb_image_shape = [H, W, 3] self.debug = debug + self.event_file = open("events.pkl", "ab") + self.event_fmt = "%1.7f", "%d", "%d", "%d" + if debug: self.fig, self.ax = plt.subplots(1, 1) @@ -49,21 +55,21 @@ def convert_event_img_rgb(self, image): return out - -def _stop_event_gen(signal, frame): - print("\nCtrl+C received. Stopping event sim...") - sys.exit(0) + def _stop_event_gen(self, signal, frame): + print("\nCtrl+C received. Stopping event sim...") + self.event_file.close() + sys.exit(0) if __name__ == "__main__": args = parser.parse_args() - event_generator = AirSimEventGen(args.W, args.H, debug=args.debug) + event_generator = AirSimEventGen(args.width, args.height, debug=args.debug) i = 0 start_time = 0 t_start = time.time() - signal.signal(signal.SIGINT, event_generator._stop_gen) + signal.signal(signal.SIGINT, event_generator._stop_event_gen) while True: image_request = airsim.ImageRequest("0", airsim.ImageType.Scene, False, False) @@ -90,14 +96,15 @@ def _stop_event_gen(signal, frame): ts = time.time_ns() ts_delta = (ts - event_generator.start_ts) * 1e-3 - start = time.time() + # start = time.time() event_img, events = event_generator.ev_sim.image_callback(img, ts_delta) - print(f"Time: {time.time() - start}") - bytestream = [] + # print(f"event gen time: {time.time() - start}") if events is not None and events.shape[0] > 0: - bytestream = events.tolist() + if event_generator.save: + # Using pickle dump in a per-frame fashion to save time, instead of savetxt + # Optimizations possible + pickle.dump(events, event_generator.event_file) if event_generator.debug: - print(len(bytestream)) event_generator.visualize_events(event_img) From 140689f991b76b6ee5e447a989b255ca8f6f1149 Mon Sep 17 00:00:00 2001 From: Sai Vemprala Date: Tue, 15 Dec 2020 14:36:44 -0800 Subject: [PATCH 5/6] Fixes --- PythonClient/eventcamera_sim/test_event_sim.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/PythonClient/eventcamera_sim/test_event_sim.py b/PythonClient/eventcamera_sim/test_event_sim.py index c19bc04bf9..a310c6b1a3 100644 --- a/PythonClient/eventcamera_sim/test_event_sim.py +++ b/PythonClient/eventcamera_sim/test_event_sim.py @@ -10,14 +10,14 @@ from event_simulator import * parser = argparse.ArgumentParser(description="Simulate event data from AirSim") -parser.add_argument("--debug", action="store_false") -parser.add_argument("--save", action="store_false") +parser.add_argument("--debug", action="store_true") +parser.add_argument("--save", action="store_true") parser.add_argument("--height", type=int, default=144) parser.add_argument("--width", type=int, default=256) class AirSimEventGen: - def __init__(self, W, H, debug=False): + def __init__(self, W, H, save=False, debug=False): self.ev_sim = EventSimulator(W, H) self.W = W self.H = H @@ -33,6 +33,7 @@ def __init__(self, W, H, debug=False): self.rgb_image_shape = [H, W, 3] self.debug = debug + self.save = save self.event_file = open("events.pkl", "ab") self.event_fmt = "%1.7f", "%d", "%d", "%d" @@ -64,7 +65,7 @@ def _stop_event_gen(self, signal, frame): if __name__ == "__main__": args = parser.parse_args() - event_generator = AirSimEventGen(args.width, args.height, debug=args.debug) + event_generator = AirSimEventGen(args.width, args.height, save=args.save, debug=args.debug) i = 0 start_time = 0 t_start = time.time() @@ -92,13 +93,14 @@ def _stop_event_gen(self, signal, frame): ) img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY).astype(np.float32) + # Add small number to avoid issues with log(I) img = cv2.add(img, 0.001) ts = time.time_ns() ts_delta = (ts - event_generator.start_ts) * 1e-3 - # start = time.time() + + # Event sim keeps track of previous image automatically event_img, events = event_generator.ev_sim.image_callback(img, ts_delta) - # print(f"event gen time: {time.time() - start}") if events is not None and events.shape[0] > 0: if event_generator.save: From 6c3fffcc8fce1959af8a5530dacdbf0574d7f417 Mon Sep 17 00:00:00 2001 From: Sai Vemprala Date: Tue, 15 Dec 2020 14:40:33 -0800 Subject: [PATCH 6/6] PEP8 compliance --- .../eventcamera_sim/event_simulator.py | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/PythonClient/eventcamera_sim/event_simulator.py b/PythonClient/eventcamera_sim/event_simulator.py index 10619524cf..3156bb7ef6 100644 --- a/PythonClient/eventcamera_sim/event_simulator.py +++ b/PythonClient/eventcamera_sim/event_simulator.py @@ -35,7 +35,7 @@ def esim( n_pix_row, ): count = 0 - maxSpikes = int(delta_time / (refractory_period_ns * 1e-3)) + max_spikes = int(delta_time / (refractory_period_ns * 1e-3)) for x in prange(x_end): itdt = np.log(current_image[x]) it = np.log(previous_image[x]) @@ -46,34 +46,34 @@ def esim( pol = np.sign(deltaL) - crossUpdate = pol * TOL - crossings[x] = np.log(crossings[x]) + crossUpdate + cross_update = pol * TOL + crossings[x] = np.log(crossings[x]) + cross_update lb = crossings[x] - it ub = crossings[x] - itdt - posCheck = lb > 0 and (pol == 1) and ub < 0 - negCheck = lb < 0 and (pol == -1) and ub > 0 + pos_check = lb > 0 and (pol == 1) and ub < 0 + neg_check = lb < 0 and (pol == -1) and ub > 0 - spikeNums = (itdt - crossings[x]) / TOL - crossCheck = posCheck + negCheck - spikeNums = np.abs(int(spikeNums * crossCheck)) + spike_nums = (itdt - crossings[x]) / TOL + cross_check = pos_check + neg_check + spike_nums = np.abs(int(spike_nums * cross_check)) - crossings[x] = itdt - crossUpdate - if spikeNums > 0: + crossings[x] = itdt - cross_update + if spike_nums > 0: spikes[x] = pol - spikeNums = maxSpikes if spikeNums > maxSpikes else spikeNums + spike_nums = max_spikes if spike_nums > max_spikes else spike_nums current_time = last_time - for i in range(spikeNums): + for i in range(spike_nums): output_events[count].x = x // n_pix_row output_events[count].y = x % n_pix_row output_events[count].timestamp = np.round(current_time * 1e-6, 6) output_events[count].polarity = 1 if pol > 0 else -1 count += 1 - current_time += (delta_time) / spikeNums + current_time += (delta_time) / spike_nums if count == max_events_per_frame: return count