问题描述
假设有一组匹配好的3D点，\(P=\left\{ p_1 \dots p_n \right\}\)，\(P^{'}=\left\{ p_1^{'} \dots p_n^{'} \right\}\)。需要找到一个欧氏变换\(R,t\)，使得误差的平方\(\frac{1}{2} \sum\limits_{i=1}^{n} || p_i - (Rp_i^{'} + t) ||^2\)最小。
矩阵的迹
矩阵的迹定义为一个\(n \times n\)的矩阵\(A\)主对角线上元素的和，记为\(tr(A)\)。
矩阵的迹有两个性质：
- \(tr(AB) = tr(BA)\)
- \(tr(mA + nB) = m\ tr(A) + n\ tr(B)\)
SVD方法求解
首先定义两组点的质心，
\[\bar p = \frac{1}{n} \sum_{i=1}^{n}p_i , \ \bar p^{'} = \frac{1}{n} \sum_{i=1}^{n}p_i^{'}.
\]
之后将误差函数进行如下变换，
\[\begin{align*}
\frac{1}{2} \sum_{i=1}^{n} || p_i - (Rp_i^{'} + t) ||^2 &= \frac{1}{2} \sum_{i=1}^{n} || p_i - Rp_i^{'} - t - \bar p + R\bar p_i^{'} + \bar p - R\bar p_i^{'} ||^2 \\
&= \frac{1}{2} \sum_{i=1}^{n} || ((p_i - \bar p) - R(p_i^{'} - \bar p_i^{'})) + (\bar p - R\bar p_i^{'} - t)||^2 \\
&= \frac{1}{2} \sum_{i=1}^{n} (|| ((p_i - \bar p) - R(p_i^{'} - \bar p_i^{'})) ||^2 + ||(\bar p - R\bar p_i^{'} - t)||^2 + 2((p_i - \bar p) - R(p_i^{'} - \bar p_i^{'}))^T(\bar p - R\bar p_i^{'} - t))
\end{align*}.
\]
交叉项部分\(((p_i - \bar p) - R(p_i^{'} - \bar p_i^{'}))\)求和后为零，优化目标函数简化为，
\[\frac{1}{2} \sum_{i=1}^{n} || ((p_i - \bar p) - R(p_i^{'} - \bar p_i^{'})) ||^2 + ||(\bar p - R\bar p_i^{'} - t)||^2.
\]
只有第二项与\(t\)有关，因此可以先通过优化第一项求出\(R\)再令第二项为零求出\(t\)，首先得到去质心坐标，
\[q_i = p_i - \bar p, \ q_i^{'} = p_i^{'} - \bar p^{'}.
\]
之后优化函数表示为，
\[\frac{1}{2} \sum_{i=1}^{n} || q_i - Rq_i^{'} ||^2 = \frac{1}{2} \sum_{i=1}^{n} (q_i^T q_i + q_i^{'T}R^TRq_i^{'} - 2q_i^{T}Rq_i^{'}).
\]
因为\(R^TR=I\)，可以得到前两项为定值，需要优化第三项使其最大，
\[\sum_{i=1}^{n} q_i^{T}Rq_i^{'} = tr(Q^TRQ^{'}),
\]
其中\(Q = \left[ q_1, \dots ,q_n \right], \ Q^{'} = \left[ q_1^{'} \dots q_n^{'} \right]\)。
根据矩阵迹的性质
\[tr(Q^T(RQ^{'})) = tr(RQ^{'}Q^T)
\]
令\(W = Q^{'}Q^T\)，并对其进行SVD分解\(W = U \Sigma V^T\)，上面的式子可以写成，
\[tr(RQ^{'}Q^T) = tr(RW) = tr(RU \Sigma V^T) = tr(RU (\Sigma V^T)) = tr(\Sigma V^T RU)
\]
其中\(\Sigma\)为对角矩阵，\(V^T,R,U\)都是正交矩阵，因而\(V^T RU\)也是正交矩阵。参考[1]中的证明，\(V^T RU=I\)时取得最小值，此时\(R = VU^T\)。注意如果\(R\)的行列式为负，需要将其中一行取相反数使其行列式变成正数。之后再根据\(t=\bar p - R \bar p^{'}\)求出\(t\)。
SLAM十四讲推导对比
前面大部分推导主要参考了SLAM十四讲的ICP部分内容，但是SLAM十四讲中给出的\(W^{'}=QQ^{'T}=W^T\)。因此得出的\(R\)表达形式不同，但是本质相同。
参考资料
[1] 博客 http://www.liuxiao.org/2019/08/使用-svd-方法求解-icp-问题/
