c++ - 二维点云中没有异常值的最近邻

Question

我试图找到时间 t 点云中的一个点(假设检测)与时间 T != t 点云中另一个点之间的对应关系，以估计点的运动(速度和方向)在 ROS 节点中(我们在时间 T 没有检测，所以我必须找到哪个可以是最近/ future 帧中的检测点)。
问题是目前我在一定半径内使用简单的最近邻方法，即

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 中已经实现了一些可以帮助我的功能？
我正在考虑对从半径中所有点到检测点的距离进行排序，并丢弃最后一个和第一个，但我猜这不是继续进行的好方法，而且如果我对距离 vector 进行排序，我将无法再进行检索对应于每个距离的点。
希望我能很好地解释问题，否则我很抱歉。
我需要的应该是能够丢弃搜索范围内的噪声点/异常值的最近邻算法。

这是一个点云的例子，汽车左上方的两个小云是 2 个步行的行人。

Answer 1

这是一种非常特殊的点云，您希望将您的算法应用到其中。

这就是你的推理出错的地方:

and furthermore if I sort the vector of distances I will no more be able to retrieve the points corresponding to each distance.

通过适当的结构，您可以跟踪 vector 并为每个点关闭点。

假设您有一个点与邻居 (n1,n2,..) 和相关距离 (d1, d2 ,...) 像这样:

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

这意味着每个点都有一个 vector(c 对象，不是移动)pair of (point, distance)。以下是您可以如何实现它:

#include <vector>
#include <iostream>

class mPoint
{
  public:
    mPoint();

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

然后 C++ 非常适合计算和处理周围的对象。祝你好运。