ICP配准_o3d.pipelines.registration.registration_icp

点云配准可以分为粗配准（Coarse Registration）和精配准（Fine Registration）两步。粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准，目的主要是为精配准提供较好的变换初值；精配准则是给定一个初始变换，进一步优化得到更精确的变换。

整体上来看，ICP 把点云配准问题拆分成了两个子问题：

• 找最近点
• 找最优变换

首先把图片转为pcd格式的文件

import open3d as o3d
import numpy as np
import copy
 
#输入是两个点云和一个初始转换，该转换将源点云和目标点云大致对齐，输出是精确的变换，使两点云紧密对齐。
#可视化帮助函数
#将目标点云和源点云可视化，并通过对齐变换对其进行转换。
#目标点云和源点云分别用青色和黄色绘制。两点云重叠的越紧密，对齐的结果就越好。
def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    source_temp.transform(transformation)
    o3d.visualization.draw_geometries([source_temp, target_temp],
                                      zoom=0.4459,
                                      front=[0.9288, -0.2951, -0.2242],
                                      lookat=[1.6784, 2.0612, 1.4451],
                                      up=[-0.3402, -0.9189, -0.1996])
#输入
source = o3d.io.read_point_cloud("/cloud/cloud_disk/users/huh/pcb/shuixin_11_26/pcd/temp.pcd")
target = o3d.io.read_point_cloud("/cloud/cloud_disk/users/huh/pcb/shuixin_11_26/pcd/test_aug.pcd")
 
threshold = 0.02
trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],
                         [-0.139, 0.967, -0.215, 0.7],
                         [0.487, 0.255, 0.835, -1.4],
                         [0.0, 0.0, 0.0, 1.0]])
draw_registration_result(source, target, trans_init)
#该函数evaluate_registration计算两个主要指标。fitness计算重叠区域（内点对应关系/目标点数）。越高越好。inlier_rmse计算所有内在对应关系的均方根误差RMSE。越低越好。
reg_p2p = o3d.pipelines.registration.registration_icp(source, target, threshold, trans_init,
        o3d.pipelines.registration.TransformationEstimationPointToPoint(),
        o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration = 2000))
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
draw_registration_result(source, target, reg_p2p.transformation)
 
 
 
 

二维 ICP 配准算法具体步骤

① ICP算法简单来说就是对源点云，B点云，不停进行矩阵变换，把它变换到和目标点云，A点云，很接近就停止。

② 算法步骤：

1) 点云归一化。

2) 找到两片点云中的对应点对。

3) 根据对应点对的信息，计算得到这次的变换矩阵。

4) 变换点云。

5) 计算当前的损失。

6) 是否决定继续迭代，如果损失小于设定阈值或者达到最大迭代数停止，否则返回第1步。

icp.py

import numpy as np
from sklearn.neighbors import NearestNeighbors
 
 
def best_fit_transform(A, B):
    '''
    Calculates the least-squares best-fit transform that maps corresponding points A to B in m spatial dimensions
    Input:
      A: Nxm numpy array of corresponding points
      B: Nxm numpy array of corresponding points
    Returns:
      T: (m+1)x(m+1) homogeneous transformation matrix that maps A on to B
      R: mxm rotation matrix
      t: mx1 translation vector
    '''
 
    assert A.shape == B.shape
 
    # get number of dimensions
    m = A.shape[1]
 
    # translate points to their centroids
    centroid_A = np.mean(A, axis=0)
    centroid_B = np.mean(B, axis=0)
    AA = A - centroid_A
    BB = B - centroid_B
 
    # rotation matrix
    H = np.dot(AA.T, BB)
    U, S, Vt = np.linalg.svd(H)
    R = np.dot(Vt.T, U.T)
 
    # special reflection case
    if np.linalg.det(R) < 0:
       Vt[m-1,:] *= -1
       R = np.dot(Vt.T, U.T)
 
    # translation
    t = centroid_B.T - np.dot(R,centroid_A.T)
 
    # homogeneous transformation
    T = np.identity(m+1)
    T[:m, :m] = R
    T[:m, m] = t
 
    return T, R, t
 
 
def nearest_neighbor(src, dst):
    '''
    Find the nearest (Euclidean) neighbor in dst for each point in src
    Input:
        src: Nxm array of points
        dst: Nxm array of points
    Output:
        distances: Euclidean distances of the nearest neighbor
        indices: dst indices of the nearest neighbor
    '''
 
    assert src.shape == dst.shape
 
    neigh = NearestNeighbors(n_neighbors=1)
    neigh.fit(dst)
    distances, indices = neigh.kneighbors(src, return_distance=True)
    return distances.ravel(), indices.ravel()
 
 
def icp(A, B, init_pose=None, max_iterations=20, tolerance=0.001):
    '''
    The Iterative Closest Point method: finds best-fit transform that maps points A on to points B
    Input:
        A: Nxm numpy array of source mD points
        B: Nxm numpy array of destination mD point
        init_pose: (m+1)x(m+1) homogeneous transformation
        max_iterations: exit algorithm after max_iterations
        tolerance: convergence criteria
    Output:
        T: final homogeneous transformation that maps A on to B
        distances: Euclidean distances (errors) of the nearest neighbor
        i: number of iterations to converge
    '''
 
    assert A.shape == B.shape
 
    # get number of dimensions
    m = A.shape[1]
 
    # make points homogeneous, copy them to maintain the originals
    src = np.ones((m+1,A.shape[0]))
    dst = np.ones((m+1,B.shape[0]))
    src[:m,:] = np.copy(A.T)
    dst[:m,:] = np.copy(B.T)
 
    # apply the initial pose estimation
    if init_pose is not None:
        src = np.dot(init_pose, src)
 
    prev_error = 0
 
    for i in range(max_iterations):
        # find the nearest neighbors between the current source and destination points
        distances, indices = nearest_neighbor(src[:m,:].T, dst[:m,:].T)
 
        # compute the transformation between the current source and nearest destination points
        T,_,_ = best_fit_transform(src[:m,:].T, dst[:m,indices].T)
 
        # update the current source
        src = np.dot(T, src)
 
        # check error
        mean_error = np.mean(distances)
        if np.abs(prev_error - mean_error) < tolerance:
            break
        prev_error = mean_error
 
    # calculate final transformation
    T,_,_ = best_fit_transform(A, src[:m,:].T)
 
    return T, distances, i

main.py

import numpy as np
import time
import icp
import cv2
from skimage import io
 
# Constants
N = 640                                # number of random points in the dataset
num_tests = 100                             # number of test iterations
dim = 3                                     # number of dimensions of the points
noise_sigma = .01                           # standard deviation error to be added
translation = .1                            # max translation of the test set
rotation = .1                               # max rotation (radians) of the test set
 
 
def rotation_matrix(axis, theta):
    axis = axis/np.sqrt(np.dot(axis, axis))
    a = np.cos(theta/2.)
    b, c, d = -axis*np.sin(theta/2.)
 
    return np.array([[a*a+b*b-c*c-d*d, 2*(b*c-a*d), 2*(b*d+a*c)],
                  [2*(b*c+a*d), a*a+c*c-b*b-d*d, 2*(c*d-a*b)],
                  [2*(b*d-a*c), 2*(c*d+a*b), a*a+d*d-b*b-c*c]])
 
 
def test_best_fit():
 
    # Generate a random dataset
    A = np.random.rand(N, dim)
 
    total_time = 0
 
    for i in range(num_tests):
 
        B = np.copy(A)
 
        # Translate
        t = np.random.rand(dim)*translation
        B += t
 
        # Rotate
        R = rotation_matrix(np.random.rand(dim), np.random.rand()*rotation)
        B = np.dot(R, B.T).T
 
        # Add noise
        B += np.random.randn(N, dim) * noise_sigma
 
        # Find best fit transform
        start = time.time()
        T, R1, t1 = icp.best_fit_transform(B, A)
        total_time += time.time() - start
 
        # Make C a homogeneous representation of B
        C = np.ones((N, 4))
        C[:,0:3] = B
 
        # Transform C
        C = np.dot(T, C.T).T
 
        assert np.allclose(C[:,0:3], A, atol=6*noise_sigma) # T should transform B (or C) to A
        assert np.allclose(-t1, t, atol=6*noise_sigma)      # t and t1 should be inverses
        assert np.allclose(R1.T, R, atol=6*noise_sigma)     # R and R1 should be inverses
 
    print('best fit time: {:.3}'.format(total_time/num_tests))
 
    return
 
 
def test_icp():
    temp_path = 'temp.jpg'
    temp_img = io.imread(temp_path)
    A = np.uint8(temp_img)
    test_aug_img_path = 'test_aug.jpg'
    test_aug_img = io.imread(test_aug_img_path)
    B = np.uint8(test_aug_img)
    
 
    T, distances, iterations = icp.icp(B, A, tolerance=0.000001)
 
 
    # T: (m+1)x(m+1) homogeneous transformation matrix that maps A on to B
    # R: mxm rotation matrix
    # t: mx1 translation vector
    T, R1, t1 = icp.best_fit_transform(B, A)
    print(T.shape)
    print(R1.shape)
    print(t1.shape)
    return
 
 
if __name__ == "__main__":
    #test_best_fit()
    test_icp()