Найближчий сусід без викидів у хмарах 2-х точок - c ++, алгоритм, найближчі сусіди, точка-хмари, виходи

Я намагаюся знайти відповідники між aточка (скажімо, виявлення) у хмарній точці в момент часу t та інша точка у хмарній точці в момент часу T! = t для оцінки руху точки (швидкості та орієнтації) у вузлі ROS (у нас немає виявлення в час T, тому я повинен знайти, що може бути точкою виявлення в кадрі, що минуло / майбутнє).
Проблема полягає в тому, що наразі я використовую простий підхід найближчого сусіда в певному радіусі, тобто

for all detections P at t
for all points of the PC at time T
take all points in PC within the radius R
find the nearest to P
compute velocity and orientation according to the different timestamps of t and T

Моя проблема полягає в тому, що використовуючи цю дуже наївнупідходити до прийнятного рішення лише в тому випадку, якщо точка виявлення та відповідна хмара досить віддалені від інших точок (в основному, якщо в радіусі немає лише шумних точок).
Що я шукав - це метод знайти найближчу сусідню точку, відкидаючи потоки, які можуть забезпечити нездійсненний модуль орієнтації та швидкості руху об'єкта.
Отже, в основному роблячи щось подібне:

for all detections P at t
for all points of the PC at time T
take all points in PC within the radius R
compute all the distances from such points to P and discard the outliers
find the group of points which has "similar" distance (even if not the minimum)
compute the centroid of such points
use the centroid to compute speed and orientation

Проблема в тому, що я не можу зрозуміти, як це зробити в c ++. Можливо, є якась функція, вже реалізована в BOOST, яка може мені допомогти?
Я думав про сортування відстаней відвсі точки в радіусі на виявлення та відкидання останньої та першої, але це не гарний спосіб продовжуватись, мабуть, і, якщо я сортую вектор відстаней, я більше не зможу отримати точки, що відповідають кожній відстані .
Сподіваюся, я добре пояснив проблему, якщо ні, то мені шкода.
Що мені знадобиться, це алгоритм "Найближчий сусід", здатний викидати шумні точки / залишки в діапазоні пошуку.

Це приклад точкової хмари, а дві маленькі хмаринки у верхній лівій частині автомобіля - це 2 пішоходи.

Відповіді:

Це дуже своєрідна хмара точок, до яких ви хочете застосувати свій алгоритм.

Ось де ваші міркування йдуть не так:

і, крім того, якщо я сортую вектор відстаней, я більше не зможу отримати точки, що відповідають кожній відстані.

За допомогою відповідної структури ви можете відстежувати вектори та точки закриття для кожної точки.

Скажімо, ви маєте справу з сусідами (n1,n2, ..) та пов’язані з ними відстані (d1, d2,...) подобається це:

point
---> n1, d1
---> n2, d2
---> n3, d3
---> n4, d4

Це означає, що кожен пункт має a vector (c об'єкт, а не рух) pair з (точка, відстань). Ось як ви могли це реалізувати:

#include <vector>
#include <iostream>

class mPoint
{
public:
mPoint();

private:
int x;
int y;
std::vector<std::pair<mPoint, float>> neighbors;
};
...

Тоді C ++ чудово обчислює і грає з предметами навколо. Удачі.