Implement frontend simplification

src/apps/sfm_map_creator/ui.py

@@ -57,13 +57,17 @@ def __init__(
         interactive: bool = True,
         debug: bool = True,
     ):
+        """TODO: Clean up."""
         QtWidgets.QMainWindow.__init__(self, parent)
         try:
             self._init_ui()
         except AttributeError as e:
             print(f"UI setup failed! e={e}.")
             raise
 
+        self._mapper_thread = None
+        self._ipython_thread = None
+
         self._image_update_signal.connect(self._show_image)
 
         self._state = _ExecutionState()
@@ -73,10 +77,14 @@ def __init__(
         self._output_path = Path(output_path)
 
         # TODO: remove hardcoded data type.
-        calibration_data = kitti.KittiCalibration(Path(self._calibration_path))
-        camera_calibration0 = calibration_data.stereo_calibration.calibrations[0]
-        self._output_path = self._output_path / str(uuid.uuid4())
-        os.makedirs(self._output_path)
+        try:
+            calibration_data = kitti.KittiCalibration(Path(self._calibration_path))
+            camera_calibration0 = calibration_data.stereo_calibration.calibrations[0]
+            self._output_path = self._output_path / str(uuid.uuid4())
+            os.makedirs(self._output_path)
+        except FileNotFoundError as e:
+            print(e)
+            raise
 
         # Mapper
         self._debug = debug
@@ -91,15 +99,13 @@ def __init__(
                 ),
             ),
         )
-        self._mapper_thread = None
         self._prev_frame_count = 0
 
         # Monitor
         self._monitor_timer = None
         self._reset_monitor()
 
         # IPython
-        self._ipython_thread = None
         if interactive:
             self._ipython_thread = threading.Thread(target=self._start_ipython)
             self._ipython_thread.start()
@@ -138,7 +144,7 @@ def _run(self):
         self._mapper_thread.start()
 
     def __del__(self):
-        if self._mapper_thread.is_alive():
+        if self._mapper_thread is not None and self._mapper_thread.is_alive():
             self._mapper_thread.join()
         if self._ipython_thread is not None and self._ipython_thread.is_alive():
             self._ipython_thread.join()
@@ -148,55 +154,53 @@ def _run_mapping(self):
         while True:
             if not self._step_run():
                 break
-            time.sleep(0.01)
+            time.sleep(0.001)
+        print("Start map optimization.")
+        self._update_viewer()
+
+    def _update_viewer(self):
+        if self._debug:
+            # NOTE: The points are estimated in reference frame.
+            frames = self._mapper.reconstruction.frames
+            if len(frames) < 2:
+                return
+            ref_frame = frames[-2]
+            points3d = ref_frame.points3d
+            if points3d is None or len(points3d) == 0:
+                return
+
+            pose_G = ref_frame.pose_G
+            image_points = ref_frame.camera.project_points(points3d)
+            image_points = image_points.round().astype(np.int32)
+
+            colors = ref_frame.image.data[image_points[:, 1], image_points[:, 0]]
+            self._plotter.add_points(
+                pose_G @ points3d,
+                scalars=colors,
+                rgb=True,
+                point_size=5.0,
+            )
+            _plot_pose(self._plotter, pose_G)
+        else:
+            points = self._mapper.reconstruction.get_landmark_positions_G()
+            if len(points):
+                self._plotter.add_points(
+                    points, point_size=5.0, color="g", render_points_as_spheres=True
+                )
 
     def _update(self):
         """
         TODO: implement visualization module.
         """
         if self._mapper.state.frame_count != self._prev_frame_count:
             self._prev_frame_count = self._mapper.state.frame_count
-
-            if self._debug:
-                # NOTE: The points are estimated in reference frame.
-                frames = self._mapper.reconstruction.frames
-                if len(frames) < 2:
-                    return
-                ref_frame = frames[-2]
-                points3d = ref_frame.points3d
-                if points3d is None or len(points3d) == 0:
-                    return
-
-                pose_G = ref_frame.pose_G
-                image_points = ref_frame.camera.project_points(points3d)
-                image_points = image_points.round().astype(np.int32)
-
-                colors = (
-                    ref_frame.image.data[image_points[:, 1], image_points[:, 0]]
-                    .reshape(-1, 3)
-                    .astype(np.float)
-                )
-
-                self._plotter.add_points(
-                    pose_G @ points3d,
-                    # render_points_as_spheres=True,
-                    scalars=colors,
-                    rgb=True,
-                    point_size=5.0,
-                )
-
-                _plot_pose(self._plotter, pose_G)
-
-            else:
-                points = self._mapper.reconstruction.get_landmark_positions_G()
-                if len(points):
-                    self._plotter.add_points(
-                        points, point_size=5.0, color="g", render_points_as_spheres=True
-                    )
+            self._update_viewer()
 
     def _step_run(self):
         self._state.mode = _ExecutionMode.Step
         r = self._mapper.step_run()
+        self._mapper.bundle_adjustment()
+        self._update_viewer()
         self._image_update_signal.emit()
         return r
 

src/mvg/mapping/common/frame.py

@@ -20,6 +20,13 @@ class Frame:
     image: Optional[Image] = None
     points3d: Optional[np.ndarray] = None
 
+    def __repr__(self) -> str:
+        num_points3d = len(self.points3d) if self.points3d is not None else None
+        return (
+            f"Frame(id={self.id}, #keypoints={len(self.keypoints)}, "
+            f"#points3d={num_points3d}, pose_G={self.pose_G})"
+        )
+
     def has_point(self, point: np.ndarray):
         # TODO
         return True

src/mvg/mapping/common/reconstruction.py

@@ -1,7 +1,7 @@
 import uuid
 from dataclasses import dataclass
 from pathlib import Path
-from typing import List, Optional
+from typing import Dict, List, Tuple
 
 import numpy as np
 import plyfile
@@ -13,20 +13,23 @@
 class Landmark:
     id: uuid.UUID
     pose_G: SE3
-    descriptor: np.ndarray
-    frame_id: uuid.UUID
-    num_matches: Optional[int] = -1  # TODO
-    num_observations: Optional[int] = -1  # TODO
+    descriptor: np.ndarray  # TODO: Compute a representative from the observations
+    observations: Dict[int, Tuple[Frame, Dict]]
 
 
 class Reconstruction:
     def __init__(self):
         # TODO: Use pandas.DataFrame
         self._landmarks_G: List[Landmark] = list()
         self._frames: List[Frame] = list()
+        self._frame_index_lookup = dict()
 
     def add_frame(self, frame: Frame):
         self._frames.append(frame)
+        self._frame_index_lookup[frame.id] = len(self._frames) - 1
+
+    def get_frame_index_from_id(self, frame_id: uuid.UUID):
+        return self._frame_index_lookup.get(frame_id)
 
     def add_landmark_G(self, landmark: Landmark):
         self._landmarks_G.append(landmark)
@@ -55,10 +58,15 @@ def landmarks_G(self) -> np.ndarray:
 
     def dump(self, path: Path):
         points = self.get_landmark_positions_G()
-        filepath = path / "reconstruction.ply"
-        print(f"Writing to {filepath}.")
+        model_filepath = path / "reconstruction.ply"
+        print(f"Writing model to {model_filepath}.")
         vertices = np.array(
             [tuple(e) for e in points], dtype=[("x", "<f4"), ("y", "<f4"), ("z", "<f4")]
         )
         el = plyfile.PlyElement.describe(vertices, "vertex")
-        plyfile.PlyData([el], text=True).write(filepath)
+        plyfile.PlyData([el], text=True).write(model_filepath)
+
+        trajectory_filepath = path / "trajectory.txt"
+        print(f"Writing trajectory to {trajectory_filepath}.")
+        poses = np.vstack([frame.pose_G.as_rotvec_pose() for frame in self._frames])
+        np.savetxt(trajectory_filepath, poses)