From 418572ca2be4c228440da3c17272be51aee95fcf Mon Sep 17 00:00:00 2001 From: Alexander Berntsson Date: Mon, 25 Mar 2024 22:23:40 +0100 Subject: [PATCH] Fix bug where point would match with random point (#300) * Fix bug where point would match with random point * Use closest neightbor distance * Switch order * Refactor ROS parametrization (#282) * Move ros params from launch file to YAML * Reuse arguments for debug clouds * Rename odometry_node to kiss_icp_node odometry is way to generic * Rename node agin * Voxel size is optional paramter * Reads better like this * New parameter proposal * Add odometry covariance to ros node (#283) * Add fixed covariance to odometry msg * Add default value just in case * pre-commit * Expose number of icp iterations in ros (#292) * Expose registration params to ROS node * More merge conflicts * Default to multithread * Revert "Refactor ROS parametrization (#282)" This reverts commit 7b8b09511f903e1f32ba6ae7e79962ee03942101. * Fix and improve ROS visualization (#285) * Fix this madness Simplify implementation by debugging in an ego-centric way always. Since the KISS-ICP internal map is on the global coordinates, fetch the last ego-centric pose and apply it to the map. Seen from the cloud_frame_id (which is the sensor frame) everything should always work in terms of visualization, no matter the multi-sensor setup. * Now is safe to disable this by default * Simplify, borrow the header from the input pointcloud msg This actually makes the visualization closer to the Python visualizer * Disable path/odom visualization by default In the case where we are not computing the poses in an egocentric world (base_frame != "") and we are not publishing to the TF tree, then the visualization wouldn't make sense. Therefore, disable it by default * Changed my mind If someone doesn't have that particular frame defined, then the visualization won't work. Leave this default * Move responsability of handling tf frames out of Registration (#288) * Move responsability of handling tf frames out of Registration Since with this new changes PublishOdometry is the only member that requieres to know the user specified target-frame, it is not necesary to handle all this bits. This makes the implementation cleaner, easier to read and reduces the lines of code. Now RegisterFrame is a simple callback as few months ago one can read in seconds. * typo * Easier to read * We need this for LookupTransform * Remove unused variable * Revert "Remove unused variable" This reverts commit 424ee901c5442ef1f1651f48079c8cbf5a3f10bf. * Remove unnecessary check * Remove unnecessary exposed utility function from ROS API With this change this function is not exposed (which was never the intention to) to the header. This way we can also "hide" this into a private unnamed namesapces and benefit from inlining the simple function into the translation unit * Revert "Remove unnecessary exposed utility function from ROS API" This reverts commit 23cd7efafd133c4f209f8575d079dee2d14702ae. * Revert "Remove unnecessary check" This reverts commit d1dcb48762392aae879e84d849c8f4b9c6e6cc10. * merge conflicts :0 * Remove unnecessary exposed utility function from ROS API (#289) * Move responsability of handling tf frames out of Registration Since with this new changes PublishOdometry is the only member that requieres to know the user specified target-frame, it is not necesary to handle all this bits. This makes the implementation cleaner, easier to read and reduces the lines of code. Now RegisterFrame is a simple callback as few months ago one can read in seconds. * typo * Easier to read * We need this for LookupTransform * Remove unused variable * Revert "Remove unused variable" This reverts commit 424ee901c5442ef1f1651f48079c8cbf5a3f10bf. * Remove unnecessary check * Remove unnecessary exposed utility function from ROS API With this change this function is not exposed (which was never the intention to) to the header. This way we can also "hide" this into a private unnamed namesapces and benefit from inlining the simple function into the translation unit * Revert "Remove unnecessary exposed utility function from ROS API" This reverts commit 23cd7efafd133c4f209f8575d079dee2d14702ae. * Remove unnecessary exposed utility function from ROS API With this change this function is not exposed (which was never the intention to) to the header. This way we can also "hide" this into a private unnamed namesapces and benefit from inlining the simple function into the translation unit * Revert "Remove unnecessary check" This reverts commit d1dcb48762392aae879e84d849c8f4b9c6e6cc10. --------- Co-authored-by: tizianoGuadagnino * too many merges * Merge Rviz and Python colors * Just make the default construction more clear --------- Co-authored-by: tizianoGuadagnino * Revert "Fix and improve ROS visualization (#285)" This reverts commit 20797deb34cb283cf45ef0d4c899a94eb708f5f1. --------- Co-authored-by: tizianoGuadagnino Co-authored-by: Benedikt Mersch Co-authored-by: Ignacio Vizzo --- cpp/kiss_icp/core/Registration.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index b3d8d1cf..46f22e74 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -65,8 +65,8 @@ std::vector GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1 return voxel_neighborhood; } -Eigen::Vector3d GetClosestNeighbor(const Eigen::Vector3d &point, - const kiss_icp::VoxelHashMap &voxel_map) { +std::tuple GetClosestNeighbor(const Eigen::Vector3d &point, + const kiss_icp::VoxelHashMap &voxel_map) { // Convert the point to voxel coordinates const auto &voxel = voxel_map.PointToVoxel(point); // Get nearby voxels on the map @@ -76,15 +76,15 @@ Eigen::Vector3d GetClosestNeighbor(const Eigen::Vector3d &point, // Find the nearest neighbor Eigen::Vector3d closest_neighbor; - double closest_distance2 = std::numeric_limits::max(); + double closest_distance = std::numeric_limits::max(); std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) { - double distance = (neighbor - point).squaredNorm(); - if (distance < closest_distance2) { + double distance = (neighbor - point).norm(); + if (distance < closest_distance) { closest_neighbor = neighbor; - closest_distance2 = distance; + closest_distance = distance; } }); - return closest_neighbor; + return std::make_tuple(closest_neighbor, closest_distance); } Associations FindAssociations(const std::vector &points, @@ -101,12 +101,12 @@ Associations FindAssociations(const std::vector &points, // 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) { + 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