Feature/use kd tree for landmark map search

[PaulVerhoeckx]

Proposed changes

Implement efficient nearest-neighbor landmark search using per-category KD-trees with nanoflann. Provides O(log n) spatial queries instead of O(n) linear search for position-based landmark matching.

Tested for a particle filter with:

• 1000 particles
• 3 landmark detections
• landmark map containing 50.000 landmarks

This reduced the filter update time from 1.4 seconds to 0.005 seconds. (12th Gen Intel® Core™ i7-1255U × 12)

Changes

• Added category-based KD-tree indices for fast spatial nearest-neighbor search
• Implemented find_nearest_landmark() using KD-tree queries for efficient position-based matching
• Cached per-category indices with lazy construction during map initialization
• Added PositionCloud adapter and CategoryIndex structure for nanoflann integration
• Deprecated copy operations to prevent expensive kd-tree reconstruction

Type of change

• 🐛 Bugfix (change which fixes an issue)
• 🚀 Feature (change which adds functionality)
• 📚 Documentation (change which fixes or extends documentation)

Checklist

Put an x in the boxes that apply. This is simply a reminder of what we will require before merging your code.

• Lint and unit tests (if any) pass locally with my changes
• I have added tests that prove my fix is effective or that my feature works
• I have added necessary documentation (if appropriate)
• All commits have been signed for DCO

Additional comments

Note on find_closest_bearing_landmark() implementation:
This method currently uses O(n) linear search instead of the KD-tree indices because bearing-based matching requires angular distance metrics, which are fundamentally incompatible with the Euclidean L2 distance metric used by the position-based KD-trees.

[glpuga]

Thanks for the PR, @PaulVerhoeckx .

Notice that you can run the test suite very similarly to how CI builds and runs it by executing ./src/beluga/tools/build-and-test.sh in the development container. That way you don't need to wait for us to approve the CI run.

[PaulVerhoeckx]

Thanks for the PR, @PaulVerhoeckx .

Notice that you can run the test suite very similarly to how CI builds and runs it by executing ./src/beluga/tools/build-and-test.sh in the development container. That way you don't need to wait for us to approve the CI run.

Thanks for the tip, I think it will pass now;)

[glpuga]

Nice job! we created this model for a proof of concept demo a long time ago and we did not put a lot performance into it, it's good to see it getting some dedication.

[glpuga]

It may be worth it to do a first pass counting how many landmarks for each category are there, to reserve the size in the pts vector and that way avoid reallocations as it grows.

[glpuga]

Feels wasteful to store the landmarks along with the nanoflann index, since both contain the same information in different forms, and we only use the index later. We only use the landmarks struct to recreate the index in case of copy/move operations.

We can probably remove landmarks_ if we refactor the category_indices_ storage. Maybe we would have to split build_category_indices in two, one step to fill the pts vectors, and another separate one to create the indices.

The first one is only done in the constructor that takes in the landmarks data. The map of categories and the points for each become trivially copyable/movable in all constructor/assignment operators, and we need only build the nanoflann index from that data.

Does it make sense?

[glpuga]

@PaulVerhoeckx ignore the rolling failure. If you rebase to the current main we no longer run CI against rolling as it is a moving target.