Adaptive point cloud registration (RANSAC, ICP)

Point cloud registration

Complete code: https://github.com/kang-0909/point-cloud-registration/tree/main, remember to give a star~

Experimental goals

Task 1: Register two point clouds of the same shape and size, and then estimate the pose between the two point clouds.
Task 2: Back-project a series of depth maps to obtain point clouds. After registration, obtain the pose transformation between each depth map and fuse the corresponding point clouds together.

Compile and run environment

The project is written in Python3 to implement RANSAC registration and ICP registration, using the open3d and numpy libraries. Runpython main.py src.ply tgt.ply save_path.ply, read source point cloudsrc.ply, target point cloudtgt.ply, save pathsave_path.ply . Three pictures will be displayed, namely the original point cloud (the source point cloud is marked in red, the target point cloud is marked in green), the coarse registration result, and the fine registration result.

algorithm design

The main ideas of Task 1 and Task 2 are the same. They both downsample and filter the point cloud and then calculate the point cloud feature information (including normal vectors, feature histograms, and several features required for subsequent feature matching), and then use the extracted The features are roughly aligned using the RANSAC method, and finally the ICP method is used for fine alignment.

Feature extraction

[PCL Study Notes] Fast Point Feature Histogram FPFH - pcl::FPFHSignature33 - Eba_

Use FPFH (Fast Point Feature Histogram) algorithm. The PFH (Point Feature Histogram) algorithm extracts geometric features (some angle information) between pairs of points in the neighborhood of a given point and draws a histogram. FPFH retains most of the information of PFH, but ignores the calculation between adjacent points. Instead, it weights the histogram of points in a neighborhood according to the inverse proportion of the distance to obtain a 33-dimensional vector, which is more efficient. I tried using PCA for dimensionality reduction to improve the speed of subsequent feature matching, but found that it had little effect, so I gave up.

Coarse registration

Using the RANSAC method, in short, for the three points in the source point cloud, "mask" them to correspond to the points in the target point cloud, and then calculate the transformation matrix to test its advantages and disadvantages. The brief process is as follows:

Randomly select 3 points in the source point cloud.
Find the points whose feature vectors are closest to each other in the target point cloud. "Close" here is in the sense of Euclidean distance, and the query data structure uses KD trees.
Computes the transformation matrix between two triangles. In fact, it is to find the optimal transformation matrix in the sense of the least squares method according to the ICP precise registration process.
Find the point cloud of the source point cloud after this transformation, and calculate the degree of coincidence with the target point cloud (defined as the number of points in the source point cloud that have the target point within the threshold radius).
Return to 1, repeat several times, maintain and return the transformation with the highest degree of coincidence.

In actual coding, due to the relatively slow query speed of KD tree in 4 (close to $O (n)$ ), due to the improvement in It is a good practice to choose the quality of triangle pairs in 1, 2. Here, the quality of the triangle pair refers to the possibility of their matching, and the second is whether the transformation matrix calculated by them is accurate. There are many methods that can be thought of, such as the sides of a triangle not being too short, the corresponding side lengths of two triangles not being too different, giving priority to points whose feature vectors are far from the mean, etc. The first two are used in my implementation. In addition, registration on the downsampled point cloud can also improve efficiency.

Fine registration

[Point Cloud Precision Registration] Iterative Closest Point (ICP) - Forrest Ding

The specific derivation process will not be described in detail.

Depth map fusion

The above algorithm can better complete the registration of point clouds with the same shape and size. However, if applied directly to the depth map, there will be a problem of mismatching (the following is a top view of the single-depth matching result):

Insert image description here

This is because ICP finds the closest point from the target point cloud to match it for each point in the source point cloud. But in the depth map, there are many points in the two point clouds that are inherently mismatched. If they are forced to match, then the optimal solution in the sense of the least squares method is as shown in the picture above. One point cloud is moved to the "center" position of the other.

The improvement method is that for a point in the source point cloud, if there is no point with a distance smaller than a certain threshold in the target point cloud, then the point will not be considered in this iteration. At the same time, this threshold continues to decrease (has a lower limit) as the iteration proceeds, which makes the registration more and more refined.

key code

RANSAC

There are two more details involved in the implementation.

First, what is the appropriate setting for the matching threshold? Since different data have different sizes, we want to get the average distance of the point cloud (on a surface). A very violent method is used here, considering voxel downsampling oldPcd.voxel_down_sample(voxel_size). After sampling to a certain extent, the average distance between dense point clouds is voxel_size. Therefore set the matching threshold for RANSAC to 2.5 * voxel_size.

Second, how to determine the difference between two triangles? In the code, if the difference in corresponding side lengths is greater than lengthThreshold * average length.

In this way, 50 effective samples can calculate a good transfer matrix. As can be seen from the effect below, even the coarse registration effect is very good.

def RANSAC():
    maxCount = 0
    jisuan=0
    j = 0
    print("RANSACing...")
    while True:
        j += 1
        // 随机选取三个点
        srcCorr = random.sample(range(srcNum), 3)
        // 如果某两个点距离太近，舍弃
        if not notTooClose3([srcPoints[x] for x in srcCorr]):
            continue
            
        // KD 树特征匹配
        tgtCorr = []
        for id in srcCorr:
            k, idx, dis2 = tgtFpfhKDT.search_knn_vector_xd(srcFpfh[id], knn=1)
            tgtCorr.append(random.choice(idx[0]))
            
        // 如果两个三角形差距太大，也舍弃
        if True in [farAway(srcPoints[i[0]] - srcPoints[j[0]], 
                            tgtPoints[i[1]] - tgtPoints[j[1]]) 
                    for i in zip(srcCorr, tgtCorr) 
                    for j in zip(srcCorr, tgtCorr)]:
            continue
        jisuan += 1
        
        // 得到变换矩阵
        R, T = calculateTrans(np.array([srcPoints[i] for i in srcCorr]), 
                              np.array([tgtPoints[i] for i in tgtCorr]))
        A = np.transpose((R @ srcPoints.T) + np.tile(T, (1, srcNum)))
        
        //计算匹配数
        count = 0
        for point in range(0, srcNum, 1):
            k, idx, dis2 = tgtKDT.search_hybrid_vector_3d(A[point], 
                                                          radius=fitThreshold, max_nn=1)
            count += k
        
        // 更新最大匹配
        if count > maxCount:
            maxCount = count
            bestR, bestT = R, T
        if jisuan > 50 and j > 1000:
            break
        
    print("RANSAC calculated %d times, maximum matches: %d" % (jisuan, maxCount))
    return bestR, bestT

ICP

def ICP(src, tgt):
    limit = fitThreshold
    retR = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
    retT = np.array([[0], [0], [0]])
    trace = []
    for _ in range(400):
        tgtCorr = []
        srcCorr = []
        // 筛选距离近的点对
        for point in src:
            k, idx, dis2 = tgtKDT.search_knn_vector_3d(point, knn=1)
            if dis2[0] < (limit)**2:
                srcCorr.append(point)
                tgtCorr.append(tgt[idx[0]])
        trace.append([limit, len(srcCorr)])
        R, T = calculateTrans(np.array(srcCorr), np.array(tgtCorr))
        retR = R @ retR
        retT = R @ retT + T
        src = np.transpose((R @ src.T) + np.tile(T, (1, srcNum)))
        // 阈值的衰减
        limit = (limit - fitThreshold/1.5) * 0.95 + fitThreshold/1.5
        // 自适应过程，如果匹配点对不变化说明算法收敛，退出迭代
        if len(trace) > 50 and len(set([x[1] for x in trace[-20:]])) == 1:	
            break
    print("ICP trace is:", trace)
    return retR, retT

Effect

In all test data, the algorithm can achieve satisfactory results. Three groups of data including airplane, person and single-depth are selected for display. In addition, if the original data is directly registered, the source point cloud and the target point cloud will completely overlap, and only a single color will be displayed, which is not conducive to the observation effect. Therefore, the algorithm does an appropriate amount of downsampling for each set of data and then performs registration, so it can be seen that it is the result of matching two point clouds.

airplane:

Before registration:
Insert image description here