reading conf from python

mad_icp/apps/cpp_runners/bin_runner.cpp

@@ -44,7 +44,7 @@
 std::string data_path;
 std::string estimate_path;
 std::string dataset_config;
-std::string mad_icp_config = "../../../configurations/params.cfg";
+std::string mad_icp_config = "../../../configurations/default.cfg";
 int num_cores              = 4;
 int num_keyframes          = 4;
 bool realtime              = false;

mad_icp/apps/mad_icp.py

@@ -41,6 +41,8 @@
 from mad_icp.apps.utils.ros_reader import Ros1Reader
 from mad_icp.apps.utils.kitti_reader import KittiReader
 from mad_icp.apps.utils.visualizer import Visualizer
+from mad_icp.configurations.datasets.dataset_configurations import DatasetConfiguration_lut
+from mad_icp.configurations.mad_params import MADConfiguration_lut
 # binded Odometry
 from mad_icp.src.pybind.pypeline import Pipeline, VectorEigen3d
 
@@ -50,7 +52,7 @@
 
 class InputDataInterface(str, Enum):
     kitti = "kitti",
-    ros1 = "ros1",
+    ros1 = "ros1"
     # Can insert additional conversion formats
 
 
@@ -65,9 +67,9 @@ def main(data_path: Annotated[
         estimate_path: Annotated[
         Path, typer.Option(help="trajectory estimate output path (folder path)", show_default=False)],
         dataset_config: Annotated[
-        Path, typer.Option(help="dataset configuration file", show_default=False)],
-        mad_icp_config: Annotated[
-        Path, typer.Option(help="parameters for mad icp", show_default=True)] = "../configurations/params.cfg",
+        Path, typer.Option(help="dataset configuration name or path to config file", show_default=False)],
+        mad_icp_params: Annotated[
+        Path, typer.Option(help="parameters for mad icp", show_default=True)] = "default",
         num_cores: Annotated[
         int, typer.Option(help="how many threads to use for icp (suggest maximum num)", show_default=True)] = 4,
         num_keyframes: Annotated[
@@ -85,11 +87,6 @@ def main(data_path: Annotated[
             f"[yellow] Output directory {estimate_path} does not exist. Creating new directory")
         estimate_path.mkdir(parents=True, exist_ok=True)
 
-    if not dataset_config.is_file():
-        console.print(
-            f"[red] Dataset config file {dataset_config} does not exist!")
-        sys.exit(-1)
-
     visualizer = None
     if not noviz:
         visualizer = Visualizer()
@@ -101,28 +98,49 @@ def main(data_path: Annotated[
     else:
         console.print("[yellow] The dataset is in kitti format")
 
-    console.print("[green] Parsing dataset configuration file")
-    data_config_file = open(dataset_config, 'r')
-    data_cf = yaml.safe_load(data_config_file)
+
+    console.print("[green] Parsing dataset configuration")
+    if dataset_config.is_file():
+        data_config_file = open(dataset_config, 'r')
+        data_cf = yaml.safe_load(data_config_file)
+    else:
+        dataset_config_str = str(dataset_config)
+        if dataset_config_str in DatasetConfiguration_lut:
+            data_cf = DatasetConfiguration_lut[dataset_config_str]
+        else:
+            console.print(f"[red] Error: Dataset '{dataset_config_str}' not found in the configuration lookup table. Possible configurations: {', '.join(DatasetConfiguration_lut.keys())}")
+            sys.exit(-1)
     min_range = data_cf["min_range"]
     max_range = data_cf["max_range"]
     sensor_hz = data_cf["sensor_hz"]
     deskew = data_cf["deskew"]
+    # apply_correction = data_cf["apply_correction"]
+    apply_correction = data_cf.get("apply_correction", False)
     topic = None
     if reader_type == InputDataInterface.ros1:
         topic = data_cf["rosbag_topic"]
     lidar_to_base = np.array(data_cf["lidar_to_base"])
 
-    console.print("[green] Parsing mad-icp configuration file")
-    mad_icp_config_file = open(mad_icp_config, 'r')
-    mad_icp_cf = yaml.safe_load(mad_icp_config_file)
+
+    console.print("[green] Parsing mad-icp parameters")
+    if mad_icp_params.is_file():
+        mad_icp_params_file = open(mad_icp_params, 'r')
+        mad_icp_cf = yaml.safe_load(mad_icp_params_file)
+    else:
+        mad_icp_params_str = str(mad_icp_params)
+        if mad_icp_params_str in MADConfiguration_lut:
+            mad_icp_cf = MADConfiguration_lut[mad_icp_params_str]
+        else:
+            console.print(f"[red] Error: mad-icp '{mad_icp_params_str}' not found in the configuration lookup table. Possible configurations: {', '.join(MADConfiguration_lut.keys())}")
+            sys.exit(-1)
     b_max = mad_icp_cf["b_max"]
     b_min = mad_icp_cf["b_min"]
     b_ratio = mad_icp_cf["b_ratio"]
     p_th = mad_icp_cf["p_th"]
     rho_ker = mad_icp_cf["rho_ker"]
     n = mad_icp_cf["n"]
 
+
     # check some params for machine
     if (realtime and num_keyframes > num_cores):
         console.print(
@@ -136,7 +154,7 @@ def main(data_path: Annotated[
     estimate_file_name = estimate_path / "estimate.txt"
     estimate_file = open(estimate_file_name, 'w')
 
-    with InputDataInterface_lut[reader_type](data_path, min_range, max_range, topic=topic, sensor_hz=sensor_hz) as reader:
+    with InputDataInterface_lut[reader_type](data_path, min_range, max_range, topic=topic, sensor_hz=sensor_hz, apply_correction=apply_correction) as reader:
         t_start = datetime.now()
         for ts, points in track(reader, description="processing..."):
 

mad_icp/apps/utils/kitti_reader.py

@@ -31,10 +31,11 @@
 import natsort
 import pickle
 import numpy as np
+from scipy.spatial.transform import Rotation as R
 
 class KittiReader:
     def __init__(self, data_dir: Path, min_range = 0, 
-                 max_range = 200, *args, **kwargs):
+                 max_range = 200, apply_correction = False, *args, **kwargs):
         """
         :param data_dir: Directory containing rosbags or path to a rosbag file
         :param topics: None for kitti
@@ -48,11 +49,13 @@ def __init__(self, data_dir: Path, min_range = 0,
 
         self.min_range = min_range
         self.max_range = max_range
+        self.apply_correction = apply_correction
         self.time = 0.
         self.time_inc = 1./sensor_hz
         self.file_index = 0
         self.data_dir = data_dir
         self.cdtype = np.float32
+        self.vertical_angle_offset = np.radians(0.205)
         if (self.data_dir / ".dtype.pkl").exists():
             f = open(self.data_dir / ".dtype.pkl", "rb")
             self.cdtype = pickle.load(f)
@@ -66,18 +69,27 @@ def __enter__(self):
     def __exit__(self, exc_type, exc_val, exc_tb):
         return
 
+    # apply kitti magic correction (not documented)
+    def apply_rotation_correction(self, points: np.ndarray) -> np.ndarray:
+        rotation_vectors = np.cross(points, np.array([0., 0., 1.]))
+        norms = np.linalg.norm(rotation_vectors, axis=1).reshape(-1, 1)
+        rotation_vectors_normalized = rotation_vectors / norms
+        rotations = R.from_rotvec(self.vertical_angle_offset * rotation_vectors_normalized)
+        corrected_points = rotations.apply(points)
+        return corrected_points
+
     def __getitem__(self, item) -> Tuple[float, Tuple[np.ndarray, np.ndarray]]:
         cloud_np = np.fromfile(self.file_names[self.file_index], dtype=self.cdtype)
-        # with open(self.file_names[self.file_index], 'rb') as f:
-        #     buffer = f.read()
-        #     cloud_np = np.frombuffer(buffer, dtype=self.cdtype)
         cloud_np = cloud_np.reshape(-1, 4)[:, :3]
-        # if (point.norm() < min_range || point.norm() > max_range || std::isnan(point.x()) || std::isnan(point.y()) ||
-        #   std::isnan(point.z()))
+
         norms = np.linalg.norm(cloud_np, axis=1)
         mask = (norms >= self.min_range) & (norms <= self.max_range)
         # Use the mask to filter the points
         filtered_points = cloud_np[mask]
+
+        if self.apply_correction:
+            filtered_points = self.apply_rotation_correction(filtered_points)
+
         self.time += self.time_inc
         self.file_index += 1
         return self.time, filtered_points

mad_icp/configurations/datasets/dataset_configurations.py

@@ -0,0 +1,147 @@
+# Copyright 2024 R(obots) V(ision) and P(erception) group
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+#    this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+#    this list of conditions and the following disclaimer in the documentation
+#    and/or other materials provided with the distribution.
+#
+# 3. Neither the name of the copyright holder nor the names of its contributors
+#    may be used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from enum import Enum
+
+hilti_2021_conf = {
+    "min_range": 0.7,
+    "max_range": 100,
+    "sensor_hz": 10,
+    "deskew": False,
+    "rosbag_topic": "/os_cloud_node/points",
+    "lidar_to_base": [
+        [1, 0.0025, -0.0065, 0.0100],
+        [0.0025, -1, 0.0003, -0.0066],
+        [-0.0065, -0.0003, -1, 0.0947],
+        [0, 0, 0, 1]
+    ]
+}
+
+kitti_conf = {
+    "min_range": 0.7,
+    "max_range": 120,
+    "sensor_hz": 10,
+    "deskew": False,
+    "apply_correction": True,
+    "lidar_to_base": [
+        [-0.7071, -0.7071, 0, -0.0843],
+        [0.7071, -0.7071, 0, -0.0250],
+        [0, 0, 1, 0.0502],
+        [0, 0, 0, 1]
+    ]
+}
+
+mulran_conf = {
+    "min_range": 0.7,
+    "max_range": 120,
+    "sensor_hz": 10,
+    "deskew": True,
+    "lidar_to_base": [
+        [-1, -0.0058, 0, 1.7042],
+        [0.0058, -1, 0, -0.0210],
+        [0, 0, 1, 1.8047],
+        [0, 0, 0, 1]
+    ]
+}
+
+newer_college_os0_conf = {
+    "min_range": 0.7,
+    "max_range": 50,
+    "sensor_hz": 10,
+    "deskew": False,
+    "rosbag_topic": "/os_cloud_node/points",
+    "lidar_to_base": [
+        [1, 0, 0, 0.001],
+        [0, 1, 0, 0],
+        [0, 0, 1, 0.090683],
+        [0, 0, 0, 1]
+    ]
+}
+
+newer_college_os1_conf = {
+    "min_range": 0.7,
+    "max_range": 120,
+    "sensor_hz": 10,
+    "deskew": False,
+    "rosbag_topic": "/os1_cloud_node/points",
+    "lidar_to_base": [
+        [-0.7071, -0.7071, 0, -0.0843],
+        [0.7071, -0.7071, 0, -0.0250],
+        [0, 0, 1, 0.0502],
+        [0, 0, 0, 1]
+    ]
+}
+
+vbr_os0_conf = {
+    "min_range": 0,
+    "max_range": 50,
+    "sensor_hz": 10,
+    "deskew": False,
+    "rosbag_topic": "/ouster/points",
+    "lidar_to_base": [
+        [1, 0, 0, 0],
+        [0, 1, 0, 0],
+        [0, 0, 1, 0],
+        [0, 0, 0, 1]
+    ]
+}
+
+vbr_os1_conf = {
+    "min_range": 1.3,
+    "max_range": 120,
+    "sensor_hz": 20,
+    "deskew": True,
+    "rosbag_topic": "/ouster/points",
+    "lidar_to_base": [
+        [1, 0, 0, 0],
+        [0, 1, 0, 0],
+        [0, 0, 1, 0],
+        [0, 0, 0, 1]
+    ]
+}
+
+class DatasetConfiguration(str, Enum):
+    hilti_2021 = "hilti_2021",
+    kitti = "kitti",
+    mulran = "mulran",
+    newer_college_os0 = "newer_college_os0",
+    newer_college_os1 = "newer_college_os1",
+    vbr_os0 = "vbr_os0",
+    vbr_os1 = "vbr_os1"
+    # Can insert additional dataset configurations
+
+
+DatasetConfiguration_lut = {
+    DatasetConfiguration.hilti_2021: hilti_2021_conf,
+    DatasetConfiguration.kitti: kitti_conf,
+    DatasetConfiguration.mulran: mulran_conf,
+    DatasetConfiguration.newer_college_os0: newer_college_os0_conf,
+    DatasetConfiguration.newer_college_os1: newer_college_os1_conf,
+    DatasetConfiguration.vbr_os0: vbr_os0_conf,
+    DatasetConfiguration.vbr_os1: vbr_os1_conf
+}

mad_icp/configurations/datasets/hilti_2021.cfg

@@ -1,4 +1,4 @@
-# newer college os0-64 params 
+# Hilti 2021 os0-64 params 
 min_range : 0.7
 max_range : 100
 sensor_hz : 10

mad_icp/configurations/datasets/kitti.cfg

@@ -3,6 +3,7 @@ min_range : 0.7
 max_range : 120
 sensor_hz : 10
 deskew : False
+apply_correction: True
 lidar_to_base: 
   - [-0.7071, -0.7071, 0, -0.0843]
   - [0.7071, -0.7071, 0, -0.0250]

mad_icp/configurations/datasets/newer_college_os0.cfg

@@ -1,4 +1,4 @@
-# newer college os0-128 params 
+# Newer College os0-128 params 
 min_range : 0.7
 max_range : 50
 sensor_hz : 10

mad_icp/configurations/datasets/newer_college_os1.cfg

@@ -1,4 +1,4 @@
-# newer college os1-64 params 
+# Newer College os1-64 params 
 min_range : 0.7
 max_range : 120
 sensor_hz : 10

mad_icp/configurations/params.cfg → mad_icp/configurations/default.cfg

@@ -1,7 +1,7 @@
-# mad-icp params
+# mad-icp default params
 b_max : 0.2 # [m] max size of kd leaves
 b_min : 0.1 # [m] when a node is flatten than this param, propagate normal
 b_ratio : 0.02 # the increase factor of search radius needed in data association 
 p_th : 0.8 # [%] ensuring an update when the curr point cloud is registered less than this param
 rho_ker : 0.1 # huber threshold in mad-icp
-n : 10 # the number of last poses to smooth velocity
+n : 10 # the number of last poses to smooth velocity