diff --git a/mad_icp/apps/cpp_runners/bin_runner.cpp b/mad_icp/apps/cpp_runners/bin_runner.cpp index 831164f..e2108dd 100644 --- a/mad_icp/apps/cpp_runners/bin_runner.cpp +++ b/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; diff --git a/mad_icp/apps/mad_icp.py b/mad_icp/apps/mad_icp.py index d4e46ed..88b6c8d 100755 --- a/mad_icp/apps/mad_icp.py +++ b/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,21 +98,41 @@ 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"] @@ -123,6 +140,7 @@ def main(data_path: Annotated[ 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..."): diff --git a/mad_icp/apps/utils/kitti_reader.py b/mad_icp/apps/utils/kitti_reader.py index da6cabc..fbb61ef 100644 --- a/mad_icp/apps/utils/kitti_reader.py +++ b/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 \ No newline at end of file diff --git a/mad_icp/configurations/datasets/dataset_configurations.py b/mad_icp/configurations/datasets/dataset_configurations.py new file mode 100644 index 0000000..2d3681f --- /dev/null +++ b/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 +} \ No newline at end of file diff --git a/mad_icp/configurations/datasets/hilti_2021.cfg b/mad_icp/configurations/datasets/hilti_2021.cfg index 1150809..32baabd 100755 --- a/mad_icp/configurations/datasets/hilti_2021.cfg +++ b/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 diff --git a/mad_icp/configurations/datasets/kitti.cfg b/mad_icp/configurations/datasets/kitti.cfg index 916a27c..1206567 100755 --- a/mad_icp/configurations/datasets/kitti.cfg +++ b/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] diff --git a/mad_icp/configurations/datasets/newer_college_os0.cfg b/mad_icp/configurations/datasets/newer_college_os0.cfg index 9287061..2acc4a3 100755 --- a/mad_icp/configurations/datasets/newer_college_os0.cfg +++ b/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 diff --git a/mad_icp/configurations/datasets/newer_college_os1.cfg b/mad_icp/configurations/datasets/newer_college_os1.cfg index 3dd28c4..ad9134c 100755 --- a/mad_icp/configurations/datasets/newer_college_os1.cfg +++ b/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 diff --git a/mad_icp/configurations/params.cfg b/mad_icp/configurations/default.cfg similarity index 81% rename from mad_icp/configurations/params.cfg rename to mad_icp/configurations/default.cfg index 23667eb..0dd605d 100755 --- a/mad_icp/configurations/params.cfg +++ b/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 \ No newline at end of file +n : 10 # the number of last poses to smooth velocity diff --git a/mad_icp/configurations/mad_params.py b/mad_icp/configurations/mad_params.py new file mode 100644 index 0000000..da4c712 --- /dev/null +++ b/mad_icp/configurations/mad_params.py @@ -0,0 +1,47 @@ +# 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 + +default_conf = { + "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 +} + +class MADConfiguration(str, Enum): + default = "default" + # Can insert additional MAD configurations + + +MADConfiguration_lut = { + MADConfiguration.default: default_conf +} \ No newline at end of file