Support eigen factors

project/Visualisation.py

@@ -41,13 +41,14 @@ def visualize(self, pcd_s: List[Pcd], graph_estimated_state):
         )
 
         pc_answ = o3d.geometry.PointCloud()
+        graph_estimated_state = graph_estimated_state[:len(pcd_s)]
         for i in range(num_of_nodes):
             pc = self.visualize_pcd(
                 pcd_s[-(i + 1)], [graph_estimated_state[-(i + 1)], reflection]
             )
             pc = pc.voxel_down_sample(0.05)
 
-            pcd_s[-(i + 1)].points = None
+            # pcd_s[-(i + 1)].points = None
             pc_answ += pc
         pc_answ = pc_answ.voxel_down_sample(0.05)
 

project/main.py

@@ -6,7 +6,6 @@
 import numpy as np
 
 from project import read_office, config
-from project.SLAMGraph import SLAMGraph
 from project.Visualisation import Visualisation
 from project.annotators.AnnotatorImage import AnnotatorImage
 from project.annotators.AnnotatorPointCloud import AnnotatorPointCloud
@@ -23,6 +22,8 @@
 from project.postprocessing.PlaneInfoPrinter import PlaneInfoPrinter
 from project.postprocessing.PlaneRemover import PlaneRemover
 from project.postprocessing.SmallPlanesFilter import SmallPlanesFilter
+from project.slam.EigenPointsSLAMGraph import EigenPointsSLAMGraph
+from project.slam.LandmarksSLAMGraph import LandmarksSLAMGraph
 from project.utils.intervals import load_evaluation_intervals, dump_evaluation_intervals, ids_list_to_intervals
 
 FORMAT_ICL_TUM = 1
@@ -204,15 +205,19 @@ def main(
 
         # max_tracks = PostProcessing.post_process(pcds)
 
-        slam_graph = SLAMGraph()
+        slam_graph_old = LandmarksSLAMGraph()
+        # slam_graph = EigenPointsSLAMGraph()
         # graph_estimated_state = slam_graph.estimate_graph(pcds, max_tracks)
-        graph_estimated_state = slam_graph.estimate_graph(pcds)
+        graph_estimated_state_old = slam_graph_old.estimate_graph(pcds)
+        # graph_estimated_state = slam_graph.estimate_graph(pcds)
 
         # 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)
-        visualisation.visualize(pcds, 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)
 
 
 if __name__ == "__main__":

project/slam/EigenPointsSLAMGraph.py

@@ -0,0 +1,31 @@
+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()
+
+        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:
+                    continue
+
+                cur_indx = self.plane_index_to_real_index[plane.track]
+                plane_points = pcd.points[plane.plane_indices]
+                self.graph.eigen_factor_plane_add_points_array(
+                    planeEigenId=cur_indx,
+                    nodePoseId=self.graph_trajectory[i],
+                    pointsArray=plane_points,
+                    W=1.0
+                )
+
+    def _solve(self):
+        self.graph.solve(LM_ELLIPS)

project/slam/LandmarksSLAMGraph.py

@@ -0,0 +1,27 @@
+from typing import List
+from mrob.mrob import FGraph, LM
+
+import numpy as np
+
+from project.dto.Pcd import Pcd
+from project.slam.SLAMGraph import SLAMGraph
+
+
+class LandmarksSLAMGraph(SLAMGraph):
+
+    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:
+                    continue
+
+                cur_indx = self.plane_index_to_real_index[plane.track]
+                self.graph.add_factor_1pose_1plane_4d(
+                    plane.equation, self.graph_trajectory[i], cur_indx, w_z
+                )
+
+    def _solve(self):
+        self.graph.solve(LM)

project/SLAMGraph.py → project/slam/SLAMGraph.py

@@ -1,3 +1,4 @@
+from abc import abstractmethod
 from typing import List
 from mrob.mrob import FGraph, geometry, LM
 from dto.Pcd import Pcd
@@ -18,41 +19,37 @@ def __init__(self):
         self.graph_trajectory = []
         self.plane_index_to_real_index = {}
 
+    @abstractmethod
+    def _add_planes(self, pcd_s: List[Pcd], needed_indices: List[int]):
+        pass
+
+    @abstractmethod
+    def _solve(self):
+        pass
+
     def __build_graph(self, pcd_s: List[Pcd], needed_indices: List[int] = None):
         for pcd in pcd_s:
             real_indx = len(self.plane_index_to_real_index)
             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
+                        (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
 
-        for _ in self.plane_index_to_real_index:
-            self.graph.add_node_plane_4d(np.array([1, 0, 0, 0]))
-
         for _ in pcd_s:
             next_node = self.graph.add_node_pose_3d(geometry.SE3())
             self.graph_trajectory.append(next_node)
+
         self.graph.add_factor_1pose_3d(
             geometry.SE3(), self.graph_trajectory[0], 1e6 * np.identity(6)
         )
 
     def estimate_graph(self, pcd_s: List[Pcd], needed_indices: List[int] = None):
-
         self.__build_graph(pcd_s, needed_indices)
-        w_z = np.identity(4)  # weight matrix
-
-        for i, pcd in enumerate(pcd_s):
-            for plane in pcd.planes:
-                if needed_indices is None or plane.track in needed_indices:
-                    cur_indx = self.plane_index_to_real_index[plane.track]
-                    self.graph.add_factor_1pose_1plane_4d(
-                        plane.equation, self.graph_trajectory[i], cur_indx, w_z
-                    )
-
-        self.graph.solve(LM)
+        self._add_planes(pcd_s, needed_indices)
+        self._solve()
         graph_estimated_state = self.graph.get_estimated_state()
 
         return graph_estimated_state