CN101082988A - 自动的深度图像配准方法 - Google Patents

Abstract

一种自动的深度图像配准方法，步骤为：对深度图像进行三角化处理，将点云数据转换成三角网格数据；根据所述的三角网格数据，找出深度图像中的边界点；计算深度图像中非边界顶点的特征值；根据非边界顶点的特征值，提取深度图像中具有突出表面特征的特征点集；对于待配准的两幅深度图像，寻找它们的特征点之间的匹配关系，即对应点，找出至少三对对应的顶点；根据所述的对应点计算出这两幅深度图像间运动的估计值；最后采用改进的ICP算法优化前一步的结果，完成两幅深度图像的精确配准。本发明是一种自动的深度图像配准方法，提高了配准的速度和精确性，同时计算量小和计算简单。

Description

自动的深度图像配准方法

技术领域

本发明属于计算机虚拟现实和计算机图形学技术领域，具体地说，是一种根据深度图像数据自动重建物体三维模型的方法，该方法成功地应用于各种真实物体三维模型的重建，特别是应用于数字博物馆中珍贵文物三维模型的重建。

背景技术

重建真实世界中物体的三维模型，被越来越广泛的应用在虚拟仿真、计算机动画、反求工程、计算机辅助设计以及数字博物馆等方面。随着三维扫描设备的不断发展，基于扫描设备采集到的点云数据，即深度图像，重建真实物体的模型，逐渐成为一种流行的三维建模方法。真实物体的几何形状往往较复杂，而三维扫描仪的视角又有限，因此，为了获取三维物体表面全部的几何信息，需要在多个不同的视点扫描，再将每一次采集到的深度图像拼接起来，恢复成一个完整的点云数据，这一过程就是深度图像的配准。因为在不同视点处扫描得到的深度图像分别处于不同的坐标系中，因此配准两幅深度图像的关键，就是找出它们之间的运动矩阵，在该运动矩阵的作用下，将第二幅深度图像中所有顶点变换到第一幅图像的坐标系中。

在深度图像配准方法的研究中，Besl，P.J.，McKay，N.D.：A method forregistration of 3-d shapes.IEEE Trans.Pattern Anal.Mach.Intell.14(2)(1992)239-256和Chen，Y.，Medioni，G.：Object modelling by registration of multiplerange images.Image Vision Comput.10(3)(1992)145-155等人首先提出了配准两幅深度图像的ICP(Iterative Closest Point)算法。ICP算法通过一个迭代过程，不断的减小两个点云集合间预定义的距离函数，直到达到某个阀值，从而计算出两幅深度图像间的运动矩阵。ICP算法是一种不基于特征的迭代方法，它需要有较好的初始运动估计值。目前，它主要被用来完成两幅深度图像间的精细配准。

近年来，如何实现自动化的深度图像的配准，成为了配准领域研究的新热点。对于两幅深度图像的自动配准，在没有初始位置估计和其他信息(如每次扫描仪的位置或旋转的角度等)的情况下，一般需要先自动完成两幅深度图像的粗略配准，即估计出它们之间运动的初始值；然后再使用ICP或其他算法进行优化。在这方面的研究中，Chen，C.S.，Hung，Y.P.，Cheng，J.B.：A fastautomatic method for registration of partially-overlapping range images.In：ICCV′98：Proceedings of the Sixth Inter-national Conference on ComputerVision，Washington，DC，USA，IEEE Computer Society(1998)242利用两点间的空间距离作为约束条件，找出两幅图像中对应的特征点，然后计算出这两幅图像间运动的近似值。Huber，D.F.：Automatic three-dimensional modelingfrom reality.PhD thesis，Carnegie Mellon University(2002)Chair-MartialHebert.采用旋转图像(Spin Image)描述模型表面的特征，然后结合统计学的方法，粗略配准两幅深度图像。Gelfand，N.，Mitra，N.J.，Guibas，L.J.，Pottmann，H.：Robust global registration.In：Symposium on Geometry Processing.(2005)197-206则提出使用积分的方法来描述模型表面的特征，再跟据计算出的特征值，寻找出对应的特征点。Sun，Y.，Paik，J.，Koschan，A.，Page，D.，Abidi，M.：Point fingerprint：a new 3-d object representation scheme.Systems，Manand Cybernetics，Part B，IEEE Transactions on 33(4)(2003)712-717采用了一种自定义的定点指纹来寻找两幅深度图像间的对应点。

在上述已有的各方法中，Chen的方法没有考虑模型表面的特征，而仅依靠几何约束做全局搜索，配准效率不高。Huber提出的旋转图像尽管能够较好的描述模型表面的特征，但其属于高维的特征，计算量大且耗费大量存储空间。Gelfand和Sun等人提出的特征描述符在计算上依然比较复杂，且在做特征点匹配时，普遍依靠特征自身的信息。但是，在实际应用中，模型表面往往存在相似区域，仅依靠局部特征很难准确区分这些区域。还有，已有方法在确定特征点集时，常采用均匀采样或随机采样的方法，前者往往会引入大量的特征点，从而进一步增加特征值的计算量；而后者会造成选取的特征点质量的不稳定，从而影响最终的配准结果。

发明内容

本发明的技术解决问题：克服现有技术的不足，提供一种自动的深度图像配准方法，该方法提高了配准的速度和精确性，同时计算量小和计算简单。

本发明的技术解决方案：自动的深度图像配准方法，其特点在于步骤如下：

(1)对深度图像进行三角化处理，将点云数据转换成三角网格数据；

(2)根据所述的三角网格数据，找出深度图像中的边界点；

(3)计算深度图像中非边界顶点的特征值；

(4)根据非边界顶点的特征值，提取深度图像中具有突出表面特征的特征点集；

(5)对于待配准的两幅深度图像，寻找它们的特征点之间的匹配关系，即对应点，找出至少三对对应的顶点；

(6)根据所述的对应点计算出这两幅深度图像间运动的估计值；

(7)采用改进的ICP算法优化步骤(6)中的结果，完成两幅深度图像的精确配准。

所述步骤(1)中，将点云数据转换成三角网格数据的方法是：将每幅深度图像投影到一个二维平面上，然后使用二维的Delaunay三角剖分，获得所有点之间的连接关系，再将这种连接关系映射回三维空间，就完成了对点云数据的三角化处理。

所述步骤(2)中，根据所述的三角网格数据，找出深度图像中的边界点的方法为：在已知三角网格数据的情况下，通过遍历整个的三角网格数据，找出所有的孤立边，即那些只属于一个三角形的边，这些孤立边的集合就是网格数据的边界，属于这些孤立边的两个顶点即为边界点。

所述步骤(4)中，提取任意深度图像中特征点的方法是：首先采用八叉树细分深度图像，然后在八叉树的每个叶节点中，选取特征值最大的顶点作为该节点的特征点，所有叶节点的特征点的集合，构成这幅深度图像的特征点集。

本发明的原理：本发明的核心是特征点的选取和匹配。实际上，只要在待配准两幅深度图像间，找到三对对应的特征点，就可以利用它们的空间坐标计算出这两幅图像间的运动矩阵的估计值。本发明的步骤(3)、(4)和(5)就是计算特征值、选取特征点和做特征点匹配的过程，在计算出运动矩阵的估计值后，就可以用步骤(7)中改进的ICP算法进一步优化配准结果。另外，因为准确计算边界点处的特征值很困难，所以步骤(1)、(2)剔除了深度图像中的边界点，这样可以有效避免它们对配准结果的不利影响，提高了配准精度。

本发明与现有技术相比的有益效果是：

(1)本发明在配准前先剔除边界点，避免了边界点处不准确的特征值对特征点匹配过程中的负面影响，从而提高了粗略配准的效率和准确性；同时，剔除边界点也提高了精确配准的速度和精确性。

(2)本发明采用采样点处的主曲率作为特征描述符，和Huber、Gelfand和Sun的方法相比，这种特征描述符更加简单直观，易于计算和比较，使得计算量小和计算简单。

(3)本发明的基于八叉树的特征点选取方法，在保证了选取特征点质量的基础上，有效的减少了特征点的数量，使得计算更加简单。

(4)本发明在特征点的匹配过程中，结合了对比特征值和全局几何一致性测试两种方法，有效的剔除了由相似区域特征点造成的错误的匹配结果，更加提高了配准的精确性。

(5)改进了经典的ICP算法，在迭代过程中，根据误差矩阵动态的调整距离阀值，从而加快了整个算法的收敛速度，提高了配准速度。

附图说明

图1为本发明采用深度为3的八叉树细分深度图像并提取特征点的结果；

图2为本发明采用深度为4的八叉树细分深度图像并提取特征点的结果；

图3为采用本发明配准Buddha模型的结果，其中图3a为图像数据R₁和R₂配准前的状态；图3b为R₁和R₂配准后的状态；图3c为R₃和R₄配准前的状态；图3d为最终的配准结果。

具体实施方式

(1)对深度图像进行三角化处理，将点云数据转换成网格数据。

深度图像是点云数据，只包含了顶点的坐标信息，而没有它们之间的连接关系，即拓扑信息。为了方便后面步骤中的寻找边界点和计算顶点特征值等操作，需要先对它们进行网格化的处理。

网格化的方法是：对每幅深度图像，设P＝{P₁，...，P_n}为其中所有的顶点。对于Pi＝(x_i，y_i，z_i)，忽略P_i的Z坐标，即将P_i投影到二维平面上的P_i’＝(x_i，y_i)，设P’＝{P₁’，...，P_n’}为投影得到的新点集，然后使用二维的Delaunay三角剖分(B.Delaunay，Sur la sphère vide，lzvestia Akademii Nauk SSSR，OtdelenieMatematicheskikh i Estestvennykh Nauk， 7：793-800，1934)，获得P’中顶点间的连接关系，若P_i’和P_j’间存在连接关系，则P_i和P_j间也应存在连接关系，如此将二维平面上的连接关系直接映射到三维空间中，从而完成了点云数据的三角化。

(2)根据三角网格，找出深度图像中的边界点。

由于深度图像边界点附近的几何信息不完整，故难以准确的计算出它们的特征值。为了保证配准结果的准确性，需要在计算顶点的特征值以及对应顶点匹配等操作之前，找出并剔除这些边界点。

在已知三角网格数据的情况下，通过遍历整个的三角网格数据，找出所有的孤立边，即那些只属于一个三角形的边，这些孤立边的集合就是网格数据的边界，属于这些孤立边的两个顶点即为边界点。

具体的步骤是：首先定义一个空集合E，然后对网格数据中的每个三角面片T_i(i＝1，...，n；n为三角面片总数)，对T_i的每一条边e_j(j＝1，2，3)，判断E中是否包含e_j，若E中已包含e_j，表明e_j已经属于另一个三角面片，故e_j不是孤立边，在E中删除e_j，若E中不包含e_j，将e_j加入E。如此处理所有的三角面片后，E中剩余的边即为网格数据的孤立边，而属于这些孤立边的顶点即为边界点。

(3)计算深度图像中非边界顶点的特征值。

本发明采用物体表面的微分特性作为特征描述符。曲率是物体表面微分特性的主要描述手段，其中又包括主曲率、平均曲率、高斯曲率等，由于主曲率中蕴含着其他曲率，所以采用顶点处的主曲率作为该顶点特征值。

设p是曲面S上的一个点，考虑通过p点的所有曲线C_i，可以计算出每条曲线在p点处的曲率k_i。令这些曲率中最大的是k₁，最小的是k₂，则k₁、k₂组成了曲面S的p点处的主曲率。经典微分几何理论中的曲率是定义在正则曲面上的，对于网格数据，本发明采用曲面拟合的方法计算顶点处的曲率。首先用多项式近似表示曲面，再用最小二乘法对曲面进行拟合。对于网格表面的任意顶点p，设p的k个最近点为{p₁，p₂，......，p_k}，待拟合多项式为f(x，y)＝z，将k个点的坐标代入f(x，y)＝z，得到一个线性方程组：Ax＝b，其中A是一个k×n矩阵，n为f(x，y)中系数的个数，b为这k个点的z坐标构成的向量。方程的最小二乘解即为f(x，y)＝z的拟合系数。

一般的，使用二次曲面f(x，y)＝a₀+a₁x+a₂y+a₃xy+a₄x²+a₅y²作为待拟合曲面，在计算任意顶点p点处的主曲率时，首先将二次曲面表示为参数形式r(u，v)＝(u，v，a₀+a₁u+a₂v+a₃uv+a₄u²+a₅v²)，接着计算出第一类和第二类基本参数：E＝r_u·r_u，F＝r_u·r_v，G＝r_v·r_v，L＝r_uu·n，M＝r_uv·n，N＝r_vv·n，然后计算Weingarten矩阵 $\left[\begin{array}{cc}\frac{\mathrm{LG}-\mathrm{MF}}{\mathrm{EG}-F^2}& \frac{\mathrm{MG}-\mathrm{NF}}{\mathrm{EG}-F^2}\\ \frac{\mathrm{ME}-\mathrm{LF}}{\mathrm{EG}-F^2}& \frac{\mathrm{NE}-\mathrm{MF}}{\mathrm{EG}-F^2}\end{array}\right],$ 最后计算出其特征值即为待计算的主曲率(κ₁，κ₂)。

(4)根据顶点的特征值，选取具有突出表面特征的特征点集。

使用八叉树细分每一幅深度图像，并在八叉树的每个叶节点中，选取特征值最大的顶点作为该节点的特征点，所有八叉树叶节点的特征点的集合，即构成这幅深度图像的特征点集。八叉树的深度决定特征点的数目和整个配准过程的耗时，即深度越大，提取的特征点越多，配准结果越准确，但配准时间越长。一般地，八叉树的深度取3或4，即可合理的时间内取得较好的配准结果。图1和图2分别是采用深度为3和4的八叉树细分深度图像并提取特征点的结果。

(5)对于第一幅图像的每个特征点，在第二幅图像的所有顶点中搜索主曲率最相近的点作为候选对应点，并采用欧式距离和夹角等全局几何约束条件，剔除错误的对应关系；最后找出至少三对对应的特征点。

具体步骤是：首先，将第二幅深度图像中每个顶点的主曲率看作二维空间的一个点，构建一棵kd-tree，接着对第一幅图像中所有的特征点，在kd-tree中搜索第二幅图像中与它们特征值最接近的特征点，组成特征点对，设(p_i，q_i){i＝1，......，n}是这两幅深度图像间的n对特征对应点；然后对这n对特征对应点进行以下两个几何一致性测试：

|‖p_i-p_j‖-‖q_i-q_j‖|＜ε₁

|p_i·p_j-q_i·q_j|＜ε₂

满足条件的点对即为对应特征点，其中的ε₁和ε₂为预先指定的阀值。

(6)根据这些对应点计算出这两幅深度图像间运动的估计值。

本发明采用Horn，B.：Closed-form solution of absolute orientation usingunit quaternions.Journal of the Optical Society of America 4(1987)629-642中的方法，根据两幅深度图像间至少三对对应点计算出它们之间的运动矩阵。当存在不唯一的对应点对时，需要计算出每种对应关系下的运动矩阵，并计算在该运动矩阵变换下，两幅深度图像间的距离，然后找出距离最小时的运动矩阵，并将其作为粗略配准的结果。

(7)使用一种改进的ICP算法优化步骤(6)中的结果，完成两幅深度图像的精确配准。本方法在Besl，P.J.，McKay，N.D.：A method for registration of3-d shapes.IEEE Trans.Pattern Anal.Mach.Intell.14(2)(1992)239-256中ICP经典算法上做了以下改进：在做迭代之前，先提取出深度图像的边界点，在后面的迭代过程中，不再考虑这些点，从而提高了配准结果的准确性；在每一次迭代过程中，根据误差矩阵动态的调整距离阀值，从而加快了算法的收敛速度。

具体步骤如下：

第一步，初始化，本发明设精确配准所求的变换矩阵T为单位矩阵，初始的距离阈值ε由用户指定。

第二步，选取参考点：在第一幅图像中均匀选取非边界点作为参考点，因为扫描的深度图像中包含密集采样点，为了提高计算效率，仅选择其中一些点参与精确配准，根据本文的实验结果，可以选取全部点的10％左右作为参考点。第三步，设定距离阈值：计算上次迭代后的误差 $e=\sqrt{\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|p_i-q_i\left|\right|}^2},$ 设新的距离阈值ε＝3e。

第四步，匹配：为每个参考点在第二幅图像中匹配对应点。由于每次迭代后误差减小，这样在下次迭代时，减小了搜索范围，提高搜索速度。使用kd-tree在第二幅图像中搜索距离阈值范围内的最近点作为对应点，如果阈值内没有点或最近点为边界点，则该参考点为无效点，否则为有效参考点的对应点。

第五步，最小化误差函数。计算出对第一幅图像的刚性变换矩阵T_k，使其对第一幅图像变换后误差 $\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|T_k{p}_i-q_i\left|\right|}^2$ 最小。其中平移变换是点集p_i和q_i重心重合的平移变换，平移变换后，使用四元数计算旋转变换。设p_ixp_iy分别表示p_i的x、y坐标，q_ixq_iy分别表示q_i的x、y坐标， $S_{\mathrm{mn}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{p}_{\mathrm{im}}{q}_{\mathrm{in}},$ 为了计算旋转变换，计算矩阵

$N=\left[\begin{array}{cccc}{S}_{\mathrm{xx}}+S_{\mathrm{yy}}+S_{\mathrm{zz}}& S_{\mathrm{yz}}-S_{\mathrm{zy}}& S_{\mathrm{zx}}-S_{\mathrm{xz}}& S_{\mathrm{xy}}-S_{\mathrm{ux}}\\ S_{\mathrm{yz}}-S_{\mathrm{zy}}& S_{\mathrm{xx}}-S_{\mathrm{yy}}-S_{\mathrm{zz}}& S_{\mathrm{xy}}+S_{\mathrm{yx}}& S_{\mathrm{zx}}+S_{\mathrm{xz}}\\ S_{\mathrm{zx}}-S_{\mathrm{xz}}& S_{\mathrm{xy}}-S_{\mathrm{yx}}& -S_{\mathrm{xx}}+S_{\mathrm{yy}}-S_{\mathrm{zz}}& S_{\mathrm{yz}}+S_{\mathrm{zy}}\\ S_{\mathrm{xy}}-S_{\mathrm{yx}}& S_{\mathrm{zx}}+S_{\mathrm{xz}}& S_{\mathrm{yz}}+S_{\mathrm{zy}}& -S_{\mathrm{xx}}-S_{\mathrm{yy}}+S_{\mathrm{zz}}\end{array}\right]$

矩阵N的最大特征值对应的特征向量作为旋转变换的四元数q＝(w，x，y，z)，保证了是最优变换，设q所表示的旋转变换的矩阵表示为R_3×3，t_3×1表示平移向量，由旋转变换和平移变换得到这次迭代的刚性变换矩阵 $T_k={\left[\begin{array}{cccc}R& & & t\\ 0& 0& 0& 1\end{array}\right]}_{4\×4}.$ 所求的总的变换矩阵变为T＝T_kT。若误差大于预先指定阈值，转第2步，否则，退出。

如图3所示，以Buddha模型的配准过程进行说明。Buddha模型共包含15幅原始的深度图像数据，按扫描顺序设它们依次为R₁，...，R₁₅，即相邻两幅深度图像存在重合区域。设T_i(i＝1，...，14)为R_i和R_i+1间的运动矩阵，即通过R_kT_k-1(k＝2，...，14)即可将R_k变换到R_k-1的坐标系中。

采用本文方法，依次对R_i和R_i+1(i＝1...14)做两两配准，获得它们间的运动矩阵T_i，然后通过R_i+1’＝R_i+1T_iT_i-1...T₁，将R_i+1转换到R₁的坐标系中，R_i+1’即为R_i+1在R₁坐标系中对应的图像。最后合并R₁，R₂’，...，R₁₅’，即可以得到Buddha模型的完整点云数据。

其中图3a为R1和R2配准前的状态；图3b为R₁和R₂配准后的状态；图3c为R₂和R₃配准前的状态；图3d为最终的配准结果。

Claims (9)

1、一种自动的深度图像配准方法，其特征在于它包括步骤如下：

(1)对深度图像进行三角化处理，将点云数据转换成三角网格数据；

(2)根据所述的三角网格数据，找出深度图像中的边界点；

(3)计算深度图像中非边界顶点的特征值；

(4)根据非边界顶点的特征值，提取深度图像中具有突出表面特征的特征点集；

(5)对于待配准的两幅深度图像，寻找它们的特征点之间的匹配关系，即对应点，找出至少三对对应的顶点；

(6)根据所述的对应点计算出这两幅深度图像间运动的估计值；

(7)采用改进的ICP算法优化步骤(6)中的结果，完成两幅深度图像的精确配准。

2、根据权利要求1所述的自动的深度图像配准方法，其特征在于：所述步骤(1)中，将点云数据转换成三角网格数据的方法是：将每幅深度图像投影到一个二维平面上，然后使用二维的Delaunay三角剖分，获得所有点之间的连接关系，再将这种连接关系映射回三维空间，就完成了对点云数据的三角化处理。

3、根据权利要求1所述的自动的深度图像配准方法，其特征在于：所述步骤(2)中，根据所述的三角网格数据，找出深度图像中的边界点的方法为：在已知三角网格数据的情况下，通过遍历整个的三角网格数据，找出所有的孤立边，即那些只属于一个三角形的边，这些孤立边的集合就是网格数据的边界，属于这些孤立边的两个顶点即为边界点。

4、根据权利要求1所述的自动的深度图像配准方法，其特征在于：所述步骤(3)中，采用模型表面每一顶点处的主曲率作为该顶点的特征值。

5、根据权利要求1所述的自动的深度图像配准方法，其特征在于：所述步骤(4)中，提取任意深度图像中特征点的方法是：首先采用八叉树细分深度图像，然后在八叉树的每个叶节点中，选取特征值最大的顶点作为该节点的特征点，所有叶节点的特征点的集合，构成这幅深度图像的特征点集。

6、根据权利要求5所述的自动的深度图像配准方法，其特征在于：所述的八叉树的深度取3或4。

7、根据权利要求1所述的自动的深度图像配准方法，其特征在于：所述步骤(5)中，寻找两幅深度图像对应点的方法是：

(1)将第二幅深度图像中每个顶点的主曲率看作二维空间的一个点，构建一棵kd-tree，接着对第一幅图像中所有的特征点，在kd-tree中搜索第二幅图像中与它们特征值最接近的特征点，组成特征点对，设(p_i，q_i){i＝1，……，n}是这两幅深度图像间的n对特征对应点；

(2)对这n对特征对应点进行以下两个几何一致性测试：

|‖p_i-p_j‖-‖q_i-q_j‖|＜ε₁

|p_i·p_j-q_i·q_j|＜ε₂

满足条件的点对即为对应特征点，其中的ε₁和ε₂为预先指定的阀值。

8、根据权利要求1所述的自动的深度图像配准方法，其特征在于：所述步骤(6)中，根据所述的对应点计算出这两幅深度图像间运动的估计值的方法为：估算出一个运动矩阵，并计算在这个运动矩阵下，两幅深度图像的距离；若该距离小于当前的最小值，取该距离为当前最小值，并记录当前的运动矩阵；如此计算所有满足的点对，找出距离最小时的运动矩阵，并将其作为粗略配准的结果。

9、根据权利要求1所述的自动的深度图像配准方法，其特征在于：所述步骤(7)中改进的ICP算法为：

(1)初始化，设精确配准所求的变换矩阵T为单位矩阵，初始的距离阈值ε由用户指定；

(2)选取参考点：在第一幅图像中均匀选取非边界点作为参考点；

(3)设定距离阈值：计算上次迭代后的误差 $e=\sqrt{\underset{n}{\overset{1}{-}}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left|\right|p_i-q_i|{|}^2},$ 设新的距离阈值ε＝3e；

(4)匹配：为每个参考点在第二幅图像中匹配对应点，使用kd-tree在第二幅图像中搜索距离阈值范围内的最近点作为对应点，如果阈值内没有点或最近点为边界点，则该参考点为无效点，否则为有效参考点的对应点；

(5)最小化误差函数：计算出对第一幅图像的刚性变换矩阵T_k，使其对第一幅图像变换后误差 $\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|T_k{p}_i-q_i\left|\right|}^2$ 最小，其中平移变换是点集p_i和q_i重心重合的平移变换，平移变换后，使用四元数计算旋转变换，设p_ixp_iy分别表示p_i的x、y坐标，q_ixq_iy分别表示q_i的x、y坐标， $S_{\mathrm{mn}}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{p}_{\mathrm{im}}{q}_{\mathrm{in}},$ 计算矩阵

$N=\left[\begin{array}{cccc}{S}_{\mathrm{xx}}+S_{\mathrm{yy}}+S_{\mathrm{zz}}& S_{\mathrm{yz}}-S_{\mathrm{zy}}& S_{\mathrm{zx}}-S_{\mathrm{xz}}& S_{\mathrm{xy}}-S_{\mathrm{ux}}\\ S_{\mathrm{yz}}-S_{\mathrm{zy}}& {S_{\mathrm{xx}}-S}_{\mathrm{yy}}-S_{\mathrm{zz}}& S_{\mathrm{xy}}+S_{\mathrm{yx}}& S_{\mathrm{zx}}+S_{\mathrm{xz}}\\ S_{\mathrm{zx}}-S_{\mathrm{xz}}& S_{\mathrm{xy}}-S_{\mathrm{yx}}& -S_{\mathrm{xx}}+S_{\mathrm{yy}}-S_{\mathrm{zz}}& S_{\mathrm{yz}}+S_{\mathrm{zy}}\\ S_{\mathrm{xy}}-S_{\mathrm{yx}}& S_{\mathrm{zx}}+S_{\mathrm{xz}}& S_{\mathrm{yz}}+S_{\mathrm{zy}}& {-S}_{\mathrm{xx}}-S_{\mathrm{yy}}+S_{\mathrm{zz}}\end{array}\right]$

矩阵N的最大特征值对应的特征向量作为旋转变换的四元数q＝(w，x，y，z)，设q所表示的旋转变换的矩阵表示为R_3×3，t_3×1表示平移向量，由旋转变换和平移变换得到这次迭代的刚性变换矩阵 $T_k={\left[\begin{array}{cccc}R& & & t\\ & & & \\ & & & \\ 0& 0& 0& 1\end{array}\right]}_{4\×4},$ 所求的总的变换矩阵变为T＝T_kT，若误差大于预先指定阈值，转入第2步，否则，退出。

Images