該方法源於《Least-Squares Rigid Motion Using SVD》，原文推導十分詳細，這里自己也仔細推導了一遍，有些地方加以注釋整理。
問題定義
假設我們有兩個點雲集合\(\mathcal{P}=\left\{\mathbf{p}_{1}, \mathbf{p}_{2}, \ldots, \mathbf{p}_{n}\right\}\)和\(\mathcal{Q}=\left\{\mathbf{q}_{1}, \mathbf{q}_{2}, \ldots, \mathbf{q}_{n}\right\}\)，則我們定義的 ICP 問題是通過最小化點對之間距離獲得相應的位姿\((R,\mathbf{t})\)：
\[(R, \mathbf{t})=\underset{R \in SO(d), \mathbf{t} \in \mathbb{R}^{d}}{\operatorname{argmin}} \sum_{i=1}^{n} w_{i}\left\|\left(R \mathbf{p}_{i}+\mathbf{t}\right)-\mathbf{q}_{i}\right\|^{2}\tag{1} \]
其中 \(w_i\) 代表每個點的權重。R 和 t 是我們所要求的旋轉矩陣和平移向量。
計算平移向量
我們要優化的誤差函數如下:
\(F(R,\mathbf{t})=\sum_{i=1}^{n} w_{i}\left\|\left(R \mathbf{p}_{i}+\mathbf{t}\right)-\mathbf{q}_{i}\right\|^{2}\)
首先來計算對t的導數
令\(\mathcal{l}=\left\|\left(R\mathbf{p}_{i}+\mathbf{t}\right)-\mathbf{q}_{i}\right\|^{2}=(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)^T(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)\)
\(dl\)的微分為：
\[\begin{aligned} dl &=d((R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)^T)(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)+(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)^Td(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i) \\ &=\underbrace{(d(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i))^T}_{d(X^T) = (dX)^T}(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)+(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)^Td\mathbf{t} \\ &=(d\mathbf{t})^T(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)+(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)^Td\mathbf{t} \\ &=\underbrace{2(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)^Tdt}_{當A^TB為標量時，A^TB=B^TA} \end{aligned} \]
對照\(dl=\frac{\partial l}{\partial t}^Tdt\)，得\(\frac{\partial l}{\partial t}=2(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)\)。因此:
\[\begin{aligned} \frac{\partial F}{\partial t} &=\sum_{i=1}^{n} 2 w_{i}(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i) \\ &=2\mathbf{t}\sum_{i=1}^{n}w_i+2R\sum_{i=1}^{n}w_i\mathbf{p}_i-2\sum_{i=1}^{n}w_i\mathbf{q}_i\end{aligned} \tag{2} \]
令\(\frac{\partial F}{\partial t}=0\)，得
\[\mathbf{t}=\frac {\sum_{i=1}^{n}w_i\mathbf{q}_i-R\sum_{i=1}^{n}w_i\mathbf{p}_i}{\sum_{i=1}^{n}w_i} \]
記：
\[\overline{\mathbf{p}}=\frac{\sum_{i=1}^{n} w_{i} \mathbf{p}_{i}}{\sum_{i=1}^{n} w_{i}}, \quad \overline{\mathbf{q}}=\frac{\sum_{i=1}^{n} w_{i} \mathbf{q}_{i}}{\sum_{i=1}^{n} w_{i}}\tag{3} \]
也就是加權平均的質心，並再次帶回 (2) 式可以得到：
\(\mathbf{t}=\overline{\mathbf{q}}-R \overline{\mathbf{p}}\tag{4}\)
將(4)帶回(1)可得：
\[\begin{aligned} \sum_{i=1}^{n} w_{i}\left\|\left(R \mathbf{p}_{i}+\mathbf{t}\right)-\mathbf{q}_{i}\right\|^{2} &=\sum_{i=1}^{n} w_{i}\left\|R \mathbf{p}_{i}+\overline{\mathbf{q}}-R \overline{\mathbf{p}}-\mathbf{q}_{i}\right\|^{2}=\\ &=\sum_{i=1}^{n} w_{i}\left\|R\left(\mathbf{p}_{i}-\overline{\mathbf{p}}\right)-\left(\mathbf{q}_{i}-\overline{\mathbf{q}}\right)\right\|^{2} \end{aligned}\tag{5} \]
令\(\mathbf{x}_i:=\mathbf{p}_i-\overline{\mathbf{p}},\mathbf{y}_i:=\mathbf{q}_i-\overline{\mathbf{q}}\)，則問題轉變為：
\[R=\underset{R \in S O(d)}{\operatorname{argmin}} \sum_{i=1}^{n} w_{i}\left\|R \mathbf{x}_{i}-\mathbf{y}_{i}\right\|^{2}\tag{6} \]
計算旋轉矩陣
(6)式是不是很眼熟，還記得最小二乘問題嗎？
\[\begin{aligned}\left\|R \mathbf{x}_{i}-\mathbf{y}_{i}\right\|^{2} &=\left(R \mathbf{x}_{i}-\mathbf{y}_{i}\right)^{\top}\left(R \mathbf{x}_{i}-\mathbf{y}_{i}\right)=\left(\mathbf{x}_{i}^{\top} R^{\top}-\mathbf{y}_{i}^{\top}\right)\left(R \mathbf{x}_{i}-\mathbf{y}_{i}\right) \\ &=\underbrace{\mathbf{x}_{i}^{\top}R^{\top}R\mathbf{x}_{i}}_{R是單位正交陣，R^{\top}R=\mathbf{I}}-\mathbf{y}_{i}^{\top} R \mathbf{x}_{i}-\mathbf{x}_{i}^{\top} R^{\top} \mathbf{y}_{i}+\mathbf{y}_{i}^{\top} \mathbf{y}_{i} \\ &=\mathbf{x}_{i}^{\top} \mathbf{x}_{i}-\mathbf{y}_{i}^{\top}R\mathbf{x}_{i}-\underbrace{(R\mathbf{x}_{i})^{\top}\mathbf{y}_{i}}_{(AB)^{\top}=B^{\top}A^{\top}}+\mathbf{y}_{i}^{\top} \mathbf{y}_{i} \\ &=\mathbf{x}_{i}^{\top} \mathbf{x}_{i}\underbrace{-2\mathbf{y}_{i}^{\top}R\mathbf{x}_{i}}_{當A^TB為標量時，A^TB=B^TA} + \mathbf{y}_{i}^{\top}\mathbf{y}_{i} \end{aligned}\tag{7} \]
因此問題轉換為：
\[\begin{aligned}\underset{R\in SO(d)}{\operatorname{argmin}} \sum_{i=1}^{n}w_i\left\|R \mathbf{x}_{i}-\mathbf{y}_{i}\right\|^{2}&=\underset{R\in SO(d)}{\operatorname{argmin}} \:\sum_{i=1}^{n}w_i(\underbrace{\mathbf{x}_{i}^{\top} \mathbf{x}_{i}-2\mathbf{y}_{i}^{\top}R\mathbf{x}_i + \mathbf{y}_{i}^{\top}\mathbf{y}_{i}}_{\mathbf{x}_i,\mathbf{y}_i與R無關}) \\ &=\underset{R\in SO(d)}{\operatorname{argmax}} \:\sum_{i=1}^{n}w_i\mathbf{y}_{i}^{\top}R\mathbf{x}_i \end{aligned}\tag{8} \]
令\(W_{n\times n}=\left[\begin{array}{ccccc}{w_{1}} & {} & {} & {} & {} \\ {} & {w_{2}} & {} & {} & {} \\ {} & {} & {} & {\ddots} & {} \\ {} & {} & {} & {} & {w_{n}}\end{array}\right],Y^{\top}_{n\times 3} = \left[\begin{array}{c}{-\mathbf{y}_{1}^{\top}-} \\ {-\mathbf{y}_{2}^{\top}-} \\ {\vdots} \\ {-\mathbf{y}_{n}^{\top}-}\end{array}\right],X_{3\times n}=\begin{bmatrix}| & | & & |\\\mathbf{x}_1 & \mathbf{x}_2 & \cdots & \mathbf{x}_n\\ | & | & & |\end{bmatrix}\)
有
\[\begin{align*}W_{n\times n}Y^{\top}_{n\times 3} R_{3\times 3} X_{3\times n} &= \left[\begin{array}{ccccc}{w_{1}} & {} & {} & {} & {} \\ {} & {w_{2}} & {} & {} & {} \\ {} & {} & {} & {\ddots} & {} \\ {} & {} & {} & {} & {w_{n}}\end{array}\right] \left[\begin{array}{c}{-\mathbf{y}_{1}^{\top}-} \\ {-\mathbf{y}_{2}^{\top}-} \\ {\vdots} \\ {-\mathbf{y}_{n}^{\top}-}\end{array}\right] \left[\begin{array}{ccccc}{} & {} & {} \\ {} & {R} & {} \\ {} & {} & {} \end{array}\right] \begin{bmatrix}| & | & & |\\\mathbf{x}_1 & \mathbf{x}_2 & \cdots & \mathbf{x}_n\\ | & | & & |\end{bmatrix}\\ &= \left[\begin{array}{c}{-w_{1}\mathbf{y}_{1}^{\top}-} \\ {-w_{2}\mathbf{y}_{2}^{\top}-} \\ {\vdots} \\ {-w_{n}\mathbf{y}_{n}^{\top}-}\end{array}\right]_{n\times 3} \begin{bmatrix}| & | & & |\\R\mathbf{x}_1 & R\mathbf{x}_2 & \cdots & R\mathbf{x}_n\\ | & | & & |\end{bmatrix}_{3\times n}\\ &= \left[\begin{array}{cccc}{w_{1} \mathbf{y}_{1}^{\top} R \mathbf{x}_{1}} & {} & {} & {*} \\ {} & {w_{2} \mathbf{y}_{2}^{\top} R \mathbf{x}_{2}} & {} \\ {} & {} & {\ddots} & {} \\ {*} & {} & {} & {w_{n} \mathbf{y}_{n}^{\top} R \mathbf{x}_{n}}\end{array}\right] \end{align*}\tag{9} \]
因此
\[\begin{aligned}\sum_{i=1}^{n} w_{i} \mathbf{y}_{i}^{\top} R \mathbf{x}_{i}&=\operatorname{tr}\left(W Y^{\top} R X\right)\\ &=\underbrace{tr(RXWY^{\top})}_{tr(AB)=tr(BA)} \end{aligned}\tag{10}\]
令\(S=XWY^{\top}\)，而\(S_{SVD}=U\Sigma V^{\top},U與V都是單位正交陣，即UU^{\top}=I,VV^{\top}=I,\Sigma =\left(\begin{array}{cccc}{\sigma_{1}} & {} & {} & {} \\ {} & {\sigma_{2}} & {} & {} \\ {} & {} & {\ddots} & {} \\ {} & {} & {} & {\sigma_{n}}\end{array}\right)且\sigma_{1}, \sigma_{2}, \ldots, \sigma_{n} \geq 0\)，帶入(10):
\[tr(RXWY^{\top})=tr(RS)=tr(RU\Sigma V^{\top})=tr(\Sigma V^{\top}RU) \tag{11} \]
我們來看下\(M=V^{\top}RU\)，\(V^{\top},R,U\)均為單位正交陣，那么\(M\)也為單位正交陣（自己動手推導下，很簡單的～），有\(MM^{\top}=I\)，即M中每行、每列的內積都是1。假設\(\mathbf{m}_j為M的列向量\)，那么
\[\mathbf{m}_j^{\top}\mathbf{m}_j=\sum_{i}m_{ij}^2=1 \]
可見\(\forall i,j\in[0,n],|m_{ij}|\leqslant 1\)。那么
\[\operatorname{tr}(\Sigma M)=\left(\begin{array}{cccc}{\sigma_{1}} & {} & {} & {} \\ {} & {\sigma_{2}} & {} & {} \\ {} & {} & {\ddots} & {} \\ {} & {} & {} & {\sigma_{n}}\end{array}\right)\left(\begin{array}{c}{m_{11} m_{12} \ldots m_{1 n}} \\ {m_{21} m_{22} \ldots m_{2 n}} \\ {\vdots} \\ {m_{n 1} m_{n 2} \ldots m_{n n}}\end{array}\right)=\sum_{i=1}^{n} \sigma_{i} m_{i i} \leq \sum_{i=1}^{n} \sigma_{i}\tag{12} \]
顯然\(M=I\)時，\(tr(\Sigma M)\)可以取到最大值，此時
\[I=M=V^{\top} R U \Rightarrow V=R U \Rightarrow R=V U^{\top}\tag{13} \]
反射修正
前文中推導的結果一定是一個單位正交陣，但是有一個問題，並不是所有的單位正交陣都是旋轉矩陣。
鏡面反射
參考
\(A=\begin{bmatrix} \text{cos}\theta & \text{sin}\theta \\ \text{sin}\theta & -\text{cos}\theta\end{bmatrix}\)
也是一正交矩陣，仔細觀察兩個基的變化，它相當於逆時針旋轉\(\theta\)后再把\(y'\) 軸對折，物理上若不對折，無論如何旋轉也達不到依運算所得的結果，顯然這類正交矩陣既包括旋轉還包括了鏡面反射。這里是二維的情況，對於三維同樣有效，因此求解出R后還需要進行一些檢測：
如果\(\operatorname{det}\left(V U^{\top}\right)=-1\)，則所求的 R 包含了旋轉和鏡像；
如果 \(\operatorname{det}\left(V U^{\top}\right)=1\)，則所求的 R 是我們所求的旋轉矩陣。
假設包含了旋轉和鏡像，對於上節的結論：
\[M=V^{\top}RU=\left(\begin{array}{ccccc}{1} \\ {} & {1} \\ {} & {} & {\ddots} & {} \\ {} & {} & {} & {-1}\end{array}\right) \Rightarrow R=V\left(\begin{array}{ccccc}{1} \\ {} & {1} \\ {} & {} & {\ddots} & {} \\ {} & {} & {} & {-1}\end{array}\right) U^{\top}\tag{13} \]
整理上述兩種情況就可以統一成以下表達式：
\[R=V \begin{pmatrix}1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & \operatorname{det}\left(V U^{\top}\right)\end{pmatrix} U^{\top}\tag{14} \]
平移向量\(\mathbf{t}=\overline{\mathbf{q}}-R \overline{\mathbf{p}}\tag{31}\)
實踐
void pose_estimation_svd (
const vector<pair<Vec3_t, Vec3_t>>& match_pairs,
Mat33_t& R, Vec3_t& t)
{
//假設每個點的權重都是1.0
// \overline{\mathbf{p}}=\frac{\sum_{i=1}^{n} w_{i} \mathbf{p}_{i}}{\sum_{i=1}^{n} w_{i}} = \frac {\sum_{i=1}^{n} \mathbf{p}_{i}}{n}
// \overline{\mathbf{q}}=\frac{\sum_{i=1}^{n} w_{i} \mathbf{q}_{i}}{\sum_{i=1}^{n} w_{i}} = \frac {\sum_{i=1}^{n} \mathbf{q}_{i}}{n}
//1. 計算\overline{\mathbf{p}}，\overline{\mathbf{q}}
Vec3_t p{0.,0.,0.}, q{0.,0.,0.};
int N = match_pairs.size();
for ( int i=0; i<N; i++ )
{
p += match_pairs[i].first; // \sum_{i=1}^{n} \mathbf{p}_{i}
q += match_pairs[i].second; // \sum_{i=1}^{n} \mathbf{q}_{i}
}
p /= N; //\frac {\sum_{i=1}^{n} \mathbf{p}_{i}}{n}
q /= N; //\frac {\sum_{i=1}^{n} \mathbf{1}_{i}}{n}
//2. 計算\mathbf{x},\mathbf{y}
vector<Vec3_t> X( N ), Y( N ); // remove the center
for ( int i=0; i<N; i++ )
{
X[i] = match_pairs[i].first - p;
Y[i] = match_pairs[i].second - q;
}
//3. S=XWY^{\top} W=E, 因此S=XY^{\top}
Eigen::Matrix3d S = Eigen::Matrix3d::Zero();
for ( int i=0; i<N; i++ )
{
S += X[i] * Y[i].transpose();
}
cout<<"S="<<S<<endl;
//4. S進行SVD 奇異值分解
Eigen::JacobiSVD<Eigen::Matrix3d> svd ( S, Eigen::ComputeFullU|Eigen::ComputeFullV );
const Eigen::Matrix3d U = svd.matrixU();
const Eigen::Matrix3d V = svd.matrixV();
cout<<"U="<<U<<endl;
cout<<"V="<<V<<endl;
//5. 構造去鏡像矩陣
Eigen::Matrix3d remove_mirror{Eigen::Matrix3d::Identity()};
remove_mirror(2,2) = (V*U.transpose()).determinant();
cout<<"remove_mirror="<<remove_mirror<<endl;
//6. R=V*remove_mirror*U^{\top}
R = V*remove_mirror*U.transpose();
//7. 平移向量$\mathbf{t}=\overline{\mathbf{q}}-R \overline{\mathbf{p}}\tag{31}$
t = p - R * q;
cout<< "SVD method:"<<endl;
cout<<"R="<<R<<endl;
cout<<"t="<<t.transpose()<<endl;
}
參考
- 使用 SVD 方法求解 ICP 問題
