diff --git a/config/advanced.yaml b/config/advanced.yaml index a9124ae2..a1650c10 100644 --- a/config/advanced.yaml +++ b/config/advanced.yaml @@ -16,3 +16,7 @@ adaptive_threshold: fixed_threshold: 0.3 # <- optional, disables adaptive threshold # initial_threshold: 2.0 # min_motion_th: 0.1 + +registration: + max_num_iterations: 500 # <- optional + convergence_criterion: 0.0001 # <- optional diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 1d22bef0..90a6473e 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -27,89 +27,126 @@ #include #include +#include #include #include #include +#include "VoxelHashMap.hpp" + namespace Eigen { using Matrix6d = Eigen::Matrix; using Matrix3_6d = Eigen::Matrix; using Vector6d = Eigen::Matrix; } // namespace Eigen +using Associations = std::vector>; +using LinearSystem = std::pair; namespace { - inline double square(double x) { return x * x; } -struct ResultTuple { - ResultTuple() { - JTJ.setZero(); - JTr.setZero(); - } - - ResultTuple operator+(const ResultTuple &other) { - this->JTJ += other.JTJ; - this->JTr += other.JTr; - return *this; - } - - Eigen::Matrix6d JTJ; - Eigen::Vector6d JTr; -}; - void TransformPoints(const Sophus::SE3d &T, std::vector &points) { std::transform(points.cbegin(), points.cend(), points.begin(), [&](const auto &point) { return T * point; }); } -constexpr int MAX_NUM_ITERATIONS_ = 500; -constexpr double ESTIMATION_THRESHOLD_ = 0.0001; +Eigen::Vector3d GetClosestNeighbor(const Eigen::Vector3d &point, + const kiss_icp::VoxelHashMap &voxel_map) { + const auto &query_voxels = voxel_map.GetAdjacentVoxels(point); + const auto &neighbors = voxel_map.GetPoints(query_voxels); + Eigen::Vector3d closest_neighbor; + double closest_distance2 = std::numeric_limits::max(); + std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) { + double distance = (neighbor - point).squaredNorm(); + if (distance < closest_distance2) { + closest_neighbor = neighbor; + closest_distance2 = distance; + } + }); + return closest_neighbor; +} + +Associations FindAssociations(const std::vector &points, + const kiss_icp::VoxelHashMap &voxel_map, + double max_correspondance_distance) { + using points_iterator = std::vector::const_iterator; + Associations associations; + associations.reserve(points.size()); + associations = tbb::parallel_reduce( + // Range + tbb::blocked_range{points.cbegin(), points.cend()}, + // Identity + associations, + // 1st lambda: Parallel computation + [&](const tbb::blocked_range &r, Associations res) -> Associations { + res.reserve(r.size()); + for (const auto &point : r) { + Eigen::Vector3d closest_neighbor = GetClosestNeighbor(point, voxel_map); + if ((closest_neighbor - point).norm() < max_correspondance_distance) { + res.emplace_back(point, closest_neighbor); + } + } + return res; + }, + // 2nd lambda: Parallel reduction + [](Associations a, const Associations &b) -> Associations { + a.insert(a.end(), // + std::make_move_iterator(b.cbegin()), // + std::make_move_iterator(b.cend())); + return a; + }); + + return associations; +} -std::tuple BuildLinearSystem( - const std::vector &source, - const std::vector &target, - double kernel) { - auto compute_jacobian_and_residual = [&](auto i) { - const Eigen::Vector3d residual = source[i] - target[i]; +LinearSystem BuildLinearSystem(const Associations &associations, double kernel) { + auto compute_jacobian_and_residual = [](auto association) { + const auto &[source, target] = association; + const Eigen::Vector3d residual = source - target; Eigen::Matrix3_6d J_r; J_r.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); - J_r.block<3, 3>(0, 3) = -1.0 * Sophus::SO3d::hat(source[i]); + J_r.block<3, 3>(0, 3) = -1.0 * Sophus::SO3d::hat(source); return std::make_tuple(J_r, residual); }; + auto sum_linear_systems = [](LinearSystem a, const LinearSystem &b) { + a.first += b.first; + a.second += b.second; + return a; + }; + + auto GM_weight = [&](double residual2) { return square(kernel) / square(kernel + residual2); }; + + using associations_iterator = Associations::const_iterator; const auto &[JTJ, JTr] = tbb::parallel_reduce( // Range - tbb::blocked_range{0, source.size()}, + tbb::blocked_range{associations.cbegin(), associations.cend()}, // Identity - ResultTuple(), + LinearSystem(Eigen::Matrix6d::Zero(), Eigen::Vector6d::Zero()), // 1st Lambda: Parallel computation - [&](const tbb::blocked_range &r, ResultTuple J) -> ResultTuple { - auto Weight = [&](double residual2) { - return square(kernel) / square(kernel + residual2); - }; - auto &[JTJ_private, JTr_private] = J; - for (auto i = r.begin(); i < r.end(); ++i) { - const auto &[J_r, residual] = compute_jacobian_and_residual(i); - const double w = Weight(residual.squaredNorm()); - JTJ_private.noalias() += J_r.transpose() * w * J_r; - JTr_private.noalias() += J_r.transpose() * w * residual; - } - return J; + [&](const tbb::blocked_range &r, LinearSystem J) -> LinearSystem { + return std::transform_reduce( + r.begin(), r.end(), J, sum_linear_systems, [&](const auto &association) { + const auto &[J_r, residual] = compute_jacobian_and_residual(association); + const double w = GM_weight(residual.squaredNorm()); + return LinearSystem(J_r.transpose() * w * J_r, // JTJ + J_r.transpose() * w * residual); // JTr + }); }, // 2nd Lambda: Parallel reduction of the private Jacboians - [&](ResultTuple a, const ResultTuple &b) -> ResultTuple { return a + b; }); + sum_linear_systems); - return std::make_tuple(JTJ, JTr); + return {JTJ, JTr}; } } // namespace namespace kiss_icp { -Sophus::SE3d RegisterFrame(const std::vector &frame, - const VoxelHashMap &voxel_map, - const Sophus::SE3d &initial_guess, - double max_correspondence_distance, - double kernel) { +Sophus::SE3d Registration::AlignPointsToMap(const std::vector &frame, + const VoxelHashMap &voxel_map, + const Sophus::SE3d &initial_guess, + double max_correspondence_distance, + double kernel) { if (voxel_map.Empty()) return initial_guess; // Equation (9) @@ -118,11 +155,11 @@ Sophus::SE3d RegisterFrame(const std::vector &frame, // ICP-loop Sophus::SE3d T_icp = Sophus::SE3d(); - for (int j = 0; j < MAX_NUM_ITERATIONS_; ++j) { + for (int j = 0; j < max_num_iterations_; ++j) { // Equation (10) - const auto &[src, tgt] = voxel_map.GetCorrespondences(source, max_correspondence_distance); + const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance); // Equation (11) - const auto &[JTJ, JTr] = BuildLinearSystem(src, tgt, kernel); + const auto &[JTJ, JTr] = BuildLinearSystem(associations, kernel); const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr); const Sophus::SE3d estimation = Sophus::SE3d::exp(dx); // Equation (12) @@ -130,7 +167,7 @@ Sophus::SE3d RegisterFrame(const std::vector &frame, // Update iterations T_icp = estimation * T_icp; // Termination criteria - if (dx.norm() < ESTIMATION_THRESHOLD_) break; + if (dx.norm() < convergence_criterion_) break; } // Spit the final transformation return T_icp * initial_guess; diff --git a/cpp/kiss_icp/core/Registration.hpp b/cpp/kiss_icp/core/Registration.hpp index eb82d37b..25d3abf5 100644 --- a/cpp/kiss_icp/core/Registration.hpp +++ b/cpp/kiss_icp/core/Registration.hpp @@ -30,9 +30,17 @@ namespace kiss_icp { -Sophus::SE3d RegisterFrame(const std::vector &frame, - const VoxelHashMap &voxel_map, - const Sophus::SE3d &initial_guess, - double max_correspondence_distance, - double kernel); +struct Registration { + explicit Registration(int max_num_iteration, double convergence_criterion) + : max_num_iterations_(max_num_iteration), convergence_criterion_(convergence_criterion) {} + + Sophus::SE3d AlignPointsToMap(const std::vector &frame, + const VoxelHashMap &voxel_map, + const Sophus::SE3d &initial_guess, + double max_correspondence_distance, + double kernel); + + int max_num_iterations_; + double convergence_criterion_; +}; } // namespace kiss_icp diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 2212dd67..39a0e703 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -22,9 +22,6 @@ // SOFTWARE. #include "VoxelHashMap.hpp" -#include -#include - #include #include #include @@ -32,97 +29,36 @@ #include #include -// This parameters are not intended to be changed, therefore we do not expose it -namespace { -struct ResultTuple { - ResultTuple(std::size_t n) { - source.reserve(n); - target.reserve(n); - } - std::vector source; - std::vector target; -}; -} // namespace - namespace kiss_icp { -VoxelHashMap::Vector3dVectorTuple VoxelHashMap::GetCorrespondences( - const Vector3dVector &points, double max_correspondance_distance) const { - // Lambda Function to obtain the KNN of one point, maybe refactor - auto GetClosestNeighboor = [&](const Eigen::Vector3d &point) { - auto kx = static_cast(point[0] / voxel_size_); - auto ky = static_cast(point[1] / voxel_size_); - auto kz = static_cast(point[2] / voxel_size_); - std::vector voxels; - voxels.reserve(27); - for (int i = kx - 1; i < kx + 1 + 1; ++i) { - for (int j = ky - 1; j < ky + 1 + 1; ++j) { - for (int k = kz - 1; k < kz + 1 + 1; ++k) { - voxels.emplace_back(i, j, k); - } +std::vector VoxelHashMap::GetAdjacentVoxels(const Eigen::Vector3d &point, + int adjacent_voxels) const { + auto kx = static_cast(point[0] / voxel_size_); + auto ky = static_cast(point[1] / voxel_size_); + auto kz = static_cast(point[2] / voxel_size_); + std::vector voxel_neighborhood; + for (int i = kx - adjacent_voxels; i < kx + adjacent_voxels + 1; ++i) { + for (int j = ky - adjacent_voxels; j < ky + adjacent_voxels + 1; ++j) { + for (int k = kz - adjacent_voxels; k < kz + adjacent_voxels + 1; ++k) { + voxel_neighborhood.emplace_back(i, j, k); } } + } + return voxel_neighborhood; +} - using Vector3dVector = std::vector; - Vector3dVector neighboors; - neighboors.reserve(27 * max_points_per_voxel_); - std::for_each(voxels.cbegin(), voxels.cend(), [&](const auto &voxel) { - auto search = map_.find(voxel); - if (search != map_.end()) { - const auto &points = search->second.points; - if (!points.empty()) { - for (const auto &point : points) { - neighboors.emplace_back(point); - } - } - } - }); - - Eigen::Vector3d closest_neighbor; - double closest_distance2 = std::numeric_limits::max(); - std::for_each(neighboors.cbegin(), neighboors.cend(), [&](const auto &neighbor) { - double distance = (neighbor - point).squaredNorm(); - if (distance < closest_distance2) { - closest_neighbor = neighbor; - closest_distance2 = distance; - } - }); - - return closest_neighbor; - }; - using points_iterator = std::vector::const_iterator; - const auto [source, target] = tbb::parallel_reduce( - // Range - tbb::blocked_range{points.cbegin(), points.cend()}, - // Identity - ResultTuple(points.size()), - // 1st lambda: Parallel computation - [max_correspondance_distance, &GetClosestNeighboor]( - const tbb::blocked_range &r, ResultTuple res) -> ResultTuple { - auto &[src, tgt] = res; - src.reserve(r.size()); - tgt.reserve(r.size()); - for (const auto &point : r) { - Eigen::Vector3d closest_neighboors = GetClosestNeighboor(point); - if ((closest_neighboors - point).norm() < max_correspondance_distance) { - src.emplace_back(point); - tgt.emplace_back(closest_neighboors); - } +std::vector VoxelHashMap::GetPoints(const std::vector &query_voxels) const { + std::vector points; + points.reserve(query_voxels.size() * static_cast(max_points_per_voxel_)); + std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query) { + auto search = map_.find(query); + if (search != map_.end()) { + for (const auto &point : search->second.points) { + points.emplace_back(point); } - return res; - }, - // 2nd lambda: Parallel reduction - [](ResultTuple a, const ResultTuple &b) -> ResultTuple { - auto &[src, tgt] = a; - const auto &[srcp, tgtp] = b; - src.insert(src.end(), // - std::make_move_iterator(srcp.begin()), std::make_move_iterator(srcp.end())); - tgt.insert(tgt.end(), // - std::make_move_iterator(tgtp.begin()), std::make_move_iterator(tgtp.end())); - return a; - }); - - return std::make_tuple(source, target); + } + }); + return points; } std::vector VoxelHashMap::Pointcloud() const { @@ -137,13 +73,14 @@ std::vector VoxelHashMap::Pointcloud() const { return points; } -void VoxelHashMap::Update(const Vector3dVector &points, const Eigen::Vector3d &origin) { +void VoxelHashMap::Update(const std::vector &points, + const Eigen::Vector3d &origin) { AddPoints(points); RemovePointsFarFromLocation(origin); } -void VoxelHashMap::Update(const Vector3dVector &points, const Sophus::SE3d &pose) { - Vector3dVector points_transformed(points.size()); +void VoxelHashMap::Update(const std::vector &points, const Sophus::SE3d &pose) { + std::vector points_transformed(points.size()); std::transform(points.cbegin(), points.cend(), points_transformed.begin(), [&](const auto &point) { return pose * point; }); const Eigen::Vector3d &origin = pose.translation(); diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 075a676a..65a1f94f 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -34,8 +34,6 @@ namespace kiss_icp { struct VoxelHashMap { - using Vector3dVector = std::vector; - using Vector3dVectorTuple = std::tuple; using Voxel = Eigen::Vector3i; struct VoxelBlock { // buffer of points with a max limit of n_points @@ -57,8 +55,6 @@ struct VoxelHashMap { max_distance_(max_distance), max_points_per_voxel_(max_points_per_voxel) {} - Vector3dVectorTuple GetCorrespondences(const Vector3dVector &points, - double max_correspondance_distance) const; inline void Clear() { map_.clear(); } inline bool Empty() const { return map_.empty(); } void Update(const std::vector &points, const Eigen::Vector3d &origin); @@ -66,6 +62,9 @@ struct VoxelHashMap { void AddPoints(const std::vector &points); void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); std::vector Pointcloud() const; + std::vector GetPoints(const std::vector &query_voxels) const; + std::vector GetAdjacentVoxels(const Eigen::Vector3d &point, + int adjacent_voxels = 1) const; double voxel_size_; double max_distance_; diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp index 3e06f7f0..e1aaf1d8 100644 --- a/cpp/kiss_icp/pipeline/KissICP.cpp +++ b/cpp/kiss_icp/pipeline/KissICP.cpp @@ -68,11 +68,11 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector #include "kiss_icp/core/Deskew.hpp" +#include "kiss_icp/core/Registration.hpp" #include "kiss_icp/core/Threshold.hpp" #include "kiss_icp/core/VoxelHashMap.hpp" @@ -42,6 +43,10 @@ struct KISSConfig { double min_motion_th = 0.1; double initial_threshold = 2.0; + // registration params + int max_num_iterations = 500; + double convergence_criterion = 0.0001; + // Motion compensation bool deskew = false; }; @@ -54,6 +59,7 @@ class KissICP { public: explicit KissICP(const KISSConfig &config) : config_(config), + registration_(config.max_num_iterations, config.convergence_criterion), local_map_(config.voxel_size, config.max_range, config.max_points_per_voxel), adaptive_threshold_(config.initial_threshold, config.min_motion_th, config.max_range) {} @@ -77,6 +83,7 @@ class KissICP { // KISS-ICP pipeline modules std::vector poses_; KISSConfig config_; + Registration registration_; VoxelHashMap local_map_; AdaptiveThreshold adaptive_threshold_; }; diff --git a/python/kiss_icp/config/config.py b/python/kiss_icp/config/config.py index 4c9a29e9..92dc8d98 100644 --- a/python/kiss_icp/config/config.py +++ b/python/kiss_icp/config/config.py @@ -36,6 +36,11 @@ class MappingConfig(BaseModel): max_points_per_voxel: int = 20 +class RegistrationConfig(BaseModel): + max_num_iterations: Optional[int] = 500 + convergence_criterion: Optional[float] = 0.0001 + + class AdaptiveThresholdConfig(BaseModel): fixed_threshold: Optional[float] = None initial_threshold: float = 2.0 diff --git a/python/kiss_icp/config/parser.py b/python/kiss_icp/config/parser.py index 831f3039..558a01eb 100644 --- a/python/kiss_icp/config/parser.py +++ b/python/kiss_icp/config/parser.py @@ -30,13 +30,19 @@ from pydantic_settings import BaseSettings, SettingsConfigDict -from kiss_icp.config.config import AdaptiveThresholdConfig, DataConfig, MappingConfig +from kiss_icp.config.config import ( + AdaptiveThresholdConfig, + DataConfig, + MappingConfig, + RegistrationConfig, +) class KISSConfig(BaseSettings): model_config = SettingsConfigDict(env_prefix="kiss_icp_") out_dir: str = "results" data: DataConfig = DataConfig() + registration: RegistrationConfig = RegistrationConfig() mapping: MappingConfig = MappingConfig() adaptive_threshold: AdaptiveThresholdConfig = AdaptiveThresholdConfig() diff --git a/python/kiss_icp/kiss_icp.py b/python/kiss_icp/kiss_icp.py index 6bcdae0b..fd894af6 100644 --- a/python/kiss_icp/kiss_icp.py +++ b/python/kiss_icp/kiss_icp.py @@ -26,7 +26,7 @@ from kiss_icp.deskew import get_motion_compensator from kiss_icp.mapping import get_voxel_hash_map from kiss_icp.preprocess import get_preprocessor -from kiss_icp.registration import register_frame +from kiss_icp.registration import get_registration from kiss_icp.threshold import get_threshold_estimator from kiss_icp.voxelization import voxel_down_sample @@ -37,6 +37,7 @@ def __init__(self, config: KISSConfig): self.config = config self.compensator = get_motion_compensator(config) self.adaptive_threshold = get_threshold_estimator(self.config) + self.registration = get_registration(self.config) self.local_map = get_voxel_hash_map(self.config) self.preprocess = get_preprocessor(self.config) @@ -59,7 +60,7 @@ def register_frame(self, frame, timestamps): initial_guess = last_pose @ prediction # Run ICP - new_pose = register_frame( + new_pose = self.registration.align_points_to_map( points=source, voxel_map=self.local_map, initial_guess=initial_guess, diff --git a/python/kiss_icp/mapping.py b/python/kiss_icp/mapping.py index b0f7f67e..ba1dfb73 100644 --- a/python/kiss_icp/mapping.py +++ b/python/kiss_icp/mapping.py @@ -20,8 +20,6 @@ # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. -from typing import Tuple - import numpy as np from kiss_icp.config import KISSConfig @@ -68,15 +66,3 @@ def remove_far_away_points(self, origin): def point_cloud(self) -> np.ndarray: """Return the internal representaion as a np.array (pointcloud).""" return np.asarray(self._internal_map._point_cloud()) - - def get_correspondences( - self, - points: np.ndarray, - max_correspondance_distance: float, - ) -> Tuple[np.ndarray, np.ndarray]: - """Get the pair of {source, target} pointcloud of the same size.""" - _points = kiss_icp_pybind._Vector3dVector(points) - source, target = self._internal_map._get_correspondences( - _points, max_correspondance_distance - ) - return np.asarray(source), np.asarray(target) diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp index 32cb1702..c2ff3eb2 100644 --- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp +++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp @@ -57,12 +57,12 @@ PYBIND11_MODULE(kiss_icp_pybind, m) { .def("_clear", &VoxelHashMap::Clear) .def("_empty", &VoxelHashMap::Empty) .def("_update", - py::overload_cast( + py::overload_cast &, const Eigen::Vector3d &>( &VoxelHashMap::Update), "points"_a, "origin"_a) .def( "_update", - [](VoxelHashMap &self, const VoxelHashMap::Vector3dVector &points, + [](VoxelHashMap &self, const std::vector &points, const Eigen::Matrix4d &T) { Sophus::SE3d pose(T); self.Update(points, pose); @@ -70,21 +70,26 @@ PYBIND11_MODULE(kiss_icp_pybind, m) { "points"_a, "pose"_a) .def("_add_points", &VoxelHashMap::AddPoints, "points"_a) .def("_remove_far_away_points", &VoxelHashMap::RemovePointsFarFromLocation, "origin"_a) - .def("_point_cloud", &VoxelHashMap::Pointcloud) - .def("_get_correspondences", &VoxelHashMap::GetCorrespondences, "points"_a, - "max_correspondance_distance"_a); + .def("_point_cloud", &VoxelHashMap::Pointcloud); // Point Cloud registration - m.def( - "_register_point_cloud", - [](const std::vector &points, const VoxelHashMap &voxel_map, - const Eigen::Matrix4d &T_guess, double max_correspondence_distance, double kernel) { - Sophus::SE3d initial_guess(T_guess); - return RegisterFrame(points, voxel_map, initial_guess, max_correspondence_distance, - kernel) - .matrix(); - }, - "points"_a, "voxel_map"_a, "initial_guess"_a, "max_correspondance_distance"_a, "kernel"_a); + py::class_ internal_registration(m, "_Registration", "Don't use this"); + internal_registration + .def(py::init(), "max_num_iterations"_a, "convergence_criterion"_a) + .def( + "_align_points_to_map", + [](Registration &self, const std::vector &points, + const VoxelHashMap &voxel_map, const Eigen::Matrix4d &T_guess, + double max_correspondence_distance, double kernel) { + Sophus::SE3d initial_guess(T_guess); + return self + .AlignPointsToMap(points, voxel_map, initial_guess, max_correspondence_distance, + kernel) + .matrix(); + }, + "points"_a, "voxel_map"_a, "initial_guess"_a, "max_correspondance_distance"_a, + "kernel"_a); + // AdaptiveThreshold bindings py::class_ adaptive_threshold(m, "_AdaptiveThreshold", "Don't use this"); adaptive_threshold diff --git a/python/kiss_icp/registration.py b/python/kiss_icp/registration.py index ebba5706..48a05d32 100644 --- a/python/kiss_icp/registration.py +++ b/python/kiss_icp/registration.py @@ -22,21 +22,37 @@ # SOFTWARE. import numpy as np +from kiss_icp.config.parser import KISSConfig from kiss_icp.mapping import VoxelHashMap from kiss_icp.pybind import kiss_icp_pybind -def register_frame( - points: np.ndarray, - voxel_map: VoxelHashMap, - initial_guess: np.ndarray, - max_correspondance_distance: float, - kernel: float, -) -> np.ndarray: - return kiss_icp_pybind._register_point_cloud( - points=kiss_icp_pybind._Vector3dVector(points), - voxel_map=voxel_map._internal_map, - initial_guess=initial_guess, - max_correspondance_distance=max_correspondance_distance, - kernel=kernel, +def get_registration(config: KISSConfig): + return Registration( + max_num_iterations=config.registration.max_num_iterations, + convergence_criterion=config.registration.convergence_criterion, ) + + +class Registration: + def __init__(self, max_num_iterations: int, convergence_criterion: float): + self._registration = kiss_icp_pybind._Registration( + max_num_iterations=max_num_iterations, + convergence_criterion=convergence_criterion, + ) + + def align_points_to_map( + self, + points: np.ndarray, + voxel_map: VoxelHashMap, + initial_guess: np.ndarray, + max_correspondance_distance: float, + kernel: float, + ) -> np.ndarray: + return self._registration._align_points_to_map( + points=kiss_icp_pybind._Vector3dVector(points), + voxel_map=voxel_map._internal_map, + initial_guess=initial_guess, + max_correspondance_distance=max_correspondance_distance, + kernel=kernel, + )