SVD求解ICP问题

Background

ICP（Iterative Closest Point）问题，迭代最近点。已知一组三维点在两个坐标系中的坐标表示，求这两个坐标系之间的变换关系，称为ICP问题。

最开始想到这个问题，是想进行手眼标定，有一台机械臂以及一个深度相机，如何确定相机坐标系和机械臂坐标系之间的变换关系？后来想使用接口将机械臂末端移动至某个位姿，在深度相机图像中标出该点位置（通过专门的标注工具），这样得到了一个三维点在两个坐标系下的表示，这实际构建了一个方程组。设变换矩阵为$T$，点在两个坐标系下的表示为 p , p ′ p,p' p,p′，则方程组可表示为 p = T × p ′ p=T\\times p' p=T×p′。

一维情形只需要一个点，就可以基本确定两个坐标系的关系；二维情形需要两个点，只有一个点的话，两个坐标系可以绕该点进行旋转；进而推之，三维情形至少需要三个点。但是，采样过程是存在误差的，因此需要增加数据点控制误差。

ICP问题也常常在SLAM和无人驾驶的研究中出现，也称为3D点云之间的匹配问题，传感器外参的标定问题。

SVD求解ICP问题

有两组点 P = { p 1 , p 2 , . . . , p n } P=\\{p_1,p_2,...,p_n\\} P={p1​,p2​,...,pn​}和 P ′ = { p 1 ′ , p 2 ′ , . . . , p n ′ } P'=\\{p_1',p_2',...,p_n'\\} P′={p1′​,p2′​,...,pn′​}，需要找到到一组参数 { R , t } \\{R,t\\} {R,t}表示这两组点之间“最有可能的变换关系”，可以构建最小二乘问题如下

m i n R , t J = 1 2 ∑ i = 1 n ∣ ∣ p i − ( R p i ′ + t ) ∣ ∣ 2 s . t . R T R = I min_{R,t}J=\\frac{1}{2}\\sum_{i=1}^{n}||p_i-(Rp_i'+t)||^2\\\\ s.t. R^TR=I minR,t​J=21​i=1∑n​∣∣pi​−(Rpi′​+t)∣∣2s.t.RTR=I

Solution

首先，去质心坐标；
p = 1 n ∑ i = 1 n p i , p ′ = 1 n ∑ i = 1 n p i ′ p=\\frac{1}{n}\\sum_{i=1}^np_i,p'=\\frac{1}{n}\\sum_{i=1}^np_i' p=n1​∑i=1n​pi​,p′=n1​∑i=1n​pi′​
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′

误差函数化简如下
J = 1 2 ∑ i = 1 n ∣ ∣ p i − ( R p i ′ + t ) ∣ ∣ 2 J=\\frac{1}{2}\\sum_{i=1}^{n}||p_i-(Rp_i'+t)||^2 J=21​∑i=1n​∣∣pi​−(Rpi′​+t)∣∣2
= 1 2 ∑ i = 1 n ∣ ∣ p i − ( R p i ′ + t ) − p + R p ′ + p − R p ′ ∣ ∣ 2 =\\frac{1}{2}\\sum_{i=1}^{n}||p_i-(Rp_i'+t)-p+Rp'+p-Rp'||^2 =21​∑i=1n​∣∣pi​−(Rpi′​+t)−p+Rp′+p−Rp′∣∣2
= 1 2 ∑ i = 1 n ∣ ∣ ( q i − R q i ′ ) + ( p − R p ′ − t ) ∣ ∣ 2 =\\frac{1}{2}\\sum_{i=1}^{n}||(q_i-Rq_i')+(p-Rp'-t)||^2 =21​∑i=1n​∣∣(qi​−Rqi′​)+(p−Rp′−t)∣∣2
= 1 2 ∑ i = 1 n ( ∣ ∣ q i − R q i ′ ∣ ∣ 2 + ∣ ∣ p − R p ′ − t ∣ ∣ 2 + 2 ( q i − R q i ′ ) ( p − R p ′ − t ) ) =\\frac{1}{2}\\sum_{i=1}^{n}(||q_i-Rq_i'||^2+||p-Rp'-t||^2+2(q_i-Rq_i')(p-Rp'-t)) =21​∑i=1n​(∣∣qi​−Rqi′​∣∣2+∣∣p−Rp′−t∣∣2+2(qi​−Rqi′​)(p−Rp′−t))

= 1 2 ∑ i = 1 n ( ∣ ∣ q i − R q i ′ ∣ ∣ 2 + ∣ ∣ p − R p ′ − t ∣ ∣ 2 ) + ( p − R p ′ − t ) ∑ i = 1 n ( q i − R q i ′ ) =\\frac{1}{2}\\sum_{i=1}^{n}(||q_i-Rq_i'||^2+||p-Rp'-t||^2)+(p-Rp'-t)\\sum_{i=1}^{n}(q_i-Rq_i') =21​∑i=1n​(∣∣qi​−Rqi′​∣∣2+∣∣p−Rp′−t∣∣2)+(p−Rp′−t)∑i=1n​(qi​−Rqi′​)

根据定义可得最后一项为0.

∑ i = 1 n ( q i − R q i ′ ) = ∑ i = 1 n q i − R ( ∑ i = 1 n q i ′ ) = ∑ i = 1 n ( p i − p ) − R ( ∑ i = 1 n ( p i ′ − p ′ ) ) = 0 \\sum_{i=1}^{n}(q_i-Rq_i')=\\sum_{i=1}^{n}q_i-R(\\sum_{i=1}^{n}q_i')=\\sum_{i=1}^{n}(p_i-p)-R(\\sum_{i=1}^{n}(p_i'-p'))=0 ∑i=1n​(qi​−Rqi′​)=∑i=1n​qi​−R(∑i=1n​qi′​)=∑i=1n​(pi​−p)−R(∑i=1n​(pi′​−p′))=0

因此我们有

J = 1 2 ∑ i = 1 n ( ∣ ∣ q i − R q i ′ ∣ ∣ 2 + ∣ ∣ p − R p ′ − t ∣ ∣ 2 ) J=\\frac{1}{2}\\sum_{i=1}^{n}(||q_i-Rq_i'||^2+||p-Rp'-t||^2) J=21​∑i=1n​(∣∣qi​−Rqi′​∣∣2+∣∣p−Rp′−t∣∣2)

令 t = p − R p ′ t=p-Rp' t=p−Rp′可以使第二项为0，同时减少参数$t$，得到

R ∗ = a r g R m i n 1 2 ∑ i = 1 n ∣ ∣ q i − R q i ′ ∣ ∣ 2 R^*=arg_{R}\\ min\\frac{1}{2}\\sum_{i=1}^{n}||q_i-Rq_i'||^2 R∗=argR​ min21​∑i=1n​∣∣qi​−Rqi′​∣∣2

利用约束条件 R T R = I R^TR=I RTR=I，继续化简：

R ∗ = a r g R m i n 1 2 ∑ i = 1 n ∣ ∣ q i − R q i ′ ∣ ∣ 2 R^*=arg_{R}\\ min\\frac{1}{2}\\sum_{i=1}^{n}||q_i-Rq_i'||^2 R∗=argR​ min21​∑i=1n​∣∣qi​−Rqi′​∣∣2

= a r g R m i n 1 2 ∑ i = 1 n ( q i T q i + q i ′ T R T R q i ′ − 2 q i T R q i ′ ) =arg_{R}\\ min\\frac{1}{2}\\sum_{i=1}^{n}(q_i^Tq_i+q_i'^TR^TRq_i'-2q_i^TRq_i') =argR​ min21​∑i=1n​(qiT​qi​+qi′T​RTRqi′​−2qiT​Rqi′​)

= a r g R m i n 1 2 ∑ i = 1 n ( q i T q i + q i ′ T q i ′ − 2 q i T R q i ′ ) =arg_{R}\\ min\\frac{1}{2}\\sum_{i=1}^{n}(q_i^Tq_i+q_i'^Tq_i'-2q_i^TRq_i') =argR​ min21​∑i=1n​(qiT​qi​+qi′T​qi′​−2qiT​Rqi′​)

= a r g R m i n 1 2 ∑ i = 1 n ( q i T q i + q i ′ T q i ′ − 2 q i T R q i ′ ) =arg_{R}\\ min\\frac{1}{2}\\sum_{i=1}^{n}(q_i^Tq_i+q_i'^Tq_i'-2q_i^TRq_i') =argR​ min21​∑i=1n​(qiT​qi​+qi′T​qi′​−2qiT​Rqi′​)

= a r g R m a x ∑ i = 1 n q i T R q i ′ =arg_{R}\\ max\\sum_{i=1}^{n}q_i^TRq_i' =argR​ max∑i=1n​qiT​Rqi′​

利用 a T b = t r ( b a T ) a^Tb=tr(ba^T) aTb=tr(baT)继续变形，得

∑ i = 1 n q i T ( R q i ′ ) = ∑ i = 1 n t r ( R q i ′ q i T ) = t r ( R ∑ i = 1 n q i ′ q i T ) \\sum_{i=1}^{n}q_i^T(Rq_i')=\\sum_{i=1}^{n}tr(Rq_i'q_i^T)=tr(R\\sum_{i=1}^{n}q_i'q_i^T) ∑i=1n​qiT​(Rqi′​)=∑i=1n​tr(Rqi′​qiT​)=tr(R∑i=1n​qi′​qiT​)

定义 W = ∑ i = 1 n q i q i ′ T W=\\sum_{i=1}^nq_iq_i'^T W=∑i=1n​qi​qi′T​，至此ICP问题转化为
m a x t r ( R W T ) s . t . R T R = I max\\ tr(RW^T)\\\\ s.t. R^TR=I max tr(RWT)s.t.RTR=I

Method A

对矩阵$W$进行奇异值分解（SVD）得到 W = U Σ V T W=U\\Sigma V^T W=UΣVT，根据SVD的定义，$U$和$V$均是$3\times 3$的正交矩阵，根据约束，$R$矩阵也是。
t r ( R W T ) = t r ( R V Σ U T ) = t r ( S Σ U T ) tr(RW^T)=tr(RV\\Sigma U^T)=tr(S\\Sigma U^T) tr(RWT)=tr(RVΣUT)=tr(SΣUT)
其中$S=UV$， S S T = U V ( U V ) T = U V V T U T = I SS^T=UV(UV)^T=UVV^TU^T=I SST=UV(UV)T=UVVTUT=I也是正交矩阵（两个正交矩阵相乘结果仍是正交矩阵）。

将$S$和$V$展开写成3个列向量的组合的形式：

t r ( S Σ U T ) = t r ( σ 1 s 1 u 1 T + σ 2 s 2 u 2 T + σ 3 s 3 u 3 T ) tr(S\\Sigma U^T)=tr(\\sigma_1s_1u_1^T+\\sigma_2s_2u_2^T+\\sigma_3s_3u_3^T) tr(SΣUT)=tr(σ1​s1​u1T​+σ2​s2​u2T​+σ3​s3​u3T​)

= s 1 u 1 T σ 1 + s 2 u 2 T σ 2 + s 3 u 3 T σ 3 =s_1u_1^T\\sigma_1+s_2u_2^T\\sigma_2+s_3u_3^T\\sigma_3 =s1​u1T​σ1​+s2​u2T​σ2​+s3​u3T​σ3​

由于$S$和$U$均是正交矩阵，有

∣ ∣ s 1 ∣ ∣ = ∣ ∣ s 2 ∣ ∣ = ∣ ∣ s 3 ∣ ∣ = ∣ ∣ u 1 ∣ ∣ = ∣ ∣ u 2 ∣ ∣ = ∣ ∣ u 3 ∣ ∣ = 1 ||s_1||=||s_2||=||s_3||=||u_1||=||u_2||=||u_3||=1 ∣∣s1​∣∣=∣∣s2​∣∣=∣∣s3​∣∣=∣∣u1​∣∣=∣∣u2​∣∣=∣∣u3​∣∣=1

因此，满足不等式

s 1 u 1 T σ 1 + s 2 u 2 T σ 2 + s 3 u 3 T σ 3 ≤ σ 1 + σ 2 + σ 3 s_1u_1^T\\sigma_1+s_2u_2^T\\sigma_2+s_3u_3^T\\sigma_3\\leq \\sigma_1+\\sigma_2+\\sigma_3 s1​u1T​σ1​+s2​u2T​σ2​+s3​u3T​σ3​≤σ1​+σ2​+σ3​

当 ∀ s i , u i \\forall s_i,u_i ∀si​,ui​同向时，等式成立。此时，$S=U$，即$RV=U$， R = U V T R=UV^T R=UVT

因此，我们得到$R$的最优解就是$W$矩阵SVD得到两个正交矩阵$U$和 V T V^T VT的乘积。

Method B

这个问题形式和PCA十分相近，PCA最终转化得到问题是
m a x u u T S u s . t . u T u = 1 max_{u}\\ u^TSu\\\\ s.t.\\ u^Tu=1 maxu​ uTSus.t. uTu=1
这个可以通过拉格朗日乘子法求解，对上面的问题套用。

设拉格朗日函数为 L ( R , λ ) = t r ( R ∑ i = 1 n q i ′ q i T ) − λ ( R T R − I ) \\mathcal{L}(R,\\lambda)= tr(R\\sum_{i=1}^{n}q_i'q_i^T)-\\lambda(R^TR-I) L(R,λ)=tr(R∑i=1n​qi′​qiT​)−λ(RTR−I)，

对$R$求导得 ∇ L ( R , λ ) = ∑ i = 1 n q i q i ′ T − 2 λ R \\nabla\\mathcal{L}(R,\\lambda)=\\sum_{i=1}^nq_iq_i'^T-2\\lambda R ∇L(R,λ)=∑i=1n​qi​qi′T​−2λR，令$\nabla =0$，得到

（矩阵求导 ∇ A t r ( A B ) = B T , ∇ A ( A T A ) = 2 A \\nabla_A tr(AB)=B^T,\\nabla_A(A^TA)=2A ∇A​tr(AB)=BT,∇A​(ATA)=2A）

R = 1 2 λ ∑ i = 1 n q i q i ′ T = 1 2 λ W = 1 2 λ U Σ V T R=\\frac{1}{2\\lambda}\\sum_{i=1}^nq_iq_i'^T=\\frac{1}{2\\lambda}W=\\frac{1}{2\\lambda}U\\Sigma V^T R=2λ1​∑i=1n​qi​qi′T​=2λ1​W=2λ1​UΣVT

代入 R T R = I R^TR=I RTR=I得到

1 4 λ 2 U Σ V T ( U Σ V T ) T = U Σ 2 4 λ 2 U T = I \\frac{1}{4\\lambda^2}U\\Sigma V^T(U\\Sigma V^T)^T=U\\frac{\\Sigma^2}{4\\lambda^2}U^T=I 4λ21​UΣVT(UΣVT)T=U4λ2Σ2​UT=I

利用$U$为正定矩阵的性质得 Σ 2 4 λ 2 = I \\frac{\\Sigma^2}{4\\lambda^2}=I 4λ2Σ2​=I，$\Sigma =2\lambda I$，

因此， R = 1 2 λ U Σ V T = 1 2 λ U ( 2 λ I ) V T = U V T R=\\frac{1}{2\\lambda}U\\Sigma V^T=\\frac{1}{2\\lambda}U(2\\lambda I)V^T=UV^T R=2λ1​UΣVT=2λ1​U(2λI)VT=UVT

通过拉格朗日乘子法可以得到相同的结果。

Reference

用SVD求解ICP问题的完整证明：https://zhuanlan.zhihu.com/p/108858766

SVD求解ICP问题：https://zhuanlan.zhihu.com/p/188668384