欢迎关注更多精彩
关注我,学习常用算法与数据结构,一题多解,降维打击。
问题描述
之前推导过不带权重的icp计算
思路与之类似
现在有配对好的两个点集
P = p 1 , ⋯ , p n , P ′ = p 1 ′ , ⋯ , p n ′ , W = w 1 , ⋯ , w n 是每个点的权重 P={p_1, \cdots, p_n}, P'={p_1', \cdots, p_n'}, W = {w_1, \cdots, w_n} 是每个点的权重 P=p1,⋯,pn,P′=p1′,⋯,pn′,W=w1,⋯,wn是每个点的权重
权重越高,代表相应的误差要越小
想要找到一个R, t 使得E能量最小
E = m i n R , t 1 2 ∑ i = 1 n w i ∥ p i − ( R p i ′ + t ) ∥ 2 E= \displaystyle \underset{R,t}{min}\frac 1 2\sum_{i=1}^n w_i\|p_i-(Rp_i'+t)\|^2 E=R,tmin21i=1∑nwi∥pi−(Rpi′+t)∥2
变量定义和基本定理
为了后续推导方便,先定义一些常量符号和基本定理
质心定义
p = ∑ i = 1 n w i p i ∑ i = 1 n w i , p ′ = ∑ i = 1 n w i p i ′ ∑ i = 1 n w i p=\displaystyle \frac { \sum_{i=1}^nw_ip_i}{\sum_{i=1}^nw_i}, p'=\frac { \sum_{i=1}^nw_ip_i'}{\sum_{i=1}^nw_i} p=∑i=1nwi∑i=1nwipi,p′=∑i=1nwi∑i=1nwipi′
去质心替换
q i = p i − p , q i ′ = p i ′ − p ′ q_i = p_i-p, q_i'=p_i'-p' qi=pi−p,qi′=pi′−p′
Schwarz 不等式
对于向量A, B, A与B的内积不大于A与B模长的乘积。
A ⋅ B ≤ ∥ A ∥ ⋅ ∥ B ∥ A\cdot B \le \|A\|\cdot \|B\| A⋅B≤∥A∥⋅∥B∥
正交矩阵性质
R R T = 1 RR^T=1 RRT=1
引理证明
后面会用到几个公式,这里先证明一下。
1
q
T
R
q
′
=
t
r
(
R
q
′
q
T
)
q^TRq'=tr(Rq'q^T)
qTRq′=tr(Rq′qT)
tr(A) 表示A的对角线元素的和
基本思路就是把等式左右两边展开,计算出最后结果即可
设 R = [ A 11 A 12 A 13 A 21 A 22 A 23 A 31 A 32 A 33 ] R=\begin{bmatrix} A_{11}&A_{12}&A_{13}\\ A_{21}&A_{22}&A_{23}\\ A_{31}&A_{32}&A_{33} \end{bmatrix} R= A11A21A31A12A22A32A13A23A33
q = [ q 1 q 2 q 3 ] , q ′ = [ q 1 ′ q 2 ′ q 3 ′ ] q=\begin{bmatrix} q_1\\ q_2 \\q_3 \end{bmatrix}, q'=\begin{bmatrix} q_1'\\ q_2' \\q_3' \end{bmatrix} q= q1q2q3 ,q′= q1′q2′q3′
证明:
等式左边
q T R q ′ = q T [ A 11 q 1 ′ + A 12 q 2 ′ + A 13 q 3 ′ A 21 q 1 ′ + A 22 q 2 ′ + A 23 q 3 ′ A 31 q 1 ′ + A 32 q 2 ′ + A 33 q 3 ′ ] q^TRq' = q^T\begin{bmatrix} A_{11}q_1'+A_{12}q_2'+A_{13}q_3'\\ A_{21}q_1'+A_{22}q_2'+A_{23}q_3'\\ A_{31}q_1'+A_{32}q_2'+A_{33}q_3' \end{bmatrix} qTRq′=qT A11q1′+A12q2′+A13q3′A21q1′+A22q2′+A23q3′A31q1′+A32q2′+A33q3′
= A 11 q 1 q 1 ′ + A 12 q 1 q 2 ′ + A 13 q 1 q 3 ′ + A 21 q 2 q 1 ′ + A 22 q 2 q 2 ′ + A 23 q 2 q 3 ′ + A 31 q 3 q 1 ′ + A 32 q 3 q 2 ′ + A 33 q 3 q 3 ′ = A_{11}q_1q_1'+A_{12}q_1q_2'+A_{13}q_1q_3'+ A_{21}q_2q_1'+A_{22}q_2q_2'+A_{23}q_2q_3'+ A_{31}q_3q_1'+A_{32}q_3q_2'+A_{33}q_3q_3' =A11q1q1′+A12q1q2′+A13q1q3′+A21q2q1′+A22q2q2′+A23q2q3′+A31q3q1′+A32q3q2′+A33q3q3′
等式右边
R q ′ q T = [ A 11 q 1 ′ + A 12 q 2 ′ + A 13 q 3 ′ A 21 q 1 ′ + A 22 q 2 ′ + A 23 q 3 ′ A 31 q 1 ′ + A 32 q 2 ′ + A 33 q 3 ′ ] q T Rq'q^T=\begin{bmatrix} A_{11}q_1'+A_{12}q_2'+A_{13}q_3'\\ A_{21}q_1'+A_{22}q_2'+A_{23}q_3'\\ A_{31}q_1'+A_{32}q_2'+A_{33}q_3' \end{bmatrix}q^T Rq′qT= A11q1′+A12q2′+A13q3′A21q1′+A22q2′+A23q3′A31q1′+A32q2′+A33q3′ qT
= [ A 11 q 1 q 1 ′ + A 12 q 1 q 2 ′ + A 13 q 1 q 3 ′ A 11 q 2 q 1 ′ + A 12 q 2 q 2 ′ + A 13 q 2 q 3 ′ A 11 q 3 q 1 ′ + A 12 q 3 q 2 ′ + A 13 q 3 q 3 ′ A 21 q 1 q 1 ′ + A 22 q 1 q 2 ′ + A 23 q 1 q 3 ′ A 21 q 2 q 1 ′ + A 22 q 2 q 2 ′ + A 23 q 2 q 3 ′ A 21 q 3 q 1 ′ + A 22 q 3 q 2 ′ + A 23 q 3 q 3 ′ A 31 q 1 q 1 ′ + A 32 q 1 q 2 ′ + A 33 q 1 q 3 ′ A 31 q 2 q 1 ′ + A 32 q 2 q 2 ′ + A 33 q 2 q 3 ′ A 31 q 3 q 1 ′ + A 32 q 3 q 2 ′ + A 33 q 3 q 3 ′ ] =\begin{bmatrix}A_{11}q_1q_1'+A_{12}q_1q_2'+A_{13}q_1q_3' & A_{11}q_2q_1'+A_{12}q_2q_2'+A_{13}q_2q_3' & A_{11}q_3q_1'+A_{12}q_3q_2'+A_{13}q_3q_3'\\ A_{21}q_1q_1'+A_{22}q_1q_2'+A_{23}q_1q_3' & A_{21}q_2q_1'+A_{22}q_2q_2'+A_{23}q_2q_3'&A_{21}q_3q_1'+A_{22}q_3q_2'+A_{23}q_3q_3'\\ A_{31}q_1q_1'+A_{32}q_1q_2'+A_{33}q_1q_3' &A_{31}q_2q_1'+A_{32}q_2q_2'+A_{33}q_2q_3' &A_{31}q_3q_1'+A_{32}q_3q_2'+A_{33}q_3q_3'\end{bmatrix} = A11q1q1′+A12q1q2′+A13q1q3′A21q1q1′+A22q1q2′+A23q1q3′A31q1q1′+A32q1q2′+A33q1q3′A11q2q1′+A12q2q2′+A13q2q3′A21q2q1′+A22q2q2′+A23q2q3′A31q2q1′+A32q2q2′+A33q2q3′A11q3q1′+A12q3q2′+A13q3q3′A21q3q1′+A22q3q2′+A23q3q3′A31q3q1′+A32q3q2′+A33q3q3′
提取出对角线元素刚好与左边相等。
2
M是一个对称正定矩阵,那么对于任意正交矩阵R, 会有tr(M)>=tr(RM)
证明
可以设 M = A A T M=AA^T M=AAT
t r ( R A A T ) = t r ( A T R A ) = ∑ a i T ( R a i ) tr(RAA^T) = tr(A^TRA)=\sum a_i^T(Ra_i) tr(RAAT)=tr(ATRA)=∑aiT(Rai)
根据schwarz 不等式
∑ a i T ( R a i ) ≤ ∑ a i T a i ( a i T R T R a i ) = ∑ a i T a i = t r ( M ) \sum a_i^T(Ra_i) \le \sum \sqrt{a_i^Ta_i} \sqrt{(a_i^TR^TRa_i)} = \sum a_i^Ta_i=tr(M) ∑aiT(Rai)≤∑aiTai(aiTRTRai)=∑aiTai=tr(M)
定理得证。
3
等式
∑
i
=
1
n
w
i
(
p
i
−
p
−
R
(
p
i
′
−
p
′
)
)
=
0
\displaystyle \sum_{i=1}^nw_i(p_i-p-R(p_i'-p'))=0
i=1∑nwi(pi−p−R(pi′−p′))=0
对上式展开得到
( ∑ i = 1 n w i p i ) − ( ∑ i = 1 n w i ) p − R ( ∑ i = 1 n w i p i ′ ) + ( ∑ i = 1 n w i ) R p ′ (\displaystyle \sum_{i=1}^n w_i p_i)-(\sum_{i=1}^nw_i)p-R(\sum_{i=1}^n w_ip_i')+(\sum_{i=1}^nw_i)Rp' (i=1∑nwipi)−(i=1∑nwi)p−R(i=1∑nwipi′)+(i=1∑nwi)Rp′
由质心公式可以得到 ( ∑ i = 1 n w i ) p = ∑ i = 1 n w i p i , ( ∑ i = 1 n w i ) p ′ = ∑ i = 1 n w i p i ′ \displaystyle (\sum_{i=1}^nw_i)p= \sum_{i=1}^nw_ip_i, (\sum_{i=1}^nw_i)p'= \sum_{i=1}^nw_ip_i' (i=1∑nwi)p=i=1∑nwipi,(i=1∑nwi)p′=i=1∑nwipi′,所以上式等于0。
能量方程化简
利用构造法化简
1/2常数项先拿掉
∑ i = 1 n w i ∥ p i − ( R p i ′ + t ) ∥ 2 = ∑ i = 1 n w i ∥ p i − R p i ′ − t − p + R p ′ + p − R p ′ ∥ 2 \displaystyle \sum_{i=1}^nw_i\|p_i-(Rp_i'+t)\|^2= \sum_{i=1}^nw_i\|p_i-Rp_i'-t -p +Rp' +p -Rp'\|^2 i=1∑nwi∥pi−(Rpi′+t)∥2=i=1∑nwi∥pi−Rpi′−t−p+Rp′+p−Rp′∥2
= ∑ i = 1 n w i ∥ ( p i − p − R ( p i ′ − p ′ ) ) + ( p − R p ′ − t ) ∥ 2 =\displaystyle \sum_{i=1}^nw_i\|(p_i-p - R(p_i'-p')) +(p -Rp'-t)\|^2 =i=1∑nwi∥(pi−p−R(pi′−p′))+(p−Rp′−t)∥2
利用 ∥ a + b ∥ 2 = a T a + 2 a b + b T b \|\boldsymbol a+\boldsymbol b\|^2 = \boldsymbol a^T \boldsymbol a+2 \boldsymbol a \boldsymbol b+ \boldsymbol b^T \boldsymbol b ∥a+b∥2=aTa+2ab+bTb二次多项式展开
= ∑ i = 1 n w i ( ∥ p i − p − R ( p i ′ − p ′ ) ∥ 2 + ∥ ( p − R p ′ − t ) ∥ 2 ) + ∑ i = 1 n w i ( 2 ( p i − p − R ( p i ′ − p ′ ) ) T ( p − R p ′ − t ) ) =\displaystyle \sum_{i=1}^nw_i(\|p_i-p - R(p_i'-p')\|^2 +\|(p -Rp'-t)\|^2) + \sum_{i=1}^nw_i(2(p_i-p - R(p_i'-p'))^T(p -Rp'-t)) =i=1∑nwi(∥pi−p−R(pi′−p′)∥2+∥(p−Rp′−t)∥2)+i=1∑nwi(2(pi−p−R(pi′−p′))T(p−Rp′−t))
上式中根据定义引理3 后面一项结果为0.
能量方程转化为:
E = m i n R , t 1 2 ∑ i = 1 n w i ∥ p i − p − R ( p i ′ − p ′ ) ∥ 2 + m i n R , t 1 2 ∑ i = 1 n w i ∥ p − R p ′ − t ∥ 2 E=\underset{R,t}{min}\displaystyle \frac 1 2 \sum_{i=1}^n w_i\|p_i-p - R(p_i'-p')\|^2+\underset{R,t}{min} \frac 1 2 \sum_{i=1}^n w_i\|p -Rp'-t\|^2 E=R,tmin21i=1∑nwi∥pi−p−R(pi′−p′)∥2+R,tmin21i=1∑nwi∥p−Rp′−t∥2
上述方程中,对于给定的R,总是可以找到一个t使得后一项为0, 那只要优化前一项就可以了。
J = m i n R 1 2 ∑ i = 1 n w i ∥ p i − p − R ( p i ′ − p ′ ) ∥ 2 = m i n R 1 2 ∑ i = 1 n w i ∥ q i − R q i ′ ∥ 2 J=\underset{R}{min}\frac 1 2 \displaystyle \sum_{i=1}^n w_i \|p_i-p - R(p_i'-p')\|^2 = \underset{R}{min}\frac 1 2 \sum_{i=1}^n w_i \|q_i - Rq_i'\|^2 J=Rmin21i=1∑nwi∥pi−p−R(pi′−p′)∥2=Rmin21i=1∑nwi∥qi−Rqi′∥2
= m i n R 1 2 ∑ i = 1 n w i ( q i T q i + q i ′ T R T R q i ′ − 2 q i T R q i ′ ) =\underset{R}{min}\frac 1 2 \sum_{i=1}^n w_i (q_i^Tq_i + q_i'^TR^TRq_i'-2q_i^TRq_i') =Rmin21∑i=1nwi(qiTqi+qi′TRTRqi′−2qiTRqi′)
优化求解
上式中第1,2项为常数,只要优化第三项就可以了。
根据引理1(去负号时优化最小值变成优化最大值)
− m i n R ∑ i = 1 n w i ( q i T R q i ′ ) = m a x R ∑ i = 1 n w i t r ( R q i ′ q i T ) = m a x R t r ( R ∑ i = 1 n ( w i q i ′ q i T ) ) -\underset{R}{min} \displaystyle \sum_{i=1}^n w_i (q_i^TRq_i') = \underset{R}{max} \sum_{i=1}^n w_i tr(Rq_i'q_i^T)= \underset{R}{max} \ tr (R \sum_{i=1}^n (w_iq_i'q_i^T)) −Rmini=1∑nwi(qiTRqi′)=Rmaxi=1∑nwitr(Rqi′qiT)=Rmax tr(Ri=1∑n(wiqi′qiT))
根据引理2求解R
令
H
=
∑
i
=
1
n
(
w
i
q
i
′
q
i
T
)
=
U
Σ
V
T
,
X
=
V
U
T
为正交矩阵
H=\displaystyle \sum_{i=1}^n (w_iq_i'q_i^T) = U\Sigma V^T, X=VU^T为正交矩阵
H=i=1∑n(wiqi′qiT)=UΣVT,X=VUT为正交矩阵
X H = V U T U Σ V T , X = V Σ V T XH= VU^TU\Sigma V^T, X=V\Sigma V^T XH=VUTUΣVT,X=VΣVT
X H 为对称正定矩阵 XH为对称正定矩阵 XH为对称正定矩阵
设B为正交矩阵, BX也为正交矩阵
则根据引理2 可得
tr(XH)>=tr(BXH)
可知J方程的最优解为 R=BX=X。
算法步骤
- 根据定义主算出p, p’。
- 根据定义计算矩阵H。
- 对H进行svd分解。
- 求解出R。
(当R的行列式值为负数时,取-R作为最优解)
- t = p-Rp。
本人码农,希望通过自己的分享,让大家更容易学懂计算机知识。创作不易,帮忙点击公众号的链接,帮忙转发,感激不尽。