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 7b8b095.

* 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 424ee90.

* 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 23cd7ef.

* Revert "Remove unnecessary check"

This reverts commit d1dcb48.

* 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 424ee90.

* 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 23cd7ef.

* 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 d1dcb48.

---------

Co-authored-by: tizianoGuadagnino <[email protected]>

* too many merges

* Merge Rviz and Python colors

* Just make the default construction more clear

---------

Co-authored-by: tizianoGuadagnino <[email protected]>

* Revert "Fix and improve ROS visualization (#285)"

This reverts commit 20797de.

---------

Co-authored-by: tizianoGuadagnino <[email protected]>
Co-authored-by: Benedikt Mersch <[email protected]>
Co-authored-by: Ignacio Vizzo <[email protected]>

cpp/kiss_icp/core/Registration.cpp

@@ -65,8 +65,8 @@ std::vector<Voxel> 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<Eigen::Vector3d, double> 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<double>::max();
+    double closest_distance = std::numeric_limits<double>::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<Eigen::Vector3d> &points,
@@ -101,12 +101,12 @@ Associations FindAssociations(const std::vector<Eigen::Vector3d> &points,
         // 1st lambda: Parallel computation
         [&](const tbb::blocked_range<points_iterator> &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