From 525229c33d6815a1ee2c5f0bb658d382a4eeee77 Mon Sep 17 00:00:00 2001
From: Samuel <martinsa0182@gmail.com>
Date: Fri, 27 Dec 2024 09:07:06 +0000
Subject: [PATCH] Adjusted log levels Added iterator to find largest map

---
 marimapper/camera.py           | 10 +++++-----
 marimapper/detector_process.py |  6 +++++-
 marimapper/sfm.py              | 13 +++++++++----
 3 files changed, 19 insertions(+), 10 deletions(-)

diff --git a/marimapper/camera.py b/marimapper/camera.py
index 3888cd7..6000616 100644
--- a/marimapper/camera.py
+++ b/marimapper/camera.py
@@ -62,31 +62,31 @@ def set_autofocus(self, mode, focus=0):
         logger.debug(f"Setting autofocus to mode {mode} with focus {focus}")
 
         if not self.device.set(cv2.CAP_PROP_AUTOFOCUS, mode):
-            logger.error(f"Failed to set autofocus to {mode}")
+            logger.info(f"Failed to set autofocus to {mode}")
 
         if not self.device.set(cv2.CAP_PROP_FOCUS, focus):
-            logger.error(f"Failed to set focus to {focus}")
+            logger.info(f"Failed to set focus to {focus}")
 
     def set_exposure_mode(self, mode):
 
         logger.debug(f"Setting exposure to mode {mode}")
 
         if not self.device.set(cv2.CAP_PROP_AUTO_EXPOSURE, mode):
-            logger.error(f"Failed to put camera into manual exposure mode {mode}")
+            logger.info(f"Failed to put camera into manual exposure mode {mode}")
 
     def set_gain(self, gain):
 
         logger.debug(f"Setting gain to {gain}")
 
         if not self.device.set(cv2.CAP_PROP_GAIN, gain):
-            logger.error(f"failed to set camera gain to {gain}")
+            logger.info(f"failed to set camera gain to {gain}")
 
     def set_exposure(self, exposure):
 
         logger.debug(f"Setting exposure to {exposure}")
 
         if not self.device.set(cv2.CAP_PROP_EXPOSURE, exposure):
-            logger.error(f"Failed to set exposure to {exposure}")
+            logger.warning(f"Failed to set exposure to {exposure}")
 
     def eat(self, count=30):
         for _ in range(count):
diff --git a/marimapper/detector_process.py b/marimapper/detector_process.py
index 743a0ae..49a00da 100644
--- a/marimapper/detector_process.py
+++ b/marimapper/detector_process.py
@@ -128,14 +128,18 @@ def run(self):
                         if led_current is not None:
                             distance = get_distance(led_current, led_previous)
                             if distance > 0.01:  # 1% movement
+                                logger.error(
+                                    f"Camera movement of {int(distance*100)}% has been detected"
+                                )
                                 movement = True
                         else:
                             logger.error(
-                                f"went back to check led {led_previous.led_id} for movement, and led could no longer be found"
+                                f"Went back to check led {led_previous.led_id} for movement, and led could no longer be found"
                             )
                             movement = True
 
                     if movement:
+                        logger.error("Deleting scan due to camera movement")
                         self.put_in_all_output_queues(
                             DetectionControlEnum.DELETE, view_id
                         )
diff --git a/marimapper/sfm.py b/marimapper/sfm.py
index c7d31bc..5ab9d8d 100644
--- a/marimapper/sfm.py
+++ b/marimapper/sfm.py
@@ -49,10 +49,15 @@ def sfm(leds_2d: list[LED2D]) -> list[LED3D]:
         # Because of this, lots of existing functionality like inter-led distance might break
         # Leaving it out for now but perhaps something to come back to.
 
-        if not Path(temp_dir, "0", "points3D.bin").exists():
-            return []
+        leds_3d = []
+        for map_id in range(0, 10):
+            if not Path(temp_dir, f"{map_id}", "points3D.bin").exists():
+                break
 
-        leds_3d = binary_to_led_map_3d(Path(temp_dir))
-        logger.debug(f"sfm managed to reconstruct {len(leds_3d)} leds")
+            new_map = binary_to_led_map_3d(Path(temp_dir))
+
+            print(f"sfm managed to reconstruct {len(new_map)} leds in map {map_id}")
+
+            leds_3d = new_map if len(new_map) > len(leds_3d) else leds_3d
 
         return leds_3d