Skip to content

Commit

Permalink
Nacho/strip nn search from voxel hash map (#290)
Browse files Browse the repository at this point in the history 
* First draft on core library

* Update python API

* Rearange

* Remove type alias

* Fix build

* Wrap constants into a configuration struct

* Split the watters

* It's all about drafts

* Update python API

I'm alredy regreting about this

* Some renaming just because

* Changing names trying to auto-convince myself...

* Fix c++ build

* rename function

* renaming variables

* renaming, should be one neighbor only

* Tizianified a little bit

* Draft on voxelhashmap

* Rename

* Rename Correspondences -> Associations

Otherwise too much mix a match on my opinion

* Move stuff around only

* Remove redunant name

* They are not there, we need to find them!

* Shrink

* Tiziano shows to guys -> with for_each

* Tiziano shows to guys -> with transform_reduce....sexy

* Consistent naming

* Bring comments for readbilty

* rename variable

* Sacrifice name for one-liner

* AlignCloudToMap -> AlignPointsToMap

* Make rename like a book on ProbRob

* Revert "Make rename like a book on ProbRob"

This reverts commit 7a45c9d.

* estimation_threshold -> convergence_criterion

Requested by benedikt

* Rename threshold also here

* Remove comment

---------

Co-authored-by: Benedikt Mersch <benedikt.mersch@gmail.com>
Co-authored-by: tizianoGuadagnino <frevo93@gmail.com>
  • Loading branch information
@nachovizzo @benemer @tizianoGuadagnino
3 people authored Mar 5, 2024
1 parent b217173 commit 5600e7a
Show file tree
Hide file tree
Showing 13 changed files with 212 additions and 201 deletions.
4 changes: 4 additions & 0 deletions config/advanced.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,3 +16,7 @@ adaptive_threshold:
fixed_threshold: 0.3 # <- optional, disables adaptive threshold
# initial_threshold: 2.0
# min_motion_th: 0.1

registration:
max_num_iterations: 500 # <- optional
convergence_criterion: 0.0001 # <- optional
139 changes: 88 additions & 51 deletions cpp/kiss_icp/core/Registration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,89 +27,126 @@

#include <algorithm>
#include <cmath>
#include <numeric>
#include <sophus/se3.hpp>
#include <sophus/so3.hpp>
#include <tuple>

#include "VoxelHashMap.hpp"

namespace Eigen {
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 Associations = std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>>;
using LinearSystem = std::pair<Eigen::Matrix6d, Eigen::Vector6d>;

namespace {

inline double square(double x) { return x * x; }

struct ResultTuple {
ResultTuple() {
JTJ.setZero();
JTr.setZero();
}

ResultTuple operator+(const ResultTuple &other) {
this->JTJ += other.JTJ;
this->JTr += other.JTr;
return *this;
}

Eigen::Matrix6d JTJ;
Eigen::Vector6d JTr;
};

void TransformPoints(const Sophus::SE3d &T, std::vector<Eigen::Vector3d> &points) {
std::transform(points.cbegin(), points.cend(), points.begin(),
[&](const auto &point) { return T * point; });
}

constexpr int MAX_NUM_ITERATIONS_ = 500;
constexpr double ESTIMATION_THRESHOLD_ = 0.0001;
Eigen::Vector3d GetClosestNeighbor(const Eigen::Vector3d &point,
const kiss_icp::VoxelHashMap &voxel_map) {
const auto &query_voxels = voxel_map.GetAdjacentVoxels(point);
const auto &neighbors = voxel_map.GetPoints(query_voxels);
Eigen::Vector3d closest_neighbor;
double closest_distance2 = std::numeric_limits<double>::max();
std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) {
double distance = (neighbor - point).squaredNorm();
if (distance < closest_distance2) {
closest_neighbor = neighbor;
closest_distance2 = distance;
}
});
return closest_neighbor;
}

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;
Associations associations;
associations.reserve(points.size());
associations = tbb::parallel_reduce(
// Range
tbb::blocked_range<points_iterator>{points.cbegin(), points.cend()},
// Identity
associations,
// 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) {
res.emplace_back(point, closest_neighbor);
}
}
return res;
},
// 2nd lambda: Parallel reduction
[](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 associations;
}

std::tuple<Eigen::Matrix6d, Eigen::Vector6d> BuildLinearSystem(
const std::vector<Eigen::Vector3d> &source,
const std::vector<Eigen::Vector3d> &target,
double kernel) {
auto compute_jacobian_and_residual = [&](auto i) {
const Eigen::Vector3d residual = source[i] - target[i];
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();
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 = [&](double residual2) { return square(kernel) / square(kernel + residual2); };

using associations_iterator = Associations::const_iterator;
const auto &[JTJ, JTr] = tbb::parallel_reduce(
// Range
tbb::blocked_range<size_t>{0, source.size()},
tbb::blocked_range<associations_iterator>{associations.cbegin(), associations.cend()},
// Identity
ResultTuple(),
LinearSystem(Eigen::Matrix6d::Zero(), Eigen::Vector6d::Zero()),
// 1st Lambda: Parallel computation
[&](const tbb::blocked_range<size_t> &r, ResultTuple J) -> ResultTuple {
auto Weight = [&](double residual2) {
return square(kernel) / square(kernel + 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<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);
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);

return std::make_tuple(JTJ, JTr);
return {JTJ, JTr};
}
} // namespace

namespace kiss_icp {

Sophus::SE3d RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
const VoxelHashMap &voxel_map,
const Sophus::SE3d &initial_guess,
double max_correspondence_distance,
double kernel) {
Sophus::SE3d Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &frame,
const VoxelHashMap &voxel_map,
const Sophus::SE3d &initial_guess,
double max_correspondence_distance,
double kernel) {
if (voxel_map.Empty()) return initial_guess;

// Equation (9)
Expand All @@ -118,19 +155,19 @@ Sophus::SE3d RegisterFrame(const std::vector<Eigen::Vector3d> &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 associations = FindAssociations(source, voxel_map, max_correspondence_distance);
// Equation (11)
const auto &[JTJ, JTr] = BuildLinearSystem(src, tgt, 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)
TransformPoints(estimation, source);
// Update iterations
T_icp = estimation * T_icp;
// Termination criteria
if (dx.norm() < ESTIMATION_THRESHOLD_) break;
if (dx.norm() < convergence_criterion_) break;
}
// Spit the final transformation
return T_icp * initial_guess;
Expand Down
18 changes: 13 additions & 5 deletions cpp/kiss_icp/core/Registration.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,17 @@

namespace kiss_icp {

Sophus::SE3d RegisterFrame(const std::vector<Eigen::Vector3d> &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)
: max_num_iterations_(max_num_iteration), convergence_criterion_(convergence_criterion) {}

Sophus::SE3d AlignPointsToMap(const std::vector<Eigen::Vector3d> &frame,
const VoxelHashMap &voxel_map,
const Sophus::SE3d &initial_guess,
double max_correspondence_distance,
double kernel);

int max_num_iterations_;
double convergence_criterion_;
};
} // namespace kiss_icp
119 changes: 28 additions & 91 deletions cpp/kiss_icp/core/VoxelHashMap.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,107 +22,43 @@
// SOFTWARE.
#include "VoxelHashMap.hpp"

#include <tbb/blocked_range.h>
#include <tbb/parallel_reduce.h>

#include <Eigen/Core>
#include <algorithm>
#include <limits>
#include <tuple>
#include <utility>
#include <vector>

// This parameters are not intended to be changed, therefore we do not expose it
namespace {
struct ResultTuple {
ResultTuple(std::size_t n) {
source.reserve(n);
target.reserve(n);
}
std::vector<Eigen::Vector3d> source;
std::vector<Eigen::Vector3d> target;
};
} // namespace

namespace kiss_icp {

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<int>(point[0] / voxel_size_);
auto ky = static_cast<int>(point[1] / voxel_size_);
auto kz = static_cast<int>(point[2] / voxel_size_);
std::vector<Voxel> 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::Voxel> VoxelHashMap::GetAdjacentVoxels(const Eigen::Vector3d &point,
int adjacent_voxels) const {
auto kx = static_cast<int>(point[0] / voxel_size_);
auto ky = static_cast<int>(point[1] / voxel_size_);
auto kz = static_cast<int>(point[2] / voxel_size_);
std::vector<Voxel> voxel_neighborhood;
for (int i = kx - adjacent_voxels; i < kx + adjacent_voxels + 1; ++i) {
for (int j = ky - adjacent_voxels; j < ky + adjacent_voxels + 1; ++j) {
for (int k = kz - adjacent_voxels; k < kz + adjacent_voxels + 1; ++k) {
voxel_neighborhood.emplace_back(i, j, k);
}
}
}
return voxel_neighborhood;
}

using Vector3dVector = std::vector<Eigen::Vector3d>;
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<double>::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<Eigen::Vector3d>::const_iterator;
const auto [source, target] = tbb::parallel_reduce(
// Range
tbb::blocked_range<points_iterator>{points.cbegin(), points.cend()},
// Identity
ResultTuple(points.size()),
// 1st lambda: Parallel computation
[max_correspondance_distance, &GetClosestNeighboor](
const tbb::blocked_range<points_iterator> &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);
}
std::vector<Eigen::Vector3d> VoxelHashMap::GetPoints(const std::vector<Voxel> &query_voxels) const {
std::vector<Eigen::Vector3d> points;
points.reserve(query_voxels.size() * static_cast<size_t>(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);
}
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<Eigen::Vector3d> VoxelHashMap::Pointcloud() const {
Expand All @@ -137,13 +73,14 @@ std::vector<Eigen::Vector3d> VoxelHashMap::Pointcloud() const {
return points;
}

void VoxelHashMap::Update(const Vector3dVector &points, const Eigen::Vector3d &origin) {
void VoxelHashMap::Update(const std::vector<Eigen::Vector3d> &points,
const Eigen::Vector3d &origin) {
AddPoints(points);
RemovePointsFarFromLocation(origin);
}

void VoxelHashMap::Update(const Vector3dVector &points, const Sophus::SE3d &pose) {
Vector3dVector points_transformed(points.size());
void VoxelHashMap::Update(const std::vector<Eigen::Vector3d> &points, const Sophus::SE3d &pose) {
std::vector<Eigen::Vector3d> 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();
Expand Down
Loading

0 comments on commit 5600e7a

Please sign in to comment.