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]

@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.

[glpuga]

LGTM