From 00efefbabf5a124f14975a2e309a60f9dc60869a Mon Sep 17 00:00:00 2001 From: Samuel Date: Mon, 14 Oct 2024 22:59:13 +0100 Subject: [PATCH 01/26] Simplifying to processify the detector removed width and height adjustment removed motion detection - needs re-adding Things are currently a bit broken --- marimapper/camera.py | 10 +- marimapper/detector.py | 201 +++++++++++++++++++++++++ marimapper/led_identifier.py | 53 ------- marimapper/led_map_2d.py | 11 +- marimapper/reconstructor.py | 163 -------------------- marimapper/scanner.py | 84 +++-------- marimapper/scripts/check_camera_cli.py | 13 +- marimapper/utils.py | 13 +- pyproject.toml | 1 - pytest.ini | 2 - test/test_camera.py | 10 +- test/test_capture_sequence.py | 9 +- test/test_led_identifier.py | 8 +- test/test_reconstruction.py | 8 - 14 files changed, 254 insertions(+), 332 deletions(-) create mode 100644 marimapper/detector.py delete mode 100644 marimapper/led_identifier.py delete mode 100644 marimapper/reconstructor.py delete mode 100644 pytest.ini diff --git a/marimapper/camera.py b/marimapper/camera.py index 338c3f9..d771295 100644 --- a/marimapper/camera.py +++ b/marimapper/camera.py @@ -40,6 +40,11 @@ def __init__(self, device_id): self.set_resolution(self.get_width(), self.get_height()) # Don't ask + self.default_settings = CameraSettings(self) + + def reset(self): + self.default_settings.apply(self) + def get_width(self): return int(self.device.get(cv2.CAP_PROP_FRAME_WIDTH)) @@ -120,7 +125,4 @@ def read(self, color=False): logging.error("Failed to grab frame") return None - if not color: - return cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) - else: - return image + return image diff --git a/marimapper/detector.py b/marimapper/detector.py new file mode 100644 index 0000000..f1c8305 --- /dev/null +++ b/marimapper/detector.py @@ -0,0 +1,201 @@ +import cv2 +import time +import math +from multiprocessing import Process, Event, Queue + +from marimapper.camera import Camera +from marimapper.timeout_controller import TimeoutController +from marimapper.led_map_2d import LEDDetection + + +def find_led_in_image(image, led_id=-1, threshold=128): + + if len(image.shape) > 2: + image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + + _, image_thresh = cv2.threshold(image, threshold, 255, cv2.THRESH_TOZERO) + + contours, _ = cv2.findContours(image_thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + + led_response_count = len(contours) + if led_response_count == 0: + return LEDDetection(valid=False) + + moments = cv2.moments(image_thresh) + + img_height = image.shape[0] + img_width = image.shape[1] + + center_u = moments["m10"] / max(moments["m00"], 0.00001) + center_v = moments["m01"] / max(moments["m00"], 0.00001) + + center_u = center_u / img_width + v_offset = (img_width - img_height) / 2.0 + center_v = (center_v + v_offset) / img_width + + return LEDDetection(led_id, center_u, center_v, contours) + + +def draw_led_detections(image, results): + + render_image = ( + image if len(image.shape) == 3 else cv2.cvtColor(image, cv2.COLOR_GRAY2BGR) + ) + + img_height = render_image.shape[0] + img_width = render_image.shape[1] + + if results.valid: + cv2.drawContours(render_image, results.contours, -1, (255, 0, 0), 1) + + u_abs = int(results.u * img_width) + + v_offset = (img_width - img_height) / 2.0 + + v_abs = int(results.v * img_width - v_offset) + + cv2.drawMarker( + render_image, + (u_abs, v_abs), + (0, 255, 0), + markerSize=100, + ) + + return render_image + + +class Detector(Process): + + def __init__(self, device, dark_exposure, threshold, led_backend, display=True): + super().__init__() + + self.exit_event = Event() + self.detection_request = Queue() + self.detection_result = Queue() + + self.device = device + + self.led_backend = led_backend + self.display = display + + self.dark_exposure = dark_exposure + + self.threshold = threshold + self.timeout_controller = TimeoutController() + + self.cam_state = "default" + + def __del__(self): + self.close() + + @staticmethod + def show_image(image): + cv2.imshow("MariMapper", image) + cv2.waitKey(1) + + def run(self): + + cam = Camera(self.device) + + while not self.exit_event.is_set(): + + if not self.detection_request.empty(): + self.set_cam_dark(cam) + led_id = self.detection_request.get() + result = self.enable_and_find_led(led_id) + if result is not None: + self.detection_result.put(result) + else: + self.set_cam_default(cam) + image = cam.read() + self.show_image(image) + + # if we close the window + if cv2.getWindowProperty("MariMapper", cv2.WND_PROP_VISIBLE) <= 0: + self.exit_event.set() + continue + + cv2.destroyAllWindows() + self.set_cam_default(cam) + + def shutdown(self): + self.exit_event.set() + + def set_cam_default(self, cam): + if self.cam_state != "default": + cam.reset() + cam.eat() + self.cam_state = "default" + + def set_cam_dark(self, cam): + if self.cam_state != "dark": + cam.set_autofocus(0, 0) + cam.set_exposure_mode(0) + cam.set_gain(0) + cam.set_exposure(self.dark_exposure) + cam.eat() + self.cam_state = "dark" + + def find_led(self, cam, led_id=-1): + + image = cam.read() + results = find_led_in_image(image, led_id, self.threshold) + + if self.display: + rendered_image = draw_led_detections(image, results) + self.show_image(rendered_image) + + return results + + def enable_and_find_led(self, cam, led_id): + + if self.led_backend is None: + return None + + # First wait for no leds to be visible + while self.find_led(cam) is not None: + pass + + # Set the led to on and start the clock + response_time_start = time.time() + + if led_id != -1: + self.led_backend.set_led(led_id, True) + + # Wait until either we have a result or we run out of time + result = None + while ( + result is None + and time.time() < response_time_start + self.timeout_controller.timeout + ): + result = self.find_led(cam) + + self.led_backend.set_led(led_id, False) + + if result is None: + return None + + self.timeout_controller.add_response_time(time.time() - response_time_start) + + while self.find_led(cam) is not None: + pass + + return result + + def get_camera_motion(self, valid_leds, map_data_2d): + + if len(valid_leds) == 0: + return 0 + + for led_id in valid_leds: + detection_new = self.enable_and_find_led(cam, led_id) + if detection_new: + detection_orig = map_data_2d.get_detection(led_id) + + distance_between_first_and_last = math.hypot( + detection_orig.u - detection_new.u, + detection_orig.v - detection_new.v, + ) + return distance_between_first_and_last * 100 + + return 100 diff --git a/marimapper/led_identifier.py b/marimapper/led_identifier.py deleted file mode 100644 index 1ea2372..0000000 --- a/marimapper/led_identifier.py +++ /dev/null @@ -1,53 +0,0 @@ -import cv2 -from marimapper.led_map_2d import LEDDetection - - -def find_led_in_image(image, threshold=128): - - _, image_thresh = cv2.threshold(image, threshold, 255, cv2.THRESH_TOZERO) - - contours, _ = cv2.findContours(image_thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) - - led_response_count = len(contours) - if led_response_count == 0: - return None - - moments = cv2.moments(image_thresh) - - img_height = image.shape[0] - img_width = image.shape[1] - - center_u = moments["m10"] / max(moments["m00"], 0.00001) - center_v = moments["m01"] / max(moments["m00"], 0.00001) - - center_u = center_u / img_width - v_offset = (img_width - img_height) / 2.0 - center_v = (center_v + v_offset) / img_width - - return LEDDetection(center_u, center_v, contours) - - -def draw_led_detections(image, results): - - render_image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR) - - img_height = render_image.shape[0] - img_width = render_image.shape[1] - - if results: - cv2.drawContours(render_image, results.contours, -1, (255, 0, 0), 1) - - u_abs = int(results.u * img_width) - - v_offset = (img_width - img_height) / 2.0 - - v_abs = int(results.v * img_width - v_offset) - - cv2.drawMarker( - render_image, - (u_abs, v_abs), - (0, 255, 0), - markerSize=100, - ) - - return render_image diff --git a/marimapper/led_map_2d.py b/marimapper/led_map_2d.py index 1d9f3e3..8f3bb91 100644 --- a/marimapper/led_map_2d.py +++ b/marimapper/led_map_2d.py @@ -4,10 +4,12 @@ class LEDDetection: - def __init__(self, u, v, contours=()): + def __init__(self, led_id=-1, u=0.0, v=0.0, contours=(), valid=True): + self.led_id = led_id self.u = u self.v = v self.contours = contours + self.valid = valid def pos(self): return self.u, self.v @@ -48,7 +50,7 @@ def _load(self, filename): logging.warn(f"Failed to read line {i} of {filename}: {line}") continue - self.add_detection(index, LEDDetection(u, v)) + self.add_detection(LEDDetection(index, u, v)) return True @@ -64,8 +66,9 @@ def get_detections(self): def get_detection(self, led_id): return self._detections[led_id] - def add_detection(self, led_id: int, detection: LEDDetection): - self._detections[led_id] = detection + def add_detection(self, detection: LEDDetection): + if detection.valid: + self._detections[detection.led_id] = detection def write_to_file(self, filename): diff --git a/marimapper/reconstructor.py b/marimapper/reconstructor.py deleted file mode 100644 index 84b1fec..0000000 --- a/marimapper/reconstructor.py +++ /dev/null @@ -1,163 +0,0 @@ -import cv2 -import time -import math -from threading import Thread - -from marimapper.camera import Camera, CameraSettings -from marimapper.led_identifier import find_led_in_image, draw_led_detections -from marimapper.timeout_controller import TimeoutController - - -class Reconstructor: - - def __init__( - self, - device, - dark_exposure, - threshold, - led_backend, - width=-1, - height=-1, - ): - self.cam = Camera(device) - self.settings_light = CameraSettings(self.cam) - self.led_backend = led_backend - - self.dark_exposure = dark_exposure - self.light_exposure = self.cam.get_exposure() - - self.threshold = threshold - self.timeout_controller = TimeoutController() - - if width != -1 and height != -1: - self.cam.set_resolution(width, height) - - self.cam.set_autofocus(0, 0) - self.cam.set_exposure_mode(0) - self.cam.set_gain(0) - - self.settings_dark = CameraSettings(self.cam) - - self.live_feed = None - self.live_feed_running = False - - def __del__(self): - self.close() - - def close(self): - self.close_live_feed() - cv2.destroyAllWindows() - - self.light() - - def light(self): - self.settings_light.apply(self.cam) - self.cam.eat() - - def dark(self): - self.settings_dark.apply(self.cam) - self.cam.eat() - - def open_live_feed(self): - cv2.destroyAllWindows() - self.live_feed_running = True - self.live_feed = Thread(target=self._live_thread_loop) - self.live_feed.start() - - def close_live_feed(self): - self.live_feed_running = False - if self.live_feed is not None: - if self.live_feed.is_alive(): - self.live_feed.join() - - def _live_thread_loop(self): - - cv2.namedWindow("MariMapper", cv2.WINDOW_AUTOSIZE) - - while self.live_feed_running: - - if cv2.getWindowProperty("MariMapper", cv2.WND_PROP_VISIBLE) <= 0: - self.live_feed_running = False - - image = self.cam.read(color=True) - cv2.imshow("MariMapper", image) - cv2.waitKey(1) - - cv2.destroyAllWindows() - - def show_debug(self): - - self.dark() - - cv2.namedWindow("MariMapper", cv2.WINDOW_AUTOSIZE) - - while True: - - if cv2.getWindowProperty("MariMapper", cv2.WND_PROP_VISIBLE) <= 0: - break - - self.find_led(debug=True) - - def find_led(self, debug=False): - - image = self.cam.read() - results = find_led_in_image(image, self.threshold) - - if debug: - rendered_image = draw_led_detections(image, results) - cv2.imshow("MariMapper", rendered_image) - cv2.waitKey(1) - - return results - - def enable_and_find_led(self, led_id, debug=False): - - if self.led_backend is None: - return None - - # First wait for no leds to be visible - while self.find_led(debug) is not None: - pass - - # Set the led to on and start the clock - response_time_start = time.time() - - self.led_backend.set_led(led_id, True) - - # Wait until either we have a result or we run out of time - result = None - while ( - result is None - and time.time() < response_time_start + self.timeout_controller.timeout - ): - result = self.find_led(debug) - - self.led_backend.set_led(led_id, False) - - if result is None: - return None - - self.timeout_controller.add_response_time(time.time() - response_time_start) - - while self.find_led(debug) is not None: - pass - - return result - - def get_camera_motion(self, valid_leds, map_data_2d): - - if len(valid_leds) == 0: - return 0 - - for led_id in valid_leds: - detection_new = self.enable_and_find_led(led_id, debug=True) - if detection_new: - detection_orig = map_data_2d.get_detection(led_id) - - distance_between_first_and_last = math.hypot( - detection_orig.u - detection_new.u, - detection_orig.v - detection_new.v, - ) - return distance_between_first_and_last * 100 - - return 100 diff --git a/marimapper/scanner.py b/marimapper/scanner.py index bcb8f2a..8ba9c10 100644 --- a/marimapper/scanner.py +++ b/marimapper/scanner.py @@ -3,7 +3,7 @@ from tqdm import tqdm from pathlib import Path -from marimapper.reconstructor import Reconstructor +from marimapper.detector import Detector from marimapper import utils from marimapper import logging from marimapper.utils import get_user_confirmation @@ -27,13 +27,8 @@ def __init__(self, cli_args): self.led_map_2d_queue = Queue() self.led_map_3d_queue = Queue() - self.reconstructor = Reconstructor( - cli_args.device, - cli_args.exposure, - cli_args.threshold, - self.led_backend, - width=cli_args.width, - height=cli_args.height, + self.detector = Detector( + cli_args.device, cli_args.exposure, cli_args.threshold, self.led_backend ) self.renderer3d = Renderer3D(led_map_3d_queue=self.led_map_3d_queue) @@ -52,29 +47,29 @@ def __init__(self, cli_args): self.sfm.start() self.renderer3d.start() + self.detector.start() def close(self): logging.debug("marimapper closing") self.sfm.shutdown() self.renderer3d.shutdown() + self.detector.shutdown() + self.sfm.join() self.renderer3d.join() + self.detector.join() + self.sfm.terminate() self.renderer3d.terminate() - self.reconstructor.close() + self.detector.terminate() logging.debug("marimapper closed") def mainloop(self): while True: - self.reconstructor.light() - self.reconstructor.open_live_feed() - start_scan = get_user_confirmation("Start scan? [y/n]: ") - self.reconstructor.close_live_feed() - if not start_scan: return @@ -84,10 +79,9 @@ def mainloop(self): ) return - self.reconstructor.dark() - - result = self.reconstructor.find_led(debug=True) - if result is not None: + self.detector.detection_request.put(-1) + result = self.detector.detection_result.get() + if result.valid(): logging.error( f"All LEDs should be off, but the detector found one at {result.pos()}" ) @@ -100,53 +94,23 @@ def mainloop(self): led_map_2d = LEDMap2D() - total_leds_found = 0 + for led_id in self.led_id_range: + self.detector.detection_request.put(led_id) - visible_leds = [] - - last_camera_motion_check_time = time.time() - camera_motion_interval_sec = 5 - - capture_success = True - - for led_id in tqdm( + for _ in tqdm( self.led_id_range, unit="LEDs", desc=f"Capturing sequence to {filepath}", total=self.led_id_range.stop, smoothing=0, ): + result = self.detector.detection_result.get(timeout=10) + print(f"found {result}") + + led_map_2d.add_detection(result) + + led_map_2d.write_to_file(filepath) - result = self.reconstructor.enable_and_find_led(led_id, debug=True) - - if result: - visible_leds.append(led_id) - led_map_2d.add_detection(led_id, result) - total_leds_found += 1 - - is_last = led_id == self.led_id_range.stop - 1 - camera_motion_check_overdue = ( - time.time() - last_camera_motion_check_time - ) > camera_motion_interval_sec - - if camera_motion_check_overdue or is_last: - camera_motion = self.reconstructor.get_camera_motion( - visible_leds, led_map_2d - ) - last_camera_motion_check_time = time.time() - - if camera_motion > 1.0: - logging.warn(f"\nCamera moved by {int(camera_motion)}%") - if not get_user_confirmation("Continue? [y/n]: "): - capture_success = False - break - - if capture_success: - led_map_2d.write_to_file(filepath) - logging.info(f"{total_leds_found}/{self.led_id_range.stop} leds found") - - self.led_maps_2d.append(led_map_2d) - self.sfm.add_led_maps_2d(self.led_maps_2d) - self.sfm.reload() - else: - logging.error("Capture failed") + self.led_maps_2d.append(led_map_2d) + self.sfm.add_led_maps_2d(self.led_maps_2d) + self.sfm.reload() diff --git a/marimapper/scripts/check_camera_cli.py b/marimapper/scripts/check_camera_cli.py index be1e2a9..7ab55d6 100644 --- a/marimapper/scripts/check_camera_cli.py +++ b/marimapper/scripts/check_camera_cli.py @@ -1,6 +1,6 @@ import argparse -from marimapper.reconstructor import Reconstructor +from marimapper.detector import Detector from marimapper.utils import add_camera_args from marimapper import logging @@ -21,19 +21,12 @@ def main(): ) quit() - reconstructor = Reconstructor( - args.device, - args.exposure, - args.threshold, - None, - width=args.width, - height=args.height, - ) + detector = Detector(args.device, args.exposure, args.threshold, None) logging.info( "Camera connected! Hold an LED up to the camera to check LED identification" ) - reconstructor.show_debug() + detector.show_debug() # this no longer works! if __name__ == "__main__": diff --git a/marimapper/utils.py b/marimapper/utils.py index 8f87592..7519190 100644 --- a/marimapper/utils.py +++ b/marimapper/utils.py @@ -14,18 +14,7 @@ def add_camera_args(parser): help="Camera device index, set to 1 if using a laptop with a USB webcam", default=0, ) - parser.add_argument( - "--width", - type=int, - help="Camera width, usually uses 640 by default", - default=-1, - ) - parser.add_argument( - "--height", - type=int, - help="Camera height, usually uses 480 by default", - default=-1, - ) + parser.add_argument( "--exposure", type=int, diff --git a/pyproject.toml b/pyproject.toml index 7242f51..cbd2b2b 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -39,7 +39,6 @@ license = {file = "LICENSE"} develop = [ "pytest", "pytest-cov", - "pytest-mock", "black", "flake8", "flake8-bugbear" diff --git a/pytest.ini b/pytest.ini deleted file mode 100644 index 54fc9ba..0000000 --- a/pytest.ini +++ /dev/null @@ -1,2 +0,0 @@ -[pytest] -testpaths = test \ No newline at end of file diff --git a/test/test_camera.py b/test/test_camera.py index a138903..0de6535 100644 --- a/test/test_camera.py +++ b/test/test_camera.py @@ -8,13 +8,9 @@ def test_valid_device(): image = cam.read() - assert image.shape == (480, 640) # Grey - - image_colour = cam.read(color=True) - - assert ( - image_colour.shape[2] >= 3 - ) # sometimes there are 3 channels, sometimes 4 depending on platform? + assert image.shape[0] == 480 + assert image.shape[1] == 640 + assert image.shape[2] >= 3 def test_invalid_device(): diff --git a/test/test_capture_sequence.py b/test/test_capture_sequence.py index 10d744b..d7ae454 100644 --- a/test/test_capture_sequence.py +++ b/test/test_capture_sequence.py @@ -1,6 +1,6 @@ import os -from marimapper.reconstructor import Reconstructor +from marimapper.detector import Detector from marimapper.led_map_2d import LEDMap2D @@ -11,21 +11,22 @@ def test_capture_sequence(): for view_index in range(9): - reconstructor = Reconstructor( + detector = Detector( device=f"test/MariMapper-Test-Data/9_point_box/cam_{view_index}/capture_%04d.png", dark_exposure=-10, threshold=128, led_backend=None, + display=False, ) led_map_2d = LEDMap2D() for led_id in range(24): - result = reconstructor.find_led(False) + result = detector.find_led(led_id) if result: - led_map_2d.add_detection(led_id, result) + led_map_2d.add_detection(result) filepath = os.path.join(output_dir_full, f"led_map_2d_{view_index:04}.csv") diff --git a/test/test_led_identifier.py b/test/test_led_identifier.py index d2e4f63..71900f9 100644 --- a/test/test_led_identifier.py +++ b/test/test_led_identifier.py @@ -1,6 +1,6 @@ import pytest -from marimapper.led_identifier import find_led_in_image, draw_led_detections +from marimapper.detector import find_led_in_image, draw_led_detections from marimapper.camera import Camera @@ -8,7 +8,7 @@ def test_basic_image_loading(): mock_camera = Camera("test/MariMapper-Test-Data/9_point_box/cam_0/capture_0000.png") - detection = find_led_in_image(mock_camera.read(color=False)) + detection = find_led_in_image(mock_camera.read()) assert detection.u == pytest.approx(0.4029418361244019) assert detection.v == pytest.approx(0.4029538809144072) @@ -18,10 +18,10 @@ def test_none_found(): mock_camera = Camera("test/MariMapper-Test-Data/9_point_box/cam_0/capture_%04d.png") for frame_id in range(24): - frame = mock_camera.read(color=False) + frame = mock_camera.read() if frame_id in [7, 15, 23]: led_results = find_led_in_image(frame) - assert led_results is None + assert not led_results.valid def test_draw_results(): diff --git a/test/test_reconstruction.py b/test/test_reconstruction.py index 0598f0a..2e06d07 100644 --- a/test/test_reconstruction.py +++ b/test/test_reconstruction.py @@ -82,14 +82,6 @@ def test_invalid_reconstruction_views(): assert map_3d is None -def test_reconstruct_higbeam(): - highbeam_map = get_all_2d_led_maps("test/MariMapper-Test-Data/highbeam") - - map_3d = SFM.process__(highbeam_map) - - assert map_3d is not None - - # this test does a re-scale, but should keep the dimensions about the same def test_rescale(): maps = get_all_2d_led_maps("test/scan") From 9a5b7ba42e6eaa171a87de85917dec9f6bb5ff64 Mon Sep 17 00:00:00 2001 From: Samuel Date: Fri, 18 Oct 2024 14:20:32 +0100 Subject: [PATCH 02/26] pytest passes, that's a start. Lots of stuff stripped: - camera width and height - view 2d map - rescale - connected - interpolate - changed paths relative to test dir - added typing - added sfm process - Added dummy backend - Added detector process - Added camera state --- marimapper/backends/dummy/__init__.py | 0 marimapper/backends/dummy/dummy_backend.py | 10 + marimapper/camera.py | 2 + marimapper/database_populator.py | 27 +-- marimapper/detector.py | 208 ++++++++------------ marimapper/detector_process.py | 79 ++++++++ marimapper/file_tools.py | 86 +++++++++ marimapper/led.py | 210 +++++++++++++++++++++ marimapper/led_map_2d.py | 96 ---------- marimapper/led_map_3d.py | 97 ---------- marimapper/map_cleaner.py | 74 -------- marimapper/model.py | 103 +++------- marimapper/scanner.py | 53 +++--- marimapper/scripts/check_camera_cli.py | 17 +- marimapper/scripts/view_2d_map_cli.py | 48 ----- marimapper/sfm.py | 132 +++---------- marimapper/sfm_process.py | 49 +++++ marimapper/utils.py | 9 +- test/test_2d_map.py | 20 +- test/test_3d_map.py | 19 +- test/test_backend.py | 4 +- test/test_camera.py | 2 +- test/test_capture_sequence.py | 33 ++-- test/test_led_identifier.py | 18 +- test/test_reconstruction.py | 88 +++------ test/test_script_import.py | 7 - 26 files changed, 693 insertions(+), 798 deletions(-) create mode 100644 marimapper/backends/dummy/__init__.py create mode 100644 marimapper/backends/dummy/dummy_backend.py create mode 100644 marimapper/detector_process.py create mode 100644 marimapper/file_tools.py create mode 100644 marimapper/led.py delete mode 100644 marimapper/led_map_2d.py delete mode 100644 marimapper/led_map_3d.py delete mode 100644 marimapper/map_cleaner.py delete mode 100644 marimapper/scripts/view_2d_map_cli.py create mode 100644 marimapper/sfm_process.py diff --git a/marimapper/backends/dummy/__init__.py b/marimapper/backends/dummy/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/marimapper/backends/dummy/dummy_backend.py b/marimapper/backends/dummy/dummy_backend.py new file mode 100644 index 0000000..dfe70f8 --- /dev/null +++ b/marimapper/backends/dummy/dummy_backend.py @@ -0,0 +1,10 @@ +class Backend: + + def __init__(self): + pass + + def get_led_count(self): + return 1 + + def set_led(self, led_index: int, on: bool): + pass diff --git a/marimapper/camera.py b/marimapper/camera.py index d771295..24b6d88 100644 --- a/marimapper/camera.py +++ b/marimapper/camera.py @@ -42,6 +42,8 @@ def __init__(self, device_id): self.default_settings = CameraSettings(self) + self.state = "default" + def reset(self): self.default_settings.apply(self) diff --git a/marimapper/database_populator.py b/marimapper/database_populator.py index ef068f5..17bd59e 100644 --- a/marimapper/database_populator.py +++ b/marimapper/database_populator.py @@ -1,26 +1,29 @@ from itertools import combinations from math import radians, tan +import os import numpy as np from marimapper.pycolmap_tools.database import COLMAPDatabase +from marimapper.led import LED2D, get_view_ids, get_leds_with_view +ARBITRARY_SCALE = 2000 -def populate(db_path, led_maps_2d): - map_features = np.zeros((len(led_maps_2d), 1, 2)) +def populate_database(db_path: os.path, leds: list[LED2D]): - for map_index, led_map_2d in enumerate(led_maps_2d): + views = get_view_ids(leds) + map_features = np.zeros((max(views)+1, 1, 2)) - for led_index in led_map_2d.led_indexes(): + for view in views: - pad_needed = led_index - map_features.shape[1] + 1 + for led in get_leds_with_view(leds, view): + + pad_needed = led.led_id - map_features.shape[1] + 1 if pad_needed > 0: map_features = np.pad(map_features, [(0, 0), (0, pad_needed), (0, 0)]) - map_features[map_index][led_index] = ( - np.array(led_map_2d.get_detection(led_index).pos()) * 2000 - ) + map_features[view][led.led_id] = led.point.position * ARBITRARY_SCALE db = COLMAPDatabase.connect(db_path) @@ -29,8 +32,8 @@ def populate(db_path, led_maps_2d): # model=0 means that it's a "SIMPLE PINHOLE" with just 1 focal length parameter that I think should get optimised # the params here should be f, cx, cy - width = 2000 - height = 2000 + width = ARBITRARY_SCALE + height = ARBITRARY_SCALE fov = 60 # degrees, this gets optimised so doesn't //really// matter that much SIMPLE_PINHOLE = 0 @@ -45,13 +48,13 @@ def populate(db_path, led_maps_2d): # Create dummy images_all_the_same. - image_ids = [db.add_image(str(i), camera_id) for i in range(len(led_maps_2d))] + image_ids = [db.add_image(str(view), camera_id) for view in range(max(views)+1)] # Create some keypoints for i, keypoints in enumerate(map_features): db.add_keypoints(image_ids[i], keypoints) - for view_1_id, view_2_id in combinations(range(len(led_maps_2d)), 2): + for view_1_id, view_2_id in combinations(views, 2): view_1_keypoints = map_features[view_1_id] view_2_keypoints = map_features[view_2_id] diff --git a/marimapper/detector.py b/marimapper/detector.py index f1c8305..cf621db 100644 --- a/marimapper/detector.py +++ b/marimapper/detector.py @@ -1,14 +1,15 @@ import cv2 import time -import math -from multiprocessing import Process, Event, Queue +import typing from marimapper.camera import Camera from marimapper.timeout_controller import TimeoutController -from marimapper.led_map_2d import LEDDetection +from marimapper.led import Point2D, LED2D +DETECTOR_WINDOW_NAME = "MariMapper - Detector" -def find_led_in_image(image, led_id=-1, threshold=128): + +def find_led_in_image(image: cv2.Mat, threshold: int = 128) -> typing.Optional[Point2D]: if len(image.shape) > 2: image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) @@ -18,8 +19,9 @@ def find_led_in_image(image, led_id=-1, threshold=128): contours, _ = cv2.findContours(image_thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) led_response_count = len(contours) + if led_response_count == 0: - return LEDDetection(valid=False) + return None moments = cv2.moments(image_thresh) @@ -33,10 +35,12 @@ def find_led_in_image(image, led_id=-1, threshold=128): v_offset = (img_width - img_height) / 2.0 center_v = (center_v + v_offset) / img_width - return LEDDetection(led_id, center_u, center_v, contours) + brightness = 1.0 + + return Point2D(center_u, center_v, contours, brightness) # todo, normalise contours -def draw_led_detections(image, results): +def draw_led_detections(image: cv2.Mat, led_detection: Point2D) -> cv2.Mat: render_image = ( image if len(image.shape) == 3 else cv2.cvtColor(image, cv2.COLOR_GRAY2BGR) @@ -45,157 +49,99 @@ def draw_led_detections(image, results): img_height = render_image.shape[0] img_width = render_image.shape[1] - if results.valid: - cv2.drawContours(render_image, results.contours, -1, (255, 0, 0), 1) + cv2.drawContours( + render_image, led_detection.contours, -1, (255, 0, 0), 1 + ) # TODO, de-normalise contours once normalised - u_abs = int(results.u * img_width) + u_abs = int(led_detection.u() * img_width) - v_offset = (img_width - img_height) / 2.0 + v_offset = (img_width - img_height) / 2.0 - v_abs = int(results.v * img_width - v_offset) + v_abs = int(led_detection.v() * img_width - v_offset) - cv2.drawMarker( - render_image, - (u_abs, v_abs), - (0, 255, 0), - markerSize=100, - ) + cv2.drawMarker( + render_image, + (u_abs, v_abs), + (0, 255, 0), + markerSize=100, + ) return render_image -class Detector(Process): - - def __init__(self, device, dark_exposure, threshold, led_backend, display=True): - super().__init__() - - self.exit_event = Event() - self.detection_request = Queue() - self.detection_result = Queue() - - self.device = device - - self.led_backend = led_backend - self.display = display - - self.dark_exposure = dark_exposure - - self.threshold = threshold - self.timeout_controller = TimeoutController() - - self.cam_state = "default" - - def __del__(self): - self.close() - - @staticmethod - def show_image(image): - cv2.imshow("MariMapper", image) - cv2.waitKey(1) - - def run(self): - - cam = Camera(self.device) - - while not self.exit_event.is_set(): - - if not self.detection_request.empty(): - self.set_cam_dark(cam) - led_id = self.detection_request.get() - result = self.enable_and_find_led(led_id) - if result is not None: - self.detection_result.put(result) - else: - self.set_cam_default(cam) - image = cam.read() - self.show_image(image) - - # if we close the window - if cv2.getWindowProperty("MariMapper", cv2.WND_PROP_VISIBLE) <= 0: - self.exit_event.set() - continue - - cv2.destroyAllWindows() - self.set_cam_default(cam) - - def shutdown(self): - self.exit_event.set() +def show_image(image: cv2.Mat) -> None: + cv2.imshow(DETECTOR_WINDOW_NAME, image) + key = cv2.waitKey(1) - def set_cam_default(self, cam): - if self.cam_state != "default": - cam.reset() - cam.eat() - self.cam_state = "default" + if key == 27: # esc + raise KeyboardInterrupt - def set_cam_dark(self, cam): - if self.cam_state != "dark": - cam.set_autofocus(0, 0) - cam.set_exposure_mode(0) - cam.set_gain(0) - cam.set_exposure(self.dark_exposure) - cam.eat() - self.cam_state = "dark" - def find_led(self, cam, led_id=-1): +def set_cam_default(cam: Camera) -> None: + if cam.state != "default": + cam.reset() + cam.eat() + cam.state = "default" - image = cam.read() - results = find_led_in_image(image, led_id, self.threshold) - if self.display: - rendered_image = draw_led_detections(image, results) - self.show_image(rendered_image) +def set_cam_dark(cam: Camera, exposure: int) -> None: + if cam.state != "dark": + cam.set_autofocus(0, 0) + cam.set_exposure_mode(0) + cam.set_gain(0) + cam.set_exposure(exposure) + cam.eat() + cam.state = "dark" - return results - def enable_and_find_led(self, cam, led_id): +def find_led(cam: Camera, threshold: int = 128, display: bool = True) -> typing.Optional[Point2D]: - if self.led_backend is None: - return None + image = cam.read() + results = find_led_in_image(image, threshold) - # First wait for no leds to be visible - while self.find_led(cam) is not None: - pass + if display: + rendered_image = draw_led_detections(image, results) + show_image(rendered_image) - # Set the led to on and start the clock - response_time_start = time.time() + return results - if led_id != -1: - self.led_backend.set_led(led_id, True) - # Wait until either we have a result or we run out of time - result = None - while ( - result is None - and time.time() < response_time_start + self.timeout_controller.timeout - ): - result = self.find_led(cam) +def enable_and_find_led( + cam: Camera, + led_backend, + led_id: int, + view_id: int, + timeout_controller: TimeoutController, + threshold: int, +) -> typing.Optional[LED2D]: - self.led_backend.set_led(led_id, False) + if led_backend is None: + return None - if result is None: - return None + # First wait for no leds to be visible + while find_led(cam, threshold) is not None: + pass - self.timeout_controller.add_response_time(time.time() - response_time_start) + # Set the led to on and start the clock + response_time_start = time.time() - while self.find_led(cam) is not None: - pass + led_backend.set_led(led_id, True) - return result + # Wait until either we have a result or we run out of time + point = None + while ( + point is None and time.time() < response_time_start + timeout_controller.timeout + ): + point = find_led(cam, threshold) - def get_camera_motion(self, valid_leds, map_data_2d): + led_backend.set_led(led_id, False) - if len(valid_leds) == 0: - return 0 + if point is None: + return None - for led_id in valid_leds: - detection_new = self.enable_and_find_led(cam, led_id) - if detection_new: - detection_orig = map_data_2d.get_detection(led_id) + timeout_controller.add_response_time(time.time() - response_time_start) - distance_between_first_and_last = math.hypot( - detection_orig.u - detection_new.u, - detection_orig.v - detection_new.v, - ) - return distance_between_first_and_last * 100 + while find_led(cam, threshold) is not None: + pass - return 100 + return LED2D(led_id, view_id, point) diff --git a/marimapper/detector_process.py b/marimapper/detector_process.py new file mode 100644 index 0000000..efee859 --- /dev/null +++ b/marimapper/detector_process.py @@ -0,0 +1,79 @@ +from multiprocessing import Process, Event, Queue +from marimapper.detector import ( + show_image, + set_cam_default, + Camera, + TimeoutController, + set_cam_dark, + enable_and_find_led, + DETECTOR_WINDOW_NAME, +) +from marimapper.utils import get_backend +import cv2 + + +class DetectorProcess(Process): + + def __init__( + self, + device: str, + dark_exposure: int, + threshold: int, + led_backend_name: str, + led_backend_server: str, + display: bool = True, + ): + super().__init__() + + self.exit_event = Event() + self.detection_request = Queue() # {led_id, view_id} + self.detection_result = Queue() # LED3D + + self._device = device + self._dark_exposure = dark_exposure + self._threshold = threshold + self._led_backend_name = led_backend_name + self._led_backend_server = led_backend_server + self._display = display + + def __del__(self): + self.close() + + def run(self): + + led_backend = get_backend(self._led_backend_name, self._led_backend_server) + + cam = Camera(self._device) + + timeout_controller = TimeoutController() + + while not self.exit_event.is_set(): + + if not self.detection_request.empty(): + set_cam_dark(cam, self._dark_exposure) + led_id, view_id = self.detection_request.get() + result = enable_and_find_led( + cam, + led_backend, + led_id, + view_id, + timeout_controller, + self._threshold, + ) + if result is not None: + self.detection_result.put(result) + else: + set_cam_default(cam) + image = cam.read() + show_image(image) + + # if we close the window + if cv2.getWindowProperty(DETECTOR_WINDOW_NAME, cv2.WND_PROP_VISIBLE) <= 0: + self.exit_event.set() + continue + + cv2.destroyAllWindows() + set_cam_default(cam) + + def shutdown(self): + self.exit_event.set() diff --git a/marimapper/file_tools.py b/marimapper/file_tools.py new file mode 100644 index 0000000..9b26593 --- /dev/null +++ b/marimapper/file_tools.py @@ -0,0 +1,86 @@ +import os +from marimapper.led import Point2D, LED3D, LED2D +from marimapper import logging +import typing + + +def load_detections(filename: os.path, view_id) -> typing.Optional[list[LED2D]]: + + if not os.path.exists(filename): + return None + + if not filename.endswith(".csv"): + return None + + with open(filename, "r") as f: + lines = f.readlines() + + headings = lines[0].strip().split(",") + + if headings != ["index", "u", "v"]: + return None + + leds = [] + + for i in range(1, len(lines)): + + line = lines[i].strip().split(",") + + try: + index = int(line[0]) + u = float(line[1]) + v = float(line[2]) + except (IndexError, ValueError): + logging.warn(f"Failed to read line {i} of {filename}: {line}") + continue + + leds.append(LED2D(index, view_id, Point2D(u, v))) + + return leds + + +def get_all_2d_led_maps(directory: os.path) -> list[LED2D]: + points = [] + + for view_id, filename in enumerate(sorted(os.listdir(directory))): + full_path = os.path.join(directory, filename) + + detections = load_detections(full_path, view_id) # this is wrong + + if detections is not None: + points.extend(detections) + + return points + + +def write_3d_leds_to_file(leds: list[LED3D], filename: str): + logging.debug(f"Writing 3D map with {len(leds)} leds to {filename}...") + + lines = ["index,x,y,z,xn,yn,zn,error"] + + for led in sorted(leds, key=lambda led_t: led_t.led_id): + lines.append( + f"{led.led_id}," + f"{led.point.position[0]:f}," + f"{led.point.position[1]:f}," + f"{led.point.position[2]:f}," + f"{led.point.normal[0]:f}," + f"{led.point.normal[1]:f}," + f"{led.point.normal[2]:f}," + f"{led.point.error:f}" + ) + + with open(filename, "w") as f: + f.write("\n".join(lines)) + + +def write_2d_leds_to_file(leds: list[LED2D], filename: str): + + lines = ["index,u,v"] + + for led in sorted(leds, key=lambda led_t: led_t.led_id): + + lines.append(f"{led.led_id}," f"{led.point.u():f}," f"{led.point.v():f}") + + with open(filename, "w") as f: + f.write("\n".join(lines)) diff --git a/marimapper/led.py b/marimapper/led.py new file mode 100644 index 0000000..99e37ad --- /dev/null +++ b/marimapper/led.py @@ -0,0 +1,210 @@ +import numpy as np +import math +import typing + + +class Point2D: + def __init__(self, u, v, contours=(), brightness=1.0): + self.position: np.array = np.array([u, v]) + self.contours = contours + self.brightness: float = brightness + + def u(self): + return self.position[0] + + def v(self): + return self.position[1] + + +class LED2D: + def __init__(self, led_id: int, view_id: int, point: Point2D): + self.led_id: int = led_id + self.view_id: int = view_id + self.point: Point2D = point + + +class Point3D: + def __init__(self): + self.position = np.zeros(3) + self.normal = np.zeros(3) + self.error = 0.0 + + def __add__(self, other): + new = Point3D() + new.position = self.position + other.position + new.normal = self.normal + other.normal + new.error = self.error + other.error + return new + + def __sub__(self, other): + new = Point3D() + new.position = self.position - other.position + new.normal = self.normal - other.normal + new.error = self.error - other.error + return new + + def __mul__(self, other): + new = Point3D() + new.position = self.position * other.position + new.normal = self.normal * other.normal + new.error = self.error * other.error + return new + + +class LED3D: + def __init__(self, led_id): + self.led_id = led_id + self.point = Point3D() + self.views = [] + + +# returns none if there isn't that led in the list! +def get_led(leds: list[LED2D | LED3D], led_id: int) -> typing.Optional[LED2D | LED3D]: + for led in leds: + if led.led_id == led_id: + return led + return None + + +def get_leds(leds: list[LED2D | LED3D], led_id: int) -> list[LED2D | LED3D]: + return [led for led in leds if led.led_id == led_id] + + +# returns none if it's the end! +def get_next( + led_prev: LED2D | LED3D, leds: list[LED2D | LED3D] +) -> typing.Optional[LED2D | LED3D]: + + closest = None + for led in leds: + + if led.led_id > led_prev.led_id: + + if closest is None: + closest = led + else: + if led.led_id - led_prev.led_id < closest.led_id - led_prev.led_id: + closest = led + + return closest + + +def get_gap(led_a: LED2D | LED3D, led_b: LED2D | LED3D) -> int: + return abs(led_a.led_id - led_b.led_id) + + +def get_max_led_id(leds: list[LED2D | LED3D]) -> int: + return max([led.led_id for led in leds]) + + +def get_min_led_id(leds: list[LED2D | LED3D]) -> int: + return min([led.led_id for led in leds]) + + +def get_distance(led_a: LED2D | LED3D, led_b: LED2D | LED3D): + return math.hypot(*(led_a.point.position - led_b.point.position)) + + +def get_view_ids(leds: list[LED2D]) -> set[int]: + return set([led.view_id for led in leds]) + + +def get_leds_with_view(leds: list[LED2D], view_id: int) -> list[LED2D]: + return [led for led in leds if led.view_id == view_id] + +def last_view(leds:list[LED2D]): + return max([led.view_id for led in leds]) + +def find_inter_led_distance(leds: list[LED2D | LED3D]): + + distances = [] + + for led in leds: + next_led = get_next(led, leds) + if get_gap(led, next_led) == 0: + dist = get_distance(led, next_led) + distances.append(dist) + + return np.median(distances) + + +def fill_gap(start_led: LED3D, end_led: LED3D): + + total_missing_leds = end_led.led_id - start_led.led_id - 1 + + new_leds = [] + for led_offset in range(1, total_missing_leds + 1): + + new_led = LED3D(start_led.led_id + led_offset) + fraction = led_offset / (total_missing_leds + 1) + + new_led.position = start_led.point * (1 - fraction) + end_led.point * fraction + + new_leds.append(new_led) + + return new_leds + + +def fill_gaps(leds: list[LED3D], max_dist_err=0.2, max_missing=5) -> list[LED3D]: + + led_to_led_distance = find_inter_led_distance(leds) + min_distance = (1 - max_dist_err) * led_to_led_distance + max_distance = (1 + max_dist_err) * led_to_led_distance + + new_leds = [] + + for led in leds: + + next_led = get_next(led, leds) + gap = get_gap(led, next_led) + if gap == 0: + continue + + distance = get_distance(led, next_led) + + distance_per_led = distance / (gap + 1) + + if (min_distance < distance_per_led < max_distance) and gap < max_missing: + new_leds += fill_gap(led, next_led) + + return new_leds + + +def merge(leds: list[LED3D]) -> LED3D: + + # don't merge if it's a list of 1 + if len(leds) == 1: + return leds[0] + + # ensure they all have the same ID + assert all(led.led_id == leds[0].led_id for led in leds) + + new_led = LED3D(leds[0].led_id) + + new_led.views = [view for led in leds for view in led.views] + + new_led.point.position = np.average([led.point.position for led in leds], axis=0) + new_led.point.normal = np.average([led.point.normal for led in leds], axis=0) + new_led.point.error = sum([led.point.error for led in leds]) + + return new_led + + +def remove_duplicates(leds: list[LED3D]) -> list[LED3D]: + + new_leds = [] + + for led in leds: + leds_found = get_leds(new_leds, led.led_id) + if leds_found: + new_leds.append(merge(leds_found)) + else: + new_leds.append(led) + + return new_leds + +def get_leds_with_view(leds:list[LED2D], view_id) -> list[LED2D]: + return [led for led in leds if led.view_id == view_id] + +def get_leds_with_views(leds:list[LED2D], view_ids) -> list[LED2D]: + return [led for led in leds if led.view_id in view_ids] \ No newline at end of file diff --git a/marimapper/led_map_2d.py b/marimapper/led_map_2d.py deleted file mode 100644 index 8f3bb91..0000000 --- a/marimapper/led_map_2d.py +++ /dev/null @@ -1,96 +0,0 @@ -import os -from marimapper import logging - - -class LEDDetection: - - def __init__(self, led_id=-1, u=0.0, v=0.0, contours=(), valid=True): - self.led_id = led_id - self.u = u - self.v = v - self.contours = contours - self.valid = valid - - def pos(self): - return self.u, self.v - - -class LEDMap2D: - - def __init__(self, filepath=None): - self._detections = {} - self.valid = True - if filepath: - self.valid = self._load(filepath) - - def _load(self, filename): - if not os.path.exists(filename): - return False - - if not filename.endswith(".csv"): - return False - - with open(filename, "r") as f: - lines = f.readlines() - - headings = lines[0].strip().split(",") - - if headings != ["index", "u", "v"]: - return False - - for i in range(1, len(lines)): - - line = lines[i].strip().split(",") - - try: - index = int(line[0]) - u = float(line[1]) - v = float(line[2]) - except (IndexError, ValueError): - logging.warn(f"Failed to read line {i} of {filename}: {line}") - continue - - self.add_detection(LEDDetection(index, u, v)) - - return True - - def __len__(self): - return len(self._detections) - - def led_indexes(self): - return sorted(self._detections.keys()) - - def get_detections(self): - return self._detections - - def get_detection(self, led_id): - return self._detections[led_id] - - def add_detection(self, detection: LEDDetection): - if detection.valid: - self._detections[detection.led_id] = detection - - def write_to_file(self, filename): - - lines = ["index,u,v"] - - for led_id in sorted(self.get_detections().keys()): - detection = self.get_detection(led_id) - lines.append(f"{led_id}," f"{detection.u:f}," f"{detection.v:f}") - - with open(filename, "w") as f: - f.write("\n".join(lines)) - - -def get_all_2d_led_maps(directory): - led_maps_2d = [] - - for filename in sorted(os.listdir(directory)): - - full_path = os.path.join(directory, filename) - - led_map_2d = LEDMap2D(full_path) - if led_map_2d.valid: - led_maps_2d.append(led_map_2d) - - return led_maps_2d diff --git a/marimapper/led_map_3d.py b/marimapper/led_map_3d.py deleted file mode 100644 index b860428..0000000 --- a/marimapper/led_map_3d.py +++ /dev/null @@ -1,97 +0,0 @@ -import numpy as np -import math -from marimapper import logging - - -class LEDMap3D: - - def __init__(self, data=None): - - self.valid = True - self.data = {} - self.cameras = None - if data is not None: - self.data = data - - def __setitem__(self, led_index, led_data): # pragma: no cover - self.data[led_index] = led_data - - def __getitem__(self, led_index): # pragma: no cover - return self.data[led_index] - - def __contains__(self, led_index): # pragma: no cover - return led_index in self.data - - def __len__(self): - return len(self.data) - - def keys(self): - return sorted(list(self.data.keys())) - - def get_connected_leds(self, max_ratio=1.5): - connections = [] - - inter_led_distance = self.get_inter_led_distance() - - led_ids = self.keys() - - for led_index in range(len(led_ids)): - current_id = led_ids[led_index] - next_id = led_ids[led_index] + 1 - if next_id in led_ids: - if led_ids[led_index + 1] == next_id: - - distance = math.hypot( - *(self[current_id]["pos"] - self[next_id]["pos"]) - ) - if distance < inter_led_distance * max_ratio: - connections.append((led_index, led_index + 1)) - - return connections - - def rescale(self, target_inter_distance=1.0): - - scale = (1.0 / self.get_inter_led_distance()) * target_inter_distance - - for led_id in self.data: - self[led_id]["pos"] *= scale - self[led_id]["normal"] = self[led_id]["normal"] / np.linalg.norm( - self[led_id]["normal"] - ) - self[led_id]["normal"] *= target_inter_distance - self[led_id]["error"] *= scale - - for cam in self.cameras: - cam[1] *= scale - - def get_inter_led_distance(self): - max_led_id = max(self.keys()) - - distances = [] - - for led_id in range(max_led_id): - if led_id in self.keys() and led_id + 1 in self.keys(): - dist = math.hypot(*(self[led_id]["pos"] - self[led_id + 1]["pos"])) - distances.append(dist) - - return np.median(distances) - - def write_to_file(self, filename): - logging.debug(f"Writing 3D map with {len(self.data)} leds to {filename}...") - - lines = ["index,x,y,z,xn,yn,zn,error"] - - for led_id in sorted(self.data.keys()): - lines.append( - f"{led_id}," - f"{self.data[led_id]['pos'][0]:f}," - f"{self.data[led_id]['pos'][1]:f}," - f"{self.data[led_id]['pos'][2]:f}," - f"{self.data[led_id]['normal'][0]:f}," - f"{self.data[led_id]['normal'][1]:f}," - f"{self.data[led_id]['normal'][2]:f}," - f"{self.data[led_id]['error']:f}" - ) - - with open(filename, "w") as f: - f.write("\n".join(lines)) diff --git a/marimapper/map_cleaner.py b/marimapper/map_cleaner.py deleted file mode 100644 index 71217c2..0000000 --- a/marimapper/map_cleaner.py +++ /dev/null @@ -1,74 +0,0 @@ -import numpy as np -import math - - -def _distance_between_leds(led_a, led_b): - return math.hypot(*(led_a["pos"] - led_b["pos"])) - - -def _fill_gaps(led_map, start_led_id, end_led_id): - - missing_leds = end_led_id - start_led_id - 1 - - for i in range(1, missing_leds + 1): - led_map[start_led_id + i] = {} - - led_map[start_led_id + i]["pos"] = led_map[start_led_id]["pos"] + ( - led_map[end_led_id]["pos"] - led_map[start_led_id]["pos"] - ) * (i / (missing_leds + 1)) - led_map[start_led_id + i]["error"] = led_map[start_led_id]["error"] + ( - led_map[end_led_id]["error"] - led_map[start_led_id]["error"] - ) * (i / (missing_leds + 1)) - led_map[start_led_id + i]["normal"] = led_map[start_led_id]["normal"] + ( - led_map[end_led_id]["normal"] - led_map[start_led_id]["normal"] - ) * (i / (missing_leds + 1)) - - -def find_inter_led_distance(led_map): - max_led_id = max(led_map.keys()) - - distances = [] - - for led_id in range(max_led_id): - if led_id in led_map and led_id + 1 in led_map: - dist = _distance_between_leds(led_map[led_id], led_map[led_id + 1]) - distances.append(dist) - - return np.median(distances) - - -def fill_gaps(led_map, max_dist_err=0.2, max_missing=5): - - total_leds_filled = 0 - - led_to_led_distance = find_inter_led_distance(led_map) - min_distance = (1 - max_dist_err) * led_to_led_distance - max_distance = (1 + max_dist_err) * led_to_led_distance - - max_led_id = max(led_map.keys()) - min_led_id = min(led_map.keys()) - - led_id = min_led_id - while led_id < max_led_id: - - while led_id in led_map and led_id < max_led_id: - led_id += 1 - - # now we've hit a not-led - start = led_id - 1 - while led_id not in led_map and led_id < max_led_id: - led_id += 1 - - end = led_id - - leds_missing = end - start - 1 - - distance = _distance_between_leds(led_map[start], led_map[end]) - - c = distance / (leds_missing + 1) - - if (min_distance < c < max_distance) and leds_missing < max_missing: - _fill_gaps(led_map, start, end) - total_leds_filled += leds_missing - - return total_leds_filled diff --git a/marimapper/model.py b/marimapper/model.py index d735505..1154ace 100644 --- a/marimapper/model.py +++ b/marimapper/model.py @@ -1,109 +1,58 @@ import os -import numpy as np - from marimapper.pycolmap_tools.read_write_model import ( qvec2rotmat, read_images_binary, read_points3D_binary, ) -from marimapper.led_map_3d import LEDMap3D -import open3d -import math - - -def fix_normals(led_map): - - pcd = open3d.geometry.PointCloud() - - led_ids = list(led_map.keys()) +from marimapper.led import LED3D, remove_duplicates - xyz = [led_map[led_id]["pos"] for led_id in led_ids] - normals_from_camera = [led_map[led_id]["normal"] for led_id in led_ids] - pcd.points = open3d.utility.Vector3dVector(xyz) +def binary_to_led_map_3d(path: os.path) -> list[LED3D]: - pcd.normals = open3d.utility.Vector3dVector(np.zeros((len(pcd.normals), 3))) - - pcd.estimate_normals() # This needs to be written back to the led map somehow - - assert len(pcd.normals) == len(normals_from_camera) - - for i in range(len(normals_from_camera)): - normal_from_camera = normals_from_camera[i] / np.linalg.norm( - normals_from_camera[i] - ) - normal_from_estimator = pcd.normals[i] / np.linalg.norm(pcd.normals[i]) + points_bin = read_points3D_binary(os.path.join(path, "0", "points3D.bin")) - angle = np.arccos( - np.clip(np.dot(normal_from_camera, normal_from_estimator), -1.0, 1.0) - ) + leds: list[LED3D] = [] - led_map[led_ids[i]]["normal"] = pcd.normals[i] * ( - -1 if angle > math.pi / 2.0 else 1 - ) + for ( + led_data + ) in points_bin.values(): # this will overwrite previous data! needs filtering + led_id = led_data.point2D_idxs[0] - return led_map + led = LED3D(led_id) + led.point.position = led_data.xyz + led.point.error = led_data.error + led.views = led_data.image_ids -def binary_to_led_map_3d(path): - led_map = {} + leds.append(led) - points_bin = read_points3D_binary(os.path.join(path, "0", "points3D.bin")) + leds = remove_duplicates(leds) - for ( - point - ) in points_bin.values(): # this will overwrite previous data! needs filtering - led_id = point.point2D_idxs[0] - if led_id not in led_map: - led_map[point.point2D_idxs[0]] = {"pos": [], "error": [], "views": []} + return leds - led_map[point.point2D_idxs[0]]["pos"].append(point.xyz) - led_map[point.point2D_idxs[0]]["error"].append(point.error) - led_map[point.point2D_idxs[0]]["views"].extend(point.image_ids) - for led_id in led_map: - led_map[led_id]["pos"] = np.average(led_map[led_id]["pos"], axis=0) - led_map[led_id]["error"] = np.average(led_map[led_id]["error"], axis=0) - led_map[led_id]["views"] = list(set(led_map[led_id]["views"])) +class ReconstructedCamera: + def __init__(self, camera_id, translation, rotation): + self.camera_id = camera_id + self.rotation = rotation + self.translation = translation - translation_offset = np.average( - [led_map[led_id]["pos"] for led_id in led_map], axis=0 - ) - for led_id in led_map: - led_map[led_id]["pos"] -= translation_offset +def binary_to_cameras(path: os.path) -> list[ReconstructedCamera]: cameras = [] images_bin = read_images_binary(os.path.join(path, "0", "images.bin")) - camera_positions = {} - for img in images_bin.values(): - rotation = qvec2rotmat(img.qvec) - - translation = -rotation.T @ img.tvec - rotation = rotation.T - - translation -= translation_offset - - camera_positions[img.id] = translation - - cameras.append([rotation, translation]) - for led_id in led_map: - all_views = np.array( - [camera_positions[view] for view in led_map[led_id]["views"]] - ) - led_map[led_id]["normal"] = ( - np.average(all_views, axis=0) - led_map[led_id]["pos"] - ) + rotation = qvec2rotmat(img.qvec).T + translation = -rotation @ img.tvec - led_map[led_id]["normal"] /= np.linalg.norm(led_map[led_id]["normal"]) + camera = ReconstructedCamera(img.id, translation, rotation) - led_map_3d = LEDMap3D(fix_normals(led_map)) - led_map_3d.cameras = cameras + cameras.append(camera) - return led_map_3d + return cameras diff --git a/marimapper/scanner.py b/marimapper/scanner.py index 8ba9c10..50083b7 100644 --- a/marimapper/scanner.py +++ b/marimapper/scanner.py @@ -3,32 +3,32 @@ from tqdm import tqdm from pathlib import Path -from marimapper.detector import Detector -from marimapper import utils +from marimapper.detector_process import DetectorProcess from marimapper import logging +from marimapper.file_tools import get_all_2d_led_maps, write_2d_leds_to_file from marimapper.utils import get_user_confirmation -from marimapper.led_map_2d import LEDMap2D -from marimapper.sfm import SFM from marimapper.visualize_model import Renderer3D from multiprocessing import Queue -from marimapper.led_map_2d import get_all_2d_led_maps +from marimapper.sfm_process import SFM class Scanner: def __init__(self, cli_args): self.output_dir = cli_args.dir - self.led_backend = utils.get_backend(cli_args.backend, cli_args.server) - if self.led_backend is not None: - self.led_id_range = range( - cli_args.start, min(cli_args.end, self.led_backend.get_led_count()) - ) os.makedirs(self.output_dir, exist_ok=True) + + self.led_id_range = range(cli_args.start, cli_args.end) + self.led_map_2d_queue = Queue() self.led_map_3d_queue = Queue() - self.detector = Detector( - cli_args.device, cli_args.exposure, cli_args.threshold, self.led_backend + self.detector = DetectorProcess( + cli_args.device, + cli_args.exposure, + cli_args.threshold, + cli_args.backend, + cli_args.server, ) self.renderer3d = Renderer3D(led_map_3d_queue=self.led_map_3d_queue) @@ -41,9 +41,10 @@ def __init__(self, cli_args): led_map_3d_queue=self.led_map_3d_queue, ) - self.led_maps_2d = get_all_2d_led_maps(Path(self.output_dir)) + self.leds_2d = get_all_2d_led_maps(Path(self.output_dir)) - self.sfm.add_led_maps_2d(self.led_maps_2d) + for led in self.leds_2d: + self.led_map_2d_queue.put(led) self.sfm.start() self.renderer3d.start() @@ -73,12 +74,6 @@ def mainloop(self): if not start_scan: return - if self.led_backend is None: - logging.warn( - "Cannot start backend as no backend has been defined. Re-run marimapper with --backend " - ) - return - self.detector.detection_request.put(-1) result = self.detector.detection_result.get() if result.valid(): @@ -92,10 +87,12 @@ def mainloop(self): filepath = os.path.join(self.output_dir, f"led_map_2d_{string_time}.csv") - led_map_2d = LEDMap2D() + leds = [] + + view_id = 0 # change for led_id in self.led_id_range: - self.detector.detection_request.put(led_id) + self.detector.detection_request.put((view_id, led_id)) for _ in tqdm( self.led_id_range, @@ -104,13 +101,11 @@ def mainloop(self): total=self.led_id_range.stop, smoothing=0, ): - result = self.detector.detection_result.get(timeout=10) - print(f"found {result}") + led = self.detector.detection_result.get(timeout=10) + print(f"found {led}") - led_map_2d.add_detection(result) + leds.append(led) - led_map_2d.write_to_file(filepath) + self.led_map_2d_queue.put(led) - self.led_maps_2d.append(led_map_2d) - self.sfm.add_led_maps_2d(self.led_maps_2d) - self.sfm.reload() + write_2d_leds_to_file(leds, filepath) diff --git a/marimapper/scripts/check_camera_cli.py b/marimapper/scripts/check_camera_cli.py index 7ab55d6..df95d9e 100644 --- a/marimapper/scripts/check_camera_cli.py +++ b/marimapper/scripts/check_camera_cli.py @@ -1,9 +1,9 @@ import argparse -from marimapper.detector import Detector +from marimapper.detector import find_led, set_cam_dark from marimapper.utils import add_camera_args from marimapper import logging - +from marimapper.camera import Camera def main(): @@ -15,19 +15,18 @@ def main(): args = parser.parse_args() - if args.width * args.height < 0: - logging.error( - "Failed to start camera checker as both camera width and height need to be provided" - ) - quit() - detector = Detector(args.device, args.exposure, args.threshold, None) + cam = Camera(args.device) + + set_cam_dark(cam, args.exposure) + logging.info( "Camera connected! Hold an LED up to the camera to check LED identification" ) - detector.show_debug() # this no longer works! + while True: + find_led(cam, args.threshold) if __name__ == "__main__": main() diff --git a/marimapper/scripts/view_2d_map_cli.py b/marimapper/scripts/view_2d_map_cli.py deleted file mode 100644 index 609adb5..0000000 --- a/marimapper/scripts/view_2d_map_cli.py +++ /dev/null @@ -1,48 +0,0 @@ -import argparse -import numpy as np -import cv2 -import colorsys -from marimapper.led_map_2d import LEDMap2D - - -def render_2d_model(led_map): - display = np.ones((640, 640, 3)) * 0.2 - - max_id = max(led_map.get_detections().keys()) - - for led_id in led_map.get_detections(): - col = colorsys.hsv_to_rgb(led_id / max_id, 0.5, 1) - pos = np.array( - (led_map.get_detection(led_id).u, led_map.get_detection(led_id).v) - ) - image_point = (pos * 640).astype(int) - cv2.drawMarker(display, image_point, color=col) - cv2.putText( - display, - str(led_id), - image_point, - cv2.FONT_HERSHEY_SIMPLEX, - 1, - color=col, - ) - - cv2.imshow("MariMapper", display) - cv2.waitKey(0) - - -def main(): - parser = argparse.ArgumentParser(description="Visualises 2D maps") - - parser.add_argument( - "--filename", type=str, help="The 2d_map file to visualise", required=True - ) - - args = parser.parse_args() - - map_data = LEDMap2D(filepath=args.filename) - - render_2d_model(map_data) - - -if __name__ == "__main__": - main() diff --git a/marimapper/sfm.py b/marimapper/sfm.py index f6ed81c..3ac6782 100644 --- a/marimapper/sfm.py +++ b/marimapper/sfm.py @@ -1,124 +1,44 @@ import os from tempfile import TemporaryDirectory -from pathlib import Path import pycolmap -from multiprocessing import Process, Event -from marimapper.database_populator import populate +from marimapper.database_populator import populate_database from marimapper.model import binary_to_led_map_3d from marimapper.utils import SupressLogging -from marimapper import map_cleaner -from marimapper.led_map_2d import get_all_2d_led_maps from marimapper import logging -class SFM(Process): +def sfm(maps_2d): + logging.debug("SFM process starting sfm process") + if len(maps_2d) < 2: + logging.debug("SFM process failed to run sfm process as not enough maps") + return None - def __init__( - self, - directory: Path, - rescale=False, - interpolate=False, - event_on_update=None, - led_map_2d_queue=None, - led_map_3d_queue=None, - ): - logging.debug("SFM initialising") - super().__init__() - self.directory = directory - self.rescale = rescale - self.interpolate = interpolate - self.exit_event = Event() - self.reload_event = Event() - self.event_on_update = event_on_update - self.led_map_3d_queue = led_map_3d_queue - self.led_map_2d_queue = led_map_2d_queue + with TemporaryDirectory() as temp_dir: + database_path = os.path.join(temp_dir, "database.db") - self.led_map_2d_queue.put(get_all_2d_led_maps(self.directory)) + populate_database(database_path, maps_2d) - logging.debug("SFM initialised") + options = pycolmap.IncrementalPipelineOptions() + options.triangulation.ignore_two_view_tracks = False # used to be true + options.min_num_matches = 9 # default 15 + options.mapper.abs_pose_min_num_inliers = 9 # default 30 + options.mapper.init_min_num_inliers = 50 # used to be 100 - def add_led_maps_2d(self, maps): - self.led_map_2d_queue.put(maps) + with SupressLogging(): + pycolmap.incremental_mapping( + database_path=database_path, + image_path=temp_dir, + output_path=temp_dir, + options=options, + ) - def shutdown(self): - logging.debug("SFM sending shutdown request") - self.exit_event.set() - - def reload(self): - logging.debug("SFM sending reload request") - self.reload_event.set() - - def run(self): - logging.debug("SFM process starting") - self.reload__() - while not self.exit_event.is_set(): - reload = self.reload_event.wait(timeout=1) - if reload: - self.reload__() - - def reload__(self): - logging.debug("SFM process reloading") - maps_2d = get_all_2d_led_maps(self.directory) - if len(maps_2d) < 2: - self.reload_event.clear() - return None - - logging.debug(f"SFM process running on {len(maps_2d)} maps") - - map_3d = self.process__(maps_2d, self.rescale, self.interpolate) - if map_3d is None or len(map_3d.keys()) == 0: - self.reload_event.clear() - return None - - map_3d.write_to_file(self.directory / "led_map_3d.csv") - - self.event_on_update.set() - self.led_map_3d_queue.put(map_3d) - self.reload_event.clear() - logging.debug("SFM process reloaded") - - @staticmethod - def process__(maps_2d, rescale=False, interpolate=False): - logging.debug("SFM process starting sfm process") - if len(maps_2d) < 2: - logging.debug("SFM process failed to run sfm process as not enough maps") + if not os.path.exists(os.path.join(temp_dir, "0", "points3D.bin")): + logging.debug( + "SFM process failed to run sfm process as reconstruction failed" + ) return None - with TemporaryDirectory() as temp_dir: - database_path = os.path.join(temp_dir, "database.db") - - populate(database_path, maps_2d) - - options = pycolmap.IncrementalPipelineOptions() - options.triangulation.ignore_two_view_tracks = False # used to be true - options.min_num_matches = 9 # default 15 - options.mapper.abs_pose_min_num_inliers = 9 # default 30 - options.mapper.init_min_num_inliers = 50 # used to be 100 - - with SupressLogging(): - pycolmap.incremental_mapping( - database_path=database_path, - image_path=temp_dir, - output_path=temp_dir, - options=options, - ) - - if not os.path.exists(os.path.join(temp_dir, "0", "points3D.bin")): - logging.debug( - "SFM process failed to run sfm process as reconstruction failed" - ) - return None - - map_3d = binary_to_led_map_3d(temp_dir) - - if rescale: - map_3d.rescale() - - if interpolate: - leds_interpolated = map_cleaner.fill_gaps(map_3d) - logging.debug(f"Interpolated {leds_interpolated} leds") - logging.debug("SFM process finished sfm process") - return map_3d + return binary_to_led_map_3d(temp_dir) diff --git a/marimapper/sfm_process.py b/marimapper/sfm_process.py new file mode 100644 index 0000000..fbaa242 --- /dev/null +++ b/marimapper/sfm_process.py @@ -0,0 +1,49 @@ +from pathlib import Path +from multiprocessing import Process, Event + + +from marimapper import logging +from marimapper.sfm import sfm + + +class SFM(Process): + + def __init__( + self, + directory: Path, + rescale=False, + interpolate=False, + event_on_update=None, + led_map_2d_queue=None, + led_map_3d_queue=None, + ): + logging.debug("SFM initialising") + super().__init__() + self.directory = directory + self.rescale = rescale + self.interpolate = interpolate + self.exit_event = Event() + self.event_on_update = event_on_update + self.led_map_3d_queue = led_map_3d_queue + self.led_map_2d_queue = led_map_2d_queue + self.leds = [] + + logging.debug("SFM initialised") + + def add_led_maps_2d(self, maps): + self.led_map_2d_queue.put(maps) + + def shutdown(self): + logging.debug("SFM sending shutdown request") + self.exit_event.set() + + def run(self): + logging.debug("SFM process starting") + + while not self.exit_event.is_set(): + if self.led_map_2d_queue.empty(): + map_3d = sfm(self.leds) + self.led_map_3d_queue.put(map_3d) + else: + led = self.led_map_3d_queue.get() + self.leds.append(led) diff --git a/marimapper/utils.py b/marimapper/utils.py index 7519190..e8f62f6 100644 --- a/marimapper/utils.py +++ b/marimapper/utils.py @@ -40,11 +40,12 @@ def add_backend_args(parser): parser.add_argument( "--start", type=int, help="Index of the first led you want to scan", default=0 ) + parser.add_argument( "--end", type=int, help="Index of the last led you want to scan up to the backends limit", - default=10000, + default=100, ) parser.add_argument("--server", type=str, help="Some backends require a server") @@ -130,8 +131,10 @@ def get_backend(backend_name, server=""): if os.path.isfile(backend_name) and backend_name.endswith(".py"): return load_custom_backend(backend_name, server) - if backend_name == "None": - return None + if backend_name == "dummy": + from marimapper.backends.dummy import dummy_backend + + return dummy_backend.Backend() raise RuntimeError("Invalid backend name") diff --git a/test/test_2d_map.py b/test/test_2d_map.py index 17f01e4..349388f 100644 --- a/test/test_2d_map.py +++ b/test/test_2d_map.py @@ -1,5 +1,6 @@ import tempfile -from marimapper.led_map_2d import LEDMap2D, get_all_2d_led_maps +from marimapper.file_tools import load_detections, get_all_2d_led_maps +from marimapper.led import get_led def test_partially_valid_data(): @@ -14,21 +15,22 @@ def test_partially_valid_data(): ) temp_led_map_file.close() - led_map = LEDMap2D(filepath=temp_led_map_file.name) + led_map = load_detections(temp_led_map_file.name, 0) - assert led_map.valid + assert led_map is not None assert len(led_map) == 2 - assert led_map.get_detection(0).pos() == (0.379490, 0.407710) - - assert led_map.get_detection(2).pos() == (0, 0) + assert get_led(led_map, 0).point.position[0] == 0.379490 + assert get_led(led_map, 0).point.position[1] == 0.407710 + assert get_led(led_map, 2).point.position[0] == 0 + assert get_led(led_map, 2).point.position[1] == 0 def test_invalid_path(): - led_map = LEDMap2D(filepath="doesnt-exist-i-hope") - assert not led_map.valid + led_map = load_detections("doesnt-exist-i-hope", 0) + assert led_map is None def test_get_all_maps(): @@ -55,7 +57,7 @@ def test_get_all_maps(): ) temp_led_map_file_invalid.close() - all_maps = get_all_2d_led_maps(directory=directory.name) + all_maps = get_all_2d_led_maps(directory.name) assert len(all_maps) == 1 diff --git a/test/test_3d_map.py b/test/test_3d_map.py index d0827c6..23dc62c 100644 --- a/test/test_3d_map.py +++ b/test/test_3d_map.py @@ -1,5 +1,7 @@ import tempfile -from marimapper.led_map_3d import LEDMap3D +import numpy as np +from marimapper.led import LED3D +from marimapper.file_tools import write_3d_leds_to_file def test_file_write(): @@ -13,19 +15,14 @@ def test_file_write(): led_z_normal = 1 led_error = 1 - led_dict = { - "pos": (led_x, led_y, led_z), - "normal": (led_x_normal, led_y_normal, led_z_normal), - "error": led_error, - } - - led_map = LEDMap3D({led_id: led_dict}) - - assert led_map.keys() == [led_id] + led = LED3D(led_id) + led.point.position = np.array([led_x, led_y, led_z]) + led.point.normal = np.array([led_x_normal, led_y_normal, led_z_normal]) + led.point.error = led_error output_file = tempfile.NamedTemporaryFile(delete=False) - led_map.write_to_file(output_file.name) + write_3d_leds_to_file([led], output_file.name) with open(output_file.name) as f: lines = f.readlines() diff --git a/test/test_backend.py b/test/test_backend.py index 1ae760d..216a548 100644 --- a/test/test_backend.py +++ b/test/test_backend.py @@ -158,9 +158,7 @@ def getPixelCount(self): get_backend("pixelblaze", "1.2.3.4") -def test_invalid_or_none_backend(): - - assert get_backend("None") is None +def test_invalid_backend(): with pytest.raises(RuntimeError): get_backend("invalid_backend") diff --git a/test/test_camera.py b/test/test_camera.py index 0de6535..713af2d 100644 --- a/test/test_camera.py +++ b/test/test_camera.py @@ -4,7 +4,7 @@ def test_valid_device(): - cam = Camera("test/MariMapper-Test-Data/9_point_box/cam_0/capture_%04d.png") + cam = Camera("MariMapper-Test-Data/9_point_box/cam_0/capture_%04d.png") image = cam.read() diff --git a/test/test_capture_sequence.py b/test/test_capture_sequence.py index d7ae454..320a98c 100644 --- a/test/test_capture_sequence.py +++ b/test/test_capture_sequence.py @@ -1,7 +1,8 @@ import os - -from marimapper.detector import Detector -from marimapper.led_map_2d import LEDMap2D +from marimapper.camera import Camera +from marimapper.detector import find_led +from marimapper.file_tools import write_2d_leds_to_file, load_detections +from marimapper.led import LED2D def test_capture_sequence(): @@ -11,26 +12,22 @@ def test_capture_sequence(): for view_index in range(9): - detector = Detector( - device=f"test/MariMapper-Test-Data/9_point_box/cam_{view_index}/capture_%04d.png", - dark_exposure=-10, - threshold=128, - led_backend=None, - display=False, + cam = Camera( + f"MariMapper-Test-Data/9_point_box/cam_{view_index}/capture_%04d.png" ) - led_map_2d = LEDMap2D() + leds = [] for led_id in range(24): - result = detector.find_led(led_id) + point = find_led(cam, display=False) - if result: - led_map_2d.add_detection(result) + if point: + leds.append(LED2D(led_id, 0, point)) filepath = os.path.join(output_dir_full, f"led_map_2d_{view_index:04}.csv") - led_map_2d.write_to_file(filepath) + write_2d_leds_to_file(leds, filepath) def test_capture_sequence_correctness(): @@ -39,13 +36,11 @@ def test_capture_sequence_correctness(): filepath = os.path.join(output_dir_full, f"led_map_2d_{view_index:04}.csv") - led_map_2d = LEDMap2D(filepath) + leds = load_detections(filepath, view_index) if view_index in [0, 4, 8]: assert ( # If it's a straight on view, there should be 9 points - len(led_map_2d) == 9 + len(leds) == 9 ) else: - assert ( # If it's a diagonal-ish view, then we see 15 points - len(led_map_2d) == 15 - ) + assert len(leds) == 15 # If it's a diagonal-ish view, then we see 15 points diff --git a/test/test_led_identifier.py b/test/test_led_identifier.py index 71900f9..fcd5291 100644 --- a/test/test_led_identifier.py +++ b/test/test_led_identifier.py @@ -6,27 +6,27 @@ def test_basic_image_loading(): - mock_camera = Camera("test/MariMapper-Test-Data/9_point_box/cam_0/capture_0000.png") + mock_camera = Camera("MariMapper-Test-Data/9_point_box/cam_0/capture_0000.png") detection = find_led_in_image(mock_camera.read()) - assert detection.u == pytest.approx(0.4029418361244019) - assert detection.v == pytest.approx(0.4029538809144072) + assert detection.u() == pytest.approx(0.4029418361244019) + assert detection.v() == pytest.approx(0.4029538809144072) def test_none_found(): - mock_camera = Camera("test/MariMapper-Test-Data/9_point_box/cam_0/capture_%04d.png") + mock_camera = Camera("MariMapper-Test-Data/9_point_box/cam_0/capture_%04d.png") for frame_id in range(24): frame = mock_camera.read() if frame_id in [7, 15, 23]: - led_results = find_led_in_image(frame) - assert not led_results.valid + led_detection = find_led_in_image(frame) + assert led_detection is None def test_draw_results(): - mock_camera = Camera("test/MariMapper-Test-Data/9_point_box/cam_0/capture_%04d.png") + mock_camera = Camera("MariMapper-Test-Data/9_point_box/cam_0/capture_%04d.png") frame = mock_camera.read() - led_results = find_led_in_image(frame) - draw_led_detections(frame, led_results) + led_detection = find_led_in_image(frame) + draw_led_detections(frame, led_detection) diff --git a/test/test_reconstruction.py b/test/test_reconstruction.py index 2e06d07..a721107 100644 --- a/test/test_reconstruction.py +++ b/test/test_reconstruction.py @@ -1,26 +1,30 @@ import numpy as np -import pytest -from marimapper.sfm import SFM -from marimapper.led_map_2d import get_all_2d_led_maps +from marimapper.sfm import sfm +from marimapper.file_tools import get_all_2d_led_maps +from marimapper.led import get_led, get_leds_with_views def check_dimensions(map_3d, max_error): cube_sides = [] - cube_sides.append(map_3d[2]["pos"] - map_3d[0]["pos"]) - cube_sides.append(map_3d[4]["pos"] - map_3d[6]["pos"]) - cube_sides.append(map_3d[18]["pos"] - map_3d[16]["pos"]) - cube_sides.append(map_3d[20]["pos"] - map_3d[22]["pos"]) - cube_sides.append(map_3d[4]["pos"] - map_3d[2]["pos"]) - cube_sides.append(map_3d[20]["pos"] - map_3d[18]["pos"]) - cube_sides.append(map_3d[6]["pos"] - map_3d[0]["pos"]) - cube_sides.append(map_3d[22]["pos"] - map_3d[16]["pos"]) - - cube_sides.append(map_3d[16]["pos"] - map_3d[0]["pos"]) - cube_sides.append(map_3d[18]["pos"] - map_3d[2]["pos"]) - cube_sides.append(map_3d[20]["pos"] - map_3d[4]["pos"]) - cube_sides.append(map_3d[22]["pos"] - map_3d[6]["pos"]) + for start, end in ( + [2, 0], + [4, 6], + [18, 16], + [20, 22], + [4, 2], + [20, 18], + [6, 0], + [22, 16], + [16, 0], + [18, 2], + [20, 4], + [22, 6], + ): + cube_sides.append( + get_led(map_3d, start).point.position - get_led(map_3d, end).point.position + ) cube_side_lengths = [np.linalg.norm(v) for v in cube_sides] @@ -35,9 +39,9 @@ def check_dimensions(map_3d, max_error): def test_reconstruction(): - maps = get_all_2d_led_maps("test/scan") + maps = get_all_2d_led_maps("scan") - map_3d = SFM.process__(maps) + map_3d = sfm(maps) assert len(map_3d) == 21 @@ -47,11 +51,11 @@ def test_reconstruction(): def test_sparse_reconstruction(): - maps = get_all_2d_led_maps("test/scan") + maps = get_all_2d_led_maps("scan") - maps_sparse = [maps[1], maps[3], maps[5], maps[7]] + maps_sparse = get_leds_with_views(maps, [1, 3, 5, 7]) - map_3d = SFM.process__(maps_sparse) + map_3d = sfm(maps_sparse) assert map_3d is not None @@ -63,9 +67,10 @@ def test_sparse_reconstruction(): def test_2_track_reconstruction(): - partial_map = get_all_2d_led_maps("test/scan")[1:3] + leds = get_all_2d_led_maps("scan") + leds_2_track = get_leds_with_views(leds, [1, 2]) - map_3d = SFM.process__(partial_map) + map_3d = sfm(leds_2_track) assert map_3d is not None @@ -73,41 +78,10 @@ def test_2_track_reconstruction(): def test_invalid_reconstruction_views(): - maps = get_all_2d_led_maps("test/scan") + leds = get_all_2d_led_maps("scan") - invalid_maps = [maps[0], maps[4], maps[8]] # no useful overlap + leds_invalid = get_leds_with_views(leds, [0, 4, 8]) - map_3d = SFM.process__(invalid_maps) + map_3d = sfm(leds_invalid) assert map_3d is None - - -# this test does a re-scale, but should keep the dimensions about the same -def test_rescale(): - maps = get_all_2d_led_maps("test/scan") - - map_3d = SFM.process__(maps, rescale=True) - - assert map_3d.get_inter_led_distance() == pytest.approx(1.0) - - -def test_connected(): - - maps = get_all_2d_led_maps("test/scan") - - map_3d = SFM.process__(maps) - - assert len(map_3d) == 21 - - connected = map_3d.get_connected_leds() - assert (6, 7) not in connected - assert (13, 14) not in connected - - -def test_interpolate(): - - maps = get_all_2d_led_maps("test/scan") - - map_3d = SFM.process__(maps, interpolate=True) - - assert len(map_3d) == 23 diff --git a/test/test_script_import.py b/test/test_script_import.py index 0889524..d20f64d 100644 --- a/test/test_script_import.py +++ b/test/test_script_import.py @@ -16,13 +16,6 @@ def test_check_camera_cli(): main() # this should fail if no cameras are available -def test_view_2d_map_cli(): - from marimapper.scripts.view_2d_map_cli import main - - with pytest.raises(SystemExit): - main() # This should fail gracefully without any arguments - - def test_upload_to_pixelblaze_cli(): from marimapper.scripts.upload_map_to_pixelblaze_cli import main From 3bb9cbb6d755aaff1038d2fefc080265641d9e95 Mon Sep 17 00:00:00 2001 From: Samuel Date: Fri, 18 Oct 2024 18:22:23 +0100 Subject: [PATCH 03/26] getting there --- marimapper/backends/fcmega/fcmega.py | 2 +- .../backends/pixelblaze/pixelblaze_backend.py | 2 +- .../pixelblaze/upload_map_to_pixelblaze.py | 2 +- marimapper/camera.py | 7 +- marimapper/database_populator.py | 4 +- marimapper/detector.py | 15 +++-- marimapper/detector_process.py | 33 ++++++--- marimapper/file_tools.py | 5 +- marimapper/led.py | 10 +-- ...{logging.py => multiprocessing_logging.py} | 0 marimapper/scanner.py | 2 +- marimapper/scripts/check_backend_cli.py | 3 +- marimapper/scripts/check_camera_cli.py | 6 +- marimapper/scripts/scanner_cli.py | 4 +- marimapper/sfm.py | 17 ++--- marimapper/sfm_process.py | 38 ++++------- marimapper/utils.py | 2 +- marimapper/visualize_model.py | 2 +- test/mock_camera.py | 67 +++++++++++++++++++ test/test_detector_process.py | 27 ++++++++ test/test_reconstruction.py | 6 +- test/test_sfm_process.py | 27 ++++++++ 22 files changed, 195 insertions(+), 86 deletions(-) rename marimapper/{logging.py => multiprocessing_logging.py} (100%) create mode 100644 test/mock_camera.py create mode 100644 test/test_detector_process.py create mode 100644 test/test_sfm_process.py diff --git a/marimapper/backends/fcmega/fcmega.py b/marimapper/backends/fcmega/fcmega.py index 90f2dcd..3b1acdb 100644 --- a/marimapper/backends/fcmega/fcmega.py +++ b/marimapper/backends/fcmega/fcmega.py @@ -1,7 +1,7 @@ import serial import struct import serial.tools.list_ports -from marimapper import logging +from marimapper import multiprocessing_logging as logging class FCMega: diff --git a/marimapper/backends/pixelblaze/pixelblaze_backend.py b/marimapper/backends/pixelblaze/pixelblaze_backend.py index 8d20cd6..55917fd 100644 --- a/marimapper/backends/pixelblaze/pixelblaze_backend.py +++ b/marimapper/backends/pixelblaze/pixelblaze_backend.py @@ -1,4 +1,4 @@ -from marimapper import logging +from marimapper import multiprocessing_logging as logging import pixelblaze diff --git a/marimapper/backends/pixelblaze/upload_map_to_pixelblaze.py b/marimapper/backends/pixelblaze/upload_map_to_pixelblaze.py index 045afd9..6640be7 100644 --- a/marimapper/backends/pixelblaze/upload_map_to_pixelblaze.py +++ b/marimapper/backends/pixelblaze/upload_map_to_pixelblaze.py @@ -1,7 +1,7 @@ import csv from marimapper import utils -from marimapper import logging +from marimapper import multiprocessing_logging as logging def read_coordinates_from_csv(csv_file_name): diff --git a/marimapper/camera.py b/marimapper/camera.py index 24b6d88..ef69cd9 100644 --- a/marimapper/camera.py +++ b/marimapper/camera.py @@ -1,12 +1,10 @@ import cv2 -from marimapper import logging +from marimapper import multiprocessing_logging as logging class CameraSettings: def __init__(self, camera): - self.width = camera.get_width() - self.height = camera.get_height() self.af_mode = camera.get_af_mode() self.focus = camera.get_focus() self.exposure_mode = camera.get_exposure_mode() @@ -14,7 +12,6 @@ def __init__(self, camera): self.gain = camera.get_gain() def apply(self, camera): - camera.set_resolution(self.width, self.height) camera.set_autofocus(self.af_mode, self.focus) camera.set_exposure_mode(self.exposure_mode) camera.set_gain(self.gain) @@ -38,8 +35,6 @@ def __init__(self, device_id): if not self.device.isOpened(): raise RuntimeError(f"Failed to connect to camera {device_id}") - self.set_resolution(self.get_width(), self.get_height()) # Don't ask - self.default_settings = CameraSettings(self) self.state = "default" diff --git a/marimapper/database_populator.py b/marimapper/database_populator.py index 17bd59e..fc0bae8 100644 --- a/marimapper/database_populator.py +++ b/marimapper/database_populator.py @@ -13,7 +13,7 @@ def populate_database(db_path: os.path, leds: list[LED2D]): views = get_view_ids(leds) - map_features = np.zeros((max(views)+1, 1, 2)) + map_features = np.zeros((max(views) + 1, 1, 2)) for view in views: @@ -48,7 +48,7 @@ def populate_database(db_path: os.path, leds: list[LED2D]): # Create dummy images_all_the_same. - image_ids = [db.add_image(str(view), camera_id) for view in range(max(views)+1)] + image_ids = [db.add_image(str(view), camera_id) for view in range(max(views) + 1)] # Create some keypoints for i, keypoints in enumerate(map_features): diff --git a/marimapper/detector.py b/marimapper/detector.py index cf621db..7150ad7 100644 --- a/marimapper/detector.py +++ b/marimapper/detector.py @@ -73,7 +73,7 @@ def show_image(image: cv2.Mat) -> None: cv2.imshow(DETECTOR_WINDOW_NAME, image) key = cv2.waitKey(1) - if key == 27: # esc + if key == 27: # esc raise KeyboardInterrupt @@ -94,12 +94,14 @@ def set_cam_dark(cam: Camera, exposure: int) -> None: cam.state = "dark" -def find_led(cam: Camera, threshold: int = 128, display: bool = True) -> typing.Optional[Point2D]: +def find_led( + cam: Camera, threshold: int = 128, display: bool = True +) -> typing.Optional[Point2D]: image = cam.read() results = find_led_in_image(image, threshold) - if display: + if display and results: rendered_image = draw_led_detections(image, results) show_image(rendered_image) @@ -113,13 +115,14 @@ def enable_and_find_led( view_id: int, timeout_controller: TimeoutController, threshold: int, + display: bool = False, ) -> typing.Optional[LED2D]: if led_backend is None: return None # First wait for no leds to be visible - while find_led(cam, threshold) is not None: + while find_led(cam, threshold, display) is not None: pass # Set the led to on and start the clock @@ -132,7 +135,7 @@ def enable_and_find_led( while ( point is None and time.time() < response_time_start + timeout_controller.timeout ): - point = find_led(cam, threshold) + point = find_led(cam, threshold, display) led_backend.set_led(led_id, False) @@ -141,7 +144,7 @@ def enable_and_find_led( timeout_controller.add_response_time(time.time() - response_time_start) - while find_led(cam, threshold) is not None: + while find_led(cam, threshold, display) is not None: pass return LED2D(led_id, view_id, point) diff --git a/marimapper/detector_process.py b/marimapper/detector_process.py index efee859..d45fdc3 100644 --- a/marimapper/detector_process.py +++ b/marimapper/detector_process.py @@ -8,6 +8,7 @@ enable_and_find_led, DETECTOR_WINDOW_NAME, ) +from marimapper.led import LED2D from marimapper.utils import get_backend import cv2 @@ -39,6 +40,12 @@ def __init__( def __del__(self): self.close() + def detect(self, led_id: int, view_id: int): + self.detection_request.put((led_id, view_id)) + + def get_results(self) -> LED2D: + return self.detection_result.get() + def run(self): led_backend = get_backend(self._led_backend_name, self._led_backend_server) @@ -59,20 +66,28 @@ def run(self): view_id, timeout_controller, self._threshold, + self._display, ) - if result is not None: - self.detection_result.put(result) + + self.detection_result.put(result) else: set_cam_default(cam) - image = cam.read() - show_image(image) + if self._display: + image = cam.read() + show_image(image) + + if self._display: + # if we close the window + if ( + cv2.getWindowProperty(DETECTOR_WINDOW_NAME, cv2.WND_PROP_VISIBLE) + <= 0 + ): + self.exit_event.set() + continue - # if we close the window - if cv2.getWindowProperty(DETECTOR_WINDOW_NAME, cv2.WND_PROP_VISIBLE) <= 0: - self.exit_event.set() - continue + if self._display: + cv2.destroyAllWindows() - cv2.destroyAllWindows() set_cam_default(cam) def shutdown(self): diff --git a/marimapper/file_tools.py b/marimapper/file_tools.py index 9b26593..32e9475 100644 --- a/marimapper/file_tools.py +++ b/marimapper/file_tools.py @@ -1,6 +1,5 @@ import os from marimapper.led import Point2D, LED3D, LED2D -from marimapper import logging import typing @@ -31,7 +30,6 @@ def load_detections(filename: os.path, view_id) -> typing.Optional[list[LED2D]]: u = float(line[1]) v = float(line[2]) except (IndexError, ValueError): - logging.warn(f"Failed to read line {i} of {filename}: {line}") continue leds.append(LED2D(index, view_id, Point2D(u, v))) @@ -45,7 +43,7 @@ def get_all_2d_led_maps(directory: os.path) -> list[LED2D]: for view_id, filename in enumerate(sorted(os.listdir(directory))): full_path = os.path.join(directory, filename) - detections = load_detections(full_path, view_id) # this is wrong + detections = load_detections(full_path, view_id) # this is wrong if detections is not None: points.extend(detections) @@ -54,7 +52,6 @@ def get_all_2d_led_maps(directory: os.path) -> list[LED2D]: def write_3d_leds_to_file(leds: list[LED3D], filename: str): - logging.debug(f"Writing 3D map with {len(leds)} leds to {filename}...") lines = ["index,x,y,z,xn,yn,zn,error"] diff --git a/marimapper/led.py b/marimapper/led.py index 99e37ad..f54d3c3 100644 --- a/marimapper/led.py +++ b/marimapper/led.py @@ -112,9 +112,11 @@ def get_view_ids(leds: list[LED2D]) -> set[int]: def get_leds_with_view(leds: list[LED2D], view_id: int) -> list[LED2D]: return [led for led in leds if led.view_id == view_id] -def last_view(leds:list[LED2D]): + +def last_view(leds: list[LED2D]): return max([led.view_id for led in leds]) + def find_inter_led_distance(leds: list[LED2D | LED3D]): distances = [] @@ -203,8 +205,6 @@ def remove_duplicates(leds: list[LED3D]) -> list[LED3D]: return new_leds -def get_leds_with_view(leds:list[LED2D], view_id) -> list[LED2D]: - return [led for led in leds if led.view_id == view_id] -def get_leds_with_views(leds:list[LED2D], view_ids) -> list[LED2D]: - return [led for led in leds if led.view_id in view_ids] \ No newline at end of file +def get_leds_with_views(leds: list[LED2D], view_ids) -> list[LED2D]: + return [led for led in leds if led.view_id in view_ids] diff --git a/marimapper/logging.py b/marimapper/multiprocessing_logging.py similarity index 100% rename from marimapper/logging.py rename to marimapper/multiprocessing_logging.py diff --git a/marimapper/scanner.py b/marimapper/scanner.py index 50083b7..1142e36 100644 --- a/marimapper/scanner.py +++ b/marimapper/scanner.py @@ -4,7 +4,7 @@ from pathlib import Path from marimapper.detector_process import DetectorProcess -from marimapper import logging +from marimapper import multiprocessing_logging as logging from marimapper.file_tools import get_all_2d_led_maps, write_2d_leds_to_file from marimapper.utils import get_user_confirmation from marimapper.visualize_model import Renderer3D diff --git a/marimapper/scripts/check_backend_cli.py b/marimapper/scripts/check_backend_cli.py index 339b1e1..04e1a15 100644 --- a/marimapper/scripts/check_backend_cli.py +++ b/marimapper/scripts/check_backend_cli.py @@ -1,8 +1,7 @@ import argparse import time - +from marimapper import multiprocessing_logging as logging from marimapper import utils -from marimapper import logging def main(): diff --git a/marimapper/scripts/check_camera_cli.py b/marimapper/scripts/check_camera_cli.py index df95d9e..d3b209e 100644 --- a/marimapper/scripts/check_camera_cli.py +++ b/marimapper/scripts/check_camera_cli.py @@ -2,8 +2,9 @@ from marimapper.detector import find_led, set_cam_dark from marimapper.utils import add_camera_args -from marimapper import logging from marimapper.camera import Camera +from marimapper import multiprocessing_logging as logging + def main(): @@ -15,12 +16,10 @@ def main(): args = parser.parse_args() - cam = Camera(args.device) set_cam_dark(cam, args.exposure) - logging.info( "Camera connected! Hold an LED up to the camera to check LED identification" ) @@ -28,5 +27,6 @@ def main(): while True: find_led(cam, args.threshold) + if __name__ == "__main__": main() diff --git a/marimapper/scripts/scanner_cli.py b/marimapper/scripts/scanner_cli.py index 0c19d1e..00bca3c 100644 --- a/marimapper/scripts/scanner_cli.py +++ b/marimapper/scripts/scanner_cli.py @@ -1,5 +1,5 @@ from marimapper.scanner import Scanner -from marimapper import logging +from marimapper import multiprocessing_logging import os import signal import argparse @@ -11,7 +11,7 @@ def main(): - logging.info("Starting MariMapper") + multiprocessing_logging.info("Starting MariMapper") parser = argparse.ArgumentParser(description="Captures LED flashes to file") diff --git a/marimapper/sfm.py b/marimapper/sfm.py index 3ac6782..ff0e825 100644 --- a/marimapper/sfm.py +++ b/marimapper/sfm.py @@ -4,21 +4,17 @@ import pycolmap from marimapper.database_populator import populate_database +from marimapper.led import LED3D from marimapper.model import binary_to_led_map_3d from marimapper.utils import SupressLogging -from marimapper import logging -def sfm(maps_2d): - logging.debug("SFM process starting sfm process") - if len(maps_2d) < 2: - logging.debug("SFM process failed to run sfm process as not enough maps") - return None +def sfm(leds) -> list[LED3D]: with TemporaryDirectory() as temp_dir: database_path = os.path.join(temp_dir, "database.db") - populate_database(database_path, maps_2d) + populate_database(database_path, leds) options = pycolmap.IncrementalPipelineOptions() options.triangulation.ignore_two_view_tracks = False # used to be true @@ -35,10 +31,7 @@ def sfm(maps_2d): ) if not os.path.exists(os.path.join(temp_dir, "0", "points3D.bin")): - logging.debug( - "SFM process failed to run sfm process as reconstruction failed" - ) - return None + print("failed to build") + return [] - logging.debug("SFM process finished sfm process") return binary_to_led_map_3d(temp_dir) diff --git a/marimapper/sfm_process.py b/marimapper/sfm_process.py index fbaa242..d38e1e5 100644 --- a/marimapper/sfm_process.py +++ b/marimapper/sfm_process.py @@ -1,8 +1,5 @@ -from pathlib import Path from multiprocessing import Process, Event - - -from marimapper import logging +from marimapper.led import LED2D from marimapper.sfm import sfm @@ -10,40 +7,33 @@ class SFM(Process): def __init__( self, - directory: Path, - rescale=False, - interpolate=False, - event_on_update=None, led_map_2d_queue=None, led_map_3d_queue=None, ): - logging.debug("SFM initialising") super().__init__() - self.directory = directory - self.rescale = rescale - self.interpolate = interpolate self.exit_event = Event() - self.event_on_update = event_on_update self.led_map_3d_queue = led_map_3d_queue self.led_map_2d_queue = led_map_2d_queue - self.leds = [] - - logging.debug("SFM initialised") - - def add_led_maps_2d(self, maps): - self.led_map_2d_queue.put(maps) + self.leds: list[LED2D] = [] def shutdown(self): - logging.debug("SFM sending shutdown request") self.exit_event.set() def run(self): - logging.debug("SFM process starting") + + update_required = False while not self.exit_event.is_set(): if self.led_map_2d_queue.empty(): - map_3d = sfm(self.leds) - self.led_map_3d_queue.put(map_3d) + if update_required: + print( + f"just finished recieving data, trying to build with {len(self.leds)} points" + ) + leds_3d = sfm(self.leds) + print(f"reconstructed leds 3D: {len(leds_3d)}") + self.led_map_3d_queue.put(leds_3d) + update_required = False else: - led = self.led_map_3d_queue.get() + led = self.led_map_2d_queue.get() self.leds.append(led) + update_required = True diff --git a/marimapper/utils.py b/marimapper/utils.py index e8f62f6..dceffd1 100644 --- a/marimapper/utils.py +++ b/marimapper/utils.py @@ -4,7 +4,7 @@ import importlib.util from inspect import signature -from marimapper import logging +from marimapper import multiprocessing_logging as logging def add_camera_args(parser): diff --git a/marimapper/visualize_model.py b/marimapper/visualize_model.py index d555b88..1a60ad0 100644 --- a/marimapper/visualize_model.py +++ b/marimapper/visualize_model.py @@ -1,6 +1,6 @@ import numpy as np import open3d -from marimapper import logging +from marimapper import multiprocessing_logging as logging from multiprocessing import Process, Event diff --git a/test/mock_camera.py b/test/mock_camera.py new file mode 100644 index 0000000..1cf3887 --- /dev/null +++ b/test/mock_camera.py @@ -0,0 +1,67 @@ +from marimapper import multiprocessing_logging as logging +import cv2 +import numpy as np + + +class MockCamera: + + def __init__(self, device_id): + + self.frame_id = 0 + + logging.info(f"Connecting to camera {device_id} ...") + self.device_id = device_id + self.device = cv2.VideoCapture(device_id) + real_frames = [] + while True: + ret_val, image = self.device.read() + if not ret_val: + break + real_frames.append(image) + + self.frames = [] + black = np.zeros(real_frames[0].shape) + for frame in real_frames: + self.frames.append(black) + self.frames.append(frame) + self.frames.append(black) + + def reset(self): + pass + + def get_af_mode(self): + return 1 + + def get_focus(self): + return 1 + + def get_exposure_mode(self): + return 1 + + def get_exposure(self): + return 1 + + def get_gain(self): + return 1 + + def set_autofocus(self, mode, focus=0): + pass + + def set_exposure_mode(self, mode): + pass + + def set_gain(self, gain): + pass + + def set_exposure(self, exposure): + pass + + def eat(self, count=30): + pass + + def read(self, color=False): + + frame = self.frames[self.frame_id] + self.frame_id += 1 + + return frame diff --git a/test/test_detector_process.py b/test/test_detector_process.py new file mode 100644 index 0000000..8e28224 --- /dev/null +++ b/test/test_detector_process.py @@ -0,0 +1,27 @@ +import marimapper.camera +from marimapper.detector_process import DetectorProcess +import pytest +from mock_camera import MockCamera + +@pytest.mark.skip(reason="in progress") +def test_detector_process_basic(monkeypatch): + + monkeypatch.setattr(marimapper.camera.Camera, "Camera", MockCamera) + + device = "MariMapper-Test-Data/9_point_box/cam_0/capture_0000.png" + detector_process = DetectorProcess(device, 1, 128, "dummy", "none", display=False) + + detector_process.start() + + detector_process.detect(0, 0) + + results = detector_process.get_results() + + assert results.point.u() == pytest.approx(0.4029418361244019) + assert results.point.v() == pytest.approx(0.4029538809144072) + + detector_process.exit_event.set() + + +if __name__ == "__main__": + test_detector_process_basic() diff --git a/test/test_reconstruction.py b/test/test_reconstruction.py index a721107..6250fd0 100644 --- a/test/test_reconstruction.py +++ b/test/test_reconstruction.py @@ -57,8 +57,6 @@ def test_sparse_reconstruction(): map_3d = sfm(maps_sparse) - assert map_3d is not None - assert len(map_3d) == 21 check_dimensions( @@ -72,8 +70,6 @@ def test_2_track_reconstruction(): map_3d = sfm(leds_2_track) - assert map_3d is not None - assert len(map_3d) == 15 @@ -84,4 +80,4 @@ def test_invalid_reconstruction_views(): map_3d = sfm(leds_invalid) - assert map_3d is None + assert map_3d == [] diff --git a/test/test_sfm_process.py b/test/test_sfm_process.py new file mode 100644 index 0000000..732fe08 --- /dev/null +++ b/test/test_sfm_process.py @@ -0,0 +1,27 @@ +from marimapper.file_tools import get_all_2d_led_maps +from marimapper.sfm_process import SFM +from multiprocessing import Queue + + +def test_sfm_process_basic(): + + queue_2d = Queue() + queue_3d = Queue() + + sfm_process = SFM(queue_2d, queue_3d) + + sfm_process.start() + + leds = get_all_2d_led_maps("scan") + + for led in leds: # 2ms + queue_2d.put(led) + + leds_3d = queue_3d.get() # 320ms + assert len(leds_3d) == 21 + + sfm_process.exit_event.set() + + +if __name__ == "__main__": + test_sfm_process_basic() From f88ee5aa4ef8cca1a632f569ad43fdc0b8fad211 Mon Sep 17 00:00:00 2001 From: Samuel Date: Fri, 18 Oct 2024 19:28:12 +0100 Subject: [PATCH 04/26] oooh this is gross --- marimapper/scanner.py | 24 ++++++---------- marimapper/utils.py | 4 +-- marimapper/visualize_model.py | 53 ++++++++++++++--------------------- test/test_sfm_process.py | 4 +-- 4 files changed, 34 insertions(+), 51 deletions(-) diff --git a/marimapper/scanner.py b/marimapper/scanner.py index 1142e36..e830aef 100644 --- a/marimapper/scanner.py +++ b/marimapper/scanner.py @@ -1,16 +1,17 @@ +# DO NOT MOVE THIS +# FATAL WEIRD CRASH IF THIS ISN'T IMPORTED FIRST DON'T ASK +from marimapper.sfm_process import SFM + import os import time from tqdm import tqdm from pathlib import Path - from marimapper.detector_process import DetectorProcess -from marimapper import multiprocessing_logging as logging +from marimapper import multiprocessing_logging from marimapper.file_tools import get_all_2d_led_maps, write_2d_leds_to_file from marimapper.utils import get_user_confirmation from marimapper.visualize_model import Renderer3D from multiprocessing import Queue -from marimapper.sfm_process import SFM - class Scanner: @@ -32,14 +33,7 @@ def __init__(self, cli_args): ) self.renderer3d = Renderer3D(led_map_3d_queue=self.led_map_3d_queue) - self.sfm = SFM( - Path(self.output_dir), - rescale=True, - interpolate=True, - event_on_update=self.renderer3d.reload_event, - led_map_2d_queue=self.led_map_2d_queue, - led_map_3d_queue=self.led_map_3d_queue, - ) + self.sfm = SFM(self.led_map_2d_queue,self.led_map_3d_queue) self.leds_2d = get_all_2d_led_maps(Path(self.output_dir)) @@ -51,7 +45,7 @@ def __init__(self, cli_args): self.detector.start() def close(self): - logging.debug("marimapper closing") + multiprocessing_logging.debug("marimapper closing") self.sfm.shutdown() self.renderer3d.shutdown() self.detector.shutdown() @@ -63,7 +57,7 @@ def close(self): self.sfm.terminate() self.renderer3d.terminate() self.detector.terminate() - logging.debug("marimapper closed") + multiprocessing_logging.debug("marimapper closed") def mainloop(self): @@ -77,7 +71,7 @@ def mainloop(self): self.detector.detection_request.put(-1) result = self.detector.detection_result.get() if result.valid(): - logging.error( + multiprocessing_logging.error( f"All LEDs should be off, but the detector found one at {result.pos()}" ) continue diff --git a/marimapper/utils.py b/marimapper/utils.py index dceffd1..4b4a4a4 100644 --- a/marimapper/utils.py +++ b/marimapper/utils.py @@ -34,7 +34,7 @@ def add_backend_args(parser): "--backend", type=str, help="The backend used for led communication, i.e. fadecandy, wled or my_backend.py", - default="None", + default="dummy", ) parser.add_argument( @@ -136,7 +136,7 @@ def get_backend(backend_name, server=""): return dummy_backend.Backend() - raise RuntimeError("Invalid backend name") + raise RuntimeError(f"Invalid backend name {backend_name}") class SupressLogging(object): diff --git a/marimapper/visualize_model.py b/marimapper/visualize_model.py index 1a60ad0..b8600f5 100644 --- a/marimapper/visualize_model.py +++ b/marimapper/visualize_model.py @@ -11,39 +11,29 @@ def __init__(self, led_map_3d_queue): super().__init__() self._vis = None self.exit_event = Event() - self.reload_event = Event() self.led_map_3d_queue = led_map_3d_queue self.point_cloud = None self.line_set = None self.strip_set = None logging.debug("Renderer3D initialised") - def get_reload_event(self): - return self.reload_event - def shutdown(self): self.exit_event.set() - def reload(self): - logging.debug("Renderer3D reload request sent") - self.reload_event.set() - def run(self): logging.debug("Renderer3D process starting") - while not self.reload_event.wait(timeout=1): - if self.exit_event.is_set(): - return - self.initialise_visualiser__() self.reload_geometry__(True) while not self.exit_event.is_set(): - if self.reload_event.is_set(): + + if not self.led_map_3d_queue.empty(): self.reload_geometry__() - logging.debug( - "Renderer3D process received reload event, reloading geometry" - ) + + logging.debug( + "Renderer3D process received reload event, reloading geometry" + ) window_closed = not self._vis.poll_events() @@ -87,30 +77,30 @@ def reload_geometry__(self, first=False): logging.debug("Renderer3D process reloading geometry") - led_map = self.led_map_3d_queue.get() + leds = self.led_map_3d_queue.get() - logging.debug(f"Fetched led map with size {len(led_map.keys())}") + logging.debug(f"Fetched led map with size {len(leds)}") - p, l, c = camera_to_points_lines_colors(led_map.cameras) + #p, l, c = camera_to_points_lines_colors(led_map.cameras) - self.line_set.points = open3d.utility.Vector3dVector(p) - self.line_set.lines = open3d.utility.Vector2iVector(l) - self.line_set.colors = open3d.utility.Vector3dVector(c) + #self.line_set.points = open3d.utility.Vector3dVector(p) + #self.line_set.lines = open3d.utility.Vector2iVector(l) + #self.line_set.colors = open3d.utility.Vector3dVector(c) self.point_cloud.points = open3d.utility.Vector3dVector( - np.array([led_map.data[led_id]["pos"] for led_id in led_map.keys()]) + np.array([led.point.position for led in leds]) ) self.point_cloud.normals = open3d.utility.Vector3dVector( - np.array([led_map[led_id]["normal"] for led_id in led_map.keys()]) * 0.2 + np.array([led.point.normal for led in leds]) ) - self.strip_set.points = self.point_cloud.points - self.strip_set.lines = open3d.utility.Vector2iVector( - led_map.get_connected_leds() - ) - self.strip_set.colors = open3d.utility.Vector3dVector( - [[0.8, 0.8, 0.8] for _ in range(len(self.strip_set.lines))] - ) + #self.strip_set.points = self.point_cloud.points + #self.strip_set.lines = open3d.utility.Vector2iVector( + # led_map.get_connected_leds() + #) + #self.strip_set.colors = open3d.utility.Vector3dVector( + # [[0.8, 0.8, 0.8] for _ in range(len(self.strip_set.lines))] + #) if first: self._vis.add_geometry(self.point_cloud) @@ -121,7 +111,6 @@ def reload_geometry__(self, first=False): self._vis.update_geometry(self.line_set) self._vis.update_geometry(self.strip_set) - self.reload_event.clear() logging.debug("Renderer3D process reloaded geometry") diff --git a/test/test_sfm_process.py b/test/test_sfm_process.py index 732fe08..d58e649 100644 --- a/test/test_sfm_process.py +++ b/test/test_sfm_process.py @@ -1,7 +1,7 @@ from marimapper.file_tools import get_all_2d_led_maps from marimapper.sfm_process import SFM from multiprocessing import Queue - +import time def test_sfm_process_basic(): @@ -16,7 +16,7 @@ def test_sfm_process_basic(): for led in leds: # 2ms queue_2d.put(led) - + time.sleep(1) # wait for all to be consumed leds_3d = queue_3d.get() # 320ms assert len(leds_3d) == 21 From 3225b64638001cc6759d5c454e11a36f0e1868f6 Mon Sep 17 00:00:00 2001 From: Samuel Date: Fri, 18 Oct 2024 21:44:49 +0100 Subject: [PATCH 05/26] getting there, I should stop --- marimapper/led.py | 34 +++++++++++++++++++++------ marimapper/multiprocessing_logging.py | 2 +- marimapper/scanner.py | 11 ++++++--- marimapper/sfm.py | 2 +- marimapper/sfm_process.py | 32 +++++++++++++++---------- marimapper/visualize_model.py | 24 ++++++++----------- test/test_detector_process.py | 3 +++ test/test_sfm_process.py | 3 ++- 8 files changed, 72 insertions(+), 39 deletions(-) diff --git a/marimapper/led.py b/marimapper/led.py index f54d3c3..7f8797f 100644 --- a/marimapper/led.py +++ b/marimapper/led.py @@ -45,9 +45,9 @@ def __sub__(self, other): def __mul__(self, other): new = Point3D() - new.position = self.position * other.position - new.normal = self.normal * other.normal - new.error = self.error * other.error + new.position = self.position * other + new.normal = self.normal * other + new.error = self.error * other return new @@ -118,18 +118,38 @@ def last_view(leds: list[LED2D]): def find_inter_led_distance(leds: list[LED2D | LED3D]): - distances = [] for led in leds: next_led = get_next(led, leds) - if get_gap(led, next_led) == 0: - dist = get_distance(led, next_led) - distances.append(dist) + if next_led is not None: + if get_gap(led, next_led) == 1: + dist = get_distance(led, next_led) + distances.append(dist) return np.median(distances) +def rescale(leds: list[LED3D], target_inter_distance=1.0) -> None: + + inter_led_distance = find_inter_led_distance(leds) + print(inter_led_distance) + scale = (1.0 / inter_led_distance) * target_inter_distance + + for led in leds: + led.point *= scale + + +def recenter(leds: list[LED3D]): + + center = Point3D() + + center.position = np.median([led.point.position for led in leds], axis=0) + print(f"center: {center.position}") + for led in leds: + led.point -= center + + def fill_gap(start_led: LED3D, end_led: LED3D): total_missing_leds = end_led.led_id - start_led.led_id - 1 diff --git a/marimapper/multiprocessing_logging.py b/marimapper/multiprocessing_logging.py index 58fa874..518faad 100644 --- a/marimapper/multiprocessing_logging.py +++ b/marimapper/multiprocessing_logging.py @@ -20,7 +20,7 @@ def colorise(string, string_format): def debug(string): - pass # print(colorise(string, Col.BOLD)) + print(colorise(string, Col.BOLD)) def error(string): diff --git a/marimapper/scanner.py b/marimapper/scanner.py index e830aef..6e3de4a 100644 --- a/marimapper/scanner.py +++ b/marimapper/scanner.py @@ -13,6 +13,7 @@ from marimapper.visualize_model import Renderer3D from multiprocessing import Queue + class Scanner: def __init__(self, cli_args): @@ -33,11 +34,11 @@ def __init__(self, cli_args): ) self.renderer3d = Renderer3D(led_map_3d_queue=self.led_map_3d_queue) - self.sfm = SFM(self.led_map_2d_queue,self.led_map_3d_queue) + self.sfm = SFM(self.led_map_2d_queue, self.led_map_3d_queue) self.leds_2d = get_all_2d_led_maps(Path(self.output_dir)) - for led in self.leds_2d: + for led in self.leds_2d[0:2000]: self.led_map_2d_queue.put(led) self.sfm.start() @@ -68,6 +69,10 @@ def mainloop(self): if not start_scan: return + for led in self.leds_2d[2000:]: + self.led_map_2d_queue.put(led) + time.sleep(0.01) + self.detector.detection_request.put(-1) result = self.detector.detection_result.get() if result.valid(): @@ -86,7 +91,7 @@ def mainloop(self): view_id = 0 # change for led_id in self.led_id_range: - self.detector.detection_request.put((view_id, led_id)) + self.detector.detect(led_id, view_id) for _ in tqdm( self.led_id_range, diff --git a/marimapper/sfm.py b/marimapper/sfm.py index ff0e825..fb65734 100644 --- a/marimapper/sfm.py +++ b/marimapper/sfm.py @@ -31,7 +31,7 @@ def sfm(leds) -> list[LED3D]: ) if not os.path.exists(os.path.join(temp_dir, "0", "points3D.bin")): - print("failed to build") + # print("failed to build") return [] return binary_to_led_map_3d(temp_dir) diff --git a/marimapper/sfm_process.py b/marimapper/sfm_process.py index d38e1e5..94230d3 100644 --- a/marimapper/sfm_process.py +++ b/marimapper/sfm_process.py @@ -1,19 +1,19 @@ from multiprocessing import Process, Event -from marimapper.led import LED2D +from marimapper.led import LED2D, rescale, recenter from marimapper.sfm import sfm class SFM(Process): def __init__( - self, - led_map_2d_queue=None, - led_map_3d_queue=None, + self, led_map_2d_queue=None, led_map_3d_queue=None, rescale=True, recenter=True ): super().__init__() self.exit_event = Event() self.led_map_3d_queue = led_map_3d_queue self.led_map_2d_queue = led_map_2d_queue + self.rescale = rescale + self.recenter = recenter self.leds: list[LED2D] = [] def shutdown(self): @@ -25,14 +25,22 @@ def run(self): while not self.exit_event.is_set(): if self.led_map_2d_queue.empty(): - if update_required: - print( - f"just finished recieving data, trying to build with {len(self.leds)} points" - ) - leds_3d = sfm(self.leds) - print(f"reconstructed leds 3D: {len(leds_3d)}") - self.led_map_3d_queue.put(leds_3d) - update_required = False + + if not update_required: + continue + + leds_3d = sfm(self.leds) + + if len(leds_3d) == 0: + continue + + if self.rescale: + rescale(leds_3d) + if self.recenter: + recenter(leds_3d) + + self.led_map_3d_queue.put(leds_3d) + update_required = False else: led = self.led_map_2d_queue.get() self.leds.append(led) diff --git a/marimapper/visualize_model.py b/marimapper/visualize_model.py index b8600f5..274a9f4 100644 --- a/marimapper/visualize_model.py +++ b/marimapper/visualize_model.py @@ -31,10 +31,6 @@ def run(self): if not self.led_map_3d_queue.empty(): self.reload_geometry__() - logging.debug( - "Renderer3D process received reload event, reloading geometry" - ) - window_closed = not self._vis.poll_events() if window_closed: @@ -81,26 +77,26 @@ def reload_geometry__(self, first=False): logging.debug(f"Fetched led map with size {len(leds)}") - #p, l, c = camera_to_points_lines_colors(led_map.cameras) + # p, l, c = camera_to_points_lines_colors(led_map.cameras) - #self.line_set.points = open3d.utility.Vector3dVector(p) - #self.line_set.lines = open3d.utility.Vector2iVector(l) - #self.line_set.colors = open3d.utility.Vector3dVector(c) + # self.line_set.points = open3d.utility.Vector3dVector(p) + # self.line_set.lines = open3d.utility.Vector2iVector(l) + # self.line_set.colors = open3d.utility.Vector3dVector(c) self.point_cloud.points = open3d.utility.Vector3dVector( np.array([led.point.position for led in leds]) ) self.point_cloud.normals = open3d.utility.Vector3dVector( - np.array([led.point.normal for led in leds]) + np.array([led.point.normal for led in leds]) * 0.2 ) - #self.strip_set.points = self.point_cloud.points - #self.strip_set.lines = open3d.utility.Vector2iVector( + # self.strip_set.points = self.point_cloud.points + # self.strip_set.lines = open3d.utility.Vector2iVector( # led_map.get_connected_leds() - #) - #self.strip_set.colors = open3d.utility.Vector3dVector( + # ) + # self.strip_set.colors = open3d.utility.Vector3dVector( # [[0.8, 0.8, 0.8] for _ in range(len(self.strip_set.lines))] - #) + # ) if first: self._vis.add_geometry(self.point_cloud) diff --git a/test/test_detector_process.py b/test/test_detector_process.py index 8e28224..0397e84 100644 --- a/test/test_detector_process.py +++ b/test/test_detector_process.py @@ -3,6 +3,9 @@ import pytest from mock_camera import MockCamera + +# This is tricky because the capture sequence needs to include black scenes +# hmmmm @pytest.mark.skip(reason="in progress") def test_detector_process_basic(monkeypatch): diff --git a/test/test_sfm_process.py b/test/test_sfm_process.py index d58e649..90734d0 100644 --- a/test/test_sfm_process.py +++ b/test/test_sfm_process.py @@ -3,6 +3,7 @@ from multiprocessing import Queue import time + def test_sfm_process_basic(): queue_2d = Queue() @@ -16,7 +17,7 @@ def test_sfm_process_basic(): for led in leds: # 2ms queue_2d.put(led) - time.sleep(1) # wait for all to be consumed + time.sleep(1) # wait for all to be consumed leds_3d = queue_3d.get() # 320ms assert len(leds_3d) == 21 From 5ad182e525825715b039c918d34cc7b6dddcf6a7 Mon Sep 17 00:00:00 2001 From: Samuel Date: Fri, 18 Oct 2024 21:58:25 +0100 Subject: [PATCH 06/26] That's enough for tonight --- marimapper/detector.py | 17 +++++++++-------- marimapper/led.py | 2 +- marimapper/scanner.py | 35 +++++++++++++++++------------------ 3 files changed, 27 insertions(+), 27 deletions(-) diff --git a/marimapper/detector.py b/marimapper/detector.py index 7150ad7..1126251 100644 --- a/marimapper/detector.py +++ b/marimapper/detector.py @@ -116,10 +116,12 @@ def enable_and_find_led( timeout_controller: TimeoutController, threshold: int, display: bool = False, -) -> typing.Optional[LED2D]: +) -> LED2D: + + led = LED2D(led_id, view_id) if led_backend is None: - return None + return led # First wait for no leds to be visible while find_led(cam, threshold, display) is not None: @@ -131,20 +133,19 @@ def enable_and_find_led( led_backend.set_led(led_id, True) # Wait until either we have a result or we run out of time - point = None while ( - point is None and time.time() < response_time_start + timeout_controller.timeout + led.point is None and time.time() < response_time_start + timeout_controller.timeout ): - point = find_led(cam, threshold, display) + led.point = find_led(cam, threshold, display) led_backend.set_led(led_id, False) - if point is None: - return None + if led.point is None: + return led timeout_controller.add_response_time(time.time() - response_time_start) while find_led(cam, threshold, display) is not None: pass - return LED2D(led_id, view_id, point) + return led diff --git a/marimapper/led.py b/marimapper/led.py index 7f8797f..96544b0 100644 --- a/marimapper/led.py +++ b/marimapper/led.py @@ -17,7 +17,7 @@ def v(self): class LED2D: - def __init__(self, led_id: int, view_id: int, point: Point2D): + def __init__(self, led_id: int, view_id: int, point: typing.Optional[Point2D]=None): self.led_id: int = led_id self.view_id: int = view_id self.point: Point2D = point diff --git a/marimapper/scanner.py b/marimapper/scanner.py index 6e3de4a..ce9c029 100644 --- a/marimapper/scanner.py +++ b/marimapper/scanner.py @@ -12,6 +12,7 @@ from marimapper.utils import get_user_confirmation from marimapper.visualize_model import Renderer3D from multiprocessing import Queue +from marimapper.led import last_view class Scanner: @@ -41,6 +42,8 @@ def __init__(self, cli_args): for led in self.leds_2d[0:2000]: self.led_map_2d_queue.put(led) + self.current_view = last_view(self.leds_2d) + 1 + self.sfm.start() self.renderer3d.start() self.detector.start() @@ -73,38 +76,34 @@ def mainloop(self): self.led_map_2d_queue.put(led) time.sleep(0.01) - self.detector.detection_request.put(-1) - result = self.detector.detection_result.get() - if result.valid(): - multiprocessing_logging.error( - f"All LEDs should be off, but the detector found one at {result.pos()}" - ) - continue - - # The filename is made out of the date, then the resolution of the camera - string_time = time.strftime("%Y%m%d-%H%M%S") - - filepath = os.path.join(self.output_dir, f"led_map_2d_{string_time}.csv") leds = [] - view_id = 0 # change - for led_id in self.led_id_range: - self.detector.detect(led_id, view_id) + self.detector.detect(led_id, self.current_view) for _ in tqdm( self.led_id_range, unit="LEDs", - desc=f"Capturing sequence to {filepath}", + desc="Capturing sequence", total=self.led_id_range.stop, smoothing=0, ): - led = self.detector.detection_result.get(timeout=10) + led = self.detector.detection_result.get() + + if led.point is None: + continue + print(f"found {led}") leds.append(led) self.led_map_2d_queue.put(led) - write_2d_leds_to_file(leds, filepath) + self.current_view += 1 + + + # The filename is made out of the date, then the resolution of the camera + string_time = time.strftime("%Y%m%d-%H%M%S") + filepath = os.path.join(self.output_dir, f"led_map_2d_{string_time}.csv") + write_2d_leds_to_file(leds, filepath) \ No newline at end of file From 29e77c59e759868cc301fda6ba4cdd56b4f8522c Mon Sep 17 00:00:00 2001 From: Samuel Date: Fri, 18 Oct 2024 21:59:06 +0100 Subject: [PATCH 07/26] That's enough for tonight --- marimapper/detector.py | 3 ++- marimapper/led.py | 4 +++- marimapper/scanner.py | 4 +--- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/marimapper/detector.py b/marimapper/detector.py index 1126251..1e63abf 100644 --- a/marimapper/detector.py +++ b/marimapper/detector.py @@ -134,7 +134,8 @@ def enable_and_find_led( # Wait until either we have a result or we run out of time while ( - led.point is None and time.time() < response_time_start + timeout_controller.timeout + led.point is None + and time.time() < response_time_start + timeout_controller.timeout ): led.point = find_led(cam, threshold, display) diff --git a/marimapper/led.py b/marimapper/led.py index 96544b0..ececa68 100644 --- a/marimapper/led.py +++ b/marimapper/led.py @@ -17,7 +17,9 @@ def v(self): class LED2D: - def __init__(self, led_id: int, view_id: int, point: typing.Optional[Point2D]=None): + def __init__( + self, led_id: int, view_id: int, point: typing.Optional[Point2D] = None + ): self.led_id: int = led_id self.view_id: int = view_id self.point: Point2D = point diff --git a/marimapper/scanner.py b/marimapper/scanner.py index ce9c029..185b76f 100644 --- a/marimapper/scanner.py +++ b/marimapper/scanner.py @@ -76,7 +76,6 @@ def mainloop(self): self.led_map_2d_queue.put(led) time.sleep(0.01) - leds = [] for led_id in self.led_id_range: @@ -102,8 +101,7 @@ def mainloop(self): self.current_view += 1 - # The filename is made out of the date, then the resolution of the camera string_time = time.strftime("%Y%m%d-%H%M%S") filepath = os.path.join(self.output_dir, f"led_map_2d_{string_time}.csv") - write_2d_leds_to_file(leds, filepath) \ No newline at end of file + write_2d_leds_to_file(leds, filepath) From f1cba74e59cfc81ef829511137189dbc17feef25 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sat, 19 Oct 2024 09:50:56 +0100 Subject: [PATCH 08/26] Normals are back! Next stop, cameras --- marimapper/led.py | 13 ++++++++++-- marimapper/model.py | 42 +++++++++++++-------------------------- marimapper/scanner.py | 11 +++------- marimapper/sfm_process.py | 38 ++++++++++++++++++++++++++++++++++- 4 files changed, 65 insertions(+), 39 deletions(-) diff --git a/marimapper/led.py b/marimapper/led.py index ececa68..ed769d5 100644 --- a/marimapper/led.py +++ b/marimapper/led.py @@ -1,6 +1,13 @@ import numpy as np -import math import typing +import math + + +class View: + def __init__(self, view_id, position, rotation): + self.view_id = view_id + self.rotation = rotation + self.position = position class Point2D: @@ -57,7 +64,7 @@ class LED3D: def __init__(self, led_id): self.led_id = led_id self.point = Point3D() - self.views = [] + self.views: list[View] = [] # returns none if there isn't that led in the list! @@ -116,6 +123,8 @@ def get_leds_with_view(leds: list[LED2D], view_id: int) -> list[LED2D]: def last_view(leds: list[LED2D]): + if len(leds) == 0: + return -1 return max([led.view_id for led in leds]) diff --git a/marimapper/model.py b/marimapper/model.py index 1154ace..31a574d 100644 --- a/marimapper/model.py +++ b/marimapper/model.py @@ -6,14 +6,25 @@ read_points3D_binary, ) -from marimapper.led import LED3D, remove_duplicates +from marimapper.led import LED3D, remove_duplicates, View def binary_to_led_map_3d(path: os.path) -> list[LED3D]: points_bin = read_points3D_binary(os.path.join(path, "0", "points3D.bin")) + images_bin = read_images_binary(os.path.join(path, "0", "images.bin")) + + views = {} + leds = [] + + for img in images_bin.values(): + + rotation = qvec2rotmat(img.qvec).T + translation = -rotation @ img.tvec + + view = View(img.id, translation, rotation) - leds: list[LED3D] = [] + views[img.id] = view for ( led_data @@ -24,35 +35,10 @@ def binary_to_led_map_3d(path: os.path) -> list[LED3D]: led.point.position = led_data.xyz led.point.error = led_data.error - led.views = led_data.image_ids + led.views = [views[view_id] for view_id in led_data.image_ids] leds.append(led) leds = remove_duplicates(leds) return leds - - -class ReconstructedCamera: - def __init__(self, camera_id, translation, rotation): - self.camera_id = camera_id - self.rotation = rotation - self.translation = translation - - -def binary_to_cameras(path: os.path) -> list[ReconstructedCamera]: - - cameras = [] - - images_bin = read_images_binary(os.path.join(path, "0", "images.bin")) - - for img in images_bin.values(): - - rotation = qvec2rotmat(img.qvec).T - translation = -rotation @ img.tvec - - camera = ReconstructedCamera(img.id, translation, rotation) - - cameras.append(camera) - - return cameras diff --git a/marimapper/scanner.py b/marimapper/scanner.py index 185b76f..3bb4ecc 100644 --- a/marimapper/scanner.py +++ b/marimapper/scanner.py @@ -37,12 +37,11 @@ def __init__(self, cli_args): self.renderer3d = Renderer3D(led_map_3d_queue=self.led_map_3d_queue) self.sfm = SFM(self.led_map_2d_queue, self.led_map_3d_queue) - self.leds_2d = get_all_2d_led_maps(Path(self.output_dir)) - - for led in self.leds_2d[0:2000]: + leds = get_all_2d_led_maps(Path(self.output_dir)) + for led in leds: self.led_map_2d_queue.put(led) - self.current_view = last_view(self.leds_2d) + 1 + self.current_view = last_view(leds) + 1 self.sfm.start() self.renderer3d.start() @@ -72,10 +71,6 @@ def mainloop(self): if not start_scan: return - for led in self.leds_2d[2000:]: - self.led_map_2d_queue.put(led) - time.sleep(0.01) - leds = [] for led_id in self.led_id_range: diff --git a/marimapper/sfm_process.py b/marimapper/sfm_process.py index 94230d3..77d23e6 100644 --- a/marimapper/sfm_process.py +++ b/marimapper/sfm_process.py @@ -1,6 +1,39 @@ from multiprocessing import Process, Event -from marimapper.led import LED2D, rescale, recenter +from marimapper.led import LED2D, rescale, recenter, LED3D from marimapper.sfm import sfm +import open3d +import numpy as np +import math + +# this is here for now as there is some weird import dependency going on... +def add_normals(leds: list[LED3D]): + + for led in leds: + led.point.normal = (1, 1, 1) + + pcd = open3d.geometry.PointCloud() + + xyz = [led.point.position for led in leds] + + pcd.points = open3d.utility.Vector3dVector(xyz) + + pcd.normals = open3d.utility.Vector3dVector(np.zeros((len(leds), 3))) + + pcd.estimate_normals() + + camera_normals = [] + for led in leds: + views = [view.position for view in led.views] + camera_normals.append(np.average(views, axis=0)) + + for led, camera_normal, open3d_normal in zip(leds, camera_normals, pcd.normals): + + led.point.normal = open3d_normal / np.linalg.norm(open3d_normal) + + angle = np.arccos(np.clip(np.dot(camera_normal, open3d_normal), -1.0, 1.0)) + + if angle > math.pi / 2.0: + led.point.normal *= -1 class SFM(Process): @@ -36,9 +69,12 @@ def run(self): if self.rescale: rescale(leds_3d) + if self.recenter: recenter(leds_3d) + add_normals(leds_3d) + self.led_map_3d_queue.put(leds_3d) update_required = False else: From 3e30233795c07a62544b45e2c41edbb9d31b63d5 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sat, 19 Oct 2024 10:55:14 +0100 Subject: [PATCH 09/26] cameras are back! --- marimapper/led.py | 22 +++++++++------------- marimapper/model.py | 6 ++---- marimapper/sfm.py | 5 +++-- marimapper/sfm_process.py | 4 ++-- marimapper/visualize_model.py | 28 ++++++++++++++++++---------- 5 files changed, 34 insertions(+), 31 deletions(-) diff --git a/marimapper/led.py b/marimapper/led.py index ed769d5..e68f5fd 100644 --- a/marimapper/led.py +++ b/marimapper/led.py @@ -45,17 +45,10 @@ def __add__(self, other): new.error = self.error + other.error return new - def __sub__(self, other): - new = Point3D() - new.position = self.position - other.position - new.normal = self.normal - other.normal - new.error = self.error - other.error - return new - def __mul__(self, other): new = Point3D() new.position = self.position * other - new.normal = self.normal * other + new.normal = self.normal new.error = self.error * other return new @@ -146,20 +139,23 @@ def rescale(leds: list[LED3D], target_inter_distance=1.0) -> None: inter_led_distance = find_inter_led_distance(leds) print(inter_led_distance) scale = (1.0 / inter_led_distance) * target_inter_distance - for led in leds: led.point *= scale + for view in led.views: + view.position = view.position * scale def recenter(leds: list[LED3D]): - center = Point3D() - center.position = np.median([led.point.position for led in leds], axis=0) - print(f"center: {center.position}") for led in leds: - led.point -= center + assert len(led.point.position) == 3 + center = np.median([led.point.position for led in leds], axis=0) + for led in leds: + led.point.position -= center + for view in led.views: + view.position = view.position - center def fill_gap(start_led: LED3D, end_led: LED3D): diff --git a/marimapper/model.py b/marimapper/model.py index 31a574d..f6ec266 100644 --- a/marimapper/model.py +++ b/marimapper/model.py @@ -22,9 +22,7 @@ def binary_to_led_map_3d(path: os.path) -> list[LED3D]: rotation = qvec2rotmat(img.qvec).T translation = -rotation @ img.tvec - view = View(img.id, translation, rotation) - - views[img.id] = view + views[img.id] = (img.id, translation, rotation) for ( led_data @@ -35,7 +33,7 @@ def binary_to_led_map_3d(path: os.path) -> list[LED3D]: led.point.position = led_data.xyz led.point.error = led_data.error - led.views = [views[view_id] for view_id in led_data.image_ids] + led.views = [View(*views[view_id]) for view_id in led_data.image_ids] leds.append(led) diff --git a/marimapper/sfm.py b/marimapper/sfm.py index fb65734..e108a23 100644 --- a/marimapper/sfm.py +++ b/marimapper/sfm.py @@ -31,7 +31,8 @@ def sfm(leds) -> list[LED3D]: ) if not os.path.exists(os.path.join(temp_dir, "0", "points3D.bin")): - # print("failed to build") return [] - return binary_to_led_map_3d(temp_dir) + leds = binary_to_led_map_3d(temp_dir) + print(f"sfm managed to reconstruct {len(leds)} leds") + return leds diff --git a/marimapper/sfm_process.py b/marimapper/sfm_process.py index 77d23e6..787a1cf 100644 --- a/marimapper/sfm_process.py +++ b/marimapper/sfm_process.py @@ -67,14 +67,14 @@ def run(self): if len(leds_3d) == 0: continue + add_normals(leds_3d) + if self.rescale: rescale(leds_3d) if self.recenter: recenter(leds_3d) - add_normals(leds_3d) - self.led_map_3d_queue.put(leds_3d) update_required = False else: diff --git a/marimapper/visualize_model.py b/marimapper/visualize_model.py index 274a9f4..39362d9 100644 --- a/marimapper/visualize_model.py +++ b/marimapper/visualize_model.py @@ -2,7 +2,16 @@ import open3d from marimapper import multiprocessing_logging as logging from multiprocessing import Process, Event +from marimapper.led import LED3D, View +def get_all_views(leds: list[LED3D]) -> list[View]: + views = [] + for led in leds: + for view in led.views: + if view.view_id not in [v.view_id for v in views]: + views.append(view) + + return views class Renderer3D(Process): @@ -76,12 +85,13 @@ def reload_geometry__(self, first=False): leds = self.led_map_3d_queue.get() logging.debug(f"Fetched led map with size {len(leds)}") + all_views = get_all_views(leds) - # p, l, c = camera_to_points_lines_colors(led_map.cameras) + p, l, c = view_to_points_lines_colors(all_views) - # self.line_set.points = open3d.utility.Vector3dVector(p) - # self.line_set.lines = open3d.utility.Vector2iVector(l) - # self.line_set.colors = open3d.utility.Vector3dVector(c) + self.line_set.points = open3d.utility.Vector3dVector(p) + self.line_set.lines = open3d.utility.Vector2iVector(l) + self.line_set.colors = open3d.utility.Vector3dVector(c) self.point_cloud.points = open3d.utility.Vector3dVector( np.array([led.point.position for led in leds]) @@ -110,7 +120,7 @@ def reload_geometry__(self, first=False): logging.debug("Renderer3D process reloaded geometry") -def camera_to_points_lines_colors(cameras): # returns points and lines +def view_to_points_lines_colors(views): # returns points and lines all_points = [] all_lines = [] @@ -127,13 +137,11 @@ def camera_to_points_lines_colors(cameras): # returns points and lines [[0, 1], [0, 2], [0, 3], [0, 4], [1, 2], [2, 3], [3, 4], [4, 1], [1, 5], [2, 5]] ) - for cam_id, camera in enumerate(cameras): - - rotation, translation = camera + for i, view in enumerate(views): - points_in_world = [(rotation @ p + translation) for p in camera_cone_points] + points_in_world = [(view.rotation @ p + view.position) for p in camera_cone_points] - offset = cam_id * len(camera_cone_points) + offset = i * len(camera_cone_points) all_points.extend(points_in_world) all_lines.extend(camera_cone_lines + offset) From da330b9efe9c16455daa1bd5e3cefffd4ec70624 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sat, 19 Oct 2024 12:02:24 +0100 Subject: [PATCH 10/26] sorted out a lot of multiprocessing issues --- marimapper/backends/fcmega/fcmega.py | 2 +- .../backends/pixelblaze/pixelblaze_backend.py | 2 +- .../pixelblaze/upload_map_to_pixelblaze.py | 2 +- marimapper/camera.py | 2 +- marimapper/detector_process.py | 46 +++++---------- marimapper/led.py | 2 +- ...{multiprocessing_logging.py => logging.py} | 0 marimapper/scanner.py | 36 ++++++------ marimapper/scripts/check_backend_cli.py | 2 +- marimapper/scripts/check_camera_cli.py | 2 +- marimapper/scripts/scanner_cli.py | 4 +- marimapper/sfm_process.py | 58 +++++++++---------- marimapper/utils.py | 2 +- ...isualize_model.py => visualize_process.py} | 42 ++++++++------ test/mock_camera.py | 2 +- test/test_detector_process.py | 2 - test/test_rescale.py | 11 ++++ test/test_sfm_process.py | 2 - 18 files changed, 105 insertions(+), 114 deletions(-) rename marimapper/{multiprocessing_logging.py => logging.py} (100%) rename marimapper/{visualize_model.py => visualize_process.py} (84%) create mode 100644 test/test_rescale.py diff --git a/marimapper/backends/fcmega/fcmega.py b/marimapper/backends/fcmega/fcmega.py index 3b1acdb..8f8e543 100644 --- a/marimapper/backends/fcmega/fcmega.py +++ b/marimapper/backends/fcmega/fcmega.py @@ -1,7 +1,7 @@ import serial import struct import serial.tools.list_ports -from marimapper import multiprocessing_logging as logging +from marimapper import logging as logging class FCMega: diff --git a/marimapper/backends/pixelblaze/pixelblaze_backend.py b/marimapper/backends/pixelblaze/pixelblaze_backend.py index 55917fd..0c15e6b 100644 --- a/marimapper/backends/pixelblaze/pixelblaze_backend.py +++ b/marimapper/backends/pixelblaze/pixelblaze_backend.py @@ -1,4 +1,4 @@ -from marimapper import multiprocessing_logging as logging +from marimapper import logging as logging import pixelblaze diff --git a/marimapper/backends/pixelblaze/upload_map_to_pixelblaze.py b/marimapper/backends/pixelblaze/upload_map_to_pixelblaze.py index 6640be7..8e3fa10 100644 --- a/marimapper/backends/pixelblaze/upload_map_to_pixelblaze.py +++ b/marimapper/backends/pixelblaze/upload_map_to_pixelblaze.py @@ -1,7 +1,7 @@ import csv from marimapper import utils -from marimapper import multiprocessing_logging as logging +from marimapper import logging as logging def read_coordinates_from_csv(csv_file_name): diff --git a/marimapper/camera.py b/marimapper/camera.py index ef69cd9..555aeed 100644 --- a/marimapper/camera.py +++ b/marimapper/camera.py @@ -1,5 +1,5 @@ import cv2 -from marimapper import multiprocessing_logging as logging +from marimapper import logging as logging class CameraSettings: diff --git a/marimapper/detector_process.py b/marimapper/detector_process.py index d45fdc3..cfdb2e0 100644 --- a/marimapper/detector_process.py +++ b/marimapper/detector_process.py @@ -1,4 +1,4 @@ -from multiprocessing import Process, Event, Queue +from multiprocessing import Process, Queue, Event from marimapper.detector import ( show_image, set_cam_default, @@ -6,11 +6,10 @@ TimeoutController, set_cam_dark, enable_and_find_led, - DETECTOR_WINDOW_NAME, ) from marimapper.led import LED2D from marimapper.utils import get_backend -import cv2 +from marimapper import logging class DetectorProcess(Process): @@ -25,10 +24,9 @@ def __init__( display: bool = True, ): super().__init__() - - self.exit_event = Event() - self.detection_request = Queue() # {led_id, view_id} - self.detection_result = Queue() # LED3D + self._detection_request = Queue() # {led_id, view_id} + self._detection_result = Queue() # LED3D + self._exit_event = Event() self._device = device self._dark_exposure = dark_exposure @@ -37,14 +35,14 @@ def __init__( self._led_backend_server = led_backend_server self._display = display - def __del__(self): - self.close() - def detect(self, led_id: int, view_id: int): - self.detection_request.put((led_id, view_id)) + self._detection_request.put((led_id, view_id)) def get_results(self) -> LED2D: - return self.detection_result.get() + return self._detection_result.get() + + def stop(self): + self._exit_event.set() def run(self): @@ -54,11 +52,11 @@ def run(self): timeout_controller = TimeoutController() - while not self.exit_event.is_set(): + while not self._exit_event.is_set(): - if not self.detection_request.empty(): + if not self._detection_request.empty(): set_cam_dark(cam, self._dark_exposure) - led_id, view_id = self.detection_request.get() + led_id, view_id = self._detection_request.get() result = enable_and_find_led( cam, led_backend, @@ -69,26 +67,12 @@ def run(self): self._display, ) - self.detection_result.put(result) + self._detection_result.put(result) else: set_cam_default(cam) if self._display: image = cam.read() show_image(image) - if self._display: - # if we close the window - if ( - cv2.getWindowProperty(DETECTOR_WINDOW_NAME, cv2.WND_PROP_VISIBLE) - <= 0 - ): - self.exit_event.set() - continue - - if self._display: - cv2.destroyAllWindows() - + logging.info("resetting cam!") set_cam_default(cam) - - def shutdown(self): - self.exit_event.set() diff --git a/marimapper/led.py b/marimapper/led.py index e68f5fd..3fbaf5d 100644 --- a/marimapper/led.py +++ b/marimapper/led.py @@ -147,7 +147,6 @@ def rescale(leds: list[LED3D], target_inter_distance=1.0) -> None: def recenter(leds: list[LED3D]): - for led in leds: assert len(led.point.position) == 3 @@ -157,6 +156,7 @@ def recenter(leds: list[LED3D]): for view in led.views: view.position = view.position - center + def fill_gap(start_led: LED3D, end_led: LED3D): total_missing_leds = end_led.led_id - start_led.led_id - 1 diff --git a/marimapper/multiprocessing_logging.py b/marimapper/logging.py similarity index 100% rename from marimapper/multiprocessing_logging.py rename to marimapper/logging.py diff --git a/marimapper/scanner.py b/marimapper/scanner.py index 3bb4ecc..20fb20c 100644 --- a/marimapper/scanner.py +++ b/marimapper/scanner.py @@ -7,10 +7,10 @@ from tqdm import tqdm from pathlib import Path from marimapper.detector_process import DetectorProcess -from marimapper import multiprocessing_logging +from marimapper import logging from marimapper.file_tools import get_all_2d_led_maps, write_2d_leds_to_file from marimapper.utils import get_user_confirmation -from marimapper.visualize_model import Renderer3D +from marimapper.visualize_process import VisualiseProcess from multiprocessing import Queue from marimapper.led import last_view @@ -23,7 +23,6 @@ def __init__(self, cli_args): self.led_id_range = range(cli_args.start, cli_args.end) - self.led_map_2d_queue = Queue() self.led_map_3d_queue = Queue() self.detector = DetectorProcess( @@ -34,33 +33,32 @@ def __init__(self, cli_args): cli_args.server, ) - self.renderer3d = Renderer3D(led_map_3d_queue=self.led_map_3d_queue) - self.sfm = SFM(self.led_map_2d_queue, self.led_map_3d_queue) + self.sfm = SFM() - leds = get_all_2d_led_maps(Path(self.output_dir)) - for led in leds: - self.led_map_2d_queue.put(led) + self.leds = get_all_2d_led_maps(Path(self.output_dir)) + for led in self.leds: + self.sfm.add_detection(led) - self.current_view = last_view(leds) + 1 + self.current_view = last_view(self.leds) + 1 + + self.renderer3d = VisualiseProcess(input_queue=self.sfm.get_output_queue()) self.sfm.start() self.renderer3d.start() self.detector.start() def close(self): - multiprocessing_logging.debug("marimapper closing") - self.sfm.shutdown() - self.renderer3d.shutdown() - self.detector.shutdown() + logging.debug("marimapper closing") + + self.detector.stop() + self.sfm.stop() + self.renderer3d.stop() self.sfm.join() self.renderer3d.join() self.detector.join() - self.sfm.terminate() - self.renderer3d.terminate() - self.detector.terminate() - multiprocessing_logging.debug("marimapper closed") + logging.debug("marimapper closed") def mainloop(self): @@ -83,7 +81,7 @@ def mainloop(self): total=self.led_id_range.stop, smoothing=0, ): - led = self.detector.detection_result.get() + led = self.detector.get_results() if led.point is None: continue @@ -92,7 +90,7 @@ def mainloop(self): leds.append(led) - self.led_map_2d_queue.put(led) + self.sfm.add_detection(led) self.current_view += 1 diff --git a/marimapper/scripts/check_backend_cli.py b/marimapper/scripts/check_backend_cli.py index 04e1a15..e78b879 100644 --- a/marimapper/scripts/check_backend_cli.py +++ b/marimapper/scripts/check_backend_cli.py @@ -1,6 +1,6 @@ import argparse import time -from marimapper import multiprocessing_logging as logging +from marimapper import logging as logging from marimapper import utils diff --git a/marimapper/scripts/check_camera_cli.py b/marimapper/scripts/check_camera_cli.py index d3b209e..8c2d391 100644 --- a/marimapper/scripts/check_camera_cli.py +++ b/marimapper/scripts/check_camera_cli.py @@ -3,7 +3,7 @@ from marimapper.detector import find_led, set_cam_dark from marimapper.utils import add_camera_args from marimapper.camera import Camera -from marimapper import multiprocessing_logging as logging +from marimapper import logging as logging def main(): diff --git a/marimapper/scripts/scanner_cli.py b/marimapper/scripts/scanner_cli.py index 00bca3c..0c19d1e 100644 --- a/marimapper/scripts/scanner_cli.py +++ b/marimapper/scripts/scanner_cli.py @@ -1,5 +1,5 @@ from marimapper.scanner import Scanner -from marimapper import multiprocessing_logging +from marimapper import logging import os import signal import argparse @@ -11,7 +11,7 @@ def main(): - multiprocessing_logging.info("Starting MariMapper") + logging.info("Starting MariMapper") parser = argparse.ArgumentParser(description="Captures LED flashes to file") diff --git a/marimapper/sfm_process.py b/marimapper/sfm_process.py index 787a1cf..05e2ed7 100644 --- a/marimapper/sfm_process.py +++ b/marimapper/sfm_process.py @@ -1,21 +1,17 @@ -from multiprocessing import Process, Event +from multiprocessing import Process, Event, Queue from marimapper.led import LED2D, rescale, recenter, LED3D from marimapper.sfm import sfm import open3d import numpy as np import math + # this is here for now as there is some weird import dependency going on... def add_normals(leds: list[LED3D]): - for led in leds: - led.point.normal = (1, 1, 1) - pcd = open3d.geometry.PointCloud() - xyz = [led.point.position for led in leds] - - pcd.points = open3d.utility.Vector3dVector(xyz) + pcd.points = open3d.utility.Vector3dVector([led.point.position for led in leds]) pcd.normals = open3d.utility.Vector3dVector(np.zeros((len(leds), 3))) @@ -38,46 +34,48 @@ def add_normals(leds: list[LED3D]): class SFM(Process): - def __init__( - self, led_map_2d_queue=None, led_map_3d_queue=None, rescale=True, recenter=True - ): + def __init__(self): super().__init__() - self.exit_event = Event() - self.led_map_3d_queue = led_map_3d_queue - self.led_map_2d_queue = led_map_2d_queue - self.rescale = rescale - self.recenter = recenter - self.leds: list[LED2D] = [] + self._output_queue = Queue() + self._input_queue = Queue() + self._exit_event = Event() + + def add_detection(self, led: LED2D): + self._input_queue.put(led) + + def get_output_queue(self): + return self._output_queue - def shutdown(self): - self.exit_event.set() + def stop(self): + self._exit_event.set() def run(self): update_required = False - while not self.exit_event.is_set(): - if self.led_map_2d_queue.empty(): + leds_2d = [] + while not self._exit_event.is_set(): + + if not self._input_queue.empty(): + led = self._input_queue.get() + leds_2d.append(led) + update_required = True + + else: if not update_required: continue - leds_3d = sfm(self.leds) + leds_3d = sfm(leds_2d) if len(leds_3d) == 0: continue add_normals(leds_3d) - if self.rescale: - rescale(leds_3d) + rescale(leds_3d) - if self.recenter: - recenter(leds_3d) + recenter(leds_3d) - self.led_map_3d_queue.put(leds_3d) + self._output_queue.put(leds_3d) update_required = False - else: - led = self.led_map_2d_queue.get() - self.leds.append(led) - update_required = True diff --git a/marimapper/utils.py b/marimapper/utils.py index 4b4a4a4..92166a0 100644 --- a/marimapper/utils.py +++ b/marimapper/utils.py @@ -4,7 +4,7 @@ import importlib.util from inspect import signature -from marimapper import multiprocessing_logging as logging +from marimapper import logging as logging def add_camera_args(parser): diff --git a/marimapper/visualize_model.py b/marimapper/visualize_process.py similarity index 84% rename from marimapper/visualize_model.py rename to marimapper/visualize_process.py index 39362d9..fe60acf 100644 --- a/marimapper/visualize_model.py +++ b/marimapper/visualize_process.py @@ -1,8 +1,10 @@ import numpy as np import open3d -from marimapper import multiprocessing_logging as logging +from marimapper import logging as logging from multiprocessing import Process, Event from marimapper.led import LED3D, View +import time + def get_all_views(leds: list[LED3D]) -> list[View]: views = [] @@ -13,43 +15,43 @@ def get_all_views(leds: list[LED3D]) -> list[View]: return views -class Renderer3D(Process): - def __init__(self, led_map_3d_queue): +class VisualiseProcess(Process): + + def __init__(self, input_queue): logging.debug("Renderer3D initialising") super().__init__() self._vis = None - self.exit_event = Event() - self.led_map_3d_queue = led_map_3d_queue + self._input_queue = input_queue + self._exit_event = Event() self.point_cloud = None self.line_set = None self.strip_set = None logging.debug("Renderer3D initialised") - def shutdown(self): - self.exit_event.set() + def stop(self): + self._exit_event.set() def run(self): logging.debug("Renderer3D process starting") + # wait for data to arrive sensibly + while self._input_queue.empty(): + if self._exit_event.is_set(): + return + time.sleep(0.1) + self.initialise_visualiser__() self.reload_geometry__(True) - while not self.exit_event.is_set(): + while not self._exit_event.is_set(): - if not self.led_map_3d_queue.empty(): + if not self._input_queue.empty(): self.reload_geometry__() - window_closed = not self._vis.poll_events() - - if window_closed: - logging.debug("Renderer3D process window closed, stopping process") - self.exit_event.set() - + self._vis.poll_events() self._vis.update_renderer() - self._vis.destroy_window() - def initialise_visualiser__(self): logging.debug("Renderer3D process initialising visualiser") @@ -82,7 +84,7 @@ def reload_geometry__(self, first=False): logging.debug("Renderer3D process reloading geometry") - leds = self.led_map_3d_queue.get() + leds = self._input_queue.get() logging.debug(f"Fetched led map with size {len(leds)}") all_views = get_all_views(leds) @@ -139,7 +141,9 @@ def view_to_points_lines_colors(views): # returns points and lines for i, view in enumerate(views): - points_in_world = [(view.rotation @ p + view.position) for p in camera_cone_points] + points_in_world = [ + (view.rotation @ p + view.position) for p in camera_cone_points + ] offset = i * len(camera_cone_points) diff --git a/test/mock_camera.py b/test/mock_camera.py index 1cf3887..4f9160b 100644 --- a/test/mock_camera.py +++ b/test/mock_camera.py @@ -1,4 +1,4 @@ -from marimapper import multiprocessing_logging as logging +from marimapper import logging as logging import cv2 import numpy as np diff --git a/test/test_detector_process.py b/test/test_detector_process.py index 0397e84..617ae04 100644 --- a/test/test_detector_process.py +++ b/test/test_detector_process.py @@ -23,8 +23,6 @@ def test_detector_process_basic(monkeypatch): assert results.point.u() == pytest.approx(0.4029418361244019) assert results.point.v() == pytest.approx(0.4029538809144072) - detector_process.exit_event.set() - if __name__ == "__main__": test_detector_process_basic() diff --git a/test/test_rescale.py b/test/test_rescale.py new file mode 100644 index 0000000..6560c69 --- /dev/null +++ b/test/test_rescale.py @@ -0,0 +1,11 @@ +from marimapper.led import rescale +from marimapper.sfm import sfm +from marimapper.file_tools import get_all_2d_led_maps + + +def test_rescale_basic(): + maps = get_all_2d_led_maps("scan") + + map_3d = sfm(maps) + + rescale(map_3d) diff --git a/test/test_sfm_process.py b/test/test_sfm_process.py index 90734d0..822b882 100644 --- a/test/test_sfm_process.py +++ b/test/test_sfm_process.py @@ -21,8 +21,6 @@ def test_sfm_process_basic(): leds_3d = queue_3d.get() # 320ms assert len(leds_3d) == 21 - sfm_process.exit_event.set() - if __name__ == "__main__": test_sfm_process_basic() From df2f9629f4a423ecc73b78565ace808d88407b32 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sat, 19 Oct 2024 12:04:55 +0100 Subject: [PATCH 11/26] Fixed tests --- marimapper/sfm_process.py | 3 +++ test/test_sfm_process.py | 11 +++++------ 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/marimapper/sfm_process.py b/marimapper/sfm_process.py index 05e2ed7..dbd2e5f 100644 --- a/marimapper/sfm_process.py +++ b/marimapper/sfm_process.py @@ -46,6 +46,9 @@ def add_detection(self, led: LED2D): def get_output_queue(self): return self._output_queue + def get_results(self): + return self._output_queue.get() + def stop(self): self._exit_event.set() diff --git a/test/test_sfm_process.py b/test/test_sfm_process.py index 822b882..8e84784 100644 --- a/test/test_sfm_process.py +++ b/test/test_sfm_process.py @@ -6,21 +6,20 @@ def test_sfm_process_basic(): - queue_2d = Queue() - queue_3d = Queue() - - sfm_process = SFM(queue_2d, queue_3d) + sfm_process = SFM() sfm_process.start() leds = get_all_2d_led_maps("scan") for led in leds: # 2ms - queue_2d.put(led) + sfm_process.add_detection(led) time.sleep(1) # wait for all to be consumed - leds_3d = queue_3d.get() # 320ms + leds_3d = sfm_process.get_results() assert len(leds_3d) == 21 + sfm_process.stop() + if __name__ == "__main__": test_sfm_process_basic() From b10c4aa30f6a2e4a35f96f6f1a05dded6af37296 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sat, 19 Oct 2024 13:58:12 +0100 Subject: [PATCH 12/26] I can reconstruct! merging duplicates is borked though :( --- marimapper/detector.py | 5 ++++- marimapper/detector_process.py | 6 ++++++ marimapper/led.py | 3 ++- marimapper/model.py | 7 +++++-- marimapper/scanner.py | 8 ++++---- test/test_sfm_process.py | 1 - 6 files changed, 21 insertions(+), 9 deletions(-) diff --git a/marimapper/detector.py b/marimapper/detector.py index 1e63abf..967ea5a 100644 --- a/marimapper/detector.py +++ b/marimapper/detector.py @@ -46,6 +46,9 @@ def draw_led_detections(image: cv2.Mat, led_detection: Point2D) -> cv2.Mat: image if len(image.shape) == 3 else cv2.cvtColor(image, cv2.COLOR_GRAY2BGR) ) + if led_detection is None: + return render_image + img_height = render_image.shape[0] img_width = render_image.shape[1] @@ -101,7 +104,7 @@ def find_led( image = cam.read() results = find_led_in_image(image, threshold) - if display and results: + if display: rendered_image = draw_led_detections(image, results) show_image(rendered_image) diff --git a/marimapper/detector_process.py b/marimapper/detector_process.py index cfdb2e0..edc6a8a 100644 --- a/marimapper/detector_process.py +++ b/marimapper/detector_process.py @@ -26,6 +26,7 @@ def __init__( super().__init__() self._detection_request = Queue() # {led_id, view_id} self._detection_result = Queue() # LED3D + self._led_count = Queue() self._exit_event = Event() self._device = device @@ -41,6 +42,9 @@ def detect(self, led_id: int, view_id: int): def get_results(self) -> LED2D: return self._detection_result.get() + def get_led_count(self): + return self._led_count.get() + def stop(self): self._exit_event.set() @@ -48,6 +52,8 @@ def run(self): led_backend = get_backend(self._led_backend_name, self._led_backend_server) + self._led_count.put(led_backend.get_led_count()) + cam = Camera(self._device) timeout_controller = TimeoutController() diff --git a/marimapper/led.py b/marimapper/led.py index 3fbaf5d..23c4ffa 100644 --- a/marimapper/led.py +++ b/marimapper/led.py @@ -219,8 +219,9 @@ def merge(leds: list[LED3D]) -> LED3D: return new_led +# currently broken def remove_duplicates(leds: list[LED3D]) -> list[LED3D]: - + raise NotImplementedError("currently broken") new_leds = [] for led in leds: diff --git a/marimapper/model.py b/marimapper/model.py index f6ec266..11a90db 100644 --- a/marimapper/model.py +++ b/marimapper/model.py @@ -6,7 +6,9 @@ read_points3D_binary, ) -from marimapper.led import LED3D, remove_duplicates, View +from marimapper.led import LED3D, View + +# from marimapper.led import remove_duplicates def binary_to_led_map_3d(path: os.path) -> list[LED3D]: @@ -37,6 +39,7 @@ def binary_to_led_map_3d(path: os.path) -> list[LED3D]: leds.append(led) - leds = remove_duplicates(leds) + # currently broken + # leds = remove_duplicates(leds) return leds diff --git a/marimapper/scanner.py b/marimapper/scanner.py index 20fb20c..79b5256 100644 --- a/marimapper/scanner.py +++ b/marimapper/scanner.py @@ -21,8 +21,6 @@ def __init__(self, cli_args): self.output_dir = cli_args.dir os.makedirs(self.output_dir, exist_ok=True) - self.led_id_range = range(cli_args.start, cli_args.end) - self.led_map_3d_queue = Queue() self.detector = DetectorProcess( @@ -47,6 +45,10 @@ def __init__(self, cli_args): self.renderer3d.start() self.detector.start() + self.led_id_range = range( + cli_args.start, min(cli_args.end, self.detector.get_led_count()) + ) + def close(self): logging.debug("marimapper closing") @@ -86,8 +88,6 @@ def mainloop(self): if led.point is None: continue - print(f"found {led}") - leds.append(led) self.sfm.add_detection(led) diff --git a/test/test_sfm_process.py b/test/test_sfm_process.py index 8e84784..7849272 100644 --- a/test/test_sfm_process.py +++ b/test/test_sfm_process.py @@ -1,6 +1,5 @@ from marimapper.file_tools import get_all_2d_led_maps from marimapper.sfm_process import SFM -from multiprocessing import Queue import time From 298e498c5de4af47c5d3b577ce8147261b0df7be Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 20 Oct 2024 16:30:08 +0100 Subject: [PATCH 13/26] tidied up logging --- .github/workflows/test_mac.yml | 3 +- .github/workflows/test_ubuntu.yml | 3 +- .github/workflows/test_windows.yml | 3 +- marimapper/backends/fcmega/fcmega.py | 4 +- .../backends/pixelblaze/pixelblaze_backend.py | 4 +- .../pixelblaze/upload_map_to_pixelblaze.py | 4 +- marimapper/camera.py | 49 ++++++------------- marimapper/database_populator.py | 6 ++- marimapper/detector.py | 13 +++-- marimapper/detector_process.py | 5 +- marimapper/led.py | 1 - marimapper/logging.py | 35 ------------- marimapper/scanner.py | 12 +++-- marimapper/scripts/check_backend_cli.py | 4 +- marimapper/scripts/check_camera_cli.py | 4 +- marimapper/scripts/scanner_cli.py | 18 +++++-- marimapper/sfm.py | 5 +- marimapper/sfm_process.py | 4 +- marimapper/utils.py | 4 +- marimapper/visualize_process.py | 17 +++++-- test/mock_camera.py | 5 +- test/test_sfm_process.py | 14 ++---- 22 files changed, 106 insertions(+), 111 deletions(-) delete mode 100644 marimapper/logging.py diff --git a/.github/workflows/test_mac.yml b/.github/workflows/test_mac.yml index 2b858d6..a77d5a9 100644 --- a/.github/workflows/test_mac.yml +++ b/.github/workflows/test_mac.yml @@ -6,7 +6,7 @@ on: paths-ignore: - "README.md" - "docs/**" - - "scripts/**" + - "marimapper/scripts/**" pull_request: branches: [ "main" ] @@ -37,4 +37,5 @@ jobs: - name: Pytest run: | + cd test pytest . \ No newline at end of file diff --git a/.github/workflows/test_ubuntu.yml b/.github/workflows/test_ubuntu.yml index e748aa7..06221aa 100644 --- a/.github/workflows/test_ubuntu.yml +++ b/.github/workflows/test_ubuntu.yml @@ -6,7 +6,7 @@ on: paths-ignore: - "README.md" - "docs/**" - - "scripts/**" + - "marimapper/scripts/**" pull_request: branches: [ "main" ] @@ -37,4 +37,5 @@ jobs: - name: Pytest run: | + cd test pytest . \ No newline at end of file diff --git a/.github/workflows/test_windows.yml b/.github/workflows/test_windows.yml index b4b2cff..85ebb06 100644 --- a/.github/workflows/test_windows.yml +++ b/.github/workflows/test_windows.yml @@ -6,7 +6,7 @@ on: paths-ignore: - "README.md" - "docs/**" - - "scripts/**" + - "marimapper/scripts/**" pull_request: branches: [ "main" ] @@ -37,4 +37,5 @@ jobs: - name: Pytest run: | + cd test pytest . \ No newline at end of file diff --git a/marimapper/backends/fcmega/fcmega.py b/marimapper/backends/fcmega/fcmega.py index 8f8e543..efec4aa 100644 --- a/marimapper/backends/fcmega/fcmega.py +++ b/marimapper/backends/fcmega/fcmega.py @@ -1,7 +1,9 @@ import serial import struct import serial.tools.list_ports -from marimapper import logging as logging +from multiprocessing import get_logger + +logging = get_logger() class FCMega: diff --git a/marimapper/backends/pixelblaze/pixelblaze_backend.py b/marimapper/backends/pixelblaze/pixelblaze_backend.py index 0c15e6b..47d7078 100644 --- a/marimapper/backends/pixelblaze/pixelblaze_backend.py +++ b/marimapper/backends/pixelblaze/pixelblaze_backend.py @@ -1,6 +1,8 @@ -from marimapper import logging as logging +from multiprocessing import get_logger import pixelblaze +logging = get_logger() + class Backend: diff --git a/marimapper/backends/pixelblaze/upload_map_to_pixelblaze.py b/marimapper/backends/pixelblaze/upload_map_to_pixelblaze.py index 8e3fa10..a81082b 100644 --- a/marimapper/backends/pixelblaze/upload_map_to_pixelblaze.py +++ b/marimapper/backends/pixelblaze/upload_map_to_pixelblaze.py @@ -1,7 +1,9 @@ import csv from marimapper import utils -from marimapper import logging as logging +from multiprocessing import get_logger + +logging = get_logger() def read_coordinates_from_csv(csv_file_name): diff --git a/marimapper/camera.py b/marimapper/camera.py index 555aeed..0cd3617 100644 --- a/marimapper/camera.py +++ b/marimapper/camera.py @@ -1,5 +1,7 @@ import cv2 -from marimapper import logging as logging +from multiprocessing import get_logger + +logger = get_logger() class CameraSettings: @@ -21,14 +23,14 @@ def apply(self, camera): class Camera: def __init__(self, device_id): - logging.info(f"Connecting to camera {device_id} ...") + logger.info(f"Connecting to device {device_id} ...") self.device_id = device_id for capture_method in [cv2.CAP_DSHOW, cv2.CAP_V4L2, cv2.CAP_ANY]: self.device = cv2.VideoCapture(device_id, capture_method) if self.device.isOpened(): - logging.debug( - f"Connected to camera {device_id} with capture method {capture_method}" + logger.debug( + f"Connected to device {device_id} with capture method {capture_method}" ) break @@ -63,54 +65,36 @@ def get_exposure(self): def get_gain(self): return int(self.device.get(cv2.CAP_PROP_GAIN)) - def set_resolution(self, width, height): - - logging.debug(f"Setting camera resolution to {width} x {height} ...") - - self.device.set(cv2.CAP_PROP_FRAME_WIDTH, width) - self.device.set(cv2.CAP_PROP_FRAME_HEIGHT, height) - - new_width = self.get_width() - new_height = self.get_height() - - # this is cov ignored as it's a strange position to be in but ultimately fine - if width != new_width or height != new_height: # pragma: no cover - logging.error( - f"Failed to set camera {self.device_id} resolution to {width} x {height}", - ) - - logging.debug(f"Camera resolution set to {new_width} x {new_height}") - def set_autofocus(self, mode, focus=0): - logging.debug(f"Setting autofocus to mode {mode} with focus {focus}") + logger.debug(f"Setting autofocus to mode {mode} with focus {focus}") if not self.device.set(cv2.CAP_PROP_AUTOFOCUS, mode): - logging.error(f"Failed to set autofocus to {mode}") + logger.error(f"Failed to set autofocus to {mode}") if not self.device.set(cv2.CAP_PROP_FOCUS, focus): - logging.error(f"Failed to set focus to {focus}") + logger.error(f"Failed to set focus to {focus}") def set_exposure_mode(self, mode): - logging.debug(f"Setting exposure to mode {mode}") + logger.debug(f"Setting exposure to mode {mode}") if not self.device.set(cv2.CAP_PROP_AUTO_EXPOSURE, mode): - logging.error(f"Failed to put camera into manual exposure mode {mode}") + logger.error(f"Failed to put camera into manual exposure mode {mode}") def set_gain(self, gain): - logging.debug(f"Setting gain to {gain}") + logger.debug(f"Setting gain to {gain}") if not self.device.set(cv2.CAP_PROP_GAIN, gain): - logging.error(f"failed to set camera gain to {gain}") + logger.error(f"failed to set camera gain to {gain}") def set_exposure(self, exposure): - logging.debug(f"Setting exposure to {exposure}") + logger.debug(f"Setting exposure to {exposure}") if not self.device.set(cv2.CAP_PROP_EXPOSURE, exposure): - logging.error(f"Failed to set exposure to {exposure}") + logger.error(f"Failed to set exposure to {exposure}") def eat(self, count=30): for _ in range(count): @@ -119,7 +103,6 @@ def eat(self, count=30): def read(self, color=False): ret_val, image = self.device.read() if not ret_val: - logging.error("Failed to grab frame") - return None + raise Exception("Failed to read image") return image diff --git a/marimapper/database_populator.py b/marimapper/database_populator.py index fc0bae8..e49fa3f 100644 --- a/marimapper/database_populator.py +++ b/marimapper/database_populator.py @@ -1,7 +1,7 @@ from itertools import combinations from math import radians, tan import os - +from multiprocessing import get_logger import numpy as np from marimapper.pycolmap_tools.database import COLMAPDatabase @@ -9,9 +9,11 @@ ARBITRARY_SCALE = 2000 +logger = get_logger() -def populate_database(db_path: os.path, leds: list[LED2D]): +def populate_database(db_path: os.path, leds: list[LED2D]): + logger.debug(f"Populating sfm database with {len(leds)} leds, path: {db_path}") views = get_view_ids(leds) map_features = np.zeros((max(views) + 1, 1, 2)) diff --git a/marimapper/detector.py b/marimapper/detector.py index 967ea5a..b76c859 100644 --- a/marimapper/detector.py +++ b/marimapper/detector.py @@ -1,15 +1,17 @@ import cv2 import time import typing +from multiprocessing import get_logger from marimapper.camera import Camera from marimapper.timeout_controller import TimeoutController from marimapper.led import Point2D, LED2D -DETECTOR_WINDOW_NAME = "MariMapper - Detector" +logger = get_logger() def find_led_in_image(image: cv2.Mat, threshold: int = 128) -> typing.Optional[Point2D]: + logger.debug("looking for led in image") if len(image.shape) > 2: image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) @@ -21,6 +23,7 @@ def find_led_in_image(image: cv2.Mat, threshold: int = 128) -> typing.Optional[P led_response_count = len(contours) if led_response_count == 0: + logger.debug("could not find led") return None moments = cv2.moments(image_thresh) @@ -37,11 +40,13 @@ def find_led_in_image(image: cv2.Mat, threshold: int = 128) -> typing.Optional[P brightness = 1.0 + logger.debug(f"found led at {center_u} {center_v} with brightness {brightness}") + return Point2D(center_u, center_v, contours, brightness) # todo, normalise contours def draw_led_detections(image: cv2.Mat, led_detection: Point2D) -> cv2.Mat: - + logger.debug("drawing detection") render_image = ( image if len(image.shape) == 3 else cv2.cvtColor(image, cv2.COLOR_GRAY2BGR) ) @@ -73,7 +78,7 @@ def draw_led_detections(image: cv2.Mat, led_detection: Point2D) -> cv2.Mat: def show_image(image: cv2.Mat) -> None: - cv2.imshow(DETECTOR_WINDOW_NAME, image) + cv2.imshow("MariMapper - Detector", image) key = cv2.waitKey(1) if key == 27: # esc @@ -82,6 +87,7 @@ def show_image(image: cv2.Mat) -> None: def set_cam_default(cam: Camera) -> None: if cam.state != "default": + logger.info("resetting cam to default") cam.reset() cam.eat() cam.state = "default" @@ -89,6 +95,7 @@ def set_cam_default(cam: Camera) -> None: def set_cam_dark(cam: Camera, exposure: int) -> None: if cam.state != "dark": + logger.info("setting cam to dark mode") cam.set_autofocus(0, 0) cam.set_exposure_mode(0) cam.set_gain(0) diff --git a/marimapper/detector_process.py b/marimapper/detector_process.py index edc6a8a..90a5c93 100644 --- a/marimapper/detector_process.py +++ b/marimapper/detector_process.py @@ -9,7 +9,10 @@ ) from marimapper.led import LED2D from marimapper.utils import get_backend -from marimapper import logging + +from multiprocessing import get_logger + +logging = get_logger() class DetectorProcess(Process): diff --git a/marimapper/led.py b/marimapper/led.py index 23c4ffa..a8dba59 100644 --- a/marimapper/led.py +++ b/marimapper/led.py @@ -137,7 +137,6 @@ def find_inter_led_distance(leds: list[LED2D | LED3D]): def rescale(leds: list[LED3D], target_inter_distance=1.0) -> None: inter_led_distance = find_inter_led_distance(leds) - print(inter_led_distance) scale = (1.0 / inter_led_distance) * target_inter_distance for led in leds: led.point *= scale diff --git a/marimapper/logging.py b/marimapper/logging.py deleted file mode 100644 index 518faad..0000000 --- a/marimapper/logging.py +++ /dev/null @@ -1,35 +0,0 @@ -# You might be wondering why this file exists. -# Well, this is because the default logging library does //not// like multiprocessing, so to save -# my sanity, I've implemented the worlds most basic logging library so that I can at least see something -# No this isn't multiprocessing "safe" but again, it's something - - -class Col: - PURPLE = "\033[95m" - BLUE = "\033[94m" - CYAN = "\033[96m" - GREEN = "\033[92m" - WARNING = "\033[93m" - FAIL = "\033[91m" - BOLD = "\033[1m" - UNDERLINE = "\033[4m" - - -def colorise(string, string_format): - return f"{string_format}{string}\033[0m" - - -def debug(string): - print(colorise(string, Col.BOLD)) - - -def error(string): - print(colorise(string, Col.FAIL)) - - -def warn(string): - print(colorise(string, Col.WARNING)) - - -def info(string): - print(string) diff --git a/marimapper/scanner.py b/marimapper/scanner.py index 79b5256..05d1b10 100644 --- a/marimapper/scanner.py +++ b/marimapper/scanner.py @@ -7,17 +7,20 @@ from tqdm import tqdm from pathlib import Path from marimapper.detector_process import DetectorProcess -from marimapper import logging +from multiprocessing import get_logger from marimapper.file_tools import get_all_2d_led_maps, write_2d_leds_to_file from marimapper.utils import get_user_confirmation from marimapper.visualize_process import VisualiseProcess from multiprocessing import Queue from marimapper.led import last_view +logger = get_logger() + class Scanner: def __init__(self, cli_args): + logger.debug("initialising scanner") self.output_dir = cli_args.dir os.makedirs(self.output_dir, exist_ok=True) @@ -49,8 +52,10 @@ def __init__(self, cli_args): cli_args.start, min(cli_args.end, self.detector.get_led_count()) ) + logger.debug("scanner initialised") + def close(self): - logging.debug("marimapper closing") + logger.debug("scanner closing") self.detector.stop() self.sfm.stop() @@ -60,7 +65,7 @@ def close(self): self.renderer3d.join() self.detector.join() - logging.debug("marimapper closed") + logger.debug("scanner closed") def mainloop(self): @@ -69,6 +74,7 @@ def mainloop(self): start_scan = get_user_confirmation("Start scan? [y/n]: ") if not start_scan: + print("exiting") return leds = [] diff --git a/marimapper/scripts/check_backend_cli.py b/marimapper/scripts/check_backend_cli.py index e78b879..31d229e 100644 --- a/marimapper/scripts/check_backend_cli.py +++ b/marimapper/scripts/check_backend_cli.py @@ -1,8 +1,10 @@ import argparse import time -from marimapper import logging as logging +from multiprocessing import get_logger from marimapper import utils +logging = get_logger() + def main(): diff --git a/marimapper/scripts/check_camera_cli.py b/marimapper/scripts/check_camera_cli.py index 8c2d391..29d6ebd 100644 --- a/marimapper/scripts/check_camera_cli.py +++ b/marimapper/scripts/check_camera_cli.py @@ -3,7 +3,9 @@ from marimapper.detector import find_led, set_cam_dark from marimapper.utils import add_camera_args from marimapper.camera import Camera -from marimapper import logging as logging +from multiprocessing import get_logger + +logging = get_logger() def main(): diff --git a/marimapper/scripts/scanner_cli.py b/marimapper/scripts/scanner_cli.py index 0c19d1e..cc974fd 100644 --- a/marimapper/scripts/scanner_cli.py +++ b/marimapper/scripts/scanner_cli.py @@ -1,8 +1,9 @@ from marimapper.scanner import Scanner -from marimapper import logging +from multiprocessing import log_to_stderr import os import signal import argparse +import logging from marimapper.utils import add_camera_args, add_backend_args # PYCHARM DEVELOPER WARNING! @@ -10,26 +11,37 @@ # really weird stuff happens with multiprocessing! +logger = log_to_stderr() + + def main(): - logging.info("Starting MariMapper") + logger.info("Starting MariMapper") parser = argparse.ArgumentParser(description="Captures LED flashes to file") add_camera_args(parser) add_backend_args(parser) + parser.add_argument("-v", "--verbose", action="store_true") + parser.add_argument( "--dir", type=str, help="The output folder for your capture", default="." ) args = parser.parse_args() + if not os.path.isdir(args.dir): + raise Exception(f"path {args.dir} does not exist") + + if args.verbose: + logger.setLevel(logging.DEBUG) + scanner = Scanner(cli_args=args) scanner.mainloop() scanner.close() - # For some reason python refuses to actually exit here, so I'm brute forcing it + # For some reason python refuses to actually exit here when an error is thrown, so I'm brute forcing it os.kill(os.getpid(), signal.SIGINT) os.kill(os.getpid(), signal.CTRL_C_EVENT) diff --git a/marimapper/sfm.py b/marimapper/sfm.py index e108a23..bbfc013 100644 --- a/marimapper/sfm.py +++ b/marimapper/sfm.py @@ -7,6 +7,9 @@ from marimapper.led import LED3D from marimapper.model import binary_to_led_map_3d from marimapper.utils import SupressLogging +from multiprocessing import get_logger + +logger = get_logger() def sfm(leds) -> list[LED3D]: @@ -34,5 +37,5 @@ def sfm(leds) -> list[LED3D]: return [] leds = binary_to_led_map_3d(temp_dir) - print(f"sfm managed to reconstruct {len(leds)} leds") + logger.debug(f"sfm managed to reconstruct {len(leds)} leds") return leds diff --git a/marimapper/sfm_process.py b/marimapper/sfm_process.py index dbd2e5f..b71fadb 100644 --- a/marimapper/sfm_process.py +++ b/marimapper/sfm_process.py @@ -1,10 +1,12 @@ -from multiprocessing import Process, Event, Queue +from multiprocessing import Process, Event, Queue, get_logger from marimapper.led import LED2D, rescale, recenter, LED3D from marimapper.sfm import sfm import open3d import numpy as np import math +logger = get_logger() + # this is here for now as there is some weird import dependency going on... def add_normals(leds: list[LED3D]): diff --git a/marimapper/utils.py b/marimapper/utils.py index 92166a0..ae019b8 100644 --- a/marimapper/utils.py +++ b/marimapper/utils.py @@ -4,8 +4,6 @@ import importlib.util from inspect import signature -from marimapper import logging as logging - def add_camera_args(parser): parser.add_argument( @@ -54,7 +52,7 @@ def add_backend_args(parser): def get_user_confirmation(prompt): # pragma: no coverage try: - uin = input(logging.colorise(prompt, logging.Col.BLUE)) + uin = input(prompt) while uin.lower() not in ("y", "n"): uin = input() diff --git a/marimapper/visualize_process.py b/marimapper/visualize_process.py index fe60acf..5f2af29 100644 --- a/marimapper/visualize_process.py +++ b/marimapper/visualize_process.py @@ -1,10 +1,14 @@ import numpy as np import open3d -from marimapper import logging as logging +from multiprocessing import get_logger from multiprocessing import Process, Event from marimapper.led import LED3D, View import time +logging = get_logger() + +#Temporary fix to stop the zero points issue when visualising +open3d.utility.set_verbosity_level(open3d.utility.VerbosityLevel.Error) def get_all_views(leds: list[LED3D]) -> list[View]: views = [] @@ -62,10 +66,6 @@ def initialise_visualiser__(self): height=640, ) - self.point_cloud = open3d.geometry.PointCloud() - self.line_set = open3d.geometry.LineSet() - self.strip_set = open3d.geometry.LineSet() - view_ctl = self._vis.get_view_control() view_ctl.set_up((0, 1, 0)) view_ctl.set_lookat((0, 0, 0)) @@ -91,6 +91,13 @@ def reload_geometry__(self, first=False): p, l, c = view_to_points_lines_colors(all_views) + if self.point_cloud is None: + self.point_cloud = open3d.geometry.PointCloud() + if self.line_set is None: + self.line_set = open3d.geometry.LineSet() + if self.strip_set is None: + self.strip_set = open3d.geometry.LineSet() + self.line_set.points = open3d.utility.Vector3dVector(p) self.line_set.lines = open3d.utility.Vector2iVector(l) self.line_set.colors = open3d.utility.Vector3dVector(c) diff --git a/test/mock_camera.py b/test/mock_camera.py index 4f9160b..45f12d1 100644 --- a/test/mock_camera.py +++ b/test/mock_camera.py @@ -1,7 +1,10 @@ -from marimapper import logging as logging import cv2 import numpy as np +from multiprocessing import get_logger + +logging = get_logger() + class MockCamera: diff --git a/test/test_sfm_process.py b/test/test_sfm_process.py index 7849272..06fa8c1 100644 --- a/test/test_sfm_process.py +++ b/test/test_sfm_process.py @@ -1,24 +1,16 @@ from marimapper.file_tools import get_all_2d_led_maps from marimapper.sfm_process import SFM -import time def test_sfm_process_basic(): - sfm_process = SFM() - sfm_process.start() + for led in get_all_2d_led_maps("scan"): + sfm_process.add_detection(led) - leds = get_all_2d_led_maps("scan") + sfm_process.start() - for led in leds: # 2ms - sfm_process.add_detection(led) - time.sleep(1) # wait for all to be consumed leds_3d = sfm_process.get_results() assert len(leds_3d) == 21 sfm_process.stop() - - -if __name__ == "__main__": - test_sfm_process_basic() From dda9b920cdd5364742d3f1407ca1794f075b1c92 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 20 Oct 2024 17:56:07 +0100 Subject: [PATCH 14/26] tidied up logging tidied up multiprocessing --- README.md | 5 ++-- marimapper/backends/fcmega/fcmega.py | 4 +-- .../backends/pixelblaze/pixelblaze_backend.py | 4 +-- .../pixelblaze/upload_map_to_pixelblaze.py | 10 +++---- marimapper/detector_process.py | 13 +++++++-- marimapper/scanner.py | 3 -- marimapper/scripts/check_backend_cli.py | 19 +++++++++---- marimapper/scripts/check_camera_cli.py | 12 ++++++-- marimapper/scripts/scanner_cli.py | 6 +--- marimapper/sfm_process.py | 8 ++++++ marimapper/visualize_process.py | 21 +++++++------- test/mock_camera.py | 4 +-- test/test_detector_process.py | 28 ------------------- test/test_script_import.py | 26 ----------------- test/test_sfm_process.py | 16 ----------- 15 files changed, 66 insertions(+), 113 deletions(-) delete mode 100644 test/test_detector_process.py delete mode 100644 test/test_script_import.py delete mode 100644 test/test_sfm_process.py diff --git a/README.md b/README.md index 991d0c2..7cd8ba0 100644 --- a/README.md +++ b/README.md @@ -131,10 +131,9 @@ Fill out the blanks and check it by running `marimapper_check_backend --backend ## Step 3: [It's time to thunderize!](https://youtu.be/-5KJiHc3Nuc?t=121) -Run `marimapper --dir my_scan --backend fadecandy` +In a new folder, run `marimapper --backend fadecandy` -Change `my_scan` to the directory you want to save your scan -and `fadecandy` to whatever backend you're using +and `fadecandy` to whatever backend you're using and use `--help` to show more options Set up your LEDs so most of them are in view and when you're ready, type `y` when prompted with `Start scan? [y/n]` diff --git a/marimapper/backends/fcmega/fcmega.py b/marimapper/backends/fcmega/fcmega.py index efec4aa..ba82bdb 100644 --- a/marimapper/backends/fcmega/fcmega.py +++ b/marimapper/backends/fcmega/fcmega.py @@ -3,7 +3,7 @@ import serial.tools.list_ports from multiprocessing import get_logger -logging = get_logger() +logger = get_logger() class FCMega: @@ -26,7 +26,7 @@ def __init__(self, port=None): def _get_port(self): for device in serial.tools.list_ports.comports(): if device.serial_number.startswith("FCM"): - logging.info(f"found port {device.name}") + logger.info(f"found port {device.name}") return device.name return None diff --git a/marimapper/backends/pixelblaze/pixelblaze_backend.py b/marimapper/backends/pixelblaze/pixelblaze_backend.py index 47d7078..63543ea 100644 --- a/marimapper/backends/pixelblaze/pixelblaze_backend.py +++ b/marimapper/backends/pixelblaze/pixelblaze_backend.py @@ -1,7 +1,7 @@ from multiprocessing import get_logger import pixelblaze -logging = get_logger() +logger = get_logger() class Backend: @@ -14,7 +14,7 @@ def __init__(self, pixelblaze_ip="4.3.2.1"): def get_led_count(self): pixel_count = self.pb.getPixelCount() - logging.info(f"Pixelblaze reports {pixel_count} pixels") + logger.info(f"Pixelblaze reports {pixel_count} pixels") return pixel_count def set_led(self, led_index: int, on: bool): diff --git a/marimapper/backends/pixelblaze/upload_map_to_pixelblaze.py b/marimapper/backends/pixelblaze/upload_map_to_pixelblaze.py index a81082b..751fd45 100644 --- a/marimapper/backends/pixelblaze/upload_map_to_pixelblaze.py +++ b/marimapper/backends/pixelblaze/upload_map_to_pixelblaze.py @@ -3,11 +3,11 @@ from marimapper import utils from multiprocessing import get_logger -logging = get_logger() +logger = get_logger() def read_coordinates_from_csv(csv_file_name): - logging.info(f"Loading coordinates from {csv_file_name}") + logger.info(f"Loading coordinates from {csv_file_name}") with open(csv_file_name, newline="") as csvfile: csv_reader = csv.DictReader(csvfile) list_of_leds = [] @@ -35,7 +35,7 @@ def read_coordinates_from_csv(csv_file_name): def upload_map_to_pixelblaze(cli_args): final_coordinate_list = read_coordinates_from_csv(cli_args.csv_file) - logging.info(final_coordinate_list) + logger.info(final_coordinate_list) upload_coordinates = utils.get_user_confirmation( "Upload coordinates to Pixelblaze? [y/n]: " @@ -43,9 +43,9 @@ def upload_map_to_pixelblaze(cli_args): if not upload_coordinates: return - logging.info( + logger.info( f"Uploading coordinates to pixelblaze {cli_args.server if cli_args.server is not None else ''}" ) led_backend = utils.get_backend("pixelblaze", cli_args.server) led_backend.set_map_coordinates(final_coordinate_list) - logging.info("Finished") + logger.info("Finished") diff --git a/marimapper/detector_process.py b/marimapper/detector_process.py index 90a5c93..7a9d0d1 100644 --- a/marimapper/detector_process.py +++ b/marimapper/detector_process.py @@ -12,7 +12,7 @@ from multiprocessing import get_logger -logging = get_logger() +logger = get_logger() class DetectorProcess(Process): @@ -29,7 +29,10 @@ def __init__( super().__init__() self._detection_request = Queue() # {led_id, view_id} self._detection_result = Queue() # LED3D + self._detection_request.cancel_join_thread() + self._detection_result.cancel_join_thread() self._led_count = Queue() + self._led_count.cancel_join_thread() self._exit_event = Event() self._device = device @@ -83,5 +86,11 @@ def run(self): image = cam.read() show_image(image) - logging.info("resetting cam!") + logger.info("resetting cam!") set_cam_default(cam) + + # clear the queues, don't ask why. + while not self._detection_request.empty(): + self._detection_request.get() + while not self._detection_result.empty(): + self._detection_result.get() diff --git a/marimapper/scanner.py b/marimapper/scanner.py index 05d1b10..c2aee62 100644 --- a/marimapper/scanner.py +++ b/marimapper/scanner.py @@ -11,7 +11,6 @@ from marimapper.file_tools import get_all_2d_led_maps, write_2d_leds_to_file from marimapper.utils import get_user_confirmation from marimapper.visualize_process import VisualiseProcess -from multiprocessing import Queue from marimapper.led import last_view logger = get_logger() @@ -24,8 +23,6 @@ def __init__(self, cli_args): self.output_dir = cli_args.dir os.makedirs(self.output_dir, exist_ok=True) - self.led_map_3d_queue = Queue() - self.detector = DetectorProcess( cli_args.device, cli_args.exposure, diff --git a/marimapper/scripts/check_backend_cli.py b/marimapper/scripts/check_backend_cli.py index 31d229e..c9d640b 100644 --- a/marimapper/scripts/check_backend_cli.py +++ b/marimapper/scripts/check_backend_cli.py @@ -1,9 +1,11 @@ import argparse import time -from multiprocessing import get_logger +from multiprocessing import log_to_stderr from marimapper import utils +import logging -logging = get_logger() +logger = log_to_stderr() +logger.setLevel(level=logging.INFO) def main(): @@ -21,23 +23,28 @@ def main(): default=0, ) + parser.add_argument("-v", "--verbose", action="store_true") + args = parser.parse_args() - logging.info(f"Loading {args.backend} backend") + if args.verbose: + logger.setLevel(logging.DEBUG) + + logger.info(f"Loading {args.backend} backend") led_backend = utils.get_backend(args.backend, args.server) - logging.info("Press ctrl-c to cancel") + logger.info("Press ctrl-c to cancel") while True: time.sleep(1) - logging.info(f"Turning on LED {args.reference_led}") + logger.info(f"Turning on LED {args.reference_led}") led_backend.set_led(args.reference_led, True) time.sleep(1) - logging.info(f"Turning off LED {args.reference_led}") + logger.info(f"Turning off LED {args.reference_led}") led_backend.set_led(args.reference_led, False) diff --git a/marimapper/scripts/check_camera_cli.py b/marimapper/scripts/check_camera_cli.py index 29d6ebd..57ae591 100644 --- a/marimapper/scripts/check_camera_cli.py +++ b/marimapper/scripts/check_camera_cli.py @@ -3,9 +3,11 @@ from marimapper.detector import find_led, set_cam_dark from marimapper.utils import add_camera_args from marimapper.camera import Camera -from multiprocessing import get_logger +from multiprocessing import log_to_stderr +import logging -logging = get_logger() +logger = log_to_stderr() +logger.setLevel(level=logging.INFO) def main(): @@ -15,14 +17,18 @@ def main(): ) add_camera_args(parser) + parser.add_argument("-v", "--verbose", action="store_true") args = parser.parse_args() + if args.verbose: + logger.setLevel(logging.DEBUG) + cam = Camera(args.device) set_cam_dark(cam, args.exposure) - logging.info( + logger.info( "Camera connected! Hold an LED up to the camera to check LED identification" ) diff --git a/marimapper/scripts/scanner_cli.py b/marimapper/scripts/scanner_cli.py index cc974fd..1906a49 100644 --- a/marimapper/scripts/scanner_cli.py +++ b/marimapper/scripts/scanner_cli.py @@ -1,7 +1,6 @@ from marimapper.scanner import Scanner from multiprocessing import log_to_stderr import os -import signal import argparse import logging from marimapper.utils import add_camera_args, add_backend_args @@ -12,6 +11,7 @@ logger = log_to_stderr() +logger.setLevel(level=logging.ERROR) def main(): @@ -41,10 +41,6 @@ def main(): scanner.mainloop() scanner.close() - # For some reason python refuses to actually exit here when an error is thrown, so I'm brute forcing it - os.kill(os.getpid(), signal.SIGINT) - os.kill(os.getpid(), signal.CTRL_C_EVENT) - if __name__ == "__main__": main() diff --git a/marimapper/sfm_process.py b/marimapper/sfm_process.py index b71fadb..5e12100 100644 --- a/marimapper/sfm_process.py +++ b/marimapper/sfm_process.py @@ -39,7 +39,9 @@ class SFM(Process): def __init__(self): super().__init__() self._output_queue = Queue() + self._output_queue.cancel_join_thread() self._input_queue = Queue() + self._input_queue.cancel_join_thread() self._exit_event = Event() def add_detection(self, led: LED2D): @@ -84,3 +86,9 @@ def run(self): self._output_queue.put(leds_3d) update_required = False + + # clear the queues, don't ask why. + while not self._input_queue.empty(): + self._input_queue.get() + while not self._output_queue.empty(): + self._output_queue.get() diff --git a/marimapper/visualize_process.py b/marimapper/visualize_process.py index 5f2af29..467074b 100644 --- a/marimapper/visualize_process.py +++ b/marimapper/visualize_process.py @@ -5,11 +5,12 @@ from marimapper.led import LED3D, View import time -logging = get_logger() +logger = get_logger() -#Temporary fix to stop the zero points issue when visualising +# Temporary fix to stop the zero points issue when visualising open3d.utility.set_verbosity_level(open3d.utility.VerbosityLevel.Error) + def get_all_views(leds: list[LED3D]) -> list[View]: views = [] for led in leds: @@ -23,7 +24,7 @@ def get_all_views(leds: list[LED3D]) -> list[View]: class VisualiseProcess(Process): def __init__(self, input_queue): - logging.debug("Renderer3D initialising") + logger.debug("Renderer3D initialising") super().__init__() self._vis = None self._input_queue = input_queue @@ -31,13 +32,13 @@ def __init__(self, input_queue): self.point_cloud = None self.line_set = None self.strip_set = None - logging.debug("Renderer3D initialised") + logger.debug("Renderer3D initialised") def stop(self): self._exit_event.set() def run(self): - logging.debug("Renderer3D process starting") + logger.debug("Renderer3D process starting") # wait for data to arrive sensibly while self._input_queue.empty(): @@ -57,7 +58,7 @@ def run(self): self._vis.update_renderer() def initialise_visualiser__(self): - logging.debug("Renderer3D process initialising visualiser") + logger.debug("Renderer3D process initialising visualiser") self._vis = open3d.visualization.Visualizer() self._vis.create_window( @@ -78,15 +79,15 @@ def initialise_visualiser__(self): ) render_options.background_color = [0.2, 0.2, 0.2] - logging.debug("Renderer3D process initialised visualiser") + logger.debug("Renderer3D process initialised visualiser") def reload_geometry__(self, first=False): - logging.debug("Renderer3D process reloading geometry") + logger.debug("Renderer3D process reloading geometry") leds = self._input_queue.get() - logging.debug(f"Fetched led map with size {len(leds)}") + logger.debug(f"Fetched led map with size {len(leds)}") all_views = get_all_views(leds) p, l, c = view_to_points_lines_colors(all_views) @@ -126,7 +127,7 @@ def reload_geometry__(self, first=False): self._vis.update_geometry(self.line_set) self._vis.update_geometry(self.strip_set) - logging.debug("Renderer3D process reloaded geometry") + logger.debug("Renderer3D process reloaded geometry") def view_to_points_lines_colors(views): # returns points and lines diff --git a/test/mock_camera.py b/test/mock_camera.py index 45f12d1..385e001 100644 --- a/test/mock_camera.py +++ b/test/mock_camera.py @@ -3,7 +3,7 @@ from multiprocessing import get_logger -logging = get_logger() +logger = get_logger() class MockCamera: @@ -12,7 +12,7 @@ def __init__(self, device_id): self.frame_id = 0 - logging.info(f"Connecting to camera {device_id} ...") + logger.info(f"Connecting to camera {device_id} ...") self.device_id = device_id self.device = cv2.VideoCapture(device_id) real_frames = [] diff --git a/test/test_detector_process.py b/test/test_detector_process.py deleted file mode 100644 index 617ae04..0000000 --- a/test/test_detector_process.py +++ /dev/null @@ -1,28 +0,0 @@ -import marimapper.camera -from marimapper.detector_process import DetectorProcess -import pytest -from mock_camera import MockCamera - - -# This is tricky because the capture sequence needs to include black scenes -# hmmmm -@pytest.mark.skip(reason="in progress") -def test_detector_process_basic(monkeypatch): - - monkeypatch.setattr(marimapper.camera.Camera, "Camera", MockCamera) - - device = "MariMapper-Test-Data/9_point_box/cam_0/capture_0000.png" - detector_process = DetectorProcess(device, 1, 128, "dummy", "none", display=False) - - detector_process.start() - - detector_process.detect(0, 0) - - results = detector_process.get_results() - - assert results.point.u() == pytest.approx(0.4029418361244019) - assert results.point.v() == pytest.approx(0.4029538809144072) - - -if __name__ == "__main__": - test_detector_process_basic() diff --git a/test/test_script_import.py b/test/test_script_import.py deleted file mode 100644 index d20f64d..0000000 --- a/test/test_script_import.py +++ /dev/null @@ -1,26 +0,0 @@ -# This is an incredibly simple test to just make sure that the scripts are interpretable by python, nothing more -import pytest - - -def test_check_backend_cli(): - from marimapper.scripts.check_backend_cli import main - - with pytest.raises(SystemExit): - main() # This should fail gracefully without any arguments - - -def test_check_camera_cli(): - from marimapper.scripts.check_camera_cli import main - - with pytest.raises(SystemExit): - main() # this should fail if no cameras are available - - -def test_upload_to_pixelblaze_cli(): - from marimapper.scripts.upload_map_to_pixelblaze_cli import main - - with pytest.raises(SystemExit): - main() # This should fail gracefully without any arguments - - -# Do not test scanner_cli due to it calling system level sig-kill commands! diff --git a/test/test_sfm_process.py b/test/test_sfm_process.py deleted file mode 100644 index 06fa8c1..0000000 --- a/test/test_sfm_process.py +++ /dev/null @@ -1,16 +0,0 @@ -from marimapper.file_tools import get_all_2d_led_maps -from marimapper.sfm_process import SFM - - -def test_sfm_process_basic(): - sfm_process = SFM() - - for led in get_all_2d_led_maps("scan"): - sfm_process.add_detection(led) - - sfm_process.start() - - leds_3d = sfm_process.get_results() - assert len(leds_3d) == 21 - - sfm_process.stop() From 440354d3c98bb3d639969f28d12a4083a55f3e4c Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 20 Oct 2024 19:06:39 +0100 Subject: [PATCH 15/26] just a bit of tidying --- README.md | 14 -------------- marimapper/scripts/scanner_cli.py | 4 ---- pyproject.toml | 4 ++-- 3 files changed, 2 insertions(+), 20 deletions(-) diff --git a/README.md b/README.md index 7cd8ba0..0e2a390 100644 --- a/README.md +++ b/README.md @@ -166,20 +166,6 @@ Here is an example reconstruction of a test tube of LEDs I have - Use `1`, `2` & `3` keys to change colour scheme -# Random other stuff that doesn't really fit anywhere - -There's also a tool to turn your 3D scan into a 3D model, run it with `marimapper_remesh my_3d_map.csv` - -You can visualise 2D scans with `marimapper_view_2d_scan led_map_2d_0.csv` - -If you want to develop with MariMapper, you can use -`pip install "marimapper[develop] @ git+http://github.com/themariday/marimapper"` -to grab all the tools you need. Flake8, Black, etc. - -When installing Marimapper, it will adjust your Python packages to the correct versions. -If you don't want this, then run it inside a venv. -If you're worried about library pollution then I assume you know how to use a venv. - # Feedback I would really love to hear what you think and if you have any bugs or improvements, please raise them here or drop me a diff --git a/marimapper/scripts/scanner_cli.py b/marimapper/scripts/scanner_cli.py index 1906a49..43828cb 100644 --- a/marimapper/scripts/scanner_cli.py +++ b/marimapper/scripts/scanner_cli.py @@ -5,10 +5,6 @@ import logging from marimapper.utils import add_camera_args, add_backend_args -# PYCHARM DEVELOPER WARNING! -# You MUST enable "Emulate terminal in output console" in the run configuration or -# really weird stuff happens with multiprocessing! - logger = log_to_stderr() logger.setLevel(level=logging.ERROR) diff --git a/pyproject.toml b/pyproject.toml index cbd2b2b..d4def22 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -5,7 +5,7 @@ build-backend = "hatchling.build" [project] name = "marimapper" -version = "V1.3" +version = "V2.0" dependencies = [ "numpy<1.25.0,>=1.17.3", @@ -54,8 +54,8 @@ marimapper = "marimapper.scripts.scanner_cli:main" marimapper_check_camera = "marimapper.scripts.check_camera_cli:main" marimapper_check_backend ="marimapper.scripts.check_backend_cli:main" marimapper_upload_to_pixelblaze = "marimapper.scripts.upload_map_to_pixelblaze_cli:main" -marimapper_view_2d_map = "marimapper.scripts.view_2d_map_cli:main" +# this currently does nothing because of path issues [tool.coverage.run] omit = [ "*/__init__.py", From f49911060f0d59cc29d84e1ae7733dd5a17c21f2 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 20 Oct 2024 21:19:56 +0100 Subject: [PATCH 16/26] - Added strips back into the visualiser - Added merging back in - removed dead code - --- marimapper/led.py | 21 ++++++---- marimapper/model.py | 6 +-- marimapper/utils.py | 2 +- marimapper/visualize_process.py | 22 +++++++---- test/mock_camera.py | 70 --------------------------------- test/test_led_functions.py | 18 +++++++++ 6 files changed, 49 insertions(+), 90 deletions(-) delete mode 100644 test/mock_camera.py create mode 100644 test/test_led_functions.py diff --git a/marimapper/led.py b/marimapper/led.py index a8dba59..9c1d6a0 100644 --- a/marimapper/led.py +++ b/marimapper/led.py @@ -2,6 +2,10 @@ import typing import math +from multiprocessing import get_logger + +logger = get_logger() + class View: def __init__(self, view_id, position, rotation): @@ -218,17 +222,20 @@ def merge(leds: list[LED3D]) -> LED3D: return new_led -# currently broken def remove_duplicates(leds: list[LED3D]) -> list[LED3D]: - raise NotImplementedError("currently broken") new_leds = [] - for led in leds: - leds_found = get_leds(new_leds, led.led_id) - if leds_found: - new_leds.append(merge(leds_found)) + led_ids = set([led.led_id for led in leds]) + for led_id in led_ids: + leds_found = get_leds(leds, led_id) + if len(leds_found) == 1: + new_leds.append(leds_found[0]) else: - new_leds.append(led) + new_leds.append(merge(leds_found)) + + leds_merged = len(leds) - len(new_leds) + if leds_merged > 0: + logger.debug(f"merged {len(new_leds)} leds") return new_leds diff --git a/marimapper/model.py b/marimapper/model.py index 11a90db..6083de0 100644 --- a/marimapper/model.py +++ b/marimapper/model.py @@ -7,8 +7,7 @@ ) from marimapper.led import LED3D, View - -# from marimapper.led import remove_duplicates +from marimapper.led import remove_duplicates def binary_to_led_map_3d(path: os.path) -> list[LED3D]: @@ -39,7 +38,6 @@ def binary_to_led_map_3d(path: os.path) -> list[LED3D]: leds.append(led) - # currently broken - # leds = remove_duplicates(leds) + leds = remove_duplicates(leds) return leds diff --git a/marimapper/utils.py b/marimapper/utils.py index ae019b8..70e7ccc 100644 --- a/marimapper/utils.py +++ b/marimapper/utils.py @@ -43,7 +43,7 @@ def add_backend_args(parser): "--end", type=int, help="Index of the last led you want to scan up to the backends limit", - default=100, + default=10000, ) parser.add_argument("--server", type=str, help="Some backends require a server") diff --git a/marimapper/visualize_process.py b/marimapper/visualize_process.py index 467074b..da6c4fe 100644 --- a/marimapper/visualize_process.py +++ b/marimapper/visualize_process.py @@ -2,7 +2,7 @@ import open3d from multiprocessing import get_logger from multiprocessing import Process, Event -from marimapper.led import LED3D, View +from marimapper.led import LED3D, View, get_next, get_distance import time logger = get_logger() @@ -110,13 +110,19 @@ def reload_geometry__(self, first=False): np.array([led.point.normal for led in leds]) * 0.2 ) - # self.strip_set.points = self.point_cloud.points - # self.strip_set.lines = open3d.utility.Vector2iVector( - # led_map.get_connected_leds() - # ) - # self.strip_set.colors = open3d.utility.Vector3dVector( - # [[0.8, 0.8, 0.8] for _ in range(len(self.strip_set.lines))] - # ) + self.strip_set.points = self.point_cloud.points + + strips = [] + for led_index, led in enumerate(leds): + next_led = get_next(led, leds) + if next_led is not None: + if get_distance(led, next_led) < 1.10: # + 10% + strips.append((led_index, leds.index(next_led))) + + self.strip_set.lines = open3d.utility.Vector2iVector(strips) + self.strip_set.colors = open3d.utility.Vector3dVector( + [[0.8, 0.8, 0.8] for _ in range(len(self.strip_set.lines))] + ) if first: self._vis.add_geometry(self.point_cloud) diff --git a/test/mock_camera.py b/test/mock_camera.py deleted file mode 100644 index 385e001..0000000 --- a/test/mock_camera.py +++ /dev/null @@ -1,70 +0,0 @@ -import cv2 -import numpy as np - -from multiprocessing import get_logger - -logger = get_logger() - - -class MockCamera: - - def __init__(self, device_id): - - self.frame_id = 0 - - logger.info(f"Connecting to camera {device_id} ...") - self.device_id = device_id - self.device = cv2.VideoCapture(device_id) - real_frames = [] - while True: - ret_val, image = self.device.read() - if not ret_val: - break - real_frames.append(image) - - self.frames = [] - black = np.zeros(real_frames[0].shape) - for frame in real_frames: - self.frames.append(black) - self.frames.append(frame) - self.frames.append(black) - - def reset(self): - pass - - def get_af_mode(self): - return 1 - - def get_focus(self): - return 1 - - def get_exposure_mode(self): - return 1 - - def get_exposure(self): - return 1 - - def get_gain(self): - return 1 - - def set_autofocus(self, mode, focus=0): - pass - - def set_exposure_mode(self, mode): - pass - - def set_gain(self, gain): - pass - - def set_exposure(self, exposure): - pass - - def eat(self, count=30): - pass - - def read(self, color=False): - - frame = self.frames[self.frame_id] - self.frame_id += 1 - - return frame diff --git a/test/test_led_functions.py b/test/test_led_functions.py new file mode 100644 index 0000000..f5b723c --- /dev/null +++ b/test/test_led_functions.py @@ -0,0 +1,18 @@ +from marimapper.led import LED3D, remove_duplicates + + +def test_remove_duplicates(): + + led_0 = LED3D(0) + led_0.point.position = (0, 0, 0) + led_1 = LED3D(0) + led_0.point.position = (1, 0, 0) + + removed_duplicates = remove_duplicates([led_0, led_1]) + + assert len(removed_duplicates) == 1 + + merged_pos = removed_duplicates[0].point.position + assert merged_pos[0] == 0.5 + assert merged_pos[1] == 0 + assert merged_pos[2] == 0 From ba16995babeb64259fc682d870b31ca4048c9d38 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 20 Oct 2024 22:11:38 +0100 Subject: [PATCH 17/26] - added merging back in - added interpolation - added led colour based on if it's been merged or re-interpolated --- marimapper/led.py | 63 +++++++++++++++++++++++++-------- marimapper/sfm_process.py | 18 ++++++---- marimapper/visualize_process.py | 3 ++ test/test_led_functions.py | 22 ++++++++++-- 4 files changed, 81 insertions(+), 25 deletions(-) diff --git a/marimapper/led.py b/marimapper/led.py index 9c1d6a0..aa2ef50 100644 --- a/marimapper/led.py +++ b/marimapper/led.py @@ -41,6 +41,10 @@ def __init__(self): self.position = np.zeros(3) self.normal = np.zeros(3) self.error = 0.0 + self.info = [] + + def set_position(self, x, y, z): + self.position = np.array([x, y, z]) def __add__(self, other): new = Point3D() @@ -52,16 +56,37 @@ def __add__(self, other): def __mul__(self, other): new = Point3D() new.position = self.position * other - new.normal = self.normal + new.normal = self.normal * other new.error = self.error * other return new +class LEDState: + INTERPOLATED: int = 0 + MERGED: int = 1 + + class LED3D: + def __init__(self, led_id): self.led_id = led_id self.point = Point3D() self.views: list[View] = [] + self.state = [] + + def add_state(self, state: int): + self.state.append(state) + + def has_state(self, state: int) -> bool: + return state in self.state + + def get_color(self): + if self.has_state(LEDState.INTERPOLATED): + return 255, 0, 0 + if self.has_state(LEDState.MERGED): + return 255, 0, 255 + + return 0, 255, 0 # returns none if there isn't that led in the list! @@ -169,37 +194,44 @@ def fill_gap(start_led: LED3D, end_led: LED3D): new_led = LED3D(start_led.led_id + led_offset) fraction = led_offset / (total_missing_leds + 1) + p1 = start_led.point * (1 - fraction) + p2 = end_led.point * fraction + new_led.point = p1 + p2 - new_led.position = start_led.point * (1 - fraction) + end_led.point * fraction - + new_led.add_state(LEDState.INTERPOLATED) new_leds.append(new_led) return new_leds -def fill_gaps(leds: list[LED3D], max_dist_err=0.2, max_missing=5) -> list[LED3D]: - - led_to_led_distance = find_inter_led_distance(leds) - min_distance = (1 - max_dist_err) * led_to_led_distance - max_distance = (1 + max_dist_err) * led_to_led_distance +def fill_gaps(leds: list[LED3D], max_distance: float = 1.1, max_missing=5): new_leds = [] for led in leds: next_led = get_next(led, leds) - gap = get_gap(led, next_led) - if gap == 0: + + if next_led is None: continue - distance = get_distance(led, next_led) + gap = get_gap(led, next_led) - 1 - distance_per_led = distance / (gap + 1) + if 1.0 < gap <= max_missing: - if (min_distance < distance_per_led < max_distance) and gap < max_missing: - new_leds += fill_gap(led, next_led) + distance = get_distance(led, next_led) - return new_leds + distance_per_led = distance / (gap + 1) + + if (distance_per_led < max_distance) and gap <= max_missing: + new_leds += fill_gap(led, next_led) + + new_led_count = len(new_leds) + + if new_led_count > 0: + logger.debug(f"filled {new_led_count} LEDs") + + leds += new_leds def merge(leds: list[LED3D]) -> LED3D: @@ -218,6 +250,7 @@ def merge(leds: list[LED3D]) -> LED3D: new_led.point.position = np.average([led.point.position for led in leds], axis=0) new_led.point.normal = np.average([led.point.normal for led in leds], axis=0) new_led.point.error = sum([led.point.error for led in leds]) + new_led.add_state(LEDState.MERGED) return new_led diff --git a/marimapper/sfm_process.py b/marimapper/sfm_process.py index 5e12100..bdacc23 100644 --- a/marimapper/sfm_process.py +++ b/marimapper/sfm_process.py @@ -1,5 +1,5 @@ from multiprocessing import Process, Event, Queue, get_logger -from marimapper.led import LED2D, rescale, recenter, LED3D +from marimapper.led import LED2D, rescale, recenter, LED3D, fill_gaps from marimapper.sfm import sfm import open3d import numpy as np @@ -22,16 +22,18 @@ def add_normals(leds: list[LED3D]): camera_normals = [] for led in leds: views = [view.position for view in led.views] - camera_normals.append(np.average(views, axis=0)) + camera_normals.append(np.average(views, axis=0) if views else None) for led, camera_normal, open3d_normal in zip(leds, camera_normals, pcd.normals): led.point.normal = open3d_normal / np.linalg.norm(open3d_normal) - angle = np.arccos(np.clip(np.dot(camera_normal, open3d_normal), -1.0, 1.0)) + if camera_normal is not None: - if angle > math.pi / 2.0: - led.point.normal *= -1 + angle = np.arccos(np.clip(np.dot(camera_normal, open3d_normal), -1.0, 1.0)) + + if angle > math.pi / 2.0: + led.point.normal *= -1 class SFM(Process): @@ -78,12 +80,14 @@ def run(self): if len(leds_3d) == 0: continue - add_normals(leds_3d) - rescale(leds_3d) + fill_gaps(leds_3d) + recenter(leds_3d) + add_normals(leds_3d) + self._output_queue.put(leds_3d) update_required = False diff --git a/marimapper/visualize_process.py b/marimapper/visualize_process.py index da6c4fe..830ab86 100644 --- a/marimapper/visualize_process.py +++ b/marimapper/visualize_process.py @@ -109,6 +109,9 @@ def reload_geometry__(self, first=False): self.point_cloud.normals = open3d.utility.Vector3dVector( np.array([led.point.normal for led in leds]) * 0.2 ) + self.point_cloud.colors = open3d.utility.Vector3dVector( + np.array([led.get_color() for led in leds]) + ) self.strip_set.points = self.point_cloud.points diff --git a/test/test_led_functions.py b/test/test_led_functions.py index f5b723c..c66d52d 100644 --- a/test/test_led_functions.py +++ b/test/test_led_functions.py @@ -1,12 +1,12 @@ -from marimapper.led import LED3D, remove_duplicates +from marimapper.led import LED3D, remove_duplicates, fill_gaps, get_led def test_remove_duplicates(): led_0 = LED3D(0) - led_0.point.position = (0, 0, 0) + led_0.point.set_position(0, 0, 0) led_1 = LED3D(0) - led_0.point.position = (1, 0, 0) + led_0.point.set_position(1, 0, 0) removed_duplicates = remove_duplicates([led_0, led_1]) @@ -16,3 +16,19 @@ def test_remove_duplicates(): assert merged_pos[0] == 0.5 assert merged_pos[1] == 0 assert merged_pos[2] == 0 + + +def test_fill_gaps(): + + led_0 = LED3D(0) + led_0.point.set_position(0, 0, 0) + led_6 = LED3D(6) + led_6.point.set_position(6, 0, 0) + + leds = [led_0, led_6] + fill_gaps(leds) + + assert len(leds) == 7 + + for led_id in range(7): + assert get_led(leds, led_id).point.position[0] == led_id From 48f5db7596a2ede55545bfccf1dd4f9f25648b9d Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 20 Oct 2024 22:15:14 +0100 Subject: [PATCH 18/26] quick fix to sort out normals of interpolated leds --- marimapper/led.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/marimapper/led.py b/marimapper/led.py index aa2ef50..367da74 100644 --- a/marimapper/led.py +++ b/marimapper/led.py @@ -198,6 +198,8 @@ def fill_gap(start_led: LED3D, end_led: LED3D): p2 = end_led.point * fraction new_led.point = p1 + p2 + new_led.views = start_led.views + end_led.views + new_led.add_state(LEDState.INTERPOLATED) new_leds.append(new_led) From 9549e9f9705ced78c3dbcdd5c9f1f58ac494491b Mon Sep 17 00:00:00 2001 From: Samuel Date: Wed, 23 Oct 2024 22:30:35 +0100 Subject: [PATCH 19/26] This needs a read-over --- marimapper/detector_process.py | 41 +++++++++++---------- marimapper/file_tools.py | 26 ++++++------- marimapper/file_writer_process.py | 61 +++++++++++++++++++++++++++++++ marimapper/scanner.py | 47 ++++++++++++------------ marimapper/sfm_process.py | 21 ++++++----- marimapper/visualize_process.py | 11 ++++-- 6 files changed, 137 insertions(+), 70 deletions(-) create mode 100644 marimapper/file_writer_process.py diff --git a/marimapper/detector_process.py b/marimapper/detector_process.py index 7a9d0d1..e1df99a 100644 --- a/marimapper/detector_process.py +++ b/marimapper/detector_process.py @@ -1,4 +1,4 @@ -from multiprocessing import Process, Queue, Event +from multiprocessing import get_logger, Process, Queue, Event from marimapper.detector import ( show_image, set_cam_default, @@ -7,10 +7,8 @@ set_cam_dark, enable_and_find_led, ) -from marimapper.led import LED2D -from marimapper.utils import get_backend -from multiprocessing import get_logger +from marimapper.utils import get_backend logger = get_logger() @@ -27,10 +25,9 @@ def __init__( display: bool = True, ): super().__init__() - self._detection_request = Queue() # {led_id, view_id} - self._detection_result = Queue() # LED3D - self._detection_request.cancel_join_thread() - self._detection_result.cancel_join_thread() + self._input_queue = Queue() # {led_id, view_id} + self._input_queue.cancel_join_thread() + self._output_queues: list[Queue] = [] # LED3D self._led_count = Queue() self._led_count.cancel_join_thread() self._exit_event = Event() @@ -42,11 +39,14 @@ def __init__( self._led_backend_server = led_backend_server self._display = display - def detect(self, led_id: int, view_id: int): - self._detection_request.put((led_id, view_id)) + def get_input_queue(self) -> Queue: + return self._input_queue - def get_results(self) -> LED2D: - return self._detection_result.get() + def add_output_queue(self, queue: Queue): + self._output_queues.append(queue) + + def detect(self, led_id: int, view_id: int): + self._input_queue.put((led_id, view_id)) def get_led_count(self): return self._led_count.get() @@ -66,9 +66,9 @@ def run(self): while not self._exit_event.is_set(): - if not self._detection_request.empty(): + if not self._input_queue.empty(): set_cam_dark(cam, self._dark_exposure) - led_id, view_id = self._detection_request.get() + led_id, view_id = self._input_queue.get() result = enable_and_find_led( cam, led_backend, @@ -79,7 +79,8 @@ def run(self): self._display, ) - self._detection_result.put(result) + for queue in self._output_queues: + queue.put(result) else: set_cam_default(cam) if self._display: @@ -90,7 +91,9 @@ def run(self): set_cam_default(cam) # clear the queues, don't ask why. - while not self._detection_request.empty(): - self._detection_request.get() - while not self._detection_result.empty(): - self._detection_result.get() + while not self._input_queue.empty(): + self._input_queue.get() + + for queue in self._output_queues: + while not queue.empty(): + queue.get() diff --git a/marimapper/file_tools.py b/marimapper/file_tools.py index 32e9475..f7c7efe 100644 --- a/marimapper/file_tools.py +++ b/marimapper/file_tools.py @@ -1,6 +1,7 @@ import os from marimapper.led import Point2D, LED3D, LED2D import typing +from pathlib import Path def load_detections(filename: os.path, view_id) -> typing.Optional[list[LED2D]]: @@ -51,7 +52,18 @@ def get_all_2d_led_maps(directory: os.path) -> list[LED2D]: return points -def write_3d_leds_to_file(leds: list[LED3D], filename: str): +def write_2d_leds_to_file(leds: list[LED2D], filename: Path): + + lines = ["index,u,v"] + + for led in sorted(leds, key=lambda led_t: led_t.led_id): + lines.append(f"{led.led_id}," f"{led.point.u():f}," f"{led.point.v():f}") + + with open(filename, "w") as f: + f.write("\n".join(lines)) + + +def write_3d_leds_to_file(leds: list[LED3D], filename: Path): lines = ["index,x,y,z,xn,yn,zn,error"] @@ -69,15 +81,3 @@ def write_3d_leds_to_file(leds: list[LED3D], filename: str): with open(filename, "w") as f: f.write("\n".join(lines)) - - -def write_2d_leds_to_file(leds: list[LED2D], filename: str): - - lines = ["index,u,v"] - - for led in sorted(leds, key=lambda led_t: led_t.led_id): - - lines.append(f"{led.led_id}," f"{led.point.u():f}," f"{led.point.v():f}") - - with open(filename, "w") as f: - f.write("\n".join(lines)) diff --git a/marimapper/file_writer_process.py b/marimapper/file_writer_process.py new file mode 100644 index 0000000..a33c5db --- /dev/null +++ b/marimapper/file_writer_process.py @@ -0,0 +1,61 @@ +from multiprocessing import Process, Queue, Event +import time +from marimapper.file_tools import write_3d_leds_to_file, write_2d_leds_to_file +from pathlib import Path + + +class FileWriterProcess(Process): + + def __init__(self, base_path: Path): + super().__init__() + self._input_queue_2d = Queue() + self._input_queue_2d.cancel_join_thread() + self._input_queue_3d = Queue() + self._input_queue_3d.cancel_join_thread() + self._exit_event = Event() + self._base_path = base_path + + def stop(self): + self._exit_event.set() + + def get_2d_input_queue(self): + return self._input_queue_2d + + def get_3d_input_queue(self): + return self._input_queue_3d + + def get_new_filename(self) -> Path: + string_time = time.strftime("%Y%m%d-%H%M%S") + return self._base_path / f"led_map_2d_{string_time}.csv" + + def run(self): + views = {} + view_id_to_filename = {} + + requires_update = set() + + while not self._exit_event.is_set(): + if not self._input_queue_3d.empty(): + leds = self._input_queue_3d.get() + write_3d_leds_to_file(leds, Path("led3d.csv")) + + if not self._input_queue_2d.empty(): + led = self._input_queue_2d.get() + if led.view_id not in view_id_to_filename: + view_id_to_filename[led.view_id] = self.get_new_filename() + views[led.view_id] = [] + + views[led.view_id].append(led) + requires_update.add(led.view_id) + + else: + for view_id in requires_update: + write_2d_leds_to_file(views[view_id], view_id_to_filename[view_id]) + requires_update = [] + time.sleep(1) + + # clear the queues, don't ask why. + while not self._input_queue_2d.empty(): + self._input_queue_2d.get() + while not self._input_queue_3d.empty(): + self._input_queue_3d.get() diff --git a/marimapper/scanner.py b/marimapper/scanner.py index c2aee62..326edf9 100644 --- a/marimapper/scanner.py +++ b/marimapper/scanner.py @@ -3,15 +3,15 @@ from marimapper.sfm_process import SFM import os -import time from tqdm import tqdm from pathlib import Path from marimapper.detector_process import DetectorProcess -from multiprocessing import get_logger -from marimapper.file_tools import get_all_2d_led_maps, write_2d_leds_to_file +from multiprocessing import get_logger, Queue +from marimapper.file_tools import get_all_2d_led_maps from marimapper.utils import get_user_confirmation from marimapper.visualize_process import VisualiseProcess from marimapper.led import last_view +from marimapper.file_writer_process import FileWriterProcess logger = get_logger() @@ -20,7 +20,7 @@ class Scanner: def __init__(self, cli_args): logger.debug("initialising scanner") - self.output_dir = cli_args.dir + self.output_dir = Path(cli_args.dir) os.makedirs(self.output_dir, exist_ok=True) self.detector = DetectorProcess( @@ -33,17 +33,29 @@ def __init__(self, cli_args): self.sfm = SFM() - self.leds = get_all_2d_led_maps(Path(self.output_dir)) - for led in self.leds: + self.file_writer = FileWriterProcess(self.output_dir) + + leds = get_all_2d_led_maps(self.output_dir) + + for led in leds: self.sfm.add_detection(led) - self.current_view = last_view(self.leds) + 1 + self.current_view = last_view(leds) + 1 + + self.renderer3d = VisualiseProcess() - self.renderer3d = VisualiseProcess(input_queue=self.sfm.get_output_queue()) + self.detector_update_queue = Queue() + self.detector.add_output_queue(self.sfm.get_input_queue()) + self.detector.add_output_queue(self.detector_update_queue) + self.detector.add_output_queue(self.file_writer.get_2d_input_queue()) + + self.sfm.add_output_queue(self.renderer3d.get_input_queue()) + self.sfm.add_output_queue(self.file_writer.get_3d_input_queue()) self.sfm.start() self.renderer3d.start() self.detector.start() + self.file_writer.start() self.led_id_range = range( cli_args.start, min(cli_args.end, self.detector.get_led_count()) @@ -57,11 +69,12 @@ def close(self): self.detector.stop() self.sfm.stop() self.renderer3d.stop() + self.file_writer.stop() self.sfm.join() self.renderer3d.join() self.detector.join() - + self.file_writer.join() logger.debug("scanner closed") def mainloop(self): @@ -74,8 +87,6 @@ def mainloop(self): print("exiting") return - leds = [] - for led_id in self.led_id_range: self.detector.detect(led_id, self.current_view) @@ -86,18 +97,6 @@ def mainloop(self): total=self.led_id_range.stop, smoothing=0, ): - led = self.detector.get_results() - - if led.point is None: - continue - - leds.append(led) - - self.sfm.add_detection(led) + self.detector_update_queue.get() self.current_view += 1 - - # The filename is made out of the date, then the resolution of the camera - string_time = time.strftime("%Y%m%d-%H%M%S") - filepath = os.path.join(self.output_dir, f"led_map_2d_{string_time}.csv") - write_2d_leds_to_file(leds, filepath) diff --git a/marimapper/sfm_process.py b/marimapper/sfm_process.py index bdacc23..a830044 100644 --- a/marimapper/sfm_process.py +++ b/marimapper/sfm_process.py @@ -40,20 +40,19 @@ class SFM(Process): def __init__(self): super().__init__() - self._output_queue = Queue() - self._output_queue.cancel_join_thread() self._input_queue = Queue() self._input_queue.cancel_join_thread() + self._output_queues: list[Queue] = [] self._exit_event = Event() + def get_input_queue(self) -> Queue: + return self._input_queue + def add_detection(self, led: LED2D): self._input_queue.put(led) - def get_output_queue(self): - return self._output_queue - - def get_results(self): - return self._output_queue.get() + def add_output_queue(self, queue: Queue): + self._output_queues.append(queue) def stop(self): self._exit_event.set() @@ -88,11 +87,13 @@ def run(self): add_normals(leds_3d) - self._output_queue.put(leds_3d) + for queue in self._output_queues: + queue.put(leds_3d) update_required = False # clear the queues, don't ask why. while not self._input_queue.empty(): self._input_queue.get() - while not self._output_queue.empty(): - self._output_queue.get() + for queue in self._output_queues: + while not queue.empty(): + queue.get() diff --git a/marimapper/visualize_process.py b/marimapper/visualize_process.py index 830ab86..35df8be 100644 --- a/marimapper/visualize_process.py +++ b/marimapper/visualize_process.py @@ -1,7 +1,6 @@ import numpy as np import open3d -from multiprocessing import get_logger -from multiprocessing import Process, Event +from multiprocessing import get_logger, Process, Event, Queue from marimapper.led import LED3D, View, get_next, get_distance import time @@ -23,17 +22,21 @@ def get_all_views(leds: list[LED3D]) -> list[View]: class VisualiseProcess(Process): - def __init__(self, input_queue): + def __init__(self): logger.debug("Renderer3D initialising") super().__init__() self._vis = None - self._input_queue = input_queue + self._input_queue = Queue() + self._input_queue.cancel_join_thread() self._exit_event = Event() self.point_cloud = None self.line_set = None self.strip_set = None logger.debug("Renderer3D initialised") + def get_input_queue(self) -> Queue: + return self._input_queue + def stop(self): self._exit_event.set() From d30144c4553e98b2fc012f167907c44e6ae2ba77 Mon Sep 17 00:00:00 2001 From: Samuel Date: Wed, 23 Oct 2024 22:49:44 +0100 Subject: [PATCH 20/26] fix to unions --- marimapper/led.py | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/marimapper/led.py b/marimapper/led.py index 367da74..abfb0f8 100644 --- a/marimapper/led.py +++ b/marimapper/led.py @@ -1,6 +1,7 @@ import numpy as np import typing import math +from typing import Union from multiprocessing import get_logger @@ -90,21 +91,23 @@ def get_color(self): # returns none if there isn't that led in the list! -def get_led(leds: list[LED2D | LED3D], led_id: int) -> typing.Optional[LED2D | LED3D]: +def get_led( + leds: list[Union[Union[LED2D, LED3D]]], led_id: int +) -> typing.Optional[Union[LED2D, LED3D]]: for led in leds: if led.led_id == led_id: return led return None -def get_leds(leds: list[LED2D | LED3D], led_id: int) -> list[LED2D | LED3D]: +def get_leds(leds: list[Union[LED2D, LED3D]], led_id: int) -> list[Union[LED2D, LED3D]]: return [led for led in leds if led.led_id == led_id] # returns none if it's the end! def get_next( - led_prev: LED2D | LED3D, leds: list[LED2D | LED3D] -) -> typing.Optional[LED2D | LED3D]: + led_prev: Union[LED2D, LED3D], leds: list[Union[LED2D, LED3D]] +) -> typing.Optional[Union[LED2D, LED3D]]: closest = None for led in leds: @@ -120,19 +123,19 @@ def get_next( return closest -def get_gap(led_a: LED2D | LED3D, led_b: LED2D | LED3D) -> int: +def get_gap(led_a: Union[LED2D, LED3D], led_b: Union[LED2D, LED3D]) -> int: return abs(led_a.led_id - led_b.led_id) -def get_max_led_id(leds: list[LED2D | LED3D]) -> int: +def get_max_led_id(leds: list[Union[LED2D, LED3D]]) -> int: return max([led.led_id for led in leds]) -def get_min_led_id(leds: list[LED2D | LED3D]) -> int: +def get_min_led_id(leds: list[Union[LED2D, LED3D]]) -> int: return min([led.led_id for led in leds]) -def get_distance(led_a: LED2D | LED3D, led_b: LED2D | LED3D): +def get_distance(led_a: Union[LED2D, LED3D], led_b: Union[LED2D, LED3D]): return math.hypot(*(led_a.point.position - led_b.point.position)) @@ -150,7 +153,7 @@ def last_view(leds: list[LED2D]): return max([led.view_id for led in leds]) -def find_inter_led_distance(leds: list[LED2D | LED3D]): +def find_inter_led_distance(leds: list[Union[LED2D, LED3D]]): distances = [] for led in leds: From 6b9ca38ab9694e17cf3c48af467bcc36c3a3cfe1 Mon Sep 17 00:00:00 2001 From: Samuel Date: Thu, 24 Oct 2024 07:37:57 +0100 Subject: [PATCH 21/26] Tests should now be able to run pretty much anywhere --- test/test_3d_map.py | 3 ++- test/test_camera.py | 5 ++++- test/test_capture_sequence.py | 12 ++++++++---- test/test_led_identifier.py | 13 ++++++++++--- test/test_reconstruction.py | 9 +++++---- test/test_rescale.py | 3 ++- test/utils.py | 8 ++++++++ 7 files changed, 39 insertions(+), 14 deletions(-) create mode 100644 test/utils.py diff --git a/test/test_3d_map.py b/test/test_3d_map.py index 23dc62c..5160c6c 100644 --- a/test/test_3d_map.py +++ b/test/test_3d_map.py @@ -2,6 +2,7 @@ import numpy as np from marimapper.led import LED3D from marimapper.file_tools import write_3d_leds_to_file +from pathlib import Path def test_file_write(): @@ -22,7 +23,7 @@ def test_file_write(): output_file = tempfile.NamedTemporaryFile(delete=False) - write_3d_leds_to_file([led], output_file.name) + write_3d_leds_to_file([led], Path(output_file.name)) with open(output_file.name) as f: lines = f.readlines() diff --git a/test/test_camera.py b/test/test_camera.py index 713af2d..c700f13 100644 --- a/test/test_camera.py +++ b/test/test_camera.py @@ -1,10 +1,13 @@ import pytest from marimapper.camera import Camera +from utils import get_test_dir def test_valid_device(): - cam = Camera("MariMapper-Test-Data/9_point_box/cam_0/capture_%04d.png") + cam = Camera( + get_test_dir("MariMapper-Test-Data/9_point_box/cam_0/capture_%04d.png") + ) image = cam.read() diff --git a/test/test_capture_sequence.py b/test/test_capture_sequence.py index 320a98c..a744b55 100644 --- a/test/test_capture_sequence.py +++ b/test/test_capture_sequence.py @@ -3,17 +3,20 @@ from marimapper.detector import find_led from marimapper.file_tools import write_2d_leds_to_file, load_detections from marimapper.led import LED2D +from utils import get_test_dir def test_capture_sequence(): - output_dir_full = os.path.join(os.getcwd(), "test", "scan") + output_dir_full = get_test_dir("scan") os.makedirs(output_dir_full, exist_ok=True) for view_index in range(9): cam = Camera( - f"MariMapper-Test-Data/9_point_box/cam_{view_index}/capture_%04d.png" + get_test_dir( + f"MariMapper-Test-Data/9_point_box/cam_{view_index}/capture_%04d.png" + ) ) leds = [] @@ -25,14 +28,15 @@ def test_capture_sequence(): if point: leds.append(LED2D(led_id, 0, point)) - filepath = os.path.join(output_dir_full, f"led_map_2d_{view_index:04}.csv") + filepath = output_dir_full / f"led_map_2d_{view_index:04}.csv" write_2d_leds_to_file(leds, filepath) def test_capture_sequence_correctness(): + output_dir_full = get_test_dir("scan") + for view_index in range(9): - output_dir_full = os.path.join(os.getcwd(), "test", "scan") filepath = os.path.join(output_dir_full, f"led_map_2d_{view_index:04}.csv") diff --git a/test/test_led_identifier.py b/test/test_led_identifier.py index fcd5291..cee88d2 100644 --- a/test/test_led_identifier.py +++ b/test/test_led_identifier.py @@ -2,11 +2,14 @@ from marimapper.detector import find_led_in_image, draw_led_detections from marimapper.camera import Camera +from utils import get_test_dir def test_basic_image_loading(): - mock_camera = Camera("MariMapper-Test-Data/9_point_box/cam_0/capture_0000.png") + mock_camera = Camera( + get_test_dir("MariMapper-Test-Data/9_point_box/cam_0/capture_0000.png") + ) detection = find_led_in_image(mock_camera.read()) assert detection.u() == pytest.approx(0.4029418361244019) @@ -15,7 +18,9 @@ def test_basic_image_loading(): def test_none_found(): - mock_camera = Camera("MariMapper-Test-Data/9_point_box/cam_0/capture_%04d.png") + mock_camera = Camera( + get_test_dir("MariMapper-Test-Data/9_point_box/cam_0/capture_%04d.png") + ) for frame_id in range(24): frame = mock_camera.read() @@ -26,7 +31,9 @@ def test_none_found(): def test_draw_results(): - mock_camera = Camera("MariMapper-Test-Data/9_point_box/cam_0/capture_%04d.png") + mock_camera = Camera( + get_test_dir("MariMapper-Test-Data/9_point_box/cam_0/capture_%04d.png") + ) frame = mock_camera.read() led_detection = find_led_in_image(frame) draw_led_detections(frame, led_detection) diff --git a/test/test_reconstruction.py b/test/test_reconstruction.py index 6250fd0..6b6a2d6 100644 --- a/test/test_reconstruction.py +++ b/test/test_reconstruction.py @@ -3,6 +3,7 @@ from marimapper.sfm import sfm from marimapper.file_tools import get_all_2d_led_maps from marimapper.led import get_led, get_leds_with_views +from utils import get_test_dir def check_dimensions(map_3d, max_error): @@ -39,7 +40,7 @@ def check_dimensions(map_3d, max_error): def test_reconstruction(): - maps = get_all_2d_led_maps("scan") + maps = get_all_2d_led_maps(get_test_dir("scan")) map_3d = sfm(maps) @@ -51,7 +52,7 @@ def test_reconstruction(): def test_sparse_reconstruction(): - maps = get_all_2d_led_maps("scan") + maps = get_all_2d_led_maps(get_test_dir("scan")) maps_sparse = get_leds_with_views(maps, [1, 3, 5, 7]) @@ -65,7 +66,7 @@ def test_sparse_reconstruction(): def test_2_track_reconstruction(): - leds = get_all_2d_led_maps("scan") + leds = get_all_2d_led_maps(get_test_dir("scan")) leds_2_track = get_leds_with_views(leds, [1, 2]) map_3d = sfm(leds_2_track) @@ -74,7 +75,7 @@ def test_2_track_reconstruction(): def test_invalid_reconstruction_views(): - leds = get_all_2d_led_maps("scan") + leds = get_all_2d_led_maps(get_test_dir("scan")) leds_invalid = get_leds_with_views(leds, [0, 4, 8]) diff --git a/test/test_rescale.py b/test/test_rescale.py index 6560c69..0c2586d 100644 --- a/test/test_rescale.py +++ b/test/test_rescale.py @@ -1,10 +1,11 @@ from marimapper.led import rescale from marimapper.sfm import sfm from marimapper.file_tools import get_all_2d_led_maps +from utils import get_test_dir def test_rescale_basic(): - maps = get_all_2d_led_maps("scan") + maps = get_all_2d_led_maps(get_test_dir("scan")) map_3d = sfm(maps) diff --git a/test/utils.py b/test/utils.py new file mode 100644 index 0000000..b95b39b --- /dev/null +++ b/test/utils.py @@ -0,0 +1,8 @@ +import os +from pathlib import Path + + +def get_test_dir(path: str) -> Path: + + this_path = os.path.dirname(os.path.abspath(__file__)) + return Path(this_path) / path From 8b732bff1275cbb2e3cf6dd77ad2259e3fec1a85 Mon Sep 17 00:00:00 2001 From: Samuel Date: Thu, 24 Oct 2024 08:20:02 +0100 Subject: [PATCH 22/26] - tidied up some bugs - removed passing args to a function, gross. - increased strip visualisation threshold to +50% --- marimapper/camera.py | 2 +- marimapper/file_writer_process.py | 22 ++++++++++++++-------- marimapper/scanner.py | 28 ++++++++++++++++++---------- marimapper/scripts/scanner_cli.py | 15 ++++++++++++--- marimapper/sfm_process.py | 7 ++++--- marimapper/visualize_process.py | 2 +- pyproject.toml | 2 +- 7 files changed, 51 insertions(+), 27 deletions(-) diff --git a/marimapper/camera.py b/marimapper/camera.py index 0cd3617..fa5ea9d 100644 --- a/marimapper/camera.py +++ b/marimapper/camera.py @@ -100,7 +100,7 @@ def eat(self, count=30): for _ in range(count): self.read() - def read(self, color=False): + def read(self): ret_val, image = self.device.read() if not ret_val: raise Exception("Failed to read image") diff --git a/marimapper/file_writer_process.py b/marimapper/file_writer_process.py index a33c5db..e4a4341 100644 --- a/marimapper/file_writer_process.py +++ b/marimapper/file_writer_process.py @@ -2,6 +2,9 @@ import time from marimapper.file_tools import write_3d_leds_to_file, write_2d_leds_to_file from pathlib import Path +import os + +from marimapper.led import LED2D class FileWriterProcess(Process): @@ -14,6 +17,7 @@ def __init__(self, base_path: Path): self._input_queue_3d.cancel_join_thread() self._exit_event = Event() self._base_path = base_path + os.makedirs(self._base_path, exist_ok=True) def stop(self): self._exit_event.set() @@ -28,6 +32,7 @@ def get_new_filename(self) -> Path: string_time = time.strftime("%Y%m%d-%H%M%S") return self._base_path / f"led_map_2d_{string_time}.csv" + # This function is a bit gross and needs cleaning up def run(self): views = {} view_id_to_filename = {} @@ -37,21 +42,22 @@ def run(self): while not self._exit_event.is_set(): if not self._input_queue_3d.empty(): leds = self._input_queue_3d.get() - write_3d_leds_to_file(leds, Path("led3d.csv")) + write_3d_leds_to_file(leds, self._base_path / "led_map_3d.csv") if not self._input_queue_2d.empty(): - led = self._input_queue_2d.get() - if led.view_id not in view_id_to_filename: - view_id_to_filename[led.view_id] = self.get_new_filename() - views[led.view_id] = [] + led: LED2D = self._input_queue_2d.get() + if led.point is not None: + if led.view_id not in view_id_to_filename: + view_id_to_filename[led.view_id] = self.get_new_filename() + views[led.view_id] = [] - views[led.view_id].append(led) - requires_update.add(led.view_id) + views[led.view_id].append(led) + requires_update.add(led.view_id) else: for view_id in requires_update: write_2d_leds_to_file(views[view_id], view_id_to_filename[view_id]) - requires_update = [] + requires_update = set() time.sleep(1) # clear the queues, don't ask why. diff --git a/marimapper/scanner.py b/marimapper/scanner.py index 326edf9..ed36495 100644 --- a/marimapper/scanner.py +++ b/marimapper/scanner.py @@ -2,7 +2,6 @@ # FATAL WEIRD CRASH IF THIS ISN'T IMPORTED FIRST DON'T ASK from marimapper.sfm_process import SFM -import os from tqdm import tqdm from pathlib import Path from marimapper.detector_process import DetectorProcess @@ -18,17 +17,26 @@ class Scanner: - def __init__(self, cli_args): + def __init__( + self, + output_dir: Path, + device: str, + exposure: int, + threshold: int, + backend: str, + server: str, + led_start: int, + led_end: int, + ): logger.debug("initialising scanner") - self.output_dir = Path(cli_args.dir) - os.makedirs(self.output_dir, exist_ok=True) + self.output_dir = output_dir self.detector = DetectorProcess( - cli_args.device, - cli_args.exposure, - cli_args.threshold, - cli_args.backend, - cli_args.server, + device, + exposure, + threshold, + backend, + server, ) self.sfm = SFM() @@ -58,7 +66,7 @@ def __init__(self, cli_args): self.file_writer.start() self.led_id_range = range( - cli_args.start, min(cli_args.end, self.detector.get_led_count()) + led_start, min(led_end, self.detector.get_led_count()) ) logger.debug("scanner initialised") diff --git a/marimapper/scripts/scanner_cli.py b/marimapper/scripts/scanner_cli.py index 43828cb..08906fa 100644 --- a/marimapper/scripts/scanner_cli.py +++ b/marimapper/scripts/scanner_cli.py @@ -4,7 +4,7 @@ import argparse import logging from marimapper.utils import add_camera_args, add_backend_args - +from pathlib import Path logger = log_to_stderr() logger.setLevel(level=logging.ERROR) @@ -21,7 +21,7 @@ def main(): parser.add_argument("-v", "--verbose", action="store_true") parser.add_argument( - "--dir", type=str, help="The output folder for your capture", default="." + "--dir", type=Path, help="The output folder for your capture", default="." ) args = parser.parse_args() @@ -32,7 +32,16 @@ def main(): if args.verbose: logger.setLevel(logging.DEBUG) - scanner = Scanner(cli_args=args) + scanner = Scanner( + args.dir, + args.device, + args.exposure, + args.threshold, + args.backend, + args.server, + args.start, + args.end, + ) scanner.mainloop() scanner.close() diff --git a/marimapper/sfm_process.py b/marimapper/sfm_process.py index a830044..5b66e40 100644 --- a/marimapper/sfm_process.py +++ b/marimapper/sfm_process.py @@ -66,9 +66,10 @@ def run(self): while not self._exit_event.is_set(): if not self._input_queue.empty(): - led = self._input_queue.get() - leds_2d.append(led) - update_required = True + led: LED2D = self._input_queue.get() + if led.point is not None: + leds_2d.append(led) + update_required = True else: if not update_required: diff --git a/marimapper/visualize_process.py b/marimapper/visualize_process.py index 35df8be..bef419e 100644 --- a/marimapper/visualize_process.py +++ b/marimapper/visualize_process.py @@ -122,7 +122,7 @@ def reload_geometry__(self, first=False): for led_index, led in enumerate(leds): next_led = get_next(led, leds) if next_led is not None: - if get_distance(led, next_led) < 1.10: # + 10% + if get_distance(led, next_led) < 1.50: # + 50% strips.append((led_index, leds.index(next_led))) self.strip_set.lines = open3d.utility.Vector2iVector(strips) diff --git a/pyproject.toml b/pyproject.toml index d4def22..abce2f3 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -55,7 +55,7 @@ marimapper_check_camera = "marimapper.scripts.check_camera_cli:main" marimapper_check_backend ="marimapper.scripts.check_backend_cli:main" marimapper_upload_to_pixelblaze = "marimapper.scripts.upload_map_to_pixelblaze_cli:main" -# this currently does nothing because of path issues + [tool.coverage.run] omit = [ "*/__init__.py", From 7ed22c217117c520655a498cfd66b4d59fd345a5 Mon Sep 17 00:00:00 2001 From: Samuel Date: Thu, 24 Oct 2024 12:49:20 +0100 Subject: [PATCH 23/26] - removed some dead code --- marimapper/camera.py | 6 ------ marimapper/led.py | 9 --------- 2 files changed, 15 deletions(-) diff --git a/marimapper/camera.py b/marimapper/camera.py index fa5ea9d..f7e2584 100644 --- a/marimapper/camera.py +++ b/marimapper/camera.py @@ -44,12 +44,6 @@ def __init__(self, device_id): def reset(self): self.default_settings.apply(self) - def get_width(self): - return int(self.device.get(cv2.CAP_PROP_FRAME_WIDTH)) - - def get_height(self): - return int(self.device.get(cv2.CAP_PROP_FRAME_HEIGHT)) - def get_af_mode(self): return int(self.device.get(cv2.CAP_PROP_AUTOFOCUS)) diff --git a/marimapper/led.py b/marimapper/led.py index abfb0f8..5b331ee 100644 --- a/marimapper/led.py +++ b/marimapper/led.py @@ -126,15 +126,6 @@ def get_next( def get_gap(led_a: Union[LED2D, LED3D], led_b: Union[LED2D, LED3D]) -> int: return abs(led_a.led_id - led_b.led_id) - -def get_max_led_id(leds: list[Union[LED2D, LED3D]]) -> int: - return max([led.led_id for led in leds]) - - -def get_min_led_id(leds: list[Union[LED2D, LED3D]]) -> int: - return min([led.led_id for led in leds]) - - def get_distance(led_a: Union[LED2D, LED3D], led_b: Union[LED2D, LED3D]): return math.hypot(*(led_a.point.position - led_b.point.position)) From 5cc021f6f58f4c66d7961a0be67535d1acabf66f Mon Sep 17 00:00:00 2001 From: Samuel Date: Thu, 24 Oct 2024 12:51:17 +0100 Subject: [PATCH 24/26] - removed changes to github pipeline --- .github/workflows/test_mac.yml | 1 - .github/workflows/test_ubuntu.yml | 1 - .github/workflows/test_windows.yml | 1 - 3 files changed, 3 deletions(-) diff --git a/.github/workflows/test_mac.yml b/.github/workflows/test_mac.yml index a77d5a9..a4210ce 100644 --- a/.github/workflows/test_mac.yml +++ b/.github/workflows/test_mac.yml @@ -37,5 +37,4 @@ jobs: - name: Pytest run: | - cd test pytest . \ No newline at end of file diff --git a/.github/workflows/test_ubuntu.yml b/.github/workflows/test_ubuntu.yml index 06221aa..4d5813f 100644 --- a/.github/workflows/test_ubuntu.yml +++ b/.github/workflows/test_ubuntu.yml @@ -37,5 +37,4 @@ jobs: - name: Pytest run: | - cd test pytest . \ No newline at end of file diff --git a/.github/workflows/test_windows.yml b/.github/workflows/test_windows.yml index 85ebb06..a98b14e 100644 --- a/.github/workflows/test_windows.yml +++ b/.github/workflows/test_windows.yml @@ -37,5 +37,4 @@ jobs: - name: Pytest run: | - cd test pytest . \ No newline at end of file From 7feb2b7ef2bad2cf917f8e9ca8b6fb912f3a35c4 Mon Sep 17 00:00:00 2001 From: Samuel Date: Thu, 24 Oct 2024 20:58:06 +0100 Subject: [PATCH 25/26] - style fix --- marimapper/led.py | 1 + 1 file changed, 1 insertion(+) diff --git a/marimapper/led.py b/marimapper/led.py index 5b331ee..45a83be 100644 --- a/marimapper/led.py +++ b/marimapper/led.py @@ -126,6 +126,7 @@ def get_next( def get_gap(led_a: Union[LED2D, LED3D], led_b: Union[LED2D, LED3D]) -> int: return abs(led_a.led_id - led_b.led_id) + def get_distance(led_a: Union[LED2D, LED3D], led_b: Union[LED2D, LED3D]): return math.hypot(*(led_a.point.position - led_b.point.position)) From b1c8d85834789b648e498bf26a8ab01a31251289 Mon Sep 17 00:00:00 2001 From: Samuel Date: Thu, 24 Oct 2024 22:07:30 +0100 Subject: [PATCH 26/26] - small bugs fixes --- marimapper/backends/dummy/dummy_backend.py | 2 +- marimapper/detector.py | 3 --- marimapper/scanner.py | 8 +++++++- marimapper/scripts/scanner_cli.py | 6 +++++- marimapper/sfm_process.py | 13 ++++++++----- marimapper/visualize_process.py | 2 ++ 6 files changed, 23 insertions(+), 11 deletions(-) diff --git a/marimapper/backends/dummy/dummy_backend.py b/marimapper/backends/dummy/dummy_backend.py index dfe70f8..1a9aadf 100644 --- a/marimapper/backends/dummy/dummy_backend.py +++ b/marimapper/backends/dummy/dummy_backend.py @@ -4,7 +4,7 @@ def __init__(self): pass def get_led_count(self): - return 1 + return 0 def set_led(self, led_index: int, on: bool): pass diff --git a/marimapper/detector.py b/marimapper/detector.py index b76c859..48d4362 100644 --- a/marimapper/detector.py +++ b/marimapper/detector.py @@ -11,7 +11,6 @@ def find_led_in_image(image: cv2.Mat, threshold: int = 128) -> typing.Optional[Point2D]: - logger.debug("looking for led in image") if len(image.shape) > 2: image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) @@ -23,7 +22,6 @@ def find_led_in_image(image: cv2.Mat, threshold: int = 128) -> typing.Optional[P led_response_count = len(contours) if led_response_count == 0: - logger.debug("could not find led") return None moments = cv2.moments(image_thresh) @@ -46,7 +44,6 @@ def find_led_in_image(image: cv2.Mat, threshold: int = 128) -> typing.Optional[P def draw_led_detections(image: cv2.Mat, led_detection: Point2D) -> cv2.Mat: - logger.debug("drawing detection") render_image = ( image if len(image.shape) == 3 else cv2.cvtColor(image, cv2.COLOR_GRAY2BGR) ) diff --git a/marimapper/scanner.py b/marimapper/scanner.py index ed36495..f249080 100644 --- a/marimapper/scanner.py +++ b/marimapper/scanner.py @@ -92,9 +92,15 @@ def mainloop(self): start_scan = get_user_confirmation("Start scan? [y/n]: ") if not start_scan: - print("exiting") + print("Exiting Marimapper") return + if len(self.led_id_range) == 0: + print( + "LED range is zero, have you chosen a backend with 'marimapper --backend'?" + ) + continue + for led_id in self.led_id_range: self.detector.detect(led_id, self.current_view) diff --git a/marimapper/scripts/scanner_cli.py b/marimapper/scripts/scanner_cli.py index 08906fa..e33f219 100644 --- a/marimapper/scripts/scanner_cli.py +++ b/marimapper/scripts/scanner_cli.py @@ -21,7 +21,11 @@ def main(): parser.add_argument("-v", "--verbose", action="store_true") parser.add_argument( - "--dir", type=Path, help="The output folder for your capture", default="." + "dir", + nargs="?", + type=Path, + default=os.getcwd(), + help="the location for your maps, defaults to the current working directory", ) args = parser.parse_args() diff --git a/marimapper/sfm_process.py b/marimapper/sfm_process.py index 5b66e40..1ed8f9b 100644 --- a/marimapper/sfm_process.py +++ b/marimapper/sfm_process.py @@ -4,6 +4,7 @@ import open3d import numpy as np import math +import time logger = get_logger() @@ -65,19 +66,19 @@ def run(self): while not self._exit_event.is_set(): - if not self._input_queue.empty(): + while not self._input_queue.empty(): led: LED2D = self._input_queue.get() if led.point is not None: leds_2d.append(led) update_required = True - else: - if not update_required: - continue + if update_required: + update_required = False leds_3d = sfm(leds_2d) if len(leds_3d) == 0: + logger.info("Failed to reconstruct any leds") continue rescale(leds_3d) @@ -90,7 +91,9 @@ def run(self): for queue in self._output_queues: queue.put(leds_3d) - update_required = False + + else: + time.sleep(1) # clear the queues, don't ask why. while not self._input_queue.empty(): diff --git a/marimapper/visualize_process.py b/marimapper/visualize_process.py index bef419e..d9a7caa 100644 --- a/marimapper/visualize_process.py +++ b/marimapper/visualize_process.py @@ -74,6 +74,8 @@ def initialise_visualiser__(self): view_ctl.set_up((0, 1, 0)) view_ctl.set_lookat((0, 0, 0)) view_ctl.set_zoom(0.3) + # set far distance to 200x the inter-led distance + view_ctl.set_constant_z_far(200) render_options = self._vis.get_render_option() render_options.point_show_normal = True