Fast Global Registration算法利用四元约束剔除匹配的关系集中错误的关系对,最后基于关系集优化求解实现两组点云的配准。该算法主要分为三个步骤:基于四元约束的关系集的生成、基于关系集目标函数的构建、目标函数的优化过程。
基于四元约束的关系集的生成,主要分为四个阶段: 点集的FPFH 特征描述子的计算、初始关系集的生成、关系集的互为最近邻约束、关系集的四元约束。
其中 τ = 0.95 τ = 0. 95 τ=0.95。直观地,该约束条件验证了四组关系对之间是否兼容。把满足条件的元组关系对存储到集合 K 3 K_3 K3中。图1 为四元约束图解。
经过上述两个条件约束后得到关系集 K = K 3 K = K_3 K=K3。
点云 P P P 和 Q Q Q 的配准,目标是找到一个最优的刚性变换矩阵 T T T,将点云 Q Q Q 与 P P P 配准。算法优化了基于 P P P 和 Q Q Q 之间对应关系的鲁棒的目标函数。首先,关系集是通过执行的快速特征匹配和关系约束建立而来的。在目标函数的优化期间,关系集不需要重新进行计算,因此对于密集和复杂的点云的配准,算法有很大的优势。
集合 K = ( p , q ) K = { ( p ,q) } K=(p,q)是由 P P P 和 Q Q Q 中的匹配点生成的关系集,以关系集中对应关系对之间误差的平方和为条件建立目标函数。目标函数具体形式如下:
这里 ρ ( ⋅ ) ρ(·) ρ(⋅) 是一个鲁棒的惩罚函数。鲁棒的惩罚函数的适当使用是非常重要的,因为目标函数( 2)中的许多误差项是由错误匹配关系产生的,这样可以减小错误关系对目标函数的影响,从而实现更好的配准精度。同时,为了保证计算速度,在优化过程中不希望进行下采样和验证等额外的计算。本文精心选择一个名为Geman - McClure 估计算子 ρ ρ ρ,它将自动执行验证,同时也不会增加额外的计算成本。具体的表达式为:
图2 显示了不同 μ μ μ 值的Geman - McClure 估计算子图像。从图2 可以看出,残差以最小二乘的方式进行惩罚,同时估计算子的快速平坦化中和了关系集的异常值。参数 μ μ μ 控制着残差对目标函数所产生的重要影响。
因为方程( 2) 不易直接进行优化求解,所以引入线处理。具体来说,令 L = l p , q , 0 < l p , q < 1 L = { lp ,q} ,0 <lp ,q < 1 L=lp,q,0<lp,q<1 ,线处理表示关系对 p p p和 q q q 之间的一种不连续性,当 l p , q → 0 lp,q →0 lp,q→0 不连续性存在,或者当 l p , q → 1 lp,q →1 lp,q→1 不连续性不存在。优化关于 T T T 和 L L L 的联合目标函数,具体形式如下:
这里的 Ψ ( l p , q ) Ψ( lp,q) Ψ(lp,q) 是先验项,它一个惩罚函数,表示对 p p p 和 q q q 之间不连续性的惩罚,其形式为:
为了使得 E ( T , L ) E( T,L) E(T,L) 最小化,对方程( 5) 关于 l p , q lp,q lp,q求偏导,令其导函数为零,得到如下式子:
最后,将 l p , q lp,q lp,q带入 E ( T , L ) E( T,L) E(T,L) 中,方程( 4) 变换成方程( 2) 。因此,优化目标函数( 4) 产生的变换矩阵 T T T,对于最初的目标函数( 2) 也是最优解。
方程( 4) 是非凸的,并且其形状由惩罚函数( 方程( 3) ) 的参数 μ μ μ 来控制。为了设置 μ μ μ 并减少局部最小的影响,采用渐变非凸度方法求解。从方程( 4) 可以看出 μ μ μ 平衡先验项和对齐项。较大的 μ μ μ 使得目标函数更平滑,并且允许一些错误的关系对参与优化,即使它们不能经过转换 T T T 紧密地对齐。本文算法的目标函数( 4) 优化开始于非常大的 μ = D 2 μ = D^2 μ=D2值,其中 D D D 是最大曲面的直径。参 μ μ μ在优化期间不断减小直到 μ < δ 2 μ < δ^2 μ<δ2停止优化,其中 δ δ δ是真实关系的距离阈值。
将变换矩阵T 局部地线性化为一个6 维向量 ξ = ( w , t ) = ( α , β , γ , a , b , c ) ξ=(w,t)=(\alpha,\beta,\gamma,a,b,c) ξ=(w,t)=(α,β,γ,a,b,c)该向量包含了旋转分量 w w w和平移分量 t t t。 T T T由 ξ ξ ξ的线性函数近似求得:
这里 T k T^k Tk是在上一次迭代中所求的变换矩阵。方程( 4) 变成关于 ξ ξ ξ 的最小二乘的目标函数。考虑使用高斯- 牛顿法求解,通过求解线性系统来计算 ξ ξ ξ 的值,得到:
其中 r r r 是残差向量, J r Jr Jr是其雅可比矩阵。利用方程( 9) 求得的 ξ ξ ξ 和 T k T^k Tk 的值,通过方程( 8) 来更新 T T T。两个步骤优化了相同的目标函数( 方程( 4) ) ,因此优化过程可以保证算法收敛本文算法的流程图如表1 所示。