Nacho/strip nn search from voxel hash map

[nachovizzo]

Refactor Registration Module and VoxelHashMap to allow for better configurable system

Supporting #252, this PR addresses the following:

1. Make Registration Module a class

To make the registration system "configurable" I wrapped the only function into a single class, that has (for now) just two parameters. The rest remains the same (from a logical point of view). I also changed names accordingly

struct Registration {
    explicit Registration(int max_num_iteration, double convergence_criterion)
        : max_num_iterations_(max_num_iteration), convergence_criterion_(convergence_criterion) {}

    // Register a point cloud to the given internal map representation. The config input parameter
    // contains all the necessary parametrization for the ICP loop
    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_;
};

Following the idea of #252, max_num_threads would be a registration configuration

2. Refactor VoxelHashMap, extract NN search from map representation

I extracted the NN search from the internal map representation, removing the responsibility of the map representation of providing the NN search. Instead I exposed two new functions that can be used for "other things", and thus, are a bit more generic:

VoxelHashMap::GetAdjacentVoxel: Giving a point, spits the adjacent voxels in voxel space. This can be used independently of NN search or not. Additionally, I added a (default to 1) parameter to express the intention of that floating number we had in the code

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;
}

VoxelHashMap::GetPoints(const std::vector &query_voxels) const; Given some voxels, it doesn't need to be adjacent or not, extract the the (if existing) points inside those voxels.

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 points;
}

Using those two voxel hash member functions, we can now build this utility function that performs the NN search independently of the map representation and it's easier to read in general:

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;
}

Our approach for NN search is much clearer now, and responsibilities are properly spread among the system modules. I could rewrite the few lines of GetClosestNeighbor and test a completely different strategy

3. New configuration exposed

Now, we can control OPTIONALLY, and DISABLED BY DEFAULT TO CHANGE these params from the registration, config/advanced.yaml:

registration:
  max_num_iterations: 500 # <- optional
  estimation_threshold: 0.0001 # <- optional

the max_num_threads will come after #252

4. Additional Stuff by @tizianoGuadagnino

There was a bit of refactoring happening here, that is not associated with the changes of the PR but I don't have time to rebase new branches on top of this, namely:

• 2b79370
• d834170

Rest

The rest of the changes are just refactoring changes. We can now control the entire TBB pipeline in the Registration module, which is super nice for me :)

Bonus

From a system point of view, it's nice that we now have this design:

// KISS-ICP pipeline modules
Registration registration_;
VoxelHashMap local_map_;
AdaptiveThreshold adaptive_threshold_;

Makes me wonder if we shouldn't merge Deskew into Preprocessing and convert it to another component as well 👀 , similar to how we do it in Python 🤔 , but to be discussed later

[benemer]

Overall, I like the draft! Make a lot more sense to me now to have a voxel hash map which is just a voxel hash map caring about the closest point given a query.

[nachovizzo]

Overall, I like the draft! It makes a lot more sense to me now to have a voxel hash map which is just a voxel hash map caring about the closest point given a query.

I'm still wondering if VoxelHashMap::GetClosestNeighbor should belong there or not. But looks like, since it's using the underlying map parameters (voxel_size_, and max_points_per_voxel_), and also the underlying hash table representation (map_). But I'm still not 100% convinced. If we have this function in the registration class, it's also doable by calling voxel_map.voxel_size_, and so on).

What's your view on this? I'm asking since the VoxelHashMap type without this function it's a much cleaner and smaller class. Nothing tells me that a voxel hash map should spit nearest neighbors... But again, can't decide which is better 🤣

[benemer]

I like it like this. The underlying map representation needs to provide a way to query for the closest neighbor. The way we do it is a specific realization by checking the neighboring voxels, which is an approximation of finding the closest neighbor. You might also want to provide a map that finds the actual closest neighbor (which can be further away) or a map that does an even faster nearest neighbor search by using another data structure or by approximating further, checking for example only the same voxel.

Since the implementation of this query is specific to the map, I think it should be a member function of the map class.

But I am happy to hear other views :)

[tizianoGuadagnino]

Hi both,

I slowly passing through each file, and I found some further ways of simplifying code. For now I have been working on the Registration.cpp. Two news:

1. ResultTuple/LinearSystemRes Rhaus. Now we just use a std::pair and define a lambda for the plus operator. I found this to be more clean and easy to read. I will like to hear both of you on this @nachovizzo @benemer .
2. I have transformed CorrespondeceSet from struct with two vectors to a vector of pairs, since now it will be used just in this cpp I belive it is fine. I think also in this case this further improves readability, but also here you tell me.

I still want to go to the rest properly but for now I wanted to show you this changes and see.

<3

[tizianoGuadagnino]

I am not super convinced about moving the GetCorrespondences outside of theVoxelHashMap. Also exposing a function that query for a single point is not so nice/useful for the user if he wants to use this map for something else (if we search for more than one point we need to rewrite the all for loop).

[nachovizzo]

@tizianoGuadagnino @benemer I wrote a README on this PR, please go over it on this 2nd type of iteration. I'm in favor of moving TechDebt stuff out of the PR (got huge) to move forward

[tizianoGuadagnino]

I think we can move forward for now, me and @benemer are not super happy with VoxelHashMap::GetAdjacentVoxels as we found it to be an over-generalization against the KISS principle. For now lets merge this so that other stuff can move forward. We can revert back after discussing in a separate PR. Moving desking into preprocessing looks amazing to me but lets talk about it later. I will do also a proper renaming in a separate PR.

[nachovizzo]

Amazing guys! Thanks, I have a quick fix for AdjacentVoxels thing that I'm sure you will like