该方法源于《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 = q - R * p;
cout<< "SVD method:"<<endl;
cout<<"R="<<R<<endl;
cout<<"t="<<t.transpose()<<endl;
}
参考
- 使用 SVD 方法求解 ICP 问题
