wip

prime-slam · Feb 5, 2024 · b35d0b9 · b35d0b9
1 parent c7a4e99
commit b35d0b9
Show file tree

Hide file tree

Showing 19 changed files with 284 additions and 81 deletions.
diff --git a/.gitignore b/.gitignore
@@ -1,4 +1,6 @@
 .idea
 __pycache__
 .DS_Store
-venv
+venv
+hilti
+output
diff --git a/examples/configurations/hilti.yaml b/examples/configurations/hilti.yaml
@@ -1,25 +1,27 @@
 dataset:
-  path: "evaluation/hilti"
+  path: "hilti/basement1"
   patches:
-    start: 0
-    end: 29
-    step: 10
+    start: 400
+    end: 500
+    step: 100
     iterations: 1
+undistortion:
+  segments: 10
 grid:
   voxel_edge_length: 8
 subdividers:
-  size: 2
+  size: 1
 segmenters:
   ransac:
     threshold: 0.01
     initial_points: 6
-    iterations: 5000
+    iterations: 300
 backend:
   type: "eigen_factor"
   parameters:
     iterations_number: 5000
-    robust_type: HUBER
+    robust_type: QUADRATIC
 output:
   visualization_path: "output/hilti/visualization"
   optimisation_path: "output/hilti/optimisation"
-debug: true
+debug: false
diff --git a/examples/pipeline.py b/examples/pipeline.py
@@ -19,6 +19,7 @@
 python3 examples/pipeline.py --configuration_path examples/configurations/hilti.yaml
 ```
 """
+import numpy as np
 import open3d as o3d
 from octreelib.grid import VisualizationConfig
 
@@ -39,7 +40,7 @@
     HiltiReader,
     KittiReader,
     NuscenesReader,
-    OptimisedPoseReadWriter,
+    OptimisationsReadWriter, Undistortioner,
 )
 
 if __name__ == "__main__":
@@ -58,7 +59,7 @@
         dataset_reader = NuscenesReader()
     else:
         raise ValueError("Unrecognisable type of dataset")
-    posesWriter = OptimisedPoseReadWriter()
+    optimisationsWriter = OptimisationsReadWriter()
 
     for ind in range(
         configuration_reader.patches_start,
@@ -72,7 +73,7 @@
 
         print(f"Processing {start} to {end-1}...")
 
-        point_clouds = []
+        initial_point_clouds = []
         initial_poses = []
         for s in range(start, end):
             point_cloud_path = os.path.join(
@@ -85,11 +86,30 @@
             point_cloud = dataset_reader.read_point_cloud(filename=point_cloud_path)
             initial_pose = dataset_reader.read_pose(filename=pose_path)
 
-            point_clouds.append(point_cloud)
+            initial_point_clouds.append(point_cloud)
             initial_poses.append(initial_pose)
 
         poses = copy.deepcopy(initial_poses)
+        point_clouds = copy.deepcopy(initial_point_clouds)
+
         for iteration_ind in range(configuration_reader.patches_iterations):
+            if configuration_reader.undistortion_segments is not None:
+                undistorted_point_cloud = o3d.geometry.PointCloud(o3d.utility.Vector3dVector())
+
+                undistortioner = Undistortioner(configuration_reader.undistortion_segments)
+                for undistortion_ind in range(end - start - 1):
+                    point_clouds[undistortion_ind] = undistortioner(
+                        point_clouds[undistortion_ind],
+                        poses[undistortion_ind],
+                        point_clouds[undistortion_ind + 1],
+                        poses[undistortion_ind + 1],
+                    )
+                    poses[undistortion_ind] = np.eye(4)
+
+                    undistorted_point_cloud += point_clouds[undistortion_ind].transform(poses[undistortion_ind])
+
+                #o3d.visualization.draw(undistorted_point_cloud)
+
             pipeline = SequentialPipeline(
                 point_clouds=point_clouds,
                 poses=poses,
@@ -122,7 +142,7 @@
                     o3d.utility.Vector3dVector()
                 )
                 for point_cloud, initial_pose, optimised_pose in zip(
-                    point_clouds, initial_poses, poses
+                    initial_point_clouds, initial_poses, poses
                 ):
                     color = [random.random(), random.random(), random.random()]
                     before = copy.deepcopy(point_cloud).transform(initial_pose)
@@ -139,10 +159,19 @@
                 o3d.visualization.draw(optimised_point_cloud)
 
         for optimised_pose_number in range(start, end):
-            posesWriter.write(
+            optimisationsWriter.write_pose(
                 os.path.join(
                     configuration_reader.optimisation_dir,
+                    "poses",
                     f"{optimised_pose_number}.txt",
                 ),
                 poses[optimised_pose_number - start],
             )
+            optimisationsWriter.write_point_cloud(
+                os.path.join(
+                    configuration_reader.optimisation_dir,
+                    "clouds",
+                    f"{optimised_pose_number}.pcd",
+                ),
+                point_clouds[optimised_pose_number - start],
+            )
diff --git a/slam/backend/__init__.py b/slam/backend/__init__.py
@@ -7,4 +7,9 @@
 from slam.backend.eigen_factor import *
 from slam.backend.mrob_backend import *
 
-__all__ = backend_module.__all__ + bareg_module.__all__ + eigen_factor_module.__all__ + mrob_backend_module.__all__
+__all__ = (
+    backend_module.__all__
+    + bareg_module.__all__
+    + eigen_factor_module.__all__
+    + mrob_backend_module.__all__
+)
diff --git a/slam/backend/bareg.py b/slam/backend/bareg.py
@@ -16,9 +16,7 @@ def _init_point_clouds(self, grid: GridBase) -> None:
             Represents grid with all inserted point clouds
         """
         for pose_number in range(self._poses_number):
-            leaf_voxels = grid.get_leaf_points(
-                pose_number=pose_number,
-            )
+            leaf_voxels = grid.get_leaf_points(pose_number=pose_number)
             for voxel in leaf_voxels:
                 if voxel.id not in self._planes.keys():
                     bareg_plane_id = self._graph.add_bareg_plane()

diff --git a/slam/backend/eigen_factor.py b/slam/backend/eigen_factor.py
@@ -16,9 +16,7 @@ def _init_point_clouds(self, grid: GridBase) -> None:
             Represents grid with all inserted point clouds
         """
         for pose_number in range(self._poses_number):
-            leaf_voxels = grid.get_leaf_points(
-                pose_number=pose_number,
-            )
+            leaf_voxels = grid.get_leaf_points(pose_number=pose_number)
             for voxel in leaf_voxels:
                 if voxel.id not in self._planes.keys():
                     factor_plane_id = self._graph.add_eigen_factor_plane_center()

diff --git a/slam/backend/mrob_backend.py b/slam/backend/mrob_backend.py
@@ -25,10 +25,7 @@ class MROBBackend(Backend):
     """
 
     def __init__(
-        self,
-        poses_number: int,
-        iterations_number: int,
-        robust_type: int = mrob.HUBER,
+        self, poses_number: int, iterations_number: int, robust_type: int = mrob.HUBER
     ) -> None:
         self._graph: mrob.FGraph = mrob.FGraph(robust_type)
         self._poses_number: int = poses_number
@@ -72,27 +69,19 @@ def process(self, grid: GridBase) -> BackendOutput:
         """
         self._init_poses()
         self._init_point_clouds(grid)
-        metrics = [
-            Metric(name="FGraph initial error", value=self._graph.chi2(True)),
-        ]
+        metrics = [Metric(name="FGraph initial error", value=self._graph.chi2(True))]
         converge_iterations = self._graph.solve(mrob.LM_ELLIPS, self._iterations_number)
         while converge_iterations == 0:
             print("Optimization didn't converge")
             converge_iterations = self._graph.solve(
                 mrob.LM_ELLIPS, self._iterations_number
             )
 
-        metrics.append(
-            Metric(name="Iterations to converge", value=converge_iterations),
-        )
-        metrics.append(
-            Metric(name="chi2", value=self._graph.chi2()),
-        )
+        metrics.append(Metric(name="Iterations to converge", value=converge_iterations))
+        metrics.append(Metric(name="chi2", value=self._graph.chi2()))
 
         return BackendOutput(
-            self._graph.get_estimated_state(),
-            metrics,
-            self.__get_unused_features(),
+            self._graph.get_estimated_state(), metrics, self.__get_unused_features()
         )
 
     def __get_unused_features(self) -> List[int]:

diff --git a/slam/pipeline/__init__.py b/slam/pipeline/__init__.py
@@ -4,6 +4,8 @@
 from slam.pipeline.pipeline import *
 from slam.pipeline.sequential_pipeline import *
 
-__all__ = (pipeline_module.__all__ +
-           sequential_pipeline_module.__all__ +
-           ["ConfigurationReader", "YAMLConfigurationReader"])
+__all__ = (
+    pipeline_module.__all__
+    + sequential_pipeline_module.__all__
+    + ["ConfigurationReader", "YAMLConfigurationReader"]
+)
diff --git a/slam/pipeline/configuration/reader.py b/slam/pipeline/configuration/reader.py
@@ -3,7 +3,7 @@
 
 import copy
 from abc import ABC, abstractmethod
-from typing import List
+from typing import List, Optional
 
 from slam.backend import Backend, BaregBackend, EigenFactorBackend
 from slam.filter import Filter
@@ -156,6 +156,26 @@ def patches_iterations(self) -> int:
 
         return int(value)
 
+    @property
+    def undistortion_segments(self) -> Optional[int]:
+        """
+        Represents patches iterations parameter of pipeline
+
+        Returns
+        -------
+        patches_iterations: int
+            Patches iterations
+        """
+        try:
+            undistortion_configuration = copy.deepcopy(
+                self._configuration["undistortion"]
+            )
+            value = undistortion_configuration["segments"]
+        except KeyError:
+            return None
+
+        return int(value)
+
     @property
     def visualization_dir(self) -> str:
         """
@@ -218,9 +238,7 @@ def subdividers(self) -> List[Subdivider]:
 
         for name in subdividers_configuration.keys():
             name = name.lower()
-            subdividers.append(
-                subdividers_names[name](subdividers_configuration[name]),
-            )
+            subdividers.append(subdividers_names[name](subdividers_configuration[name]))
 
         return subdividers
 
@@ -304,10 +322,7 @@ def backend(self, start: int, end: int) -> Backend:
         except KeyError:
             raise ValueError("backend_configuration must be not empty")
 
-        backend_names = {
-            "eigen_factor": EigenFactorBackend,
-            "bareg": BaregBackend,
-        }
+        backend_names = {"eigen_factor": EigenFactorBackend, "bareg": BaregBackend}
         robust_types = {
             "huber": mrob.HUBER,
             "quadratic": mrob.QUADRATIC,

diff --git a/slam/pipeline/sequential_pipeline.py b/slam/pipeline/sequential_pipeline.py
@@ -67,6 +67,6 @@ def run(self, parameters: SequentialPipelineRuntimeParameters) -> BackendOutput:
         backend_output = self._backend.process(grid)
 
         parameters.visualization_config.unused_voxels = backend_output.unused_features
-        grid.visualize(parameters.visualization_config)
+        #grid.visualize(parameters.visualization_config)
 
         return backend_output
diff --git a/slam/segmenter/__init__.py b/slam/segmenter/__init__.py
@@ -9,5 +9,10 @@
 from slam.segmenter.ransac import *
 from slam.segmenter.segmenter import *
 
-__all__ = (cape_module.__all__ + count_module.__all__ + segmenter_module.__all__ +
-           ransac_module.__all__ + identical_module.__all__)
+__all__ = (
+    cape_module.__all__
+    + count_module.__all__
+    + segmenter_module.__all__
+    + ransac_module.__all__
+    + identical_module.__all__
+)
diff --git a/slam/subdivider/__init__.py b/slam/subdivider/__init__.py
@@ -7,4 +7,9 @@
 from slam.subdivider.size import *
 from slam.subdivider.subdivider import *
 
-__all__ = count_module.__all__ + eigen_value_module.__all__ + subdivider_base_module.__all__ + size_module.__all__
+__all__ = (
+    count_module.__all__
+    + eigen_value_module.__all__
+    + subdivider_base_module.__all__
+    + size_module.__all__
+)
diff --git a/slam/typing/hints.py b/slam/typing/hints.py
@@ -3,14 +3,7 @@
 
 from typing import Annotated, Literal, TypeVar
 
-__all__ = [
-    "Array3",
-    "Array4x4",
-    "ArrayNx3",
-    "ArrayNx4",
-    "ArrayNx8",
-    "ArrayNx4x4",
-]
+__all__ = ["Array3", "Array4x4", "ArrayNx3", "ArrayNx4", "ArrayNx8", "ArrayNx4x4"]
 
 DType = TypeVar("DType", bound=np.generic)
 

diff --git a/slam/utils/__init__.py b/slam/utils/__init__.py
@@ -1,13 +1,16 @@
-import slam.pipeline.configuration as configuration_module
-import slam.utils.pose_readwriter as pose_readwriter_module
+import slam.utils.optimisations_readwriter as optimisations_readwriter_module
+import slam.utils.undistortioner as undistortioner_module
 from slam.utils.dataset_reader import (
     DatasetReader,
     HiltiReader,
     KittiReader,
     NuscenesReader,
 )
-from slam.utils.pose_readwriter import *
+from slam.utils.optimisations_readwriter import *
+from slam.utils.undistortioner import *
 
-__all__ = (configuration_module.__all__
-           + pose_readwriter_module.__all__ +
-           ["DatasetReader", "HiltiReader", "KittiReader", "NuscenesReader"])
+__all__ = (
+    optimisations_readwriter_module.__all__
+    + undistortioner_module.__all__
+    + ["DatasetReader", "HiltiReader", "KittiReader", "NuscenesReader"]
+)
diff --git a/slam/utils/dataset_reader/__init__.py b/slam/utils/dataset_reader/__init__.py
@@ -7,4 +7,9 @@
 from slam.utils.dataset_reader.nuscenes import *
 from slam.utils.dataset_reader.reader import *
 
-__all__ = hilti_module.__all__ + kitti_module.__all__ + nuscenes_module.__all__ + reader_module.__all__
+__all__ = (
+    hilti_module.__all__
+    + kitti_module.__all__
+    + nuscenes_module.__all__
+    + reader_module.__all__
+)