Improved map rescaling

removed 3d map read from file as unused
Added strips to visualiser

lib/led_map_3d.py

@@ -1,20 +1,17 @@
-from parse import parse
 import numpy as np
-import os
+import math
 from lib import logging
 
 
 class LEDMap3D:
 
-    def __init__(self, data=None, filename=None):
+    def __init__(self, data=None):
 
         self.valid = True
         self.data = {}
         self.cameras = None
         if data is not None:
             self.data = data
-        if filename is not None:
-            self.valid = self._load(filename)
 
     def __setitem__(self, led_index, led_data):
         self.data[led_index] = led_data
@@ -29,7 +26,45 @@ def __len__(self):
         return len(self.data)
 
     def keys(self):
-        return self.data.keys()
+        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()
+        print(led_ids)
+
+        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:
+                        print((led_index, led_index + 1))
+                        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_xyz_list(self):
         return np.array([self[led_id]["pos"] for led_id in self.keys()])
@@ -40,47 +75,17 @@ def get_center(self):
     def get_normal_list(self):
         return np.array([self[led_id]["normal"] for led_id in self.keys()])
 
-    def _load(self, filename):
-        logging.debug(f"Reading 3D map {filename}...")
-
-        if not os.path.exists(filename):
-            logging.error(f"Cannot read 2d map {filename} as file does not exist")
-            return False
+    def get_inter_led_distance(self):
+        max_led_id = max(self.keys())
 
-        with open(filename, "r") as f:
-            lines = f.readlines()
-            headings = lines[0].strip().split(",")
+        distances = []
 
-            if headings != ["index", "x", "y", "z", "xn", "yn", "zn", "error"]:
-                logging.error(f"Cannot read 3d map {filename} as headings don't match")
-                return False
+        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)
 
-            for i in range(1, len(lines)):
-
-                line = lines[i].strip()
-
-                values = parse(
-                    "{index:^d},{x:^f},{y:^f},{z:^f},{xn:^f},{yn:^f},{zn:^f},{error:^f}",
-                    line,
-                )
-                if values is not None:
-                    pos = np.array(
-                        [values.named["x"], values.named["y"], values.named["z"]]
-                    )
-                    normal = np.array(
-                        [values.named["xn"], values.named["yn"], values.named["zn"]]
-                    )
-                    self.data[values.named["index"]] = {
-                        "pos": pos,
-                        "normal": normal,
-                        "error": values.named["error"],
-                    }
-                else:
-                    logging.error(f"Failed to read line {i} of {filename}: {line}")
-                    continue
-
-        logging.debug(f"Read {len(self.data)} lines from 3D map {filename}...")
-        return True
+        return np.median(distances)
 
     def write_to_file(self, filename):
         logging.debug(f"Writing 3D map with {len(self.data)} leds to {filename}...")

lib/map_cleaner.py

@@ -37,19 +37,6 @@ def find_inter_led_distance(led_map):
     return np.median(distances)
 
 
-def rescale(led_map, led_to_led_dist=1.0):
-
-    scale = (1.0 / find_inter_led_distance(led_map)) * led_to_led_dist
-
-    for led_id in led_map.data:
-        led_map[led_id]["pos"] *= scale
-        led_map[led_id]["normal"] *= led_to_led_dist
-        led_map[led_id]["error"] *= scale
-
-    for cam in led_map.cameras:
-        cam[1] *= scale
-
-
 def fill_gaps(led_map, max_dist_err=0.2, max_missing=5):
 
     total_leds_filled = 0

lib/sfm/sfm.py

@@ -114,7 +114,7 @@ def process__(maps_2d, rescale=False, interpolate=False):
             map_3d = binary_to_led_map_3d(temp_dir)
 
             if rescale:
-                map_cleaner.rescale(map_3d)
+                map_3d.rescale()
 
             if interpolate:
                 leds_interpolated = map_cleaner.fill_gaps(map_3d)

lib/visualize_model.py

@@ -15,6 +15,7 @@ def __init__(self, led_map_3d_queue):
         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):
@@ -66,6 +67,7 @@ def initialise_visualiser__(self):
 
         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))
@@ -100,12 +102,22 @@ def reload_geometry__(self, first=False):
             led_map.get_normal_list() * 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))]
+        )
+
         if first:
             self._vis.add_geometry(self.point_cloud)
             self._vis.add_geometry(self.line_set)
+            self._vis.add_geometry(self.strip_set)
         else:
             self._vis.update_geometry(self.point_cloud)
             self._vis.update_geometry(self.line_set)
+            self._vis.update_geometry(self.strip_set)
 
         self.reload_event.clear()
         logging.debug("Renderer3D process reloaded geometry")