在2d点云中没有异常值的最近邻

Question

我试图找到时间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个行人走路。

Answer 1

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

这是你的推理出错的地方：

  

此外，如果我对距离矢量进行排序，我将无法再检索与每个距离对应的点。

使用适当的结构，您可以跟踪每个点的向量和闭合点。

我们假设您与邻居（n1，n2，...）和相关距离（d1，d2，... ）像这样：

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

这意味着每个点都有一个vector（c对象，而非移动）pair（点，距离）。以下是如何实现它：

#include <vector>
#include <iostream>

class mPoint
{
  public:
    mPoint();

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

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