Skip to content

Commit

Permalink
Integrate Nacho's and Ben comments
Browse files Browse the repository at this point in the history
  • Loading branch information
tizianoGuadagnino committed Aug 8, 2024
1 parent bb58b89 commit ade1479
Showing 1 changed file with 6 additions and 6 deletions.
12 changes: 6 additions & 6 deletions cpp/kiss_icp/core/VoxelHashMap.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -56,14 +56,14 @@ std::tuple<Eigen::Vector3d, double> VoxelHashMap::GetClosestNeighbor(
// Find the nearest neighbor
Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero();
double closest_distance = std::numeric_limits<double>::max();
auto closer = [&query](const auto &lhs, const auto &rhs) {
return (lhs - query).norm() < (rhs - query).norm();
};
std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &qv) {
auto search = map_.find(qv);
std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query_voxel) {
auto search = map_.find(query_voxel);
if (search != map_.end()) {
const auto &points = search.value();
const auto &neighbor = *std::min_element(points.cbegin(), points.cend(), closer);
const Eigen::Vector3d &neighbor = *std::min_element(
points.cbegin(), points.cend(), [&](const auto &lhs, const auto &rhs) {
return (lhs - query).norm() < (rhs - query).norm();
});
double distance = (neighbor - query).norm();
if (distance < closest_distance) {
closest_neighbor = neighbor;
Expand Down

0 comments on commit ade1479

Please sign in to comment.