diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 574be387..b2b54d4e 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -39,7 +39,7 @@ using Matrix6d = Eigen::Matrix; using Matrix3_6d = Eigen::Matrix; using Vector6d = Eigen::Matrix; } // namespace Eigen -using CorrespondenceVector = std::vector>; +using Associations = std::vector>; using LinearSystem = std::pair; namespace { @@ -66,43 +66,42 @@ Eigen::Vector3d GetClosestNeighbor(const Eigen::Vector3d &point, return closest_neighbor; } -CorrespondenceVector DataAssociation(const std::vector &points, - const kiss_icp::VoxelHashMap &voxel_map, - double max_correspondance_distance) { +Associations FindAssociations(const std::vector &points, + const kiss_icp::VoxelHashMap &voxel_map, + double max_correspondance_distance) { using points_iterator = std::vector::const_iterator; - CorrespondenceVector correspondences; - correspondences.reserve(points.size()); - correspondences = tbb::parallel_reduce( + Associations associations; + associations.reserve(points.size()); + associations = tbb::parallel_reduce( // Range tbb::blocked_range{points.cbegin(), points.cend()}, // Identity - correspondences, + associations, // 1st lambda: Parallel computation - [&](const tbb::blocked_range &r, - CorrespondenceVector res) -> CorrespondenceVector { + [&](const tbb::blocked_range &r, Associations res) -> Associations { res.reserve(r.size()); - std::for_each(r.begin(), r.end(), [&](const Eigen::Vector3d &point) { + 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 - [](CorrespondenceVector a, const CorrespondenceVector &b) -> CorrespondenceVector { + [](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 correspondences; + return associations; } -LinearSystem BuildLinearSystem(const CorrespondenceVector &correspondences, double kernel) { - auto compute_jacobian_and_residual = [](const auto &source_and_target) { - const auto &[source, target] = source_and_target; +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(); @@ -118,15 +117,14 @@ LinearSystem BuildLinearSystem(const CorrespondenceVector &correspondences, doub auto GM_weight = [&](double residual2) { return square(kernel) / square(kernel + residual2); }; - using correspondence_iterator = CorrespondenceVector::const_iterator; + using associations_iterator = Associations::const_iterator; const auto &[JTJ, JTr] = tbb::parallel_reduce( // Range - tbb::blocked_range{correspondences.cbegin(), - correspondences.cend()}, + tbb::blocked_range{associations.cbegin(), associations.cend()}, // Identity LinearSystem(Eigen::Matrix6d::Zero(), Eigen::Vector6d::Zero()), // 1st Lambda: Parallel computation - [&](const tbb::blocked_range &r, LinearSystem J) -> LinearSystem { + [&](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); @@ -147,7 +145,7 @@ namespace kiss_icp { Sophus::SE3d Registration::AlignPointsToMap(const std::vector &frame, const VoxelHashMap &voxel_map, const Sophus::SE3d &initial_guess, - double max_distance, + double max_correspondence_distance, double kernel) { if (voxel_map.Empty()) return initial_guess; @@ -159,9 +157,9 @@ Sophus::SE3d Registration::AlignPointsToMap(const std::vector & Sophus::SE3d T_icp = Sophus::SE3d(); for (int j = 0; j < max_num_iterations_; ++j) { // Equation (10) - const auto correspondences = DataAssociation(source, voxel_map, max_distance); + const auto associations = FindAssociations(source, voxel_map, max_correspondence_distance); // Equation (11) - const auto &[JTJ, JTr] = BuildLinearSystem(correspondences, 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)