diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml index 3a4ac69c..ed3d0100 100644 --- a/.github/workflows/python.yml +++ b/.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/ diff --git a/Makefile b/Makefile index 7ff8ab0d..be57d6e5 100644 --- a/Makefile +++ b/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) diff --git a/README.md b/README.md index f0d95a05..16af3cd1 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,7 @@
Demo   •   - Install + Install   •   ROS 1   •   diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index a34094c1..1aa5a6f2 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -33,7 +33,9 @@ #include namespace Eigen { +using Matrix6d = Eigen::Matrix; using Matrix3_6d = Eigen::Matrix; +using Vector6d = Eigen::Matrix; } // 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 BuildLinearSystem( const std::vector &source, const std::vector &target, @@ -109,6 +107,9 @@ std::tuple BuildLinearSystem( return std::make_tuple(jacobian.JTJ, jacobian.JTr); } +} // namespace + +namespace kiss_icp { Sophus::SE3d RegisterFrame(const std::vector &frame, const VoxelHashMap &voxel_map, diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp index 05d1dc99..eb82d37b 100644 --- a/cpp/kiss_icp/core/Registration.hpp +++ b/cpp/kiss_icp/core/Registration.hpp @@ -28,18 +28,8 @@ #include "VoxelHashMap.hpp" -namespace Eigen { -using Matrix6d = Eigen::Matrix; -using Vector6d = Eigen::Matrix; -} // namespace Eigen - namespace kiss_icp { -std::tuple BuildLinearSystem( - const std::vector &source, - const std::vector &target, - double kernel); - Sophus::SE3d RegisterFrame(const std::vector &frame, const VoxelHashMap &voxel_map, const Sophus::SE3d &initial_guess, diff --git a/python/kiss_icp/config/parser.py b/python/kiss_icp/config/parser.py index e58c704d..7be3a17a 100644 --- a/python/kiss_icp/config/parser.py +++ b/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: diff --git a/python/kiss_icp/tools/visualizer.py b/python/kiss_icp/tools/visualizer.py index 7048ab9c..ad0900d0 100644 --- a/python/kiss_icp/tools/visualizer.py +++ b/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() diff --git a/python/pyproject.toml b/python/pyproject.toml index ac6576fa..a97062e4 100644 --- a/python/pyproject.toml +++ b/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'] diff --git a/python/tests/test_kiss_icp.py b/python/tests/test_kiss_icp.py new file mode 100644 index 00000000..1d55f470 --- /dev/null +++ b/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 diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp index dfd8b1d3..0ddedb95 100644 --- a/ros/ros2/OdometryServer.cpp +++ b/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("/kiss/odometry", qos); traj_publisher_ = create_publisher("/kiss/trajectory", qos); path_msg_.header.frame_id = odom_frame_; @@ -214,3 +214,6 @@ void OdometryServer::PublishClouds(const std::vector frame, } } } // namespace kiss_icp_ros + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(kiss_icp_ros::OdometryServer) diff --git a/ros/ros2/OdometryServer.hpp b/ros/ros2/OdometryServer.hpp index 13726d3a..418e0d2c 100644 --- a/ros/ros2/OdometryServer.hpp +++ b/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) diff --git a/ros/rviz/kiss_icp_ros2.rviz b/ros/rviz/kiss_icp_ros2.rviz index e2949cc4..918ff216 100644 --- a/ros/rviz/kiss_icp_ros2.rviz +++ b/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