icp算法及实现

---

本系列文章由@邻居张师傅 出品，转载请注明出处。

文章链接： https://editor.csdn.net/md?not_checkout=1&articleId=109714528

邮箱： zhangyh_nb@163.com

---

---

overview

一种用于最小化两点云之间的差异的算法。
应用：根据不同的扫描结果重建二维或三维表面

---

PCL例程

PCL官方例程输出如下：

	Saved 5 data points to input:
	 0.352222 -0.151883 -0.106395
	-0.397406 -0.473106 0.292602
	-0.731898 0.667105 0.441304
	-0.734766 0.854581 -0.0361733
	-0.4607 -0.277468 -0.916762
	size:5
	
	Transformed 5 data points:
	 1.05222 -0.151883 -0.106395
	 0.302594 -0.473106 0.292602
	-0.0318983 0.667105 0.441304
	-0.0347655 0.854581 -0.0361733
	0.2393 -0.277468 -0.916762
	
	[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample
	selection distance threshold of: 0.200928
	[pcl::IterativeClosestPoint::computeTransformation] Number of
	correspondences 4 [80.000000%] out of 5 points [100.0%], RANSAC rejected:
	1 [20.000000%].
	[pcl::IterativeClosestPoint::computeTransformation] Convergence reached.
	Number of iterations: 1 out of 0. Transformation difference: 0.700001
	has converged:1 score: 1.95122e-14
	// 4 X 4 的刚性变换（rigid transformation）矩阵
	          1  4.47035e-08 -3.25963e-09          0.7
	2.98023e-08            1 -1.08499e-07 -2.98023e-08
	1.30385e-08 -1.67638e-08            1  1.86265e-08
	          0            0            0            1

其中的齐次变换矩阵为什么是4 x 4？

---

基本流程

1. Selection of some set of points in one or both meshes.
2. Matching these points to samples in the other mesh.
3. Weighting the corresponding pairs appropriately.
4. Rejecting certain pairs based on looking at each pair individually
   or considering the entire set of pairs.
5. Assigning an error metric based on the point pairs.
6. Minimizing the error metric.
   本文的流程：
   i. 给定一个原点云集source，一个配准点云集target
   ii. 寻找source关于target的每个点的最近邻点，记下每个点与最近邻的距离和最近邻的下标
   iii. 计算source关于target的齐次变换矩阵（主要是旋转矩阵和平移向量）
   其中计算旋转矩阵的方法一般是先利用所有点计算出整个点集source和target的质心，根据质心使用svd方法计算旋转矩阵
   iv. 应用齐次变换矩阵到source并更新之
   v.从ii步继续迭代，直到误差（根据点与最近邻的矩阵平均值）小于一个阈值或者迭代次数大于一个值

---

代码实现

首先是点云可视化

from mayavi import mlab
import numpy as np
def show_point3d(source, target):
    '''mayavi绘制三维点云图像,红色为源图像，绿色为变换后的图像'''
    mlab.points3d(source[:,0], source[:,1], source[:,2], color=(1,0,0), scale_factor=0.5)
    mlab.points3d(target[:,0], target[:,1], target[:,2], color=(0,1,0), scale_factor=0.5)
    mlab.axes()
    mlab.show()

根据轴角求得旋转矩阵

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]])

计算A到B的变换矩阵

def find_transform(A, B):
    '''
    计算最小二乘最佳拟合变换，该变换在m个空间维度上将对应点A映射到B
    Input:
      A: Nxm  行向量的集合
      B: Nxm  行向量的集合
    Returns:
      T: (m+1)x(m+1) A 到 B 的齐次变换矩阵
      R: mxm 旋转矩阵
      t: mx1 平移向量
    '''

    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

简化起见，输入点云是一条经过（0，0，0），（9，9，9）的直线

# Generate a random dataset source  行向量形式
source = np.array([[0,0,0],[1,1,1],[2,2,2],[3,3,3],[4,4,4],[5,5,5],[6,6,6],[7,7,7],[8,8,8],[9,9,9]])

# 生成目标数据集 行向量形式
target = np.copy(source)

# 平移
t = np.array([0,0,1]) # z坐标均加1  
target += t
#show_point3d(source, target)

# 旋转
R = rotation_matrix(np.array([0,0,1]), 90*np.pi/180) # 沿着z轴旋转90°
target = np.dot(R,target.T).T
#show_point3d(source, target)

# 加入噪声
target += np.random.randn(10,3) * noise_sigma
show_point3d(source, target)

for i in range(3):  
    T, R, t = find_transform(source, target)
    print(nearest_neighbor(source,target))
    source = np.dot(R,source.T).T + t
    show_point3d(source, target)

加入噪声后的两个点云集

一次icp迭代后的两点云集

以上的代码只是最简单的版本，输入的点云集和变换也是最简单的。还有很大的改进空间，比如可以对于较为复杂的点云集可以加上一个用户指定的大概的齐次变换矩阵，在icp迭代之前先对source进行一个大概的变换。如为各个邻点添加权重、拒绝离群点等。如根据最近邻信息结束迭代，其中计算最近邻也可以用kdtree、八叉树加速。篇幅有限，抛砖引玉。