点云对齐——精选推荐

点云对齐
杨成林∗
2009年5月31日
进行数据采集时，由于受到测量设备和坏境的限制，物体表面完整测量数据的获得往往需要通过多次测量完成。

由于每次测量得到的点云数据往往只覆盖物体部分表面，并且可能出现平移错位和旋转错位，因此为了得到物体完整表面的点云数据，需要对这些局部点云数据进行对齐。

点云的对齐所用的方法可分为两种，一种是依靠测量设备进行对齐，另一种则是根据测量后的数据处理对齐。

第一种方法通过特定的设备测量待测工件或测量传感器在多次测量中的位姿。

通过测得的位姿来对齐多次测量数据。

这一方法快速方便，不需后续处理，不过需要额外的设备，而且不能完全满足测量视角的要求。

第二种方法利用测量得到的数据进行对齐，不受设备的约束，测量视角的选择可以更加自由，不过有时计算量很大，而且会因为局部最优等问题得不到正确的对齐。

本文主要讨论后一种对齐方式。

1ICP方法
对于两个点集:作为对齐基准的点集P={p1,p2,...,p n}和待对齐的点集X={x1,x2,...,x m}，两个点云的对齐是使以下目标函数的值最小
f(R,t)=
m
i=1
||Rx i+t−y i||2
2
(1)
其中R和t分别是旋转矩阵和平移向量，而y i是点x i经过变换后的对应点。

此公式中，对应点可以有多种定义方法，比如规定y i为点x i在P中的最近点。

点云对齐问题的难点在于：y i是R和t的函数，事先无法确定，而∗*****************
1
且对应点的求取通常是计算量很大的工作。

在对应点是最近点的情况，原始的方法计算复杂度为O(mn)，常用的改进方法kd-tree方法计算复杂度在最好的情况下为O(m log n)。

在点集中点数很多的情况下，计算上的消耗是很大的，所以目标函数求值是计算量很大的工作。

虽然对于一般的最优化问题，减少目标函数的计算次数也是一个重要的目标，但在点云对齐问题中，减少目标函数的计算显得尤为重要。

为了解决这一问题，对齐问题的处理通常分为初始对齐和精确对齐两步。

初始对齐又称粗对齐，其中并不计算对齐目标函数的值，而是通过求解一些较小的问题间接地使点集的位置接近最优值。

初始对齐能减少点云之间的错位，为精确对齐提供较好的初始值，提高精确对齐的效率。

而精确对齐则直接优化对齐目标函数，使对齐的误差达到最小。

常规的优化方法根据对目标函数导数的使用的可以分为两类。

一类方法不需显式地计算目标函数的导数值，称作直接法。

常用的如Powell方法、单纯形方法。

元启发算法，如模拟退火、遗传算法，也可归为此类。

使用这类方法求解点云对齐问题，通常需要几百步甚至几千步才能收敛到最优值。

这在实际工程应用中是难以接受的。

而另一类方法，即使用目标函数导数的方法，如最速下降法、共轭梯度法、变尺度法。

在没有方法直接得到函数的导数信息时，需要使用差分法来估计导数。

点云对齐问题中，因为目标函数共有6个自变量，为了估计导数需要7次目标函数求值。

而在通过导数信息得到了搜索方向后，还需在该方向上进行一维搜索，这又需要若干次目标函数求值。

所以虽然这类方法中的某些可以用较少的迭代步得到最优解，但总的目标函数计算次数还是不少。

正是在这样的背景下，ICP[1](Iterative Closest Point)方法被提出。

在ICP方法中，以最近点为对应点，将整个寻优过程分为反复迭代的两步：寻找最近点和根据点与最近点的对应关系求解最优刚体变换。

令X，P分别为相应点集组成的矩阵，Y为各个最近点组成的矩阵，则前一个步骤记为：
Y=N(X,P)
后一步骤记为：
(R,t)=T(X,Y)
定义对齐的均方差为：
e=1
m
m
i=1
||Rx i+t−y i||2
2
(2)
ICP方法的流程如下：
2
1.初始化。

令X0=X，R0=I，t0=0，k=0。

2.计算最近点Y k=N(X k,P)，并计算e k。

3.如果满足停止条件则计算结束。

4.根据最近点求最优变换：(R∗,t∗)=T(X k,Y k)。

5.更新：根据R∗,t∗，计算X k+1=R∗X k+t∗，R k+1=R∗R k，
t k+1=t∗+R∗t k。

6.令k=k+1，并返回第2步。

从ICP方法的两个基本步骤可以看出，两个步骤的目标都在于减小均方差，而其过程绝不会使均方差增大。

所以在ICP的迭代过程中，均方差是不增的，同时均方差又是恒大于等于零的，所以ICP对齐的结果是一定收敛的。

这是ICP方法的一大特点。

这一特点一方面保证了ICP方法的收敛性，另一方面又说明ICP方法只能收敛到局部最优。

采用ICP方法求解点云对齐问题，通常只需要十几次目标函数计算就可以收敛，很好地实现了减少计算对齐目标函数的次数的目标。

虽然还存在只能得到局部最优等问题，但相较之前的方法己是很大的进步。

在Besl提出ICP方法的同时，Chen和Medioni[2]提出了以点到面的距离为优化目标的点云拼合方法。

之后，多种基于ICP方法的修正方法被提出。

ICP的含义也从原始的Iterative Closest Point扩展为Iterative Correspond-ing Point[3]。

各种方法都是将点云对齐问题分解为对应点的寻找和根据对应关系求最优变换两步。

现有的方法几乎不出这种框架。

2有向距离场的微分信息估计
与ICP方法相比，常规的最优化方法之所以被弃用，在于它们的计算量太大。

如使用导数的方法虽然迭代步数和ICP方法相当，但估计导数的计算量使它仍不实用。

但相关的研究仍在进行[4–6]。

根据对距离函数的微分性质的分析，Pottmann[7]提出了估计对齐目标函数导数的有效方法。

为了分析点的运动和点到曲面距离的关系。

设点X在曲面S上的最近点为点P。

曲面S在点P处有两主方向的单位矢量e1,e2，法矢n=e1×e2。

把{P;e1,e2,n}取作空间E3中的直角坐标系，可得曲面S在点P的标准展
开[8]：
z=1
2
(κ1x2+κ2y2)+o(x2+y2)(3)
其中κ1,κ2是曲面的两个主曲率，当两者不全为0时，上式的主要部分为：
z=1
2
(κ1x2+κ2y2)(4) 3
这个二次曲面称作S在点P的近似曲面[8]。

图1曲面及其等距曲面
记与S的有向距离为d的等距曲面为S d，S d与曲面S上的点P对应的点为P d。

不考虑奇异的情形，由等距曲面的性质[9]，S d在P d处的法向、主方向和S在点P处的法向、主方向相同，而将S在P处的主曲率半径记为ρ1,ρ2，则等距曲面相应的曲率半径为ρ1−d,ρ2−d。

等距曲面S d在前述坐标系下的二阶近似为：
z=1
2
1
ρ1−d
x2+
1
2
1
ρ2−d
y2+d(5)
换个角度，则上式也可看作由x,y,z计算d的公式。

两边分别对x,y,z求导，得：
0=
1
ρ1−d
x+
1
2
x2
d x
(ρ1−d)2
+
1
2
y2
d x
(ρ2−d)2
+d x
0=1
2
x2
d y
(ρ1−d)2
+
1
ρ2−d
y+
1
2
y2
d y
(ρ2−d)2
+d y
1=1
2
x2
d z
(ρ1−d)2
+
1
2
y2
d z
(ρ2−d)2
+d z
(6)
设点p到曲面S的有向距离为h，则p在当前坐标系下的坐标为(0,0,h)。

从前式易得(d x,d y,d z)|(0,0,h)=(0,0,1)。

即在一阶近似下，只有点沿曲面法向的运动对点到曲面的距离有影响。

4
对(5)求二阶导数，则可得到有向距离场的二阶微分信息。

由于公式过于冗长，这里略去。

最终得到的结果为：
∂2d ∂p 2 (0,0,h )= 1h −ρ10001h −ρ20000
(7)得到了有向距离场的微分信息，对齐目标函数(1)的微分也可从中推出。

沿用前面点集X ，Y 的定义。

Y 中各点的法向量记为{n 1,n 2,...,n m }，对应点之间的距离记为{d 1,d 2,...,d m }。

公式(1)中f =m i =1d 2i ，那么∂f ∂t =m i =1
2d i ∂d i ∂t 。

由前面的推导，有∂d i ∂t =n T i ，所以：∂f ∂t =m i =1
2d i n T i (8)而旋转变换采用欧拉角描述[10]，采用z −y −x 的顺序：
R (α,β,γ)= cαcβcαsβsγ−sαcγcαsβcγ+sαsγ
sαcβsαsβsγ+cαcγsαsβcγ−cαsγ−sβcβsγcβcγ
(9)点的旋转运动引起的点到曲面距离的改变应等效为点的平移来处理，有：∂d i ∂α=n T i ∂R ∂α
x i (10)由此可得：∂f ∂α=m i =12d i ∂d i ∂α=m i =1
2d i n T i ∂R ∂αx i (11)
关于β、γ的公式类似。

5
3常规方法的重新考虑
至此，已得到目标函数关于各个变量的一阶偏导。

应用这些结果，常规的使用导数的优化算法便可应用到点云对齐的计算中。

其中要用到点云的法矢信息，而法矢的计算已有了不少成熟的方法[11]。

通常所用方法是将点的邻域拟合为平面，以平面的法矢作为点的法矢，而难点在于使法矢的方向一致。

可是前面的公式中对法矢的应用并不需要法矢方向一致，这就避开了这一难点。

使用如图2所示的两片点云测试算法，其中蓝色作为待对齐点云，红色作为对齐标准。

红色点云是由蓝色点云经刚体变换得到，所以在收敛到全局最优的情况下，对齐误差为0。

ICP方法和常规方法的对齐目标并不完全相同，这里为了比较结果，统一使用ICP方法中的点到点距离平方和来评价。

图2对齐点云
3.1最速下降与拟牛顿方法
由于使用欧拉角来描述旋转运动，严格意义上在求解最优问题时应对角度做一定的限制，但在实际应用中，这一点可以忽略，即对齐问题可以作为无约束最优化问题处理。

最速下降法和牛顿法是求解无约束最优化问题的最早、最基础的方法。

牛顿法需要目标函数的二阶导数，留在后面讨论。

为了克服这一困难，在牛顿法的基础上又发展了拟牛顿法，只需一阶导数信息，常用的有BFGS和DFP两种。

分别使用ICP、最速下降法、BFGS、DFP方法来计算点云对齐，如图3所示。

为了清楚展示迭代收敛的情况，分为两个坐标图显示。

图中的Steepest表示的是最速下降法。

6
Iter f
0−10
Iter f 11−30图3拟牛顿法、最速下降法与ICP 方法比较
从图3可以看到，在最开始的几步，其它三种方法的收敛速度都比ICP 方法要快，但各种方法的表现比较接近。

而在后面的迭代中，BFGS 方法收敛速度最快，ICP 次之，DFP 方法又次之，而最速下降法最慢。

这和通常对这三种常规方法的认识是一致的。

需要指出的是，在到达最终收敛位置这一点上，ICP 方法具有突出的优势。

即ICP 方法在与最优位置距离到达一定时，可以一步到达最优位置，这是ICP 方法中应用了最优变换计算的结果。

另外，最速下降法虽然可以利用前面得到的导数估计方法，但在执行一维搜索时还需计算目标函数的值，所以在时间上的耗费要远大于其他方法。

从前面的分析可以得到，在已知点云法矢的情况下，BFGS 法可以是ICP 方法的一个替代。

3.2高斯-牛顿类方法
实际上，形如f =m
i =1d 2i 的目标函数最优问题，称作非线性最小二乘问
题，还有更好的求解思路。

将6个变量统一地记作v =(α,β,γ,t x ,t y ,t z )T ，而d =(d 1,d 2,...,d m )T ，则有雅可比矩阵：
J =∂d ∂v = ∂d i ∂v j j =1,2,...,6i =1,2,...,m
(12)7
目标函数的梯度：
g=∇f=2
m
i=1
d i∇d i=2d T J(13)
目标函数的海森矩阵：
H=∇2f=2
m
i=1
(∇d T
i
∇d i+d i∇2d i)=2J T J+2
m
i=1
d i∇2d i(14)
如果将上式中的后一项略去，且记δ=v−v0，则目标函数在v0处可由二阶泰勒展开近似为：
f(v0+δ)≈f(v0)+2d T Jδ+δT J T Jδ(15)
这样目标函数的最优化问题便可转化为上式右边的线性最小二乘问题。

这一线性最二乘问题的求解又可转为求解线性方程：
J T Jδ=−J T d(16)
从以上方程中求解出δ，可以作为最优化迭代求解中的搜索方向，这种方法便是求解非线性最小二乘问题的高斯-牛顿方法。

高斯-牛顿方法利用一阶导数信息得到二阶导数的近似，一般情况下表现比拟牛顿法好。

高斯-牛顿方法有一个重要的变体：Levenberg–Marquardt方法。

L-M方法的提出主要是为了提高算法的稳定性，它将方程(16)修正为：
(J T J+µI)δ=−J T d(17)
通过µ的调整，在稳定性和收敛速度之间获得很好地平衡。

前面提到，ICP方法的一种重要修正是变为以点到面的距离为优化目标，一般情况下性能比标准的ICP方法要好[3,12]。

下面说明这种方法实际上就是高斯-牛顿方法在点云对齐中的应用。

以点到面的距离为优化目标，目
标函数为：
f(v)=
m
i=1
(n T(x i(v)−y i))2(18)
在当前位置线性化，将目标函数转化为：
f∗(δ)=
m
i=1
(n T(∇x iδ+x i−y i))2(19) 8
易得，n T∇x i=∇d i，n T(x i−y i)=d i，所以：
f∗(δ)=
m
i=1
(∇d iδ+d i)2(20)
简单的推导后可知式(20)和式(15)是等价的。

实际中应用的点面距离最优化方法有多种，可能和推导中所用的略有不同，但并不影响这里的结果：以点到面的距离为优化目标的ICP方法即高斯-牛顿方法，只是实现的细节上可能稍有不同。

使用点-点ICP、点-面ICP、高斯-牛顿法以及L-M方法求解点云对齐，这里所用的点-面ICP是Pottmann[13]使用的TDM方法。

还是采用前面的算例，得到的结果如图4。

从图中可以看出，点-点ICP方法明显慢于其它方法，而另三种方法的效果则几乎相同。

Iter
f
图4高斯-牛顿类方法与ICP方法比较
3.3牛顿法
虽然牛顿法是基础的优化方法，但由于它需要目标函数的信息，所以放在最后讨论。

在前面高斯-牛顿法的推导过程中。

略去了公式(14)的后一项，而通过前面得到的有向距离场的二阶微分(7)，可推出这一项的具体表达式。

由于比较繁琐，这里略去。

这样得到的海森矩阵较之前的更为精确。

如此，目标函数可由二阶泰勒展开近似为：
f(v0+δ)≈f(v0)+gδ+1
2
δT Hδ(21)
9
右侧二次函数的极值在以下情况下取得：
Hδ=−g(22)以此方程的解作为最优搜索当前迭代步的解，这就是求解最优化问题的牛顿方法。

图5中给出了牛顿法和高斯-牛顿法以及标准ICP方法的比较。

由于牛顿法需要曲面的局部标架以及主曲率的值，而通过点云计算这些数据比较困难繁琐，在具体的算法实现中，本文是通过将作为对齐标准的点云插值为样条曲面，以参数化的曲面表达来计算相关数据。

f
Iter
图5牛顿法与其他方法比较
从图中可以看出，牛顿法比ICP方法效率要高不少。

可是相较高斯-牛顿法，虽然牛顿法有更好的微分信息估计，但效率上依然低于高斯-牛顿法。

这和一些论文[7,13]中声称的结果并不吻合。

当然，这里所使用的方法和这些论文中的方法并不完全相同。

为了对这个问题进一步的分析，考虑一个最简单的情形，虽然难以全面说明这一问题，也可窥见一斑。

考虑平面上单位圆的对齐问题(图6)。

以左侧的圆为对齐标准，右侧为圆离散得到的点集。

由于圆的对称性，只需考虑某一方向上的平移即可。

将点集平移t，分别用三种方法计算对齐所需的平移，这里只计算一个迭代步。

然后比较实际平移与计算所得平移之间的误差，得到图7。

从图7可以看出，牛顿法的表现并不好于高斯-牛顿方法，而且在平移距离约为1时，由于圆的距离场在圆心处奇异，牛顿法的求解很不稳定,而
10
图6平面单位圆对齐
高斯-牛顿法则总能给出有效的下降方向[14]。

有趣的是，ICP方法虽然是公认的较慢的方法，但在某一区间内表现好于其他方法。

总体上则是高斯-牛顿方法的性能最佳。

4总结
通过前面的分析可以看到，通过对有向距离场的微分信息，可以得到对齐目标函数的微分信息，从而将传统的基于目标函数导数的优化方法应用到几何对齐的问题中。

虽然文中所使用的方法都是非常经典的方法，但数值优化领域还是不断有新的成果出现，充分结合通用算法和专用算法来进一步提高几何对齐问题的求解应是大有可为的。

11
t e r r 图7三种方法在单位圆对齐中的表现
参考文献
[1]Paul J.Besl and Neil D.Mckay.A method for registration of 3-d
shapes.IEEE Transactions on Pattern Analysis and Machine Intelligence ,14(2):239–256,February 1992.
[2]Yang Chen and G´e rard Medioni.Object modelling by registration of multi-
ple range images.Image Vision Comput.,10(3):145–155,1992.
[3]Szymon Rusinkiewicz and Marc Levoy.Efficient variants of the ICP algo-
rithm.In Third International Conference on 3D Digital Imaging and Mod-eling (3DIM),June 2001.
[4]Andrew W.Fitzgibbon.Robust registration of 2d and 3d point sets.In In
Proc.British Machine Vision Conference,volume II ,pages 411–420,2001.
[5]Thomas M.Tucker and Thomas R.Kurfess.Newton methods for parametric
surface registration.part puter-Aided Design ,35(1):107–114,2003.
12
[6]Thomas M.Tucker and Thomas R.Kurfess.Newton methods for parametric
surface registration.part ii.experimental puter-Aided De-sign,35(1):115–120,2003.
[7]H.Pottmann and M.Hofer.Geometry of the squared distance function to
curves and surfaces.In H.-C.Hege and K.Polthier,editors,Visualization and Mathematics III,pages223–244.Springer,2003.
[8]陈维桓.微分几何初步.北京大学出版社,1990.
[9]黄克智，薛明德，陆明万.张量分析.清华大学出版社,2003.
[10]熊有伦.机器人技术基础.华中理工大学出版社,1996.
[11]T.K.Dey,G.Li,and J.Sun.Normal estimation for point clouds:a compari-
son study for a voronoi based method.pbg,0:39–46,2005.
[12]Timoth´e e Jost.Fast Geometric Matching for Shape Registration.PhD thesis,
Universit´e de Neuchˆa tel,2002.
[13]Helmut Pottmann,Qi-Xing Huang,Yong-Liang Yang,and Shi-Min Hu.Ge-
ometry and convergence analysis of algorithms for registration of3D shapes.
puter Vision,67(3):277–296,2006.
[14]J.Nocedal and S.J.Wright.Numerical optimization.Springer,1999.
13。