Merge remote-tracking branch 'origin/main' into feature/set-num-threads

.github/workflows/python.yml

@@ -25,3 +25,7 @@ jobs:
       - name: Test installation
         run: |
           kiss_icp_pipeline --version
+      - name: Run unittests
+        run: |
+          python -m pip install --verbose './python[test]'
+          pytest -rA --verbose ./python/

Makefile

@@ -10,6 +10,9 @@ editable:
 	@pip install scikit-build-core pyproject_metadata pathspec pybind11 ninja cmake
 	@pip install --no-build-isolation -ve ./python/
 
+test:
+	@pytest -rA --verbose ./python/
+
 cpp:
 	@cmake -Bbuild cpp/kiss_icp/
 	@cmake --build build -j$(nproc --all)

README.md

@@ -9,7 +9,7 @@
     <br />
     <a href=https://user-images.githubusercontent.com/21349875/219626075-d67e9165-31a2-4a1b-8c26-9f04e7d195ec.mp4>Demo</a>
     <span>&nbsp;&nbsp;•&nbsp;&nbsp;</span>
-    <a href="https://github.com/PRBonn/kiss-icp/edit/main/README.md#install">Install</a>
+    <a href="https://github.com/PRBonn/kiss-icp/blob/main/README.md#Install">Install</a>
     <span>&nbsp;&nbsp;•&nbsp;&nbsp;</span>
     <a href="https://github.com/PRBonn/kiss-icp/blob/main/ros">ROS 1</a>
     <span>&nbsp;&nbsp;•&nbsp;&nbsp;</span>

cpp/kiss_icp/core/Registration.cpp

@@ -33,7 +33,9 @@
 #include <tuple>
 
 namespace Eigen {
+using Matrix6d = Eigen::Matrix<double, 6, 6>;
 using Matrix3_6d = Eigen::Matrix<double, 3, 6>;
+using Vector6d = Eigen::Matrix<double, 6, 1>;
 }  // namespace Eigen
 
 namespace {
@@ -65,10 +67,6 @@ constexpr int MAX_NUM_ITERATIONS_ = 500;
 constexpr double ESTIMATION_THRESHOLD_ = 0.0001;
 constexpr int NUM_THREADS_ = 16;
 
-}  // namespace
-
-namespace kiss_icp {
-
 std::tuple<Eigen::Matrix6d, Eigen::Vector6d> BuildLinearSystem(
     const std::vector<Eigen::Vector3d> &source,
     const std::vector<Eigen::Vector3d> &target,
@@ -109,6 +107,9 @@ std::tuple<Eigen::Matrix6d, Eigen::Vector6d> BuildLinearSystem(
 
     return std::make_tuple(jacobian.JTJ, jacobian.JTr);
 }
+}  // namespace
+
+namespace kiss_icp {
 
 Sophus::SE3d RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
                            const VoxelHashMap &voxel_map,

cpp/kiss_icp/core/Registration.hpp

@@ -28,18 +28,8 @@
 
 #include "VoxelHashMap.hpp"
 
-namespace Eigen {
-using Matrix6d = Eigen::Matrix<double, 6, 6>;
-using Vector6d = Eigen::Matrix<double, 6, 1>;
-}  // namespace Eigen
-
 namespace kiss_icp {
 
-std::tuple<Eigen::Matrix6d, Eigen::Vector6d> BuildLinearSystem(
-    const std::vector<Eigen::Vector3d> &source,
-    const std::vector<Eigen::Vector3d> &target,
-    double kernel);
-
 Sophus::SE3d RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
                            const VoxelHashMap &voxel_map,
                            const Sophus::SE3d &initial_guess,

python/kiss_icp/config/parser.py

@@ -28,7 +28,7 @@
 from pathlib import Path
 from typing import Any, Dict, Optional
 
-from pydantic import BaseSettings, PrivateAttr
+from pydantic_settings import BaseSettings
 
 from kiss_icp.config.config import AdaptiveThresholdConfig, DataConfig, MappingConfig
 
@@ -38,32 +38,23 @@ class KISSConfig(BaseSettings):
     data: DataConfig = DataConfig()
     mapping: MappingConfig = MappingConfig()
     adaptive_threshold: AdaptiveThresholdConfig = AdaptiveThresholdConfig()
-    _config_file: Optional[Path] = PrivateAttr()
-
-    def __init__(self, config_file: Optional[Path] = None, *args, **kwargs):
-        self._config_file = config_file
-        super().__init__(*args, **kwargs)
-
-    def _yaml_source(self) -> Dict[str, Any]:
-        data = None
-        if self._config_file is not None:
-            try:
-                yaml = importlib.import_module("yaml")
-            except ModuleNotFoundError:
-                print(
-                    "Custom configuration file specified but PyYAML is not installed on your system,"
-                    ' run `pip install "kiss-icp[all]"`. You can also modify the config.py if your '
-                    "system does not support PyYaml "
-                )
-                sys.exit(1)
-            with open(self._config_file) as cfg_file:
-                data = yaml.safe_load(cfg_file)
-        return data or {}
-
-    class Config:
-        @classmethod
-        def customise_sources(cls, init_settings, env_settings, file_secret_settings):
-            return init_settings, KISSConfig._yaml_source
+
+
+def _yaml_source(config_file: Optional[Path]) -> Dict[str, Any]:
+    data = None
+    if config_file is not None:
+        try:
+            yaml = importlib.import_module("yaml")
+        except ModuleNotFoundError:
+            print(
+                "Custom configuration file specified but PyYAML is not installed on your system,"
+                ' run `pip install "kiss-icp[all]"`. You can also modify the config.py if your '
+                "system does not support PyYaml "
+            )
+            sys.exit(1)
+        with open(config_file) as cfg_file:
+            data = yaml.safe_load(cfg_file)
+    return data or {}
 
 
 def load_config(
@@ -72,7 +63,7 @@ def load_config(
     """Load configuration from an Optional yaml file. Additionally, deskew and max_range can be
     also specified from the CLI interface"""
 
-    config = KISSConfig(config_file=config_file)
+    config = KISSConfig(**_yaml_source(config_file))
 
     # Override defaults from command line
     if deskew is not None:

python/kiss_icp/tools/visualizer.py

@@ -33,6 +33,7 @@
 RED = np.array([128, 0, 0]) / 255.0
 BLACK = np.array([0, 0, 0]) / 255.0
 BLUE = np.array([0.4, 0.5, 0.9])
+GRAY = np.array([0.4, 0.4, 0.4])
 SPHERE_SIZE = 0.20
 
 
@@ -216,7 +217,9 @@ def _update_geometries(self, source, keypoints, target, pose):
         if self.render_map:
             target = copy.deepcopy(target)
             self.target.points = self.o3d.utility.Vector3dVector(target)
-            if not self.global_view:
+            if self.global_view:
+                self.target.paint_uniform_color(GRAY)
+            else:
                 self.target.transform(np.linalg.inv(pose))
         else:
             self.target.points = self.o3d.utility.Vector3dVector()

python/pyproject.toml

@@ -38,7 +38,8 @@ dependencies = [
     "natsort",
     "numpy",
     "plyfile",
-    "pydantic <2",
+    "pydantic>=2",
+    "pydantic-settings",
     "pyquaternion",
     "rich",
     "tqdm",
@@ -53,6 +54,9 @@ all = [
     "PyYAML",
     "trimesh",
 ]
+test = [
+    "pytest",
+]
 visualizer = [
     "open3d>=0.13",
 ]
@@ -89,3 +93,6 @@ skip = ["*-musllinux*",  "pp*", "cp36-*"]
 [tool.cibuildwheel.macos]
 environment = "MACOSX_DEPLOYMENT_TARGET=10.14"
 archs = ["auto64", "arm64"]
+
+[tool.pytest.ini_options]
+testpaths = ['tests']

python/tests/test_kiss_icp.py

@@ -0,0 +1,4 @@
+def test_import():
+    from kiss_icp.kiss_icp import KissICP
+
+    assert KissICP is not None

ros/ros2/OdometryServer.cpp

@@ -81,7 +81,7 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
         std::bind(&OdometryServer::RegisterFrame, this, std::placeholders::_1));
 
     // Initialize publishers
-    rclcpp::QoS qos((rclcpp::SystemDefaultsQoS()));
+    rclcpp::QoS qos((rclcpp::SystemDefaultsQoS().keep_last(1).durability_volatile()));
     odom_publisher_ = create_publisher<nav_msgs::msg::Odometry>("/kiss/odometry", qos);
     traj_publisher_ = create_publisher<nav_msgs::msg::Path>("/kiss/trajectory", qos);
     path_msg_.header.frame_id = odom_frame_;
@@ -214,3 +214,6 @@ void OdometryServer::PublishClouds(const std::vector<Eigen::Vector3d> frame,
     }
 }
 }  // namespace kiss_icp_ros
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(kiss_icp_ros::OdometryServer)

ros/ros2/OdometryServer.hpp

@@ -94,10 +94,3 @@ class OdometryServer : public rclcpp::Node {
 };
 
 }  // namespace kiss_icp_ros
-
-#include "rclcpp_components/register_node_macro.hpp"
-
-// Register the component with class_loader.
-// This acts as a sort of entry point, allowing the component to be
-// discoverable when its library is being loaded into a running process.
-RCLCPP_COMPONENTS_REGISTER_NODE(kiss_icp_ros::OdometryServer)

ros/rviz/kiss_icp_ros2.rviz

@@ -58,7 +58,7 @@ Visualization Manager:
             Durability Policy: Volatile
             Filter size: 10
             History Policy: Keep Last
-            Reliability Policy: Reliable
+            Reliability Policy: Best Effort
             Value: /kiss/frame
           Use Fixed Frame: true
           Use rainbow: true
@@ -92,7 +92,7 @@ Visualization Manager:
             Durability Policy: Volatile
             Filter size: 10
             History Policy: Keep Last
-            Reliability Policy: Reliable
+            Reliability Policy: Best Effort
             Value: /kiss/keypoints
           Use Fixed Frame: true
           Use rainbow: true
@@ -126,7 +126,7 @@ Visualization Manager:
             Durability Policy: Volatile
             Filter size: 10
             History Policy: Keep Last
-            Reliability Policy: Reliable
+            Reliability Policy: Best Effort
             Value: /kiss/local_map
           Use Fixed Frame: true
           Use rainbow: true
@@ -171,7 +171,7 @@ Visualization Manager:
             Durability Policy: Volatile
             Filter size: 10
             History Policy: Keep Last
-            Reliability Policy: Reliable
+            Reliability Policy: Best Effort
             Value: /kiss/odometry
           Value: true
         - Alpha: 1
@@ -199,7 +199,7 @@ Visualization Manager:
             Durability Policy: Volatile
             Filter size: 10
             History Policy: Keep Last
-            Reliability Policy: Reliable
+            Reliability Policy: Best Effort
             Value: /kiss/trajectory
           Value: true
       Enabled: true