目录
1、参数平差
现有的针对ICP配准点云定权的方法都是简单地利用点对的距离绝对值的倒数作为权重值，这种方法形象直观，并且在某些情况下能起到抵御粗差的作用。但由于没有充分利用点对残差的统计分布规律，简单残差倒数的定权法不具备通用性。当ICP配准残差服从正态分布规律时，可以将残差对应的正态分布曲线函数作为残差的定权函数，即
P ( v ) = f ( v ) = 1 2 π δ ⋅ e − v 2 2 δ 2 (1) P(v)=f(v)=\frac{1}{\sqrt{2\pi\delta}}\cdot e^{\frac{-v^2}{\sqrt{2\delta^2}}}\tag{1} P(v)=f(v)=2πδ 1⋅e2δ2 −v2(1)
式(1)满足残差绝对值大的权重小、残差绝对值小的权重大的定权原则，而且能够保证权重值在0~1之间。
A x + B y + C z + D = 0 (1) Ax+By+Cz+D=0\tag{1} Ax+By+Cz+D=0(1)
其中， A , B , C , D A,B,C ,D A,B,C,D 分别为平面方程的四个参数; ( x ， y ， z ) ( x，y，z) (x，y，z)为平面上点的坐标。拟合的目的是通过观测该平面上的三个或三个以上点的坐标，获得该平面的参数。假设观测了 n n n个点的坐标，于是可以列立 n n n个观测方程，第 i i i个方程可以写成:
A x i + B y i + C z i + D = 0 (2) Ax_i+By_i+Cz_i+D=0\tag{2} Axi+Byi+Czi+D=0(2)
此时注意到式(2)中，独立的参数只有三个，这里不考虑平面经过原点情形，设 A ≠ 0 A≠0 A=0可将式(2)改写为:
a x i + b y i + c z i + 1 = 0 (3) ax_i+by_i+cz_i+1=0\tag{3} axi+byi+czi+1=0(3)
可见式(3)是线性模型，考虑到平面点 ( x i ， y i ， z i ) ( x_i，y_i，z_i) (xi，yi，zi) 观测误差，易得参数平差的误差方程:
v I = a x i + b y i + c z i + 1 (4) v_I=ax_i+by_i+cz_i+1\tag{4} vI=axi+byi+czi+1(4)
写成矩阵表达式为:
V = B x − L (5) V=Bx-L\tag{5} V=Bx−L(5)
其中，
V = v 1 v 2 ⋮ v n , B = x 1 y 1 z 1 x 2 y 2 z 2 ⋮ ⋮ ⋮ x n y n z n , x = a b c , L = − 1 − 1 ⋮ − 1 V=\left \\begin{matrix} v_1\\\\ v_2\\\\ \\vdots\\\\ v_n\\\\ \\end{matrix} \\right, B=\left \\begin{matrix} x_1 \& y_1\& z_1\\\\ x_2 \& y_2\& z_2\\\\ \\vdots \& \\vdots\& \\vdots\\\\ x_n \& y_n\& z_n\\\\ \\end{matrix} \\right, {x}=\left \\begin{matrix} a\\\\ b\\\\ c\\\\ \\end{matrix} \\right, \ L=\left \\begin{matrix} -1\\\\ -1\\\\ \\vdots\\\\ -1\\\\ \\end{matrix} \\right V= v1v2⋮vn ,B= x1x2⋮xny1y2⋮ynz1z2⋮zn ,x= abc , L= −1−1⋮−1
在最小二乘准则下，依据参数平差原理，解算式(5)得到:
x ^ = ( B T P B ) − 1 B T P L (6) \hat{x}=(B^TPB)^{-1}B^TPL\tag{6} x^=(BTPB)−1BTPL(6)
如果坐标的观测值中不存在着粗差， x ^ = ( a ^ , b ^ , c ^ ) T \hat{x} = ( \hat{a},\hat{b},\hat{c})^T x^=(a^,b^,c^)T 就是求解得到的空间平面的参数估值。
2、选权迭代法
选权迭代法的本质是在平差过程中使得观测值的权不断变化，经过多次迭代，从而使含有粗差的观测值的权函数为零或接近为零，使其在平差中作用变小，甚至不起作用。其基本步骤如下:
- 列立误差方程，令 P P P为 n n n阶单位阵。
- 组成并解算法方程，得到未知参数 x ^ \hat{x} x^的第一次估值，即:
x ^ ( 1 ) = ( B T P B ) − 1 B T P L (7) \hat{x}^{(1)}=(B^TPB)^{-1}B^TPL\tag{7} x^(1)=(BTPB)−1BTPL(7)
选取迭代法的核心是定权法则，常用的定权准则有Huber 函数、Tukey 函数和Hampel 函数等，这些方法都涉及到较繁琐的数学理论，不易理解。这里，不妨从几何意义入手来定义权。由解析几何知道，点到平面的距离公式可以表述为:
d i s t i = ∣ a ^ x i + b ^ y i + c ^ z i + 1 ∣ a ^ 2 + b ^ 2 + c ^ 2 (8) dist_i=\frac{|\hat{a}x_i+\hat{b}y_i+\hat{c}z_i+1|}{\sqrt{\hat{a}^2+\hat{b}^2+\hat{c}^2}}\tag{8} disti=a^2+b^2+c^2 ∣a^xi+b^yi+c^zi+1∣(8)
这样由第一次估值得每个点到平面的距离，我们知道粗差点到平面的距离相对其他点来说是较大的，取权:
p i = 1 d i s t i (9) p_i=\frac{1}{dist_i}\tag{9} pi=disti1(9)
定义：
P ( 1 ) = d i a g ( p 1 , p 2 , . . . , p n ) (10) P^{(1)}=diag(p_1,p_2,...,p_n)\tag{10} P(1)=diag(p1,p2,...,pn)(10)
由式(10)可见含粗差的观测值获得了较小的权，达到选权的目的。
-
运用参数平差原理得到 x ^ \hat{x} x^的第二次估值，即:
x ^ ( 2 ) = ( B T P ( 1 ) B ) − 1 B T P ( 1 ) L (11) \hat{x}^{(2)}=(B^TP^{(1)}B)^{-1}B^TP^{(1)}L\tag{11} x^(2)=(BTP(1)B)−1BTP(1)L(11) -
由 x ^ ( 2 ) \hat{x}^{(2)} x^(2)按照定权准则构造新的等价权阵 P ( 2 ) P^{(2)} P(2) ，再解算法方程，直到 ∣ ∣ x ( k + 1 ) ^ − x ( k ) ^ ∣ ∣ ||\hat{x^{(k+1)}}-\hat{x^{(k)}}|| ∣∣x(k+1)^−x(k)^∣∣的差值符合限差为止，最后结果为:
x ^ ( k ) = ( B T P ( k − 1 ) B ) − 1 B T P ( k − 1 ) L (12) \hat{x}^{(k)}=(B^TP^{(k-1)}B)^{-1}B^TP^{(k-1)}L\tag{12} x^(k)=(BTP(k−1)B)−1BTP(k−1)L(12)
3、参考文献
1王继刚. 基于选权迭代法的空间平面拟合 J. 山西建筑, 2018, 44 (10): 205-206.