视觉SLAM十四讲学习笔记-第七讲-视觉里程计-三角测量和实践

 专栏汇总

视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-开发环境搭建_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转矩阵和Eigen库_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转向量、欧拉角、四元数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-相似、仿射、射影变换和eigen程序、可视化演示_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记---前三讲学习笔记总结之SLAM的作用、变换和位姿表示_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-李代数求导与扰动模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-Sophus实践、相似变换群与李代数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-相机模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-图像和实践_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲---第五讲学习笔记总结---李群和李代数、相机_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲学习笔记总结（1）---非线性优化原理_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲学习笔记总结（2）---非线性优化应用_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第七讲-视觉里程计-特征点发和特征提取和匹配实践_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第七讲-视觉里程计-对极几何和对极约束、本质矩阵、基础矩阵

视觉SLAM十四讲学习笔记-第七讲-视觉里程计-单应矩阵和实践_goldqiu的博客-CSDN博客

7.5 三角测量

使用对极几何约束估计了相机运动之后，下一步需要用相机的运动估计特征点的空间位置。在单目SLAM中，仅通过单张图像无法获得像素的深度信息，需要通过三角测量（Triangulation）（或三角化）的方法来估计地图点的深度。

三角测量是指，通过在两处观察同一个点的夹角，确定该点的距离。三角测量最早由高斯提出并应用于测量学中，它在天文学、地理学的测量中都有应用。例如可以通过不同季节观察到星星的角度，估计它离我们的距离。在SLAM中主要用三角化来估计像素点的距离。

考虑图像 I1 和 I2，以左图为参考，右图的变换矩阵为 T。相机光心 为 O1 和 O2。在 I1 中有特征点 p1，对应 I2 中有特征点 p2。理论上直线 O1p1 与 O2p2 在场景中会相交于一点 P，该点即是两个特征点所对应的地图点在三维场景中的位置。然而由于噪声的影响，这两条直线往往无法相交。因此可以通过最二小乘去求解。 按照对极几何中的定义，设 x1, x2 为两个特征点的归一化坐标，那么它们满足： s1x1 = s2Rx2 + t.

已知 R, t，想要求解的是两个特征点的深度 s1, s2。这两个深度是可以分开求的，先算s2，那么先对上式两侧左乘一个

得：

该式左侧为零，右侧可看成 s2 的一个方程，可以根据它直接求得 s2。有了 s2，s1也可以求出。于是就得到了两个帧下的点的深度，确定了它们的空间坐标。由于噪声的存在，估得的 R, t不一定精确使方程为0，所以更常见的做法求最小二乘解而不是零解。

7.6 实践：三角测量

7.6.1 三角测量代码

代码演示了对之前根据对极几何求解的相机位姿通过三角化求出特征点的空间位置。调用 OpenCV 提供的 triangulation 函数进行三角化。

代码为：slambook/ch7/triangulation.cpp

同时在 main 函数中增加三角测量部分，并验证重投影关系。

代码输出某特征点的信息：

point in the first camera frame: [0.0844072, -0.0734976]
point projected from 3D [0.0843702, -0.0743606], d=14.9895
point in the second camera frame: [0.0431343, -0.0459876]
point reprojected from second frame: [0.04312769812378599, -0.04515455276163744, 1]

打印了每个空间点在两个相机坐标系下的投影坐标与像素坐标。这里对于这个特征点来说，通过两个图像下的一对像素坐标的三角化，能得到这个特征点的空间坐标，d为深度，利用这个空间坐标重投影能得到一个投影坐标，跟原始的像素坐标相比有一定的误差。由于误差的存在，它们会有一些微小的差异。误差的量级大约在小数点后第三位。可以看到，三角化特征点的距离大约为 15。但由于尺度不确定性，并不知道这里的 15 究竟是多少米。

7.6.2 讨论

三角测量是由平移得到的，有平移才会有对极几何中的三角形，才谈的上三角测量。因此，纯旋转是无法使用三角测量的，因为对极约束将永远满足。在平移存在的情况下，还要关心三角测量的不确定性，这会引出一个三角测量的矛盾。

如图所示。当平移很小时，像素上的不确定性将导致较大的深度不确定性。也就是说，如果特征点运动一个像素 δx，使得视线角变化了一个角度 δθ，那么测量到深度值将有 δd 的变化。从几何关系可以看到，当 t 较大时，δd 将明显变小，这说明平移较大时， 在同样的相机分辨率下，三角化测量将更精确。对该过程的定量分析可以使用正弦定理得到，但这里先考虑定性分析。 因此，要增加三角化的精度，其一是提高特征点的提取精度，也就是提高图像分辨率 ——但这会导致图像变大，提高计算成本。另一方式是使平移量增大。但是，平移量增大会导致图像的外观发生明显的变化，比如箱子原先被挡住的侧面显示出来了，比如反射光发生变化了，等等。外观变化会使得特征提取与匹配变得困难。总而言之，增大平移，会导致匹配失效；而平移太小，则三角化精度不够——这就是三角化的矛盾。

拓展：定量地计算每个特征点的位置及不确定性。假设特征点服从高斯分布，并且对它不断地进行观测，在信息正确的情况下，就能够期望它的方差会不断减小乃至收敛。这就得到了一个滤波器，称为深度滤波器（Depth Filter）。