A point cloud Registration (registration) ICP algorithm in PCL

Introduction

Given two sets of three-dimensional data points from different coordinate systems, the transformation relationships of two point set spaces are found, so that the two point sets can be unified into the same coordinate system, that is, the registration process. The goal of the registration is to find the relative position and direction of the separately acquired views in the global coordinate frame, so that the intersecting areas between them are completely overlapping. For each set of point cloud data that gets from different views, you need a single point cloud model that aligns them together, allowing you to apply subsequent processing steps, such as segmentation and object refactoring.
This blog post is based on the ICP algorithm in PCL, which realizes the registration of different view point cloud data and extends relevant knowledge. Registration Model

The registration of two (pairwise) point cloud Datasets that rotate and pan on one of the data so that the intersecting areas between them are fully coincident. The output here is a (4x4) rigid (rigid) transformation matrix.

Two data set calculation steps:
1. Identify points of interest (interest points) (i.e. key points keypoints) that best represent the scene in the two data sets
2. At each key point, calculate the feature descriptor;
3. From the set of feature descriptors and their x, Y, Z and y locations in two datasets, the corresponding relationships are estimated based on similarity between features and locations;
4. Assuming that the data is considered to contain noise, not all correspondence is valid, so the negative impact correspondence that negatively affects the registration process is discarded;
5. From the remaining set of good correspondence, estimate a transformation behavior. Key points

Feature points in a point cloud are points of interest, such as point or point sets with distinct characteristics (feature), such as the point of destination, the corner point, and so on. Feature Description

Based on the key points of discovery, the feature combination information is extracted and the vectors are generated to compare. Conformance estimation correspondence estimate

Given two sets of data to get the corresponding feature description vector, find the corresponding feature to find the overlapping parts in the data. Discard bad Consistency estimates

Not all consistency estimates are correct, and the consistency of the errors negatively affects the transformation and therefore needs to be discarded. Transform Transformation

Steps:
1. Error measurement based on conformance evaluation (error metric)
2. Estimating the Transformation matrix
3. Optimize point structure iteration nearest iterative Closest point (ICP)

The ICP algorithm is essentially an optimal registration method based on least squares. The algorithm repeats the selection of corresponding relation point pairs and calculates the optimal rigid body transformation process until the convergence accuracy of the correct registration is satisfied. ICP is a widely used registration algorithm, the main purpose is to find the rotation and peace of the parameters, the two different coordinate system of point cloud, one point cloud coordinate system as a global coordinate system, the other point cloud after the rotation and movement of two sets of point cloud coincident parts completely overlap. The ICP algorithm was first proposed by Chen and Medioni,and Besl and McKay.

The input of the algorithm: reference point cloud and Target Point cloud, stop the standard of iteration.
The output of the algorithm: the rotation and translation matrix, which is the transformation matrix. The basic process matches the nearest point in the reference point cloud (or selected collection) for each point in the target point cloud. In order to calculate the minimum mean of the RMS (root square,rms), the translational and rotational parameters of the corresponding points are obtained. Use the obtained transformation matrix to transform the target point cloud. Iterate (re-associate points) until the condition of the terminating iteration is met (the number of iterations or the error is less than the threshold value). The error here is the smallest, and can be the absolute value of the adjacent two RMS difference is less than a certain limit. the possible problem algorithm converges to the local minimum error. Noise or anomalous data can cause the algorithm to fail to converge or error. In the first step of the ICP algorithm to determine an iterative initial value, the selected initial value will have a significant impact on the final registration results, if the initial selection is not appropriate, the algorithm may be limited to local optimization, so that the iteration can not converge to the correct registration results. Rigid (rigid) transformation matrices

The rotation matrix is the matrix that performs the rotation transformation in the European Space, the ICP algorithm makes the point cloud with the time, the transformation matrix contains the translation and the rotation, therefore, it is necessary to study the coordinate system translation and rotation process. The three-dimensional transformation is similar to the two-dimensional transformation, using homogeneous coordinate technique to describe the coordinates of each point of space and its transformation, the transformation matrix describing the three-dimensional transformation of space is a 4x4 form. Thus, a series of transformations can be represented by a single matrix.

The three axes of the coordinate system, X, Y, z, x and y, respectively, are rotation axes, and the points are actually only two-dimensional rotations on the vertical axis. The three-dimensional rotation transformation matrix can be directly introduced with the two-dimension rotation formula. In the right-hand coordinate system, the positive direction of the object rotation is the right-hand helix direction, that is, from the positive half-axis of the axis to the origin of the counterclockwise direction.

Two-angle and difference formula:
Sin (α + β) =sinα⋅cosβ+cosα⋅sinβsin (α−β) =sinα⋅cosβ−cosα⋅sinβcos (α + β) =cosα⋅cosβ−sinα⋅sinβcos (α−β) =cosα⋅cosβ+ sinα⋅sinβ \sin (\alpha +\beta)=\sin \alpha \cdot \cos \beta + \cos \alpha \cdot \sin \beta\\ \sin (\alpha -\beta)=\sin \al pha \cdot \cos \beta - \cos \alpha \cdot \sin \beta\\ \cos (\alpha +\beta)=\cos \alpha \cdot \cos \beta - \sin \alpha \cdo t \sin \beta\\ \cos (\alpha -\beta)=\cos \alpha \cdot \cos \beta + \sin \alpha \cdot \sin \beta\\
Two-dimensional coordinate rotation, the derivation process:
Around the origin of the coordinates, the radius r R rotates at a certain angle α\alpha, as shown in the following figure, the point (x, Y) (x, y) is rotated counterclockwise through the rotation Α\alpha transform (x′,y′), the known rotation angle α\alpha and point (x, Y) (x, y) coordinates, calculate the point (x′,y′) (x ', y ')

X′=r⋅cos (α + β) =r⋅