From 16a30857847ee63607375fa86bb82ebc7e110b59 Mon Sep 17 00:00:00 2001 From: Benedikt Mersch Date: Thu, 25 Jul 2024 15:03:44 +0200 Subject: [PATCH] Bump version (#20) * Migrate Python side * Migrate Cpp * No need * Bump versions * Bump versions --- pyproject.toml | 4 +- src/mapmos/datasets/mapmos_dataset.py | 2 +- src/mapmos/odometry.py | 33 +++-- src/mapmos/paper_pipeline.py | 5 +- src/mapmos/pipeline.py | 7 +- src/mapmos/pybind/Registration.cpp | 176 ++++++++++++++++++-------- src/mapmos/pybind/Registration.hpp | 18 ++- src/mapmos/pybind/VoxelHashMap.cpp | 137 +++++--------------- src/mapmos/pybind/VoxelHashMap.hpp | 18 +-- src/mapmos/pybind/mapmos_pybind.cpp | 37 +++--- src/mapmos/registration.py | 49 +++++-- 11 files changed, 265 insertions(+), 221 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index f880dee..35478bf 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,6 +1,6 @@ [project] name = "mapmos" -version = "0.0.1" +version = "1.0.0" description = "Building Volumetric Beliefs for Dynamic Environments Exploiting Map-Based Moving Object Segmentation" readme = "README.md" authors = [ @@ -8,7 +8,7 @@ authors = [ ] license_files = "LICENSE" dependencies = [ - "kiss-icp<=0.4.0", + "kiss-icp>=1.0.0", "diskcache>=5.3.0", "pytorch_lightning>=1.6.4", ] diff --git a/src/mapmos/datasets/mapmos_dataset.py b/src/mapmos/datasets/mapmos_dataset.py index 6b33174..7c50859 100644 --- a/src/mapmos/datasets/mapmos_dataset.py +++ b/src/mapmos/datasets/mapmos_dataset.py @@ -236,7 +236,7 @@ def get_scan_and_map( self.gt_map.remove_voxels_far_from_location(self.odometry.current_location()) map_points = self.odometry.transform( - registered_map_points, np.linalg.inv(self.odometry.current_pose()) + registered_map_points, np.linalg.inv(self.odometry.last_pose) ) scan_timestamps = scan_index * np.ones(len(scan_points)) diff --git a/src/mapmos/odometry.py b/src/mapmos/odometry.py index 4e118d0..a5ff20e 100644 --- a/src/mapmos/odometry.py +++ b/src/mapmos/odometry.py @@ -25,8 +25,8 @@ from typing import Type from kiss_icp.config import KISSConfig -from kiss_icp.kiss_icp import KissICP -from mapmos.registration import register_frame +from kiss_icp.kiss_icp import KissICP, get_registration +from mapmos.registration import get_registration from mapmos.mapping import VoxelHashMap @@ -56,10 +56,11 @@ def __init__( max_distance=self.config.data.max_range, max_points_per_voxel=self.config.mapping.max_points_per_voxel, ) + self.registration = get_registration(kiss_config) def register_points(self, points, timestamps, scan_index): # Apply motion compensation - points = self.compensator.deskew_scan(points, self.poses, timestamps) + points = self.compensator.deskew_scan(points, timestamps, self.last_delta) # Preprocess the input cloud points_prep = self.preprocess(points) @@ -68,13 +69,12 @@ def register_points(self, points, timestamps, scan_index): source, points_downsample = self.voxelize(points_prep) # Get motion prediction and adaptive_threshold - sigma = self.get_adaptive_threshold() + sigma = self.adaptive_threshold.get_threshold() # Compute initial_guess for ICP - prediction = self.get_prediction_model() - initial_guess = self.current_pose() @ prediction + initial_guess = self.last_pose @ self.last_delta - new_pose = register_frame( + new_pose = self.registration.align_points_to_map( points=source, voxel_map=self.local_map, initial_guess=initial_guess, @@ -82,11 +82,16 @@ def register_points(self, points, timestamps, scan_index): kernel=sigma / 3, ) - self.adaptive_threshold.update_model_deviation(np.linalg.inv(initial_guess) @ new_pose) + # Compute the difference between the prediction and the actual estimate + model_deviation = np.linalg.inv(initial_guess) @ new_pose + + # Update step: threshold, local map, delta, and the last pose + self.adaptive_threshold.update_model_deviation(model_deviation) self.local_map.update(points_downsample, new_pose, scan_index) - self.poses.append(new_pose) + self.last_delta = np.linalg.inv(self.last_pose) @ new_pose + self.last_pose = new_pose - points_reg = self.transform(points, self.current_pose()) + points_reg = self.transform(points, self.last_pose) return np.asarray(points_reg) def get_map_points(self): @@ -98,11 +103,5 @@ def transform(self, points, pose): points = (pose @ points_hom.T).T[:, :3] return points - def get_poses(self): - return self.poses - - def current_pose(self): - return self.poses[-1] if self.poses else np.eye(4) - def current_location(self): - return self.current_pose()[:3, 3] + return self.last_pose[:3, 3] diff --git a/src/mapmos/paper_pipeline.py b/src/mapmos/paper_pipeline.py index a499b76..be1ad7e 100644 --- a/src/mapmos/paper_pipeline.py +++ b/src/mapmos/paper_pipeline.py @@ -72,6 +72,7 @@ def _run_pipeline(self): local_scan, timestamps, gt_labels = self._next(scan_index) map_points, map_indices = self.odometry.get_map_points() scan_points = self.odometry.register_points(local_scan, timestamps, scan_index) + self.poses[scan_index - self._first] = self.odometry.last_pose min_range_mos = self.config.mos.min_range_mos max_range_mos = self.config.mos.max_range_mos @@ -165,7 +166,7 @@ def _run_pipeline(self): pred_labels_map, belief_labels_scan, belief_labels_map, - self.odometry.current_pose(), + self.odometry.last_pose, ) # Probabilistic volumetric fusion with scan and moving map predictions and delay @@ -185,7 +186,7 @@ def _run_pipeline(self): def _run_evaluation(self): if self.has_gt: - self.results.eval_odometry(self.odometry.get_poses(), self.gt_poses) + self.results.eval_odometry(self.poses, self.gt_poses) self.results.eval_mos(self.confusion_matrix_mos, desc="\nScan Prediction") self.results.eval_fps(self.times_mos, desc="Average Frequency MOS") self.results.eval_mos(self.confusion_matrix_belief_scan_only, desc="\nBelief, Scan Only") diff --git a/src/mapmos/pipeline.py b/src/mapmos/pipeline.py index ff29031..f704d32 100644 --- a/src/mapmos/pipeline.py +++ b/src/mapmos/pipeline.py @@ -82,7 +82,7 @@ def __init__( # Results self.results = MOSPipelineResults() - self.poses = self.odometry.poses + self.poses = np.zeros((self._n_scans, 4, 4)) self.has_gt = hasattr(self._dataset, "gt_poses") self.gt_poses = self._dataset.gt_poses[self._first : self._last] if self.has_gt else None self.dataset_name = self._dataset.__class__.__name__ @@ -127,6 +127,7 @@ def _run_pipeline(self): local_scan, timestamps, gt_labels = self._next(scan_index) map_points, map_indices = self.odometry.get_map_points() scan_points = self.odometry.register_points(local_scan, timestamps, scan_index) + self.poses[scan_index - self._first] = self.odometry.last_pose min_range_mos = self.config.mos.min_range_mos max_range_mos = self.config.mos.max_range_mos @@ -187,7 +188,7 @@ def _run_pipeline(self): pred_labels_map, belief_labels_scan, belief_labels_map, - self.odometry.current_pose(), + self.odometry.last_pose, ) # Evaluate and save with delay @@ -238,7 +239,7 @@ def _next(self, idx): def _run_evaluation(self): if self.has_gt: - self.results.eval_odometry(self.odometry.get_poses(), self.gt_poses) + self.results.eval_odometry(self.poses, self.gt_poses) self.results.eval_mos(self.confusion_matrix_belief, desc="\nBelief") self.results.eval_fps(self.times_mos, desc="\nAverage Frequency MOS") self.results.eval_fps(self.times_belief, desc="Average Frequency Belief") diff --git a/src/mapmos/pybind/Registration.cpp b/src/mapmos/pybind/Registration.cpp index f67d1ea..529c894 100644 --- a/src/mapmos/pybind/Registration.cpp +++ b/src/mapmos/pybind/Registration.cpp @@ -23,92 +23,166 @@ #include "Registration.hpp" #include +#include +#include #include #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 -namespace { +using Correspondences = std::vector>; +using LinearSystem = std::pair; +namespace { inline double square(double x) { return x * x; } -struct ResultTuple { - ResultTuple() { - JTJ.setZero(); - JTr.setZero(); - } +void TransformPoints(const Sophus::SE3d &T, std::vector &points) { + std::transform(points.cbegin(), points.cend(), points.begin(), + [&](const auto &point) { return T * point; }); +} - ResultTuple operator+(const ResultTuple &other) { - this->JTJ += other.JTJ; - this->JTr += other.JTr; - return *this; +using Voxel = mapmos::VoxelHashMap::Voxel; +std::vector GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1) { + std::vector voxel_neighborhood; + for (int i = voxel.x() - adjacent_voxels; i < voxel.x() + adjacent_voxels + 1; ++i) { + for (int j = voxel.y() - adjacent_voxels; j < voxel.y() + adjacent_voxels + 1; ++j) { + for (int k = voxel.z() - adjacent_voxels; k < voxel.z() + adjacent_voxels + 1; ++k) { + voxel_neighborhood.emplace_back(i, j, k); + } + } } + return voxel_neighborhood; +} - Eigen::Matrix6d JTJ; - Eigen::Vector6d JTr; -}; +std::tuple GetClosestNeighbor(const Eigen::Vector3d &point, + const mapmos::VoxelHashMap &voxel_map) { + // Convert the point to voxel coordinates + const auto &voxel = voxel_map.PointToVoxel(point); + // Get nearby voxels on the map + const auto &query_voxels = GetAdjacentVoxels(voxel); + // Extract the points contained within the neighborhood voxels + const auto &neighbors = voxel_map.GetPoints(query_voxels); + + // Find the nearest neighbor + Eigen::Vector3d closest_neighbor; + double closest_distance = std::numeric_limits::max(); + std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) { + double distance = (neighbor - point).norm(); + if (distance < closest_distance) { + closest_neighbor = neighbor; + closest_distance = distance; + } + }); + return std::make_tuple(closest_neighbor, closest_distance); +} -void TransformPoints(const Sophus::SE3d &T, std::vector &points) { - std::transform(points.cbegin(), points.cend(), points.begin(), - [&](const auto &point) { return T * point; }); +Correspondences DataAssociation(const std::vector &points, + const mapmos::VoxelHashMap &voxel_map, + const double max_correspondance_distance) { + using points_iterator = std::vector::const_iterator; + Correspondences correspondences; + correspondences.reserve(points.size()); + correspondences = tbb::parallel_reduce( + // Range + tbb::blocked_range{points.cbegin(), points.cend()}, + // Identity + correspondences, + // 1st lambda: Parallel computation + [&](const tbb::blocked_range &r, Correspondences res) -> Correspondences { + res.reserve(r.size()); + std::for_each(r.begin(), r.end(), [&](const auto &point) { + const auto &[closest_neighbor, distance] = GetClosestNeighbor(point, voxel_map); + if (distance < max_correspondance_distance) { + res.emplace_back(point, closest_neighbor); + } + }); + return res; + }, + // 2nd lambda: Parallel reduction + [](Correspondences a, const Correspondences &b) -> Correspondences { + a.insert(a.end(), // + std::make_move_iterator(b.cbegin()), // + std::make_move_iterator(b.cend())); + return a; + }); + + return correspondences; } -Sophus::SE3d AlignClouds(const std::vector &source, - const std::vector &target, - double th) { - auto compute_jacobian_and_residual = [&](auto i) { - const Eigen::Vector3d residual = source[i] - target[i]; +LinearSystem BuildLinearSystem(const Correspondences &correspondences, const double kernel_scale) { + auto compute_jacobian_and_residual = [](const auto &correspondence) { + const auto &[source, target] = correspondence; + 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 = [&](const double &residual2) { + return square(kernel_scale) / square(kernel_scale + residual2); + }; + + using correspondence_iterator = Correspondences::const_iterator; const auto &[JTJ, JTr] = tbb::parallel_reduce( // Range - tbb::blocked_range{0, source.size()}, + tbb::blocked_range{correspondences.cbegin(), + correspondences.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(th) / square(th + 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 &correspondence) { + const auto &[J_r, residual] = compute_jacobian_and_residual(correspondence); + 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); - const Eigen::Vector6d x = JTJ.ldlt().solve(-JTr); - return Sophus::SE3d::exp(x); + return {JTJ, JTr}; } - -constexpr int MAX_NUM_ITERATIONS_ = 500; -constexpr double ESTIMATION_THRESHOLD_ = 0.0001; - } // namespace namespace mapmos { -Sophus::SE3d RegisterFrame(const std::vector &frame, - const VoxelHashMap &voxel_map, - const Sophus::SE3d &initial_guess, - double max_correspondence_distance, - double kernel) { +Registration::Registration(int max_num_iteration, double convergence_criterion, int max_num_threads) + : max_num_iterations_(max_num_iteration), + convergence_criterion_(convergence_criterion), + // Only manipulate the number of threads if the user specifies something greater than 0 + max_num_threads_(max_num_threads > 0 ? max_num_threads : tbb::info::default_concurrency()) { + // This global variable requires static duration storage to be able to manipulate the max + // concurrency from TBB across the entire class + static const auto tbb_control_settings = tbb::global_control( + tbb::global_control::max_allowed_parallelism, static_cast(max_num_threads_)); +} + +Sophus::SE3d Registration::AlignPointsToMap(const std::vector &frame, + const VoxelHashMap &voxel_map, + const Sophus::SE3d &initial_guess, + const double max_distance, + const double kernel_scale) { if (voxel_map.Empty()) return initial_guess; // Equation (9) @@ -117,17 +191,19 @@ 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 correspondences = DataAssociation(source, voxel_map, max_distance); // Equation (11) - auto estimation = AlignClouds(src, tgt, kernel); + const auto &[JTJ, JTr] = BuildLinearSystem(correspondences, kernel_scale); + const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr); + const Sophus::SE3d estimation = Sophus::SE3d::exp(dx); // Equation (12) TransformPoints(estimation, source); // Update iterations T_icp = estimation * T_icp; // Termination criteria - if (estimation.log().norm() < ESTIMATION_THRESHOLD_) break; + if (dx.norm() < convergence_criterion_) break; } // Spit the final transformation return T_icp * initial_guess; diff --git a/src/mapmos/pybind/Registration.hpp b/src/mapmos/pybind/Registration.hpp index d7f26bc..25dfd45 100644 --- a/src/mapmos/pybind/Registration.hpp +++ b/src/mapmos/pybind/Registration.hpp @@ -30,9 +30,17 @@ namespace mapmos { -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, int max_num_threads); + + Sophus::SE3d AlignPointsToMap(const std::vector &frame, + const VoxelHashMap &voxel_map, + const Sophus::SE3d &initial_guess, + const double max_correspondence_distance, + const double kernel_scale); + + int max_num_iterations_; + double convergence_criterion_; + int max_num_threads_; +}; } // namespace mapmos diff --git a/src/mapmos/pybind/VoxelHashMap.cpp b/src/mapmos/pybind/VoxelHashMap.cpp index ccc4357..4a3feb7 100644 --- a/src/mapmos/pybind/VoxelHashMap.cpp +++ b/src/mapmos/pybind/VoxelHashMap.cpp @@ -32,97 +32,20 @@ #include #include #include - -namespace { -struct ResultTuple { - ResultTuple(std::size_t n) { - source.reserve(n); - target.reserve(n); - } - std::vector source; - std::vector target; -}; -} // namespace - namespace mapmos { -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::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); } } - - 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); - } - } - 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 { @@ -154,17 +77,17 @@ std::tuple, std::vector> VoxelHashMap::Pointcl return std::make_tuple(points, timestamps); } -void VoxelHashMap::Update(const Vector3dVector &points, +void VoxelHashMap::Update(const std::vector &points, const Eigen::Vector3d &origin, const int timestamp) { AddPoints(points, timestamp); - RemoveFarAwayPoints(origin); + RemovePointsFarFromLocation(origin); } -void VoxelHashMap::Update(const Vector3dVector &points, +void VoxelHashMap::Update(const std::vector &points, const Sophus::SE3d &pose, const int timestamp) { - Vector3dVector points_transformed(points.size()); + 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(); @@ -180,25 +103,39 @@ void VoxelHashMap::AddPoints(const std::vector &points, const i voxel_block.AddPoint(point, timestamp); } else { map_.insert( - {voxel, VoxelBlock{{point}, {timestamp}, {Belief()}, max_points_per_voxel_}}); + {voxel, VoxelBlock{{point}, {timestamp}, {Belief{}}, max_points_per_voxel_}}); } }); } -void VoxelHashMap::UpdateBelief(const Vector3dVector &points, const std::vector &updates) { +void VoxelHashMap::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) { + const auto max_distance2 = max_distance_ * max_distance_; + for (auto it = map_.begin(); it != map_.end();) { + const auto &[voxel, voxel_block] = *it; + Eigen::Vector3d pt(voxel[0] * voxel_size_, voxel[1] * voxel_size_, voxel[2] * voxel_size_); + if ((pt - origin).squaredNorm() >= (max_distance2)) { + it = map_.erase(it); + } else { + ++it; + } + } +} + +void VoxelHashMap::UpdateBelief(const std::vector &points, + const std::vector &updates) { std::vector voxels_to_update; voxels_to_update.reserve(points.size()); - for (size_t i = 0; i < points.size(); i++) { auto voxel = Voxel((points[i] / voxel_size_).template cast()); voxels_to_update.emplace_back(voxel); map_[voxel].belief.accumulatePartialUpdate(updates[i]); } + tbb::parallel_for_each(voxels_to_update.cbegin(), voxels_to_update.cend(), [this](const auto &voxel) { map_[voxel].belief.update(); }); } -std::vector VoxelHashMap::GetBelief(const Vector3dVector &points) const { +std::vector VoxelHashMap::GetBelief(const std::vector &points) const { std::vector beliefs(points.size(), 0.0); std::transform(points.cbegin(), points.cend(), beliefs.begin(), [this](const auto &p) { auto voxel = Voxel((p / voxel_size_).template cast()); @@ -209,13 +146,5 @@ std::vector VoxelHashMap::GetBelief(const Vector3dVector &points) const }); return beliefs; } -void VoxelHashMap::RemoveFarAwayPoints(const Eigen::Vector3d &origin) { - for (const auto &[voxel, voxel_block] : map_) { - Eigen::Vector3d pt(voxel[0] * voxel_size_, voxel[1] * voxel_size_, voxel[2] * voxel_size_); - const auto max_distance2 = max_distance_ * max_distance_; - if ((pt - origin).squaredNorm() > (max_distance2)) { - map_.erase(voxel); - } - } -} + } // namespace mapmos diff --git a/src/mapmos/pybind/VoxelHashMap.hpp b/src/mapmos/pybind/VoxelHashMap.hpp index 6dbdcd4..8b29b1d 100644 --- a/src/mapmos/pybind/VoxelHashMap.hpp +++ b/src/mapmos/pybind/VoxelHashMap.hpp @@ -51,8 +51,6 @@ struct Belief { }; 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 @@ -60,7 +58,6 @@ struct VoxelHashMap { std::vector timestamps; Belief belief; int num_points_; - inline void AddPoint(const Eigen::Vector3d &point, int timestamp) { if (points.size() < static_cast(num_points_)) { points.push_back(point); @@ -80,10 +77,13 @@ 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(); } + inline Voxel PointToVoxel(const Eigen::Vector3d &point) const { + return Voxel(static_cast(point.x() / voxel_size_), + static_cast(point.y() / voxel_size_), + static_cast(point.z() / voxel_size_)); + } void Update(const std::vector &points, const Eigen::Vector3d &origin, const int timestamp); @@ -91,11 +91,13 @@ struct VoxelHashMap { const Sophus::SE3d &pose, const int timestamp); void AddPoints(const std::vector &points, const int timestamp); - std::vector GetBelief(const Vector3dVector &points) const; - void UpdateBelief(const Vector3dVector &points, const std::vector &updates); - void RemoveFarAwayPoints(const Eigen::Vector3d &origin); + std::vector GetBelief(const std::vector &points) const; + void UpdateBelief(const std::vector &points, + const std::vector &updates); + void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); std::vector Pointcloud() const; std::tuple, std::vector> PointcloudWithTimestamps() const; + std::vector GetPoints(const std::vector &query_voxels) const; double voxel_size_; double max_distance_; diff --git a/src/mapmos/pybind/mapmos_pybind.cpp b/src/mapmos/pybind/mapmos_pybind.cpp index 84d8f37..b295d98 100644 --- a/src/mapmos/pybind/mapmos_pybind.cpp +++ b/src/mapmos/pybind/mapmos_pybind.cpp @@ -53,36 +53,41 @@ PYBIND11_MODULE(mapmos_pybind, m) { .def("_clear", &VoxelHashMap::Clear) .def("_empty", &VoxelHashMap::Empty) .def("_update", - py::overload_cast &, const Eigen::Vector3d &, const int>(&VoxelHashMap::Update), "points"_a, "origin"_a, "timestamp"_a) .def( "_update", - [](VoxelHashMap &self, const VoxelHashMap::Vector3dVector &points, + [](VoxelHashMap &self, const std::vector &points, const Eigen::Matrix4d &T, const int timestamp) { Sophus::SE3d pose(T); self.Update(points, pose, timestamp); }, "points"_a, "pose"_a, "timestamp"_a) .def("_add_points", &VoxelHashMap::AddPoints, "points"_a, "timestamp"_a) - .def("_remove_far_away_points", &VoxelHashMap::RemoveFarAwayPoints, "origin"_a) + .def("_remove_far_away_points", &VoxelHashMap::RemovePointsFarFromLocation, "origin"_a) .def("_point_cloud", &VoxelHashMap::Pointcloud) .def("_point_cloud_with_timestamps", &VoxelHashMap::PointcloudWithTimestamps) .def("_update_belief", &VoxelHashMap::UpdateBelief, "points"_a, "updates"_a) - .def("_get_belief", &VoxelHashMap::GetBelief, "points"_a) - .def("_get_correspondences", &VoxelHashMap::GetCorrespondences, "points"_a, - "max_correspondance_distance"_a); + .def("_get_belief", &VoxelHashMap::GetBelief, "points"_a); // 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, + "max_num_threads"_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); } } // namespace mapmos diff --git a/src/mapmos/registration.py b/src/mapmos/registration.py index 7ef9944..277a53a 100644 --- a/src/mapmos/registration.py +++ b/src/mapmos/registration.py @@ -22,21 +22,44 @@ # SOFTWARE. import numpy as np +from kiss_icp.config.parser import KISSConfig from mapmos.mapping import VoxelHashMap from mapmos.pybind import mapmos_pybind -def register_frame( - points: np.ndarray, - voxel_map: VoxelHashMap, - initial_guess: np.ndarray, - max_correspondance_distance: float, - kernel: float, -) -> np.ndarray: - return mapmos_pybind._register_point_cloud( - points=mapmos_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, + max_num_threads=config.registration.max_num_threads, ) + + +class Registration: + def __init__( + self, + max_num_iterations: int, + convergence_criterion: float, + max_num_threads: int = 0, + ): + self._registration = mapmos_pybind._Registration( + max_num_iterations=max_num_iterations, + convergence_criterion=convergence_criterion, + max_num_threads=max_num_threads, + ) + + 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=mapmos_pybind._Vector3dVector(points), + voxel_map=voxel_map._internal_map, + initial_guess=initial_guess, + max_correspondance_distance=max_correspondance_distance, + kernel=kernel, + )