2次元点群の外れ値のない最近隣隣

Question

私は時刻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つの小さな雲が歩行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++はオブジェクトを使って計算して再生するのに適しています。 幸運。