SALM-ICP from scratch learning the principles and applications together

Point "Computer Vision life" concerns, the star receives dry faster! ##

White: Brother, it is busy recently what, you have not see the shadows, our classes have not updated it for a long time

Brother: I'm sorry, sorry, busy recently mundane. Together make up behind me, learning strength you have to learn from the young man ah! Saying what you do in a recent study?

White: look at the recent ICP, speaking on fourteen simple mention, the little Bubur da

Brother: Well, today to talk about it ICP algorithm. Ha move a small bench to do:

ICP stands for Iterative Closest Point, which translates iterative closest point. ICP point cloud registration applications is very wide, so using ICP-based depth camera, laser radar algorithm relatively high frequency.

White: I was learning visual SLAM, is not without concern, ah (heart OS: SLAM after all, to learn too much)

Brother: Haha, I can not steal this lazy. Now in visual SLAM with ICP is also very much. I summed up what scenarios under the ICP:

ICP in the end what use is it?

Suppose we have two set points in three-dimensional space, we are called P1, P2 Well, P1, P2 are in thousands of points, it can be divided into two cases:

1, we do not know P1, P2 how each point is corresponding. This situation is very common, for example, I in two different locations were shot in front of a Cacalia a depth map with only a depth of depth camera or lidar point cloud and into my special meow do not know what point and which corresponds to the point, but I want these two points cloud "integration" (specialized vocabulary, called registration, registration translated into some places, people talking a little bit, I recommend translated as registration) together into a more complete Cacalia.

2, we already know which points P1, P2 in which the corresponding point. For example, we are using the RGB-D camera photographing an RGB color image + depth map in two different positions, and the color map and the depth map is good alignment. FIG color because we can do a feature point matching, because each feature point corresponding to the depth values in a depth map, we can get a good three-dimensional points corresponding to the two groups.

White: So, ICP looks pretty strong, does not correspond correspondence can get ah, ICP why such a cow fork?

ICP why such a cow fork?

Brother: take a look at the ICP process to know. For convenience, we use a two-dimensional example to illustrate it, initially smiley (dark red and green) at two different angles, here is how red smile overlap by ICP process and green smiley:

White: Good magic ah

Brother: This is just an example, under normal circumstances we will not encounter this situation in SLAM, the point is the three-dimensional space. I would like to point cloud registration as an example. Suppose there are two points to be registered clouds (such as the above small rabbit), ICP algorithm is two point clouds registration:

ICP algorithm flow

First, for each point in a point cloud, and calculating a matching point on the other point cloud (last point)
Minimizing matching error between the matching point, pose calculation
Then the calculated pose role in the point cloud
Recalculated matching point
Thus iteration, until the number of iterations reaches a threshold value, or a function of minimizing the amount of change of energy less than a set threshold value

Here is the effect of a three-dimensional point cloud of ICP

White: The first calculated error estimate is large, the back will gradually become smaller, until convergence?

Brother: ah, this is a delicate process from extensive to, by the vision algorithms coarse to fine method is used. .

White: for example, an image pyramid! Haha

Brother: ah, the familiar formula, familiar taste (/ Smile)

ICP is actually the essence of iterations, and again fix the errors, and finally get a good result.

But, he says, but in general the data have a lot of noise, if noisy, the first case not be able to complete the registration correctly.

White: It sounds reasonable, do not bother, why the book is derived so complicated?

Brother: ah, this is the emotional to the rational process. Visual SLAM say fourteen books on the mathematical derivation from how to achieve specific. I quote it here in three steps to solve the ICP:

White: the principle of derivation is probably Nengkanmingbai

Brother: er er, is derived partially left you a little homework up

White: brothers want thoughtful, in order to truly grasp it (the heart OS: Zhenhen ah ..)

Brother: The above description is the simplest point and point matching ICP algorithm, in order to make the practical application of the robust algorithm, which is under a lot of noise will still be able to get the correct result, many researchers have proposed different match ideas: for example, the minimum point of the error term comprises a Euclidean distance corresponding point, and point to the corresponding point plane distance, and color values corresponding to the error minimization other point

White: so many ways, with what is good?

Brother: If you are a simple scenario, data clean and basically no noise, the most primitive little match on the line. If there is noise, then finally consider the method of complex points. In 2003, when two papers pottman Hofer and Daniel proved when two point cloud closer, minimizing the corresponding point of the point to the plane closer to the real distance between the two planes than the distance from point to point distance, i.e. the plane to be ideal point distance calculation!

The following is a schematic

This is the principle ICP friends!

Here is the Practice

Exercise

1. Proof: "visual SLAM fourteen stresses" on page 174 Formula 7.55

See the book each symbol is as defined, wherein, TR represents the trace of a matrix.

2, a given one track, the data format: timestamp tx ty tz qx qy qz qw, custom arbitrary rotation matrix and a translation vector (can try different values, and even add some noise to see the results of any changes), for track 1 is transformed to obtain a new track 2, using the ICP algorithm (Tip: take a three-dimensional translation of the spatial points) between the posture trajectory estimated bit 1, then acting posture trajectory 2

Verify: ICP algorithm to estimate the rotation matrix and translation vector accuracy; track 2 are coincident.

Here are two tracks I added after a rotating shift amount, after ICP calculate the position and orientation of the track and then the reaction after the conversion, the final two tracks are coincident drop!

The code framework, data and expected results ready for you No public "computer vision life" backstage reply: ICP , can be obtained.

Welcome messages to discuss, learn more videos, documentation, reference answers and other concerned public computer vision, life number, scan the QR code to view "from scratch learning SLAM" Planet reports, the three-dimensional vision to seize the outlet, and other small partners come together learning exchange -

Reference article:

Gao Xiang "visual SLAM fourteen stresses"

Recommended Reading