Support two step pipeline for eigen factors

project/main.py

@@ -205,19 +205,19 @@ def main(
 
         # max_tracks = PostProcessing.post_process(pcds)
 
-        slam_graph_old = LandmarksSLAMGraph()
-        # slam_graph = EigenPointsSLAMGraph()
+        slam_graph_landmarks = LandmarksSLAMGraph()
+        slam_graph = EigenPointsSLAMGraph()
         # graph_estimated_state = slam_graph.estimate_graph(pcds, max_tracks)
-        graph_estimated_state_old = slam_graph_old.estimate_graph(pcds)
-        # graph_estimated_state = slam_graph.estimate_graph(pcds)
+        graph_estimated_state_landmarks = slam_graph_landmarks.estimate_graph(pcds)
+        graph_estimated_state = slam_graph.estimate_graph(pcds, initial_poses=graph_estimated_state_landmarks)
 
         # measure_error = MeasureError(ds_filename_gt, len(annot_list), num_of_nodes)
         # measure_error.measure_error(first_node, first_gt_node, graph_estimated_state)
         #
-        visualisation = Visualisation(graph_estimated_state_old)
-        visualisation.visualize(pcds, graph_estimated_state_old)
-        # visualisation = Visualisation(graph_estimated_state)
-        # visualisation.visualize(pcds, graph_estimated_state)
+        # visualisation = Visualisation(graph_estimated_state_landmarks)
+        # visualisation.visualize(pcds, graph_estimated_state_landmarks)
+        visualisation = Visualisation(graph_estimated_state)
+        visualisation.visualize(pcds, graph_estimated_state)
 
 
 if __name__ == "__main__":

project/postprocessing/SmallPlanesFilter.py

@@ -6,8 +6,7 @@ class SmallPlanesFilter:
     def filter(pcd: Pcd) -> Pcd:
         result = Pcd(pcd.points)
         for plane in pcd.planes:
-            if len(plane.plane_indices) < 200:
-            # if len(plane.plane_indices) < 1000:
+            if len(plane.plane_indices) < 1000:
                 continue
             result.planes.append(plane)
         return result

project/slam/EigenPointsSLAMGraph.py

@@ -1,18 +1,16 @@
 from typing import List
 
-import numpy as np
-
 from project.dto.Pcd import Pcd
 from project.slam.SLAMGraph import SLAMGraph
 from mrob.mrob import LM_ELLIPS
 
 
 class EigenPointsSLAMGraph(SLAMGraph):
 
-    def _add_planes(self, pcd_s: List[Pcd], needed_indices: List[int]):
-        for _ in self.plane_index_to_real_index:
-            self.graph.add_eigen_factor_plane()
+    def _add_plane_node(self) -> int:
+        return self.graph.add_eigen_factor_plane()
 
+    def _add_planes(self, pcd_s: List[Pcd], needed_indices: List[int]):
         for i, pcd in enumerate(pcd_s):
             for plane in pcd.planes:
                 if needed_indices is not None and plane.track not in needed_indices:

project/slam/LandmarksSLAMGraph.py

@@ -8,11 +8,11 @@
 
 
 class LandmarksSLAMGraph(SLAMGraph):
+    def _add_plane_node(self) -> int:
+        return self.graph.add_node_plane_4d(np.array([1, 0, 0, 0]))
 
     def _add_planes(self, pcd_s: List[Pcd], needed_indices: List[int]):
         w_z = np.identity(4)  # weight matrix
-        for _ in self.plane_index_to_real_index:
-            self.graph.add_node_plane_4d(np.array([1, 0, 0, 0]))
         for i, pcd in enumerate(pcd_s):
             for plane in pcd.planes:
                 if needed_indices is not None and plane.track not in needed_indices:

project/slam/SLAMGraph.py

@@ -1,10 +1,13 @@
 from abc import abstractmethod
 from typing import List
+
+from mrob import mrob
 from mrob.mrob import FGraph, geometry, LM
-from dto.Pcd import Pcd
 
 import numpy as np
 
+from project.dto.Pcd import Pcd
+
 
 class SLAMGraph:
     """
@@ -23,32 +26,34 @@ def __init__(self):
     def _add_planes(self, pcd_s: List[Pcd], needed_indices: List[int]):
         pass
 
+    @abstractmethod
+    def _add_plane_node(self) -> int:
+        pass
+
     @abstractmethod
     def _solve(self):
         pass
 
-    def __build_graph(self, pcd_s: List[Pcd], needed_indices: List[int] = None):
-        for _ in pcd_s:
-            next_node = self.graph.add_node_pose_3d(geometry.SE3())
+    def __build_graph(self, pcd_s: List[Pcd], needed_indices: List[int] = None, initial_poses=None):
+        for index, _ in enumerate(pcd_s):
+            item = geometry.SE3() if initial_poses is None else geometry.SE3(initial_poses[index])
+            if index == 0:
+                next_node = self.graph.add_node_pose_3d(item, mrob.NODE_ANCHOR)
+            else:
+                next_node = self.graph.add_node_pose_3d(item)
             self.graph_trajectory.append(next_node)
 
-        self.graph.add_factor_1pose_3d(
-            geometry.SE3(), self.graph_trajectory[0], 1e6 * np.identity(6)
-        )
-
         for pcd in pcd_s:
-            # add pcd_s len as we add plane nodes after view nodes
-            real_indx = len(self.plane_index_to_real_index) + len(pcd_s)
             for plane in pcd.planes:
                 if (
                         (needed_indices is None or plane.track in needed_indices)
                         and plane.track not in self.plane_index_to_real_index
                 ):
-                    self.plane_index_to_real_index[plane.track] = real_indx
-                    real_indx += 1
+                    real_index = self._add_plane_node()
+                    self.plane_index_to_real_index[plane.track] = real_index
 
-    def estimate_graph(self, pcd_s: List[Pcd], needed_indices: List[int] = None):
-        self.__build_graph(pcd_s, needed_indices)
+    def estimate_graph(self, pcd_s: List[Pcd], needed_indices: List[int] = None, initial_poses=None):
+        self.__build_graph(pcd_s, needed_indices, initial_poses)
         self._add_planes(pcd_s, needed_indices)
         self._solve()
         graph_estimated_state = self.graph.get_estimated_state()