ICP问题 SVD方法推导(Markdown版)

1 问题描述

利用SVD求解ICP问题的详细推导。

已知三维点集 P { P 1 , P 2 , ⋯   , P n } P\{P_1,P_2,\cdots,P_n\} P{P1​,P2​,⋯,Pn​}和三维点集 Q { Q 1 , Q 2 , ⋯   , Q n } Q\{Q_1,Q_2,\cdots,Q_n\} Q{Q1​,Q2​,⋯,Qn​}，且这两个点集已经进行了匹配，求其变换矩阵$T$，
$$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\text{\hspace{1em}(1)}$$

2 解决方案

将上述问题用数学公式描述如下，
$$\underset{R,t}{\min }\sum _{i=1}^n\Vert Q_i-(R{P}_i+t){\Vert }^2=\underset{R,t}{\min }\sum _{i=1}^n(Q_i-R{P}_i-t{)}^T(Q_i-R{P}_i-t)\text{\hspace{1em}(2)}$$
又因为，
$$Q_i-R{P}_i-t=\underset{A}{\underbrace{(Q_i-\bar{Q})-R(P_i-\bar{P})}}\underset{B}{\underbrace{-t+\bar{Q}-R\bar{P}}}\text{\hspace{1em}(3)}$$

那么，$(Q_i-R{P}_i-t{)}^T(Q_i-R{P}_i-t)$可以表示为，
$$\begin{gathered} (Q_i-R{P}_i-t{)}^T(Q_i-R{P}_i-t)=(A+B{)}^T(A+B)=A^{T}A+A^{T}B+B^{T}A+B^{T}B \\ =A^{T}A+B^{T}B+2A^{T}B\text{\hspace{1em}(4)} \end{gathered}$$
由于，
$$\sum _{i=1}^{n}2A^{T}B=2\left(\sum _{i=1}^n{A}^T\right)B=2(\sum _{i=1}^{n}A{)}^{T}B\text{\hspace{1em}(5)}$$
$$\sum _{i=1}^{n}A=\sum _{i=1}^n\{(Q_i-\bar{Q})-R(P_i-\bar{P})\}=(\sum _{i=1}^n{Q}_i-n\bar{Q})-R(\sum _{i=1}^n{P}_i-n\bar{P})=\vec{0}-R\cdot \vec{0}=\vec{0}\text{\hspace{1em}(6)}$$
故，公式(2)表示的优化问题，可以写成，
$$\begin{gathered} \underset{R,t}{\min }\sum _{i=1}^n(Q_i-R{P}_i-t{)}^T(Q_i-R{P}_i-t) \\ =\underset{R,t}{\min }\sum _{i=1}^n\{(Q_i^{\prime }-R{P}_i^{\prime }{)}^T(Q_i^{\prime }-R{P}_i^{\prime })+(\bar{Q}-R\bar{P}-t{)}^T(\bar{Q}-R\bar{P}-t)\}\text{\hspace{1em}(7)} \end{gathered}$$
其中$Q_i^{\prime }$和$P_i^{\prime }$分别表示$Q_i$和$P_i$的去中心坐标。

那么，公式(7)表示的优化问题可以进一步写成如下两个优化问题，
$$\underset{R}{\min }\sum _{i=1}^n(Q_i^{\prime }-R{P}_i^{\prime }{)}^T(Q_i^{\prime }-R{P}_i^{\prime })\text{\hspace{1em}(8)}$$
$$\underset{R,t}{\min }\sum _{i=1}^n(\bar{Q}-R\bar{P}-t{)}^T(\bar{Q}-R\bar{P}-t)=n\underset{R,t}{\min }(\bar{Q}-R\bar{P}-t{)}^T(\bar{Q}-R\bar{P}-t)\text{\hspace{1em}(9)}$$

对于公式(8)所表示的优化问题，有，
$$\begin{gathered} (Q_i^{\prime }-R{P}_i^{\prime }{)}^T(Q_i^{\prime }-R{P}_i^{\prime })=(Q_i^{\prime T}-P_i^{\prime T}{R}^T)(Q_i^{\prime }-R{P}_i^{\prime }) \\ =Q_i^{\prime T}{Q}_i^{\prime }-Q_i^{\prime T}R{P}_i^{\prime }-P_i^{\prime T}{R}^T{Q}_i^{\prime }+P_i^{\prime T}{R}^{T}R{P}_i^{\prime } \\ =Q_i^{\prime T}{Q}_i^{\prime }-2Q_i^{\prime T}R{P}_i^{\prime }+P_i^{\prime T}{P}_i^{\prime }\text{\hspace{1em}(10)} \end{gathered}$$

故公式(8)的最优值$R^{\ast }$相当于如下优化问题的解，
$$\underset{R}{\max }\sum _{i=1}^n{Q}_i^{\prime T}R{P}_i^{\prime }=\underset{R}{\max }\sum _{i=1}^{n}trace\left(Q_i^{\prime T}R{P}_i^{\prime }\right)\text{\hspace{1em}(11)}$$
由于矩阵的迹具有性质$trace(AB)=trace(BA)$，故上式可以写成，
$$\underset{R}{\max }\sum _{i=1}^{n}trace\left(R{P}_i^{\prime }{Q}_i^{\prime T}\right)=\underset{R}{\max }trace\left(R\sum _{i=1}^n{P}_i^{\prime }{Q}_i^{\prime T}\right)\text{\hspace{1em}(12)}$$
记$W={\sum }_{i=1}^n{P}_i^{\prime }{Q}_i^{\prime T}$，上式可以写成，
$$\underset{R}{\max }trace\left(RW\right)\text{\hspace{1em}(13)}$$

考虑到如下定理，

---

定理：若有正定矩阵$A{A}^T$，则对于任意正定矩阵$B$，有$trace(BA{A}^T)\le trace(A{A}^T)$。

---

那么，依据上述定理，公式(13)的最优值$R^{\ast }$满足，矩阵$R^{\ast }W$是一个正定矩阵。不妨对矩阵$W$进行奇异值分解，有，
$$W=U\Sigma V^T\text{\hspace{1em}(14)}$$
那么当矩阵$R$取为$V{U}^T$时，$RW$可以表示为，
$$RW=V{U}^T\cdot U\Sigma V^T=V\Sigma V^T=\left(V{\Sigma }^{\frac{1}{2}}\right)(V{\Sigma }^{\frac{1}{2}}{)}^T\text{\hspace{1em}(15)}$$
为正定矩阵，取最大值。

故，
$$R^{\ast }=V{U}^T\text{\hspace{1em}(16)}$$
即证。