Simplify GetClosestPoint

[tizianoGuadagnino]

Sorry guys, this was a piece of the previous PR that I split away. Just a small simplification, but it looks better to my eyes.

[nachovizzo]

I don't see much where the simplification is, and yet again, I fail to see the motivation for this change.

I'm approving as you probably went further than 15 branches, which would be hard to revert.

But as a measure, try not to think about this module for two days, then read the current implementation and tell me if it's readable.

std::tuple<Eigen::Vector3d, double> VoxelHashMap::GetClosestNeighbor(
    const Eigen::Vector3d &query) const {
    // Get nearby voxels on the map
    const auto &query_voxels = GetAdjacentVoxels(query, *this);
    // Find the nearest neighbor
    Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero();
    double closest_distance = std::numeric_limits<double>::max();
    std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query_voxel) {
        const auto &points = map_.at(query_voxel);
        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;
            closest_distance = distance;
        }
    });
    return std::make_tuple(closest_neighbor, closest_distance);
}

As a baseline, in the last release, v1.0.0 this is the implementation we had

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 = kiss_icp::PointToVoxel(point, voxel_map.voxel_size_);
    // Get nearby voxels on the map
    const auto &query_voxels = GetAdjacentVoxels(voxel);
    // Extract the points contained within the neighborhood voxels
    const auto &neighbors = voxel_map.GetPoints(query_voxels);

    // Find the nearest neighbor
    Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero();
    double closest_distance = std::numeric_limits<double>::max();
    std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) {
        double distance = (neighbor - point).norm();
        if (distance < closest_distance) {
            closest_neighbor = neighbor;
            closest_distance = distance;
        }
    });
    return std::make_tuple(closest_neighbor, closest_distance);
}

Sometimes, I feel like it's a race to get the fewest lines of code while maximizing the number of STL algorithms we use. I don't particularly enjoy where this is going, but let's be democratic, and as Benedikt already approves, let's move forward.

[nachovizzo]

This is relatively unsafe. I know that up here, you only provide the voxels that already exist in the grid, but within this method, nothing prevents us from making a mistake; in that case, we will get an exception

TSL_RH_THROW_OR_TERMINATE(std::out_of_range, "Couldn't find key.");

[nachovizzo]

Why pass this as an argument? How should I read this line?

    const auto &query_voxels = GetAdjacentVoxels(query, *this);

[tizianoGuadagnino]

Sorry for changing stuff, you are right we should revert everything