问题描述
假设有一组匹配好的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-问题/