《视觉SLAM14讲》学习小结——供小白快速入门ch7(下)

Zealot[胜利][憨笑][疑问]

已于 2024-08-02 11:34:14 修改

阅读量606

点赞数 13

分类专栏：《视觉SLAM14讲》学习小结——供小白快速入门：理论部分文章标签：计算机视觉

于 2024-08-01 06:50:33 首次发布

本文链接：https://blog.csdn.net/weixin_44469383/article/details/140833522

版权

《视觉SLAM14讲》学习小结——供小白快速入门：理论部分专栏收录该内容

13 篇文章 2 订阅

订阅专栏

视觉里程计1--特征点法（下）

这篇博文是上一篇的后半篇。上半篇谈到如何确定特征点，以及如何根据2D-2D信息实现SLAM。这一篇继续3D-2D问题和3D-3D问题。

三、3D-2D:n点透视定位法(Perspective-n-Point，PnP)

这是基于三维空间n个点坐标及其在像素平面对应的二维坐标，确定相机位置的办法。这种定位所需的点数要比2D-2D问题少很多，理论上只要三个点。此外，它具有丰富的工程应用背景。例如双目相机可直接获得3D数据，因此可采用此方法。又如在SLAM过程中，如果部分地图或路标已经构建或定位，则其3D坐标是已知的，这时要根据观测的2D信息来确定相机位姿就可以用这种方法。3D-2D问题的求解方法很多，这里主要讲解三种：DLT，P3P和BA。这三种方法虽然都是用于解决3D-2D问题，但应用的场合不同。为了明确起见，我们在每一小节的开头就对问题进行明确描述。

1.直接线性变换法DLT

问题描述：已知空间点在世界坐标系中的三维坐标 $\mathbf{P}^w=[X^w,Y^w,Z^w]^\mathrm{T}$ 以及这些点在相机像素坐标系中的坐标 $\mathbf{p}$ ，要确定描述相机位姿的矩阵 $\mathbf{R},\mathbf{t}$ 。

合并ch5中的式（4）和（5）可得 $Z\mathbf{K}\mathbf{p}^1=\mathbf{K}(\mathbf{R}\mathbf{P}^w+\mathbf{t})$ ，消去矩阵 $\mathbf{K}$ 并定义增广矩阵 $\mathbf{r}=[\mathbf{R}|\mathbf{t}]$ 可得 $Z\mathbf{p}^1=\mathbf{r}\hat{\mathbf{P}}^w$ 。其中 $\hat{\mathbf{P}}_w=[X_w,Y_w,Z_w,1]^\mathrm{T}$ 是 $\mathbf{P}_w$ 的齐次坐标，展开得

$Z\begin{bmatrix} x^1 \\y^1 \\1 \end{bmatrix}= \begin{bmatrix} r_1 & r_2 &r_3 & r_4\\r_5 &r_6 & r_7 &r_8\\ r_9 & r_{10} & r_{11} & r_{12} \end{bmatrix} \begin{bmatrix} X^w \\ Y^w\\ Z^w\\ 1 \end{bmatrix}$ (1)

根据ch5中的公式（4）可知 $\mathbf{p}=\mathbf{K}\mathbf{p}^1$ ，由此容易根据内参 $\mathbf{K}$ 和像素坐标 $\mathbf{p}$ 求得归一化坐标 $\mathbf{p}_1$ 。这样方程（1）中的未知数为 $Z$ ， $\mathbf{r}_1= \begin{bmatrix} r_1 & r_2 &r_3 & r_4 \end{bmatrix} ^\mathrm{T}$ ， $\mathbf{r}_2= \begin{bmatrix} r_4 & r_5 &r_6 & r_8 \end{bmatrix} ^\mathrm{T}$ 和 $\mathbf{r}_3= \begin{bmatrix} r_9 & r_{10} &r_{11} & r_{12} \end{bmatrix} ^\mathrm{T}$ 。上式矩阵可改写为三个矩阵方程 $Zx^1=\mathbf{r}_1\hat{\mathbf{P}}$ ， $Zy^1=\mathbf{r}_2\hat{\mathbf{P}}$ 和 $Z=\mathbf{r}_3\hat{\mathbf{P}}$ 。利用第三个方程消去前两个方程中的 $Z$ 可得

$\mathbf{r}_3\hat{\mathbf{P}}x^1=\mathbf{r}_1\hat{\mathbf{P}},~\mathbf{r}_3\hat{\mathbf{P}}y^1=\mathbf{r}_2\hat{\mathbf{P}}$ (2)

一对3D-2D特征点可以提供如上两个方程，如果有6对3D-2D特征点可构建12个这样的线性方程。这样方程组封闭可求解其中的 $\mathbf{r}$ 并得到 $\mathbf{R},\mathbf{t}$ 。

这种方法直接将 $\mathbf{R}$ 中元素作为未知数来求解，忽略了其为正交阵对元素的约束关系，因此这样求解得到的 $\mathbf{R}$ 不一定是正交阵。为了得到正交阵 $\mathbf{R}^\prime$ ，可采用QR分解法或这样的投影方法 $\mathbf{R}^\prime\leftarrow (\mathbf{R}\mathbf{R}^\mathrm{T})^{-1/2}\mathbf{R}$ 。

2.P3P法

问题描述：已知空间三点在世界坐标系中的三维坐标 $\mathbf{P}_w$ 以及这些点在相机位姿下的坐标 $\mathbf{P}_{u,v}$ ，要确定这三个点在相机坐标系下的坐标 $\mathbf{P}$ 。

P3P法根据三个特征点在世界坐标系中的坐标 $\mathbf{P}_w$ 来获取它们在相机坐标系下的坐标 $\mathbf{P}$ 。有了这种对应关系，就把3D-2D的相机位姿定位问题，即位姿矩阵 $\mathbf{R},\mathbf{t}$ 的求解转化为3D-3D问题。后者使用下一节要讨论的ICP方法可容易解决。

P3P方法的原理可根据图5来阐述。其中A,B,C为空间中的三个点，a,b,c为像素平面上的三个点。

图5：P3P方法原理示意图

根据相似关系，可记

$u=\frac{BC^2}{AB^2}=\frac{bc^2}{ab^2},~w=\frac{AC^2}{AB^2}=\frac{ac^2}{ab^2},~v=\frac{AB^2}{OC^2}$ (3)

利用余弦定理有

$OA^2+OB^2-2OA\cdot OB\cdot\cos<a,b>=AB^2\\ OB^2+OC^2-2OB\cdot OC\cdot\cos<b,c>=BC^2\\ OA^2+OC^2-2OA\cdot OC\cdot\cos<a,c>=AC^2$ (4)

记 $\bar{x}=OA/OC,~\bar{y}=OB/OC$ ，将以上三式两边都除以 $OC^2$ 并利用式（3）整理可得

$(1-u)\bar{y}^2-u\bar{x}^2-\cos<b,c>\bar{y}+2u\bar{x}\bar{y}\cos<a,b>+1=0\\ (1-w)\bar{x}^2-w\bar{y}^2-\cos<a,c>x+2w\bar{x}\bar{y}\cos<a,b>+1=0$ (5)

上式已利用一个方程消去 $v$ ，因此方程个数少了一个。上式中的 $u,w$ 可根据A,B,C点的世界坐标系计算， $\cos<a,b>,\cos<a,c>,\cos<b,c>$ 可根据像素坐标计算得到，因此只有 $\bar{x},\bar{y}$ 是未知的。这是一个二元二次方程组，通过消元法可转化为一元四次方程并解析求解。计算得到的结果有四个根如图4所示。通过增加一个验证点可以确定哪一组数据是正确的。在获得 $\bar{x},\bar{y}$ 后事实上就确定了目标距离光心的距离，进一步结合像素坐标系中的二维坐标 $\mathbf{p}$ 即可确定相机坐标系中的三维坐标 $\mathbf{P}$ 。

P3P方法的缺点是只能利用3组特征点数据，不利于提高计算精度。目前也有改进算法以采用更多数据。但作为后面优化问题的初始值，它是有意义的。

3.优化方法BA(Bundle Adjustment)

(1)优化模型及方法

问题描述：假设 $i$ 个空间点的世界坐标系坐标为 $\mathbf{P}^w_i=[X^w_i,Y^w_i,Z^w_i]^\mathrm{T}$ ，它们在像素平面上的二维坐标 $\mathbf{p}_i=[u_i,v_i]^\mathrm{T}$ 为已知。要求确定相机的位姿 $\mathbf{R},\mathbf{t}$ 并适当调整路标点的坐标 $\mathbf{P}^w_i$ 。

将 $\mathbf{P}^w_i$ 写成齐次坐标 $\mathbf{\hat{P}}^w_i=[X^w_i,Y^w_i,Z^w_i,1]^\mathrm{T}$ ，再记 $\mathbf{T}=[\mathbf{R},\mathbf{t};\mathbf{0}^\mathrm{T},1]$ ，根据上ch5中的式（4）和（5）可知 $Z\mathbf{p}=\mathbf{K}\mathbf{T}\mathbf{\hat{P}}^w$ 。由于相机位姿未知以及观测噪声的存在，这个等式存在一个误差 $\mathbf{e}=Z\mathbf{p}-\mathbf{K}\mathbf{T}\mathbf{\hat{P}}^w$ 。考虑到齐次坐标的特性， $\mathbf{e}$ 的第三维并不存在误差，因此对于每一个点 $i$ 均提供两个标量方程或误差。为了寻找最佳的相机位姿 $\mathbf{T}$ ，应当求解优化问题

$\mathbf{T}^*=\arg\min\limits_{\mathbf{T}}|Z\mathbf{p}-\mathbf{K}\mathbf{T}\mathbf{\hat{P}}^w|_2^2.$ (6)

其中二范数是指列阵各分量的平方和求根。这个问题利用李代数可以转换成无约束优化问题。非线性问题的优化已有现成软件如ceres或g2o。在使用这些软件时如果能输入雅可比矩阵的解析表达式，则有助于提高计算速度。因此优化问题分析的重点在于计算雅可比矩阵。

(2)雅可比矩阵的计算

雅可比矩阵 $\mathbf{J}$ 是误差函数对优化变量求导得到的矩阵，即

$\mathbf{e}(\mathbf{x}+\Delta\mathbf{x})\approx \mathbf{e}(\mathbf{x})+\mathbf{J}^\mathbf{T}\Delta\mathbf{x}$ (7)

如前所述，误差函数 $\mathbf{e}$ 是二维的；如果 $\mathbf{x}$ 取6个未知数来表示的位姿 $\boldsymbol{\xi}$ ，则 $\mathbf{J}^\mathrm{T}=\partial \mathbf{e}/\partial \mathbf{x}=\partial \mathbf{e}/\partial \boldsymbol{\xi}$ 应当为 $2\times6$ 矩阵。秉承计算机视觉中的规定，下面的阐述中不再区分齐次坐标和普通坐标的符号差异，即遇到矩阵运算不兼容时默认切换。由于相机坐标 $\mathbf{P}=\mathbf{T}\mathbf{P}_w$ ，因此 $\mathbf{e}=Z\mathbf{P}_{u,v}-\mathbf{K}\mathbf{P}$ 。利用链式求导法则可知

$\frac{\partial \mathbf{e}}{\partial \boldsymbol{\xi}}=-\frac{\partial (\mathbf{K}\mathbf{P})}{\partial \boldsymbol{\xi}} =-\frac{\partial (\mathbf{K}\mathbf{P})}{\partial \mathbf{P}}\frac{\partial \mathbf{P}}{\partial \boldsymbol{\xi}}$ (8)

(a)计算第一个偏导数 ${\partial \mathbf{e}}/{\partial \mathbf{P}} ={\partial (\mathbf{K}\mathbf{P})}/{\partial \mathbf{P}}$

根据ch5中的公式（3）可知 $Z\mathbf{P}_{u,v}=\mathbf{K}\mathbf{P}$ ，展开得

$\begin{bmatrix} Zu\\ Zv\\ Z \end{bmatrix} =\begin{bmatrix} f_x &0 &c_x \\ 0&f_y &c_y \\ 0&0 &1 \end{bmatrix} \begin{bmatrix} X\\ Y\\ Z \end{bmatrix}$ (9)

利用第三行消去前两行中的 $Z$ 可得两个标量方程

$(\mathbf{K}\mathbf{P})_1=u=f_x\frac{X}{Z}+c_x,~(\mathbf{K}\mathbf{P})_2=v=f_x\frac{Y}{Z}+c_y$ (10)

于是

$\frac{\partial (\mathbf{K}\mathbf{P})}{\partial \mathbf{P}} =-\begin{bmatrix} \frac{\partial u}{\partial X} & \frac{\partial u}{\partial Y} &\frac{\partial u}{\partial z} \\ \frac{\partial v}{\partial X}&\frac{\partial v}{\partial Y} & \frac{\partial v}{\partial Z} \end{bmatrix} =\begin{bmatrix} \frac{f_x}{Z} & 0 & -\frac{f_x X}{Z^2}\\ 0&\frac{f_y}{Z} & -\frac{f_y Y}{Z^2} \end{bmatrix}$ (11)

(b)计算第二个偏导数 ${\partial \mathbf{P}}/{\partial \boldsymbol{\xi}}={\partial (\mathbf{T}\mathbf{P}^w)}/{\partial \boldsymbol{\xi}}$

根据ch4中的讨论 ${\partial (\mathbf{T}\mathbf{P}^w)}/{\partial \boldsymbol{\xi}}$ 的计算有两种方法。第一种就是李导数方法，记为 ${\partial (\mathbf{T}\mathbf{P}^w)}/{\partial \boldsymbol{\xi}}$ 。其含义是 $\boldsymbol{\xi}$ 发生微小变化后 $\mathbf{P}=\mathbf{T}\mathbf{P}^w$ 会变化多少。第二种是所谓的扰动模型法，记为 ${\partial (\mathbf{T}\mathbf{P}^w)}/{\partial \delta\boldsymbol{\xi}}$ 。其含义是 $\mathbf{T}$ 发生微小变化后 $\mathbf{P}=\mathbf{T}\mathbf{P}^w$ 会变化多少。这两个计算结果会差一个书中式（4.31）所示的 $\mathbf{J}_l$ ，它是一个取决于 $\boldsymbol{\xi}$ 中 $\mathbf{R}$ 值的量。当 $\boldsymbol{\xi}$ 在某个给定值附近微小变化时， $\boldsymbol{\xi}$ 本身几乎保持不变，因此 $\mathbf{J}_l$ 可视为恒定值。如此一来，采用两种不同导数计算方法的差别，相当于给目标函数 $\mathbf{e}$ 乘上一个常数矩阵 $\mathbf{J}_l$ 。这好比书上都会给目标函数乘以1/2而本博客并没有乘1/2一样，这种差异并不会影响优化结果。根据ch4中的讨论

$\frac{\partial (\mathbf{T}\mathbf{P}^w)}{\partial\delta \boldsymbol{\xi}} =(\mathbf{T}\mathbf{P}^w)^\odot =-\begin{bmatrix} \mathbf{I} & -\mathbf{P}^\wedge \\ \mathbf{0}^\mathrm{T} &\mathbf{0}^\mathrm{T} \end{bmatrix}$ (12)

这样将式（11）和（12）代入式（8），最终可得雅可比矩阵为

$\frac{\partial \mathbf{e}}{\partial \delta\boldsymbol{\xi}}= \begin{bmatrix} \frac{f_x}{Z} &0 &-\frac{f_xX}{Z^2} &-\frac{f_xXY}{Z^2} &f_x+\frac{f_xX^2}{Z^2} &-\frac{f_xY}{Z} \\ 0 & \frac{f_y}{Z} & -\frac{f_yY}{Z^2} &-f_y-\frac{f_yY^2}{Z^2} &\frac{f_yXY}{Z^2} & \frac{f_yX}{Z} \end{bmatrix}$ (13)

(c)路标雅可比矩阵的计算

上面计算的是位姿雅可比矩阵。如果希望对路标同时进行优化，则需要计算误差 $\mathbf{e}$ 对路标点 $\mathbf{P}^w$ 的雅可比矩阵。类似地使用链式法则

$\frac{\partial \mathbf{e}}{\partial \mathbf{P}^w}=-\frac{\partial (\mathbf{K}\mathbf{P})}{\partial \mathbf{P}}\frac{\partial \mathbf{P}}{\partial \mathbf{P}^w}$ (14)

${\partial (\mathbf{K}\mathbf{P})}/{\partial \mathbf{P}}$ 已在式（11）计算好了。注意到 $\mathbf{P}=\mathbf{T}\mathbf{P}^w=\mathbf{R}\mathbf{P}^w+\mathbf{t}$ 有 $\partial \mathbf{P}/(\partial \mathbf{P}^w)=\mathbf{R}$ 。这样

$\frac{\partial \mathbf{e}}{\partial \mathbf{P}^w}=-\begin{bmatrix} \frac{f_x}{Z} & 0 & -\frac{f_x X}{Z^2}\\ 0&\frac{f_y}{Z} & -\frac{f_y Y}{Z^2} \end{bmatrix}\mathbf{R}$ (15)

四、3D-3D问题：迭代最近点（Iterative Closest Point,ICP)

问题描述：在相机的a,b两帧中，其特征点在各自相机坐标系下的坐标 $\mathbf{P}_i^a,\mathbf{P}_i^b$ 为已知，找一个变换 $\mathbf{R},\mathbf{t}$ 使得 $\mathbf{P}_i^a=\mathbf{R}\mathbf{P}_i^b+\mathbf{t}$ 。一般情况下上标是用来表征投影坐标系的，这里将区别两帧的标记a,b也写成上标是为了方便起见，并不会引起混淆。

首先对第 $i$ 对点定义误差 $\mathbf{e}_i=\mathbf{P}_i^a-(\mathbf{R}\mathbf{P}_i^b+\mathbf{t})$ ，然后构建最小二乘问题，求使误差平方和达到最小值的 $\mathbf{R},\mathbf{t}$ ：

$\mathbf{R}^*,\mathbf{t}^*=\arg\min\limits_{\mathbf{R},\mathbf{t}}\sum\limits_{i=1}^n|\mathbf{P}_i^a-\mathbf{R}\mathbf{P}_i^b-\mathbf{t}|_2^2.$ (16)

要同时优化两个变量 $\mathbf{R},\mathbf{t}$ 比较困难，下面分析一种逐步求解的方法。令空间点的质心坐标

$\mathbf{P}^a=\frac{1}{n}\sum\limits_{i=1}^n(\mathbf{P}_i^a),~\mathbf{P}^b=\frac{1}{n}\sum\limits_{i=1}^n(\mathbf{P}_i^b)$ (17)

利用这两个变量，目标函数可改写为

$\min\limits_{\mathbf{R},\mathbf{t}}\mathbf{J}=\sum\limits_{i=1}^n|\mathbf{P}_i^a-\mathbf{P}^a-\mathbf{R}(\mathbf{P}_i^b-\mathbf{P}^b)|_2^2+|\mathbf{P}^a-\mathbf{R}\mathbf{P}^b-\mathbf{t}|_2^2 .$ (18)

注意到范数是大于零的，因此上面最小值问题可以分步求解。具体说就是先按下面最优问题求解 $\mathbf{R}$

$\min\limits_{\mathbf{R}}\mathbf{J}=\sum\limits_{i=1}^n|\mathbf{P}_i^a-\mathbf{P}^a-\mathbf{R}(\mathbf{P}_i^b-\mathbf{P}^b)|_2^2 .$ (19)

然后解方程 $\mathbf{P}^a-\mathbf{R}\mathbf{P}^b-\mathbf{t}=0$ 确定 $\mathbf{t}$ 的值。

问题（19）的求解步骤为：

(1)先定义矩阵 $\mathbf{W}=\sum\limits_{i=1}^n(\mathbf{P}_i^a-\mathbf{P}^a)(\mathbf{P}_i^b-\mathbf{P}^b)^\mathbf{T}$

(2)对 $3\times3$ 矩阵 $\mathbf{W}$ 进行奇异值分解得 $\mathbf{W}=\mathbf{U}\mathbf{\Sigma}\mathbf{V}^\mathrm{T}$ ，其中 $\mathbf{\Sigma}$ 为奇异值组成的对角阵，对角元素从大到小排列。 $\mathbf{U},\mathbf{V}$ 为对角矩阵。

(3)按 $\mathbf{R} = \mathbf{U}\mathbf{V}^\mathrm{T}$ 来确定 $\mathbf{R}$ 。若所得结构行列式为负，则取 $-\mathbf{R}$ 作为 $\mathbf{R}$ 的最优值。

五、小结

这一章讨论了基于特征点的视觉里程计，内容比较丰富所以分为上下两个半篇。其内容包括特征点的概念、构建和匹配方法，讨论了不同情况下相机位姿的估计方法。其中包括基于2D-2D数据的对极几何理论；基于2D-3D数据的直接线性变换法（DLT），转为ICP的P3P方法，参数优化的BA方法；基于3D-3D数据的ICP方法。BA方法非常重要，后面还会用到。本篇融入了作者对李导数的一些理解，希望能对读者有帮助。如有不妥之处请指正。