Iterative closest point algorithm

Coarse Association and ICP

As already mentioned, the purpose of coarse association is for algorithm to get and idea which objects in the current LiDAR scan belongs to already existing objects and static background. It is based on modified version Iterative Closest Point Algorithm (ICP), which we are going to describe more closely in this section. ICP is efficient algorithm for minimizing the difference between two clusters of points. In robotics it is mainly used for reconstructing 2D or 3D surfaces from different scans, to localize robots and achieve optimal path planning. In our work we use it to align two clusters within certain distance threshold and compute rough guess of the rigid transformation between target and source clusters. If both translation and rotation between these two clusters are lower than very small, fixed threshold within multiple iteration, we say the the target cluster is corresponding to the source cluster. ICP can be very slow if we have big initial misalignment between target and source, however with scanning frequency of out LiDAR 40Hz we are making sure that the distance and misalignment between two objects can’t be so high that ICP would fail to converge quickly, given our prediction model is correct as the ICP is run right after the prediction step. Wang et. al recommends to estimate association validity based on an ICP procedure that involves a nearest neighbor estimation. This simple nearest neighbor procedure means that for many shapes, all points in one object are associated with a single point in another. This yields bad transformation estimation. This can be seen on the following figure.

The problem using nearest neighbour in ICP, as it can generate 1-n association of points, which leads to bad transformation estimation between objects. In this case point 106 in the blue object is assigned as the closest point to every point in the green object, this leads to wrong calculation of transformation between these 2 objects.

Changes to original ICP

We propose a new estimation based on minimizing the total distance between a group of pairs, where we make sure that if we assign point from one object to point in another object, these points are not considered again as candidates for another point pair. Using this approach, we have seen significant improvement compared to the [5] not only in terms of precision, but speed of convergence of ICP as well.

With this version of ICP, we align the incoming lidar scan with clusters which already has been associated to static background. After that, we calculate a transformation between the corresponding points: if the transformation (either its translation magnitude or rotation magnitude) is too large, then the association is rejected and the cluster deemed to not be a good match with the candidate object.

The resulting transformation between found point pairs from target and source cluster is done by scikit-image library using Euclidean Transformation. The result of our associations computed by ICP on clusters from figure 4 is shown figures 5.