Added cameras back in!

lib/led_map_3d.py

@@ -10,6 +10,7 @@ def __init__(self, data=None, filename=None):
 
         self.valid = True
         self.data = {}
+        self.cameras = None
         if data is not None:
             self.data = data
         if filename is not None:

lib/map_cleaner.py

@@ -37,7 +37,7 @@ def find_inter_led_distance(led_map):
     return np.median(distances)
 
 
-def rescale(led_map, cameras=(), led_to_led_dist=1.0):
+def rescale(led_map, led_to_led_dist=1.0):
 
     scale = (1.0 / find_inter_led_distance(led_map)) * led_to_led_dist
 
@@ -46,8 +46,8 @@ def rescale(led_map, cameras=(), led_to_led_dist=1.0):
         led_map[led_id]["normal"] *= scale
         led_map[led_id]["error"] *= scale
 
-    for cam in cameras:
-        cam[2] *= scale
+    for cam in led_map.cameras:
+        cam[1] *= scale
 
 
 def fill_gaps(led_map, max_dist_err=0.2, max_missing=5):

lib/sfm/model.py

@@ -4,15 +4,14 @@
 
 from lib.sfm.read_write_model import (
     qvec2rotmat,
-    read_cameras_binary,
     read_images_binary,
     read_points3D_binary,
 )
 from lib.remesher import fix_normals
 from lib.led_map_3d import LEDMap3D
 
 
-def get_map_and_cams(path):
+def binary_to_led_map_3d(path):
     led_map = {}
 
     points_bin = read_points3D_binary(os.path.join(path, "0", "points3D.bin"))
@@ -38,56 +37,28 @@ def get_map_and_cams(path):
     for led_id in led_map:
         led_map[led_id]["pos"] -= center
 
-    cams = []
+    cameras = []
 
-    cameras_bin = read_cameras_binary(os.path.join(path, "0", "cameras.bin"))
     images_bin = read_images_binary(os.path.join(path, "0", "images.bin"))
 
     camera_positions = {}
 
     for img in images_bin.values():
         # rotation
-        R = qvec2rotmat(img.qvec)
+        rotation = qvec2rotmat(img.qvec)
 
         # translation
-        t = img.tvec
+        transformation = img.tvec
 
         # invert
-        t = -R.T @ t
-        R = R.T
-
-        t -= center
-
-        camera_positions[img.id] = t
-
-        # intrinsics
-        cam = cameras_bin[img.camera_id]
-
-        if cam.model in ("SIMPLE_PINHOLE", "SIMPLE_RADIAL", "RADIAL"):
-            fx = fy = cam.params[0]
-            cx = cam.params[1]
-            cy = cam.params[2]
-        elif cam.model in (
-            "PINHOLE",
-            "OPENCV",
-            "OPENCV_FISHEYE",
-            "FULL_OPENCV",
-        ):
-            fx = cam.params[0]
-            fy = cam.params[1]
-            cx = cam.params[2]
-            cy = cam.params[3]
-        else:
-            raise Exception("Camera model not supported")
-
-        # intrinsics
-        K = np.identity(3)
-        K[0, 0] = fx
-        K[1, 1] = fy
-        K[0, 2] = cx
-        K[1, 2] = cy
-
-        cams.append([K, R, t, cam.width, cam.height])
+        transformation = -rotation.T @ transformation
+        rotation = rotation.T
+
+        transformation -= center
+
+        camera_positions[img.id] = transformation
+
+        cameras.append([rotation, transformation])
 
     for led_id in led_map:
         all_views = np.array(
@@ -97,6 +68,7 @@ def get_map_and_cams(path):
             np.average(all_views, axis=0) - led_map[led_id]["pos"]
         )
 
-    led_map = fix_normals(led_map)
+    led_map_3d = LEDMap3D(fix_normals(led_map))
+    led_map_3d.cameras = cameras
 
-    return LEDMap3D(led_map), cams
+    return led_map_3d

lib/sfm/sfm.py

@@ -6,7 +6,7 @@
 from multiprocessing import Process, Event
 
 from lib.sfm.database_populator import populate
-from lib.sfm.model import get_map_and_cams
+from lib.sfm.model import binary_to_led_map_3d
 from lib.utils import SupressLogging
 from lib import map_cleaner
 from lib.led_map_2d import get_all_2d_led_maps
@@ -111,10 +111,10 @@ def process__(maps_2d, rescale=False, interpolate=False):
                 )
                 return None
 
-            map_3d, cams = get_map_and_cams(temp_dir)
+            map_3d = binary_to_led_map_3d(temp_dir)
 
             if rescale:
-                map_cleaner.rescale(map_3d, cams)
+                map_cleaner.rescale(map_3d)
 
             if interpolate:
                 leds_interpolated = map_cleaner.fill_gaps(map_3d)

lib/visualize_model.py

@@ -14,6 +14,7 @@ def __init__(self, led_map_3d_queue):
         self.reload_event = Event()
         self.led_map_3d_queue = led_map_3d_queue
         self.point_cloud = None
+        self.line_set = None
         logging.debug("Renderer3D initialised")
 
     def get_reload_event(self):
@@ -64,6 +65,7 @@ def initialise_visualiser__(self):
         )
 
         self.point_cloud = open3d.geometry.PointCloud()
+        self.line_set = open3d.geometry.LineSet()
 
         view_ctl = self._vis.get_view_control()
         view_ctl.set_up((0, 1, 0))
@@ -80,12 +82,19 @@ def initialise_visualiser__(self):
         logging.debug("Renderer3D process initialised visualiser")
 
     def reload_geometry__(self, first=False):
+
         logging.debug("Renderer3D process reloading geometry")
 
         led_map = self.led_map_3d_queue.get()
 
         logging.debug(f"Fetched led map with size {len(led_map.keys())}")
 
+        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)
+
         xyz = []
         normals = []
         for led_id in led_map.keys():
@@ -98,65 +107,52 @@ def reload_geometry__(self, first=False):
         self.point_cloud.normals = open3d.utility.Vector3dVector(normals)
 
         if first:
+            self._vis.add_geometry(self.line_set)
             self._vis.add_geometry(self.point_cloud, reset_bounding_box=True)
 
         self._vis.update_geometry(self.point_cloud)
+        self._vis.update_geometry(self.line_set)
 
         self.reload_event.clear()
         logging.debug("Renderer3D process reloaded geometry")
 
 
-def draw_camera(K, R, t, w, h):
-    """Create axis, plane and pyramed geometries in Open3D format.
-    :param K: calibration matrix (camera intrinsics)
-    :param R: rotation matrix
-    :param t: translation
-    :param w: image width
-    :param h: image height
-    :return: camera model geometries (axis, plane and pyramid)
-    """
-
-    # scale = 1
-    color = [0.8, 0.8, 0.8]
-
-    # intrinsics
-    K = K.copy()
-    Kinv = np.linalg.inv(K)
-
-    # 4x4 transformation
-    T = np.column_stack((R, t))
-    T = np.vstack((T, (0, 0, 0, 1)))
-
-    # axis
-    # axis = open3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5 * scale)
-    # axis.transform(T)
-
-    # points in pixel
-    points_pixel = [
-        [0, 0, 0],
-        [0, 0, 1],
-        [w, 0, 1],
-        [0, h, 1],
-        [w, h, 1],
-    ]
-
-    # pixel to camera coordinate system
-    points = [Kinv @ p for p in points_pixel]
-
-    # pyramid
-    points_in_world = [(R @ p + t) for p in points]
-    lines = [
-        [0, 1],
-        [0, 2],
-        [0, 3],
-        [0, 4],
-    ]
-    colors = [color for _ in range(len(lines))]
-    line_set = open3d.geometry.LineSet(
-        points=open3d.utility.Vector3dVector(points_in_world),
-        lines=open3d.utility.Vector2iVector(lines),
-    )
-    line_set.colors = open3d.utility.Vector3dVector(colors)
-
-    # return as list in Open3D format
-    return [line_set]
+def camera_to_points_lines_colors(cameras):  # returns points and lines
+
+    all_points = []
+    all_lines = []
+
+    for cam_id, camera in enumerate(cameras):
+
+        R, t = camera
+
+        Kinv = np.array([[0.0005, 0, -0.5], [0, 0.0005, -0.5], [0, 0, 1]])
+
+        # points in pixel
+        points_pixel = [
+            [0, 0, 0],
+            [0, 0, 1],
+            [2000, 0, 1],
+            [0, 2000, 1],
+            [2000, 2000, 1],
+        ]
+
+        # pixel to camera coordinate system
+        points = [Kinv @ p for p in points_pixel]
+
+        # pyramid
+        points_in_world = [(R @ p + t) for p in points]
+        offset = cam_id * 5
+        lines = [
+            [offset, offset + 1],
+            [offset, offset + 2],
+            [offset, offset + 3],
+            [offset, offset + 4],
+        ]
+
+        all_points.extend(points_in_world)
+        all_lines.extend(lines)
+
+    all_colors = [[0.8, 0.8, 0.8] for _ in range(len(all_lines))]
+
+    return all_points, all_lines, all_colors