CODEWITHSUNDEEP
c++algorithmnearest-neighborpoint-cloudsoutliers
Jacopo Lottero
Jacopo Lottero

Reputation: 190

Nearest Neighbor without outliers in 2d point cloud

I am trying to find correspondances between a point (let say a detection) in a point cloud at time t and another point in a point cloud at time T != t to estimate the motion of the point (speed and orientation) in a ROS node (we do not have detections at time T so I have to find which can be the point of the detection in an immediately past/future frame).
The problem is that for the time being I am using a simple nearest neighbor approach in a certain radius, i.e. 

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

My problem is that by using this very naive approach a get satisfiable solution only if the detection point and the corresponding cloud are well distant from other points ( basically if no noisy alone points are in the radius).
What I was looking for is a method to find the nearest neighbor point discarding the outliers which can provide a non feasible orientation and speed module for the motion of the object.
So, basically doing something like that:

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

The problem is that I cannot figure out how to do it in c++. Maybe there are some function already implemented in BOOST which can help me?
I was thinking about sorting the distances from all the points in the radius to the detection and discard the last and the first but this is not a good way to proceed I guess and furthermore if I sort the vector of distances I will no more be able to retrieve the points corresponding to each distance.
Hope I explained the problem well, if not I am sorry.
What I would need should be a Nearest Neighbor algorithm capable of discarding the noisy points/outliers in the search range.

This is how my point cloud looks like

This is an example of point cloud and the two small clouds on the top left of the car are 2 pedestrians walking.

Upvotes: 0

Views: 769

Answers (1)

J. Chomel
J. Chomel

Reputation: 8393

It is a very peculiar kind of cloud of points you want to apply your algorithm to.

This is where your reasoning goes wrong:

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

With the appropriate structure, you can track vectors and close points for each point.

Let's say you have a point with neighbors (n1,n2,..) and associated distances (d1, d2,...) like this:

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

This means each point has a vector (c object, not movement) of pair of (point, distance). Here is how you could implement it:

#include <vector>
#include <iostream>

class mPoint
{
  public:
    mPoint();

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

Then C++ is great to compute and play with objects around. Good luck.

Upvotes: 1

Related Questions