Iterative Closest Point for 2-D LIDAR Data

Question

I have two 2-D point clouds obtained from LIDAR (Light Detection And Ranging) scans at two different poses (positions and orientations) inside a circular structure, where a small object (vertical cylindrical column) is placed at a fixed location. My objective here is to match as closely as possible the two point clouds and find the planar transformation (translation and rotation) to do that. One useful technique—I believe—would be the point set registration using the ICP (Iterative Closest Point) algorithm.

The issue now is that the algorithm fails to match perfectly the two point clouds, specifically in terms of rotation because it didn't complete matching the data points related to the object inside the circle. Therefore, my question is: would this be a limitation of the ICP algorithm, or a problem in implementing it (which I doubt it since I double-checked with Matlab ICP embedded function)?

Are there other methods/algorithms that can solve this problem?

Answer 1

You can use 3D feature descriptors here to register two point clouds. I've personally used two most recent ones that performed well enough for a similar application.

Following are the references to the papers:

1. A novel binary shape context for 3D local surface description link
2. TOLDI: An effective and robust approach for 3D local shape description link

The approach is: a) Detect good keypoints from the point clouds -> b) Compute descriptors for them -> c) Match descriptors -> d) Use RANSAC to find inlier matches and compute rigid body transformation between the point clouds.

In the paper (1) above, methods for (a) and (b) are given. I found them to be robust enough in an application I used which is similar to yours. For (c), I used Hamming distance based descriptor matching algorithm BFMatcher of OpenCV (link). For (d) I developed my own RANSAC implementation and used estimateRigidTransformation method in pcl library (link).

You can follow an example implementation of point cloud registration at pcl library in this link, that uses different feature descriptors. It did not work for me, but you can give it a try.

If you need help in implementing paper (1) above, you may follow my codes in this link

Answer 2

This is what worked for me (to auto-align sparse scans, which can also be useful in SLAM when it gets lost):

1. Run a corner detector for each scan (convert the LIDAR output into a single path and run a line simplification algorithm to extract the vertexes). As an improvement, you can also detect middle-of-the-air points using a filter and create multiple paths containing only continuous points.

2. Since a single corner (POI) doesn't provide enough information for alignment, create features composed of pairs of POIs for each scan. Each feature has a >--< shape and contains the distance between the POIs, the adjacent walls' lenght and its relative angle to the line that connect the POIs. Sort them by length (between POIs).

3. Compare each feature in your current scan to all features in all other scans with about the same length (find the lower_bound and upper_bound to speed things up). Give them a cost, by similarity (between angles, walls length, compass,...), and add the matched feature lines to a vector;

4. Sort this vector and select the best candidates;
5. Create a low resolution probability map with the points from all other scans (initializing it with 0.5 and increasing the probability every time a point is "painted" in a given position)
6. For each candidate, calculate the transformation required to align the two matched lines and get the RMSD: sqrt(sum ((1-prob)^2)/n )
7. Sort by RMSD
8. Group the N best candidates to remove overlapping matches and remove bad candidates
9. As an optimization, here I create a custom map to each remaining candidate. Differently to the previous map, this one only contain points where the LIDAR position is on the same side of the wall of the scanned points. It avoids matching points that are on the other side of the walls
10. Sort again by RMSD and take the best(s) candidate(s)
11. Run a fine/local placement algorithm to calculate the final position. I use a multi-resolution probability map (created with the same technique as in item 9) and use an iterative and greedy gradient descend algorithm to snap the points to the nearest position with smaller RMSD.

Many of these steps may be optional and where implemented just to improve performance and reduce wrong matches. Using optimization techniques, I could auto-align one scan to other 50 with a very high precision in around 0.4s in today's computers.

Answer 3

Iterative closest point is iterative. You have to seed a starting match, and it will find a local minima from there. If you're getting poor results, try feeding a better seed guess or try increasing the size of the steps the algorithm is allowed to take.

Increasing the step size could be dangerous, though, because the results may not converge. It's not clear what the application of this is; I don't know if you're just doing this to do it or if you're trying to get some kind of an automatic alignment procedure to reconcile two lidar poses ("known good" and unknown, maybe?)

I would start by feeding it a better guess before looking to monkey with any of the ICP step sizes.