数学之美
首发于数学之美

SVD求解3D-3D位姿估计

在ICP中,每次迭代都需要求解一个3D-3D位姿估计问题。在之前视觉SLAM系列文章《3D-3D相机位姿估计》中曾简要提到了该方法,但没有给出证明过程。本文就来详细证明一下。


在视觉SLAM中,3D-3D匹配是可以通过特征匹配获知的。在激光SLAM中,3D-3D匹配在ICP的每一次迭代中也是可以获知的。因此,假设我们已知一对匹配好的点的集合

X=\left\{x_1,x_2,...,x_n\right\} \\ Y=\left\{y_1,y_2,...,y_n\right\}

位姿估计的目标是求解一个旋转矩阵 R 和一个平移向量 t ,使得集合 X 中的点经过旋转和平移后与 Y 中的对应点距离最短。即使得误差函数

E(R,t)=\sum_{i=1}^{n}{||y_i-Rx_i-t||^2}\\

最小。

现在,我们对误差函数做一些变换

\begin{aligned} E(R,t)&=\sum_{i=1}^{n}{||y_i-Rx_i-t||^2}\\ &=\sum_{i=1}^{n}{||y_i-Rx_i-t-y_o+y_o-Rx_o+Rx_o||^2}\\ \end{aligned}

其中, x_oy_o 分别是 XY 的重心坐标,可以通过对集合中所有元素求平均得到。进一步合并同类项,并展开\begin{aligned} E(R,t)&=\sum_{i=1}^{n}{\left\|y_i-y_o-R(x_i-x_o)+(y_o-Rx_o-t)\right\|^2}\\ &=\sum_{i=1}^{n}{\Big(\left\|y_i-y_o-R(x_i-x_o)\right\|^2+\left\|y_o-Rx_o-t\right\|^2+2(y_i-y_o-R(x_i-x_o))^T(y_o-Rx_o-t)\Big)}\\  &=\sum_{i=1}^{n}{\Big(\left\|y_i-y_o-R(x_i-x_o)\right\|^2+\left\|y_o-Rx_o-t\right\|^2\Big)}\\ &=\sum_{i=1}^{n}{\left\|y_i-y_o-R(x_i-x_o)\right\|^2}+n\left\|y_o-Rx_o-t\right\|^2\\  \end{aligned}

由于我们定义了x_oy_o 分别是 XY 的重心坐标,因此展开后的交叉项恰好等于0(第二行)。观察最后的结果,第二项与 i 无关,对于任意的 R ,都可以找到一个 t 使得该项为0。而第一项与 R 相关,我们可以优化 R ,使得该项最小。那么优化问题就变成了找到一个 R ,使得上式的第一项最小,即求

\begin{aligned} R^*&=\mathop{\arg\min}_{R} \sum_{i=1}^{n}{\left\|y_i-y_o-R(x_i-x_o)\right\|^2} \\ &=\mathop{\arg\min}_{R} \sum_{i=1}^{n}{\left\|y_i'-Rx_i'\right\|^2} \\ &=\mathop{\arg\min}_{R} \sum_{i=1}^{n}{\Big(y_i'^Ty_i'+x_i'^TR^TRx_i'-2y_i'^TRx_i'\Big)} \\ &=\mathop{\arg\min}_{R} \sum_{i=1}^{n}{\Big(-2y_i'^TRx_i'\Big)} \\ &=\mathop{\arg\max}_{R} \sum_{i=1}^{n}{\Big(y_i'^TRx_i'\Big)} \\ &=\mathop{\arg\max}_{R} \sum_{i=1}^{n}{\Big(trace(y_i'^TRx_i')\Big)} \\ &=\mathop{\arg\max}_{R} \sum_{i=1}^{n}{\Big(trace(Rx_i'y_i'^T)\Big)} \\  &= \mathop{\arg\max}_{R} trace\Big(\sum_{i=1}^{n}{Rx_i'y_i'^T\Big)} \\ &=\mathop{\arg\max}_{R} trace\Big(R\sum_{i=1}^{n}{x_i'y_i'^T\Big)} \\  &=\mathop{\arg\max}_{R} trace(RH) \\ \end{aligned}

其中,第2行进行了变量替换,令 x_i'=x_i-x_oy_i'=y_i-y_o 。第3行的前两项都与 R 无关( R^TR=I ),因此可以去掉。第5行把求最小问题转化成求最大问题。第6行的 trace 运算表示求矩阵的迹,由于第5行的矩阵乘积得到 1\times1 矩阵,那么再对它求迹仍然是它本身。第7行应用了迹的性质 trace(AB)=trace(BA) 。第8行,先求迹再累加和先累加再求迹结果一样,因此可以把迹运算提取到累加运算外面。第9行, R 与累加变量 i 无关,因此可以把 R 提取到累加运算外面。第10行,令 H=\sum_{i=1}^{n}{x_i'y_i'^T} ,是一个 3\times3 矩阵。

接下来,我们先对 H 做SVD分解,得到

H=U\Sigma V^T \\

其中 UV 都是正交矩阵, \Sigma=\begin{bmatrix} \sigma_1 & 0 & 0 \\ 0 & \sigma_2 & 0 \\ 0 & 0 & \sigma_3 \end{bmatrix} , \sigma_1,\sigma_2,\sigma_3H 的三个奇异值。现在,我们继续推导上面的误差函数

\begin{aligned} R^* &=\mathop{\arg\max}_{R} trace(RH) \\ &= \mathop{\arg\max}_{R} trace(RU\Sigma V^T) \\ &= \mathop{\arg\max}_{R} trace(V^TRU\Sigma) \\  &= \mathop{\arg\max}_{R} trace(R'\Sigma)\\ &= V\Big(\mathop{\arg\max}_{R'} trace(R'\Sigma)\Big) U^T \\ &= V\Big(\mathop{\arg\max}_{R'} r_{11}'\sigma_1+r_{22}'\sigma_2+r_{33}'\sigma_3\Big) U^T \\ &=VU^T \end{aligned}

其中,第3行仍然利用了迹的性质 trace(AB)=trace(BA) 。第4行做变量替换,令 R'=V^TRU 。第5行把优化变量改为 R' ,得到最优的 R' 后,再变换回 R 。第6行展开矩阵乘法和迹运算,其中 R'=\begin{bmatrix} r'_{11} & r'_{12} & r'_{13} \\ r'_{21} & r'_{22} & r'_{23} \\ r'_{31} & r'_{32} & r'_{33} \end{bmatrix} 。由于 R,U,V 都是正交矩阵,所以 R' 也是正交矩阵。对于正交矩阵 R' ,应满足 -1\leq r'_{11},r'_{22},r'_{33} \leq 1 。又考虑到奇异值 \sigma_1,\sigma_2,\sigma_3\geq0 ,要想使得 r_{11}'\sigma_1+r_{22}'\sigma_2+r_{33}'\sigma_3 最大,必须使每一项都最大。恰好, r'_{11},r'_{22},r'_{33} 三个数可以认为互不相关,我们可以任意对它们取值。显然,只有当 r'_{11}=r'_{22}=r'_{33}=1 时,r_{11}'\sigma_1+r_{22}'\sigma_2+r_{33}'\sigma_3 最大,且最大值为 \sigma_1+\sigma_2+\sigma_3 。此时,根据正交矩阵的定义,可求出 R 中的其余元素,得到 R'=I 。第6行得到 R

最后,还需要对 R 进行进一步的验证,检查是否满足 \left| R \right|=1 。如果 \left| R \right|=-1 ,那么 R 代表一个镜像对称(Reflection),求解失败。出现这种现象的原因通常是因为点云共面甚至共线,或者外点噪声过大导致的。当然,针对这种情况存在一些解决方案,本文不再讨论,感兴趣的同学可以看参考资料中的第三篇论文。


至此,我们成功推导了3D-3D位姿估计的求解过程。该过程在《视觉SLAM十四讲》中有所体现,但书中的推导并不完整。高翔在书中给出了提出该解法的论文Least-Squares Fitting of Two 3-D Point Sets,可以对照着看。但本文给出的解法比论文更简单,思路来自“SLAM扫地僧”肖师兄,特此感谢。


参考资料

激光SLAM的前端配准方法 曾书格

《视觉SLAM十四讲》 高翔

Least-Squares Fitting of Two 3-D Point Sets K. S. Arun, T. S. Huang, and S. D. Blostein

《线性代数及其应用》 David C. Lay

请问谁能用易于理解的语言解释下矩阵的正定及半正定? 知乎

编辑于 2018-12-12

文章被以下专栏收录