Nacho/get rid of get adjacent voxels

[nachovizzo]

Follow up from #290

New result: reads much better

Eigen::Vector3d GetClosestNeighbor(const Eigen::Vector3d &point,
                                   const kiss_icp::VoxelHashMap &voxel_map) {
    // Convert the point to voxel coordinates
    const Voxel voxel(static_cast<int>(point[0] / voxel_map.voxel_size()),
                      static_cast<int>(point[1] / voxel_map.voxel_size()),
                      static_cast<int>(point[2] / 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;
    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;
}