Skip to content

Commit

Permalink
Revert "Make rename like a book on ProbRob"
Browse files Browse the repository at this point in the history
This reverts commit 7a45c9d.
  • Loading branch information
nachovizzo committed Mar 5, 2024
1 parent 7a45c9d commit 26dd7b0
Showing 1 changed file with 22 additions and 24 deletions.
46 changes: 22 additions & 24 deletions cpp/kiss_icp/core/Registration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ using Matrix6d = Eigen::Matrix<double, 6, 6>;
using Matrix3_6d = Eigen::Matrix<double, 3, 6>;
using Vector6d = Eigen::Matrix<double, 6, 1>;
} // namespace Eigen
using CorrespondenceVector = std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
using Associations = std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
using LinearSystem = std::pair<Eigen::Matrix6d, Eigen::Vector6d>;

namespace {
Expand All @@ -66,43 +66,42 @@ Eigen::Vector3d GetClosestNeighbor(const Eigen::Vector3d &point,
return closest_neighbor;
}

CorrespondenceVector DataAssociation(const std::vector<Eigen::Vector3d> &points,
const kiss_icp::VoxelHashMap &voxel_map,
double max_correspondance_distance) {
Associations FindAssociations(const std::vector<Eigen::Vector3d> &points,
const kiss_icp::VoxelHashMap &voxel_map,
double max_correspondance_distance) {
using points_iterator = std::vector<Eigen::Vector3d>::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_iterator>{points.cbegin(), points.cend()},
// Identity
correspondences,
associations,
// 1st lambda: Parallel computation
[&](const tbb::blocked_range<points_iterator> &r,
CorrespondenceVector res) -> CorrespondenceVector {
[&](const tbb::blocked_range<points_iterator> &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();
Expand All @@ -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<correspondence_iterator>{correspondences.cbegin(),
correspondences.cend()},
tbb::blocked_range<associations_iterator>{associations.cbegin(), associations.cend()},
// Identity
LinearSystem(Eigen::Matrix6d::Zero(), Eigen::Vector6d::Zero()),
// 1st Lambda: Parallel computation
[&](const tbb::blocked_range<correspondence_iterator> &r, LinearSystem J) -> LinearSystem {
[&](const tbb::blocked_range<associations_iterator> &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);
Expand All @@ -147,7 +145,7 @@ namespace kiss_icp {
Sophus::SE3d Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &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;

Expand All @@ -159,9 +157,9 @@ Sophus::SE3d Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &
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)
Expand Down

0 comments on commit 26dd7b0

Please sign in to comment.