feat(yaml_reader): slighlty change yaml configuration

Author: pmokeev

examples/configurations/hilti.yaml

@@ -1,25 +1,25 @@
 dataset:
+  type: "hilti"
   path: "evaluation/hilti"
   patches:
     start: 0
     end: 29
     step: 10
     iterations: 1
-grid:
-  voxel_edge_length: 8
-subdividers:
-  size: 2
-segmenters:
-  ransac:
-    threshold: 0.01
-    initial_points: 6
-    iterations: 5000
-backend:
-  type: "eigen_factor"
-  parameters:
-    iterations_number: 5000
-    robust_type: HUBER
-output:
-  visualization_path: "output/hilti/visualization"
-  optimisation_path: "output/hilti/optimisation"
-debug: true
+pipeline:
+  grid:
+    voxel_edge_length: 8
+  subdividers:
+    size: 1
+  segmenters:
+    ransac:
+      threshold: 0.01
+      initial_points: 6
+      iterations: 1000
+  backend:
+    type: "eigen_factor"
+    parameters:
+      iterations_number: 5000
+      robust_type: QUADRATIC
+  output: "output/hilti"
+debug: false

examples/configurations/kitti.yaml

@@ -1,27 +1,27 @@
 dataset:
+  type: "kitti"
   path: "evaluation/kitti"
   patches:
     start: 0
     end: 100
     step: 10
     iterations: 1
-grid:
-  voxel_edge_length: 16
-subdividers:
-  size: 2
-segmenters:
-  ransac:
-    threshold: 0.01
-    initial_points: 6
-    iterations: 5000
-  cape:
-    correlation: 300
-backend:
-  type: "eigen_factor"
-  parameters:
-    iterations_number: 5000
-    robust_type: HUBER
-output:
-  visualization_path: "output/kitti/visualization"
-  optimisation_path: "output/kitti/optimisation"
+pipeline:
+  grid:
+    voxel_edge_length: 16
+  subdividers:
+    size: 2
+  segmenters:
+    ransac:
+      threshold: 0.01
+      initial_points: 6
+      iterations: 5000
+    cape:
+      correlation: 300
+  backend:
+    type: "eigen_factor"
+    parameters:
+      iterations_number: 5000
+      robust_type: HUBER
+  output: "output/kitti"
 debug: true

examples/pipeline.py

@@ -19,6 +19,8 @@
 python3 examples/pipeline.py --configuration_path examples/configurations/hilti.yaml
 ```
 """
+from typing import Tuple
+
 import open3d as o3d
 from octreelib.grid import VisualizationConfig
 
@@ -42,6 +44,23 @@
     OptimisedPoseReadWriter,
 )
 
+
+def prepare_output_directories(configuration: YAMLConfigurationReader) -> Tuple[str, str]:
+    if not os.path.exists(configuration.output_directory):
+        os.makedirs(configuration.output_directory)
+
+    poses_dir = os.path.join(configuration.output_directory, "poses")
+    if not os.path.exists(poses_dir):
+        os.makedirs(poses_dir)
+
+    visualization_dir = os.path.join(configuration.output_directory, "visualization")
+    if configuration.debug:
+        if not os.path.join(visualization_dir):
+            os.makedirs(visualization_dir)
+
+    return poses_dir, visualization_dir
+
+
 if __name__ == "__main__":
     parser = argparse.ArgumentParser(prog="Pipeline")
     parser.add_argument("--configuration_path", type=str, required=True)
@@ -50,16 +69,18 @@
     configuration_reader = YAMLConfigurationReader(args.configuration_path)
 
     dataset_reader = DatasetReader
-    if "hilti" in configuration_reader.dataset_path.lower():
+    if "hilti" in configuration_reader.dataset_type.lower():
         dataset_reader = HiltiReader()
-    elif "kitti" in configuration_reader.dataset_path.lower():
+    elif "kitti" in configuration_reader.dataset_type.lower():
         dataset_reader = KittiReader()
-    elif "nuscenes" in configuration_reader.dataset_path.lower():
+    elif "nuscenes" in configuration_reader.dataset_type.lower():
         dataset_reader = NuscenesReader()
     else:
         raise ValueError("Unrecognisable type of dataset")
     posesWriter = OptimisedPoseReadWriter()
 
+    poses_dir, visualization_dir = prepare_output_directories(configuration_reader)
+
     for ind in range(
         configuration_reader.patches_start,
         configuration_reader.patches_end,
@@ -97,13 +118,14 @@
                 segmenters=configuration_reader.segmenters,
                 filters=configuration_reader.filters,
                 backend=configuration_reader.backend(start, end),
+                debug=configuration_reader.debug,
             )
 
             output = pipeline.run(
                 SequentialPipelineRuntimeParameters(
                     grid_configuration=configuration_reader.grid_configuration,
                     visualization_config=VisualizationConfig(
-                        filepath=f"{configuration_reader.visualization_dir}/{start}-{end - 1}_{iteration_ind}.html"
+                        filepath=f"{visualization_dir}/{start}-{end - 1}_{iteration_ind}.html"
                     ),
                     initial_point_cloud_number=(end - start) // 2,
                 )
@@ -141,7 +163,7 @@
         for optimised_pose_number in range(start, end):
             posesWriter.write(
                 os.path.join(
-                    configuration_reader.optimisation_dir,
+                    poses_dir,
                     f"{optimised_pose_number}.txt",
                 ),
                 poses[optimised_pose_number - start],

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]:
@@ -107,6 +96,7 @@ def __get_unused_features(self) -> List[int]:
         try:
             robust_mask = self._graph.get_eigen_factors_robust_mask()
         except AttributeError:
+            print("[WARNING] Most likely you are not using robust optimisations")
             return []
 
         unused_features = []

slam/pipeline/configuration/reader.py

@@ -62,6 +62,24 @@ def debug(self) -> bool:
 
         return bool(value)
 
+    @property
+    def dataset_type(self) -> str:
+        """
+        Represents dataset type for pose and clouds reading
+
+        Returns
+        -------
+        dataset_type: str
+            Dataset type
+        """
+        try:
+            dataset_configuration = copy.deepcopy(self._configuration["dataset"])
+            value = dataset_configuration["type"]
+        except KeyError as e:
+            raise ValueError(f"{e} must be set")
+
+        return value
+
     @property
     def dataset_path(self) -> str:
         """
@@ -157,38 +175,20 @@ def patches_iterations(self) -> int:
         return int(value)
 
     @property
-    def visualization_dir(self) -> str:
+    def output_directory(self) -> str:
         """
-        Represents visualization directory parameter of pipeline
+        Represents output directory which contains all products of voxel-based pipeline
 
         Returns
         -------
-        visualization_dir: str
-            Path to visualisation directory to save
+        output_directory: str
+            Path to output directory
         """
         try:
-            output_configuration = copy.deepcopy(self._configuration["output"])
-            value = output_configuration["visualization_path"]
+            pipeline_configuration = copy.deepcopy(self._configuration["pipeline"])
+            value = pipeline_configuration["output"]
         except KeyError:
-            return "output/visualization"
-
-        return value
-
-    @property
-    def optimisation_dir(self) -> str:
-        """
-        Represents optimisation directory parameter of pipeline
-
-        Returns
-        -------
-        optimisation_dir: str
-            Path to optimisation poses directory to save
-        """
-        try:
-            output_configuration = copy.deepcopy(self._configuration["output"])
-            value = output_configuration["optimisation_path"]
-        except KeyError:
-            return "output/optimisation"
+            return "output"
 
         return value
 
@@ -203,9 +203,8 @@ def subdividers(self) -> List[Subdivider]:
             Subdividers list
         """
         try:
-            subdividers_configuration = copy.deepcopy(
-                self._configuration["subdividers"]
-            )
+            pipeline_configuration = copy.deepcopy(self._configuration["pipeline"])
+            subdividers_configuration = pipeline_configuration["subdividers"]
         except KeyError:
             raise ValueError("subdividers must be not empty")
 
@@ -218,9 +217,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
 
@@ -247,7 +244,8 @@ def segmenters(self) -> List[Segmenter]:
             Segmenters list
         """
         try:
-            segmenters_configuration = copy.deepcopy(self._configuration["segmenters"])
+            pipeline_configuration = copy.deepcopy(self._configuration["pipeline"])
+            segmenters_configuration = pipeline_configuration["segmenters"]
         except KeyError:
             raise ValueError("segmenters must be not empty")
 
@@ -277,7 +275,8 @@ def grid_configuration(self) -> GridConfig:
             Grid configuration
         """
         try:
-            grid_configuration = copy.deepcopy(self._configuration["grid"])
+            pipeline_configuration = copy.deepcopy(self._configuration["pipeline"])
+            grid_configuration = pipeline_configuration["grid"]
         except KeyError:
             raise ValueError("grid_configuration must be not empty")
 
@@ -300,14 +299,12 @@ def backend(self, start: int, end: int) -> Backend:
             Backend of pipeline
         """
         try:
-            backend_configuration = copy.deepcopy(self._configuration["backend"])
+            pipeline_configuration = copy.deepcopy(self._configuration["pipeline"])
+            backend_configuration = pipeline_configuration["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,

slam/pipeline/pipeline.py

@@ -51,6 +51,8 @@ class Pipeline(ABC):
         Filter conditions to filter voxels in grid
     backend: Backend
         Backend of SLAM algorithm that optimises given poses
+    debug: bool
+        Represents debug parameter. If it is specified, the pipeline will save the visualization files.
     """
 
     def __init__(
@@ -61,6 +63,7 @@ def __init__(
         segmenters: List[Segmenter],
         filters: List[Filter],
         backend: Backend,
+        debug: bool,
     ) -> None:
         if len(point_clouds) != len(poses):
             raise ValueError("Sizes of point_cloud and poses arrays must be equal")
@@ -71,6 +74,7 @@ def __init__(
         self._segmenters: List[Segmenter] = segmenters
         self._filters: List[Filter] = filters
         self._backend: Backend = backend
+        self._debug: bool = debug
 
     @abstractmethod
     def run(self, parameters: PipelineRuntimeParameters) -> BackendOutput:

slam/pipeline/sequential_pipeline.py

@@ -66,7 +66,10 @@ 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)
+        if self._debug:
+            parameters.visualization_config.unused_voxels = (
+                backend_output.unused_features
+            )
+            grid.visualize(parameters.visualization_config)
 
         return backend_output