视觉SLAM14精讲——视觉里程计5.2

视觉SLAM14精讲

1. 三维空间刚体运动1.0
2. 三维空间刚体运动1.1
3. 三维空间刚体运动1.2
4. 李群与李代数2.1
5. 相机与图像3.1
6. 相机与图像3.2
7. 相机与图像3.3
8. 非线性优化4.1

---

第五章内容大纲

2D-2D

单应矩阵

单应矩阵推导过程扩展

该节描述了一段平面上的点与法向量的关系，但是没有做深入的解释与探讨。

$$n^{T}P+d=0$$
语言描述为：法向量的转置诚意面上的点等于该平面到原点距离的相反数。
若坐标归一化，则该距离为1。（X, Y, 1）

这条定理可以在标定过程中发挥奇效。比如激光雷达在棋盘格上扫描的点，往往不能够与视觉的点产生严谨的对应关系（不能说激光点与视觉点相近，就认定两点为匹配点）。但是唯一可以确定的是，通过分割后的所有点，均处在标定板的平面上，且

OPENCV求解单应矩阵问题的讨论

API问题

在上一篇讨论的内容里提到，采用对极几何方法的隐含条件是自始至终都仅有同一型号和内参的相机作为输入源。这一点在opencv的求解本质矩阵接口中的到体现。在findEssentialMat以及recoverPose接口中仅需要输入focal_length和principal_point参数，实际上这两个参数即内参的实际内容，但是稍有不同。这里焦距参数不区分是标量double类型参数，也就是说不支持x和y方向比例不同的相机（也比较罕见，但却是存在）。

例程pose_estimation_2d2d.cpp

细心的人可能会发现findEssentialMat求出的本质矩阵essential_matrix与程序最后输出的$t^{\wedge }R$有些区别。这是因为findEssentialMat求解出的结果额外乘scale尺度因子，为$t^{\wedge }R\cdot scale$。程序根据程序中显示的结果来看，这个scale值应该在-0.71左右。

对极几何讨论

从E分解到R,t的过程中，如果相机发生纯旋转，导致t为0,那么E也将为，最终误解。这一点的指导意义是在采用的单目相机纯VSLAM初始化和运行过程中，必须保证有一定的平移才能确保有解。这一点在ORBSLAM系列，仅采用MONO的情况下，纯旋转会直接导致初始化通过或者定位丢失。此时需要引入额外的传感器进行协助。

3D-2D

应用场景：3D例如lidar，单线LDS， 面阵ToF（dToF，iToF），RGBD
2D（相机）
思路：

1. 3D-2D对应点建立方程
2. 求出2D（u,v）的解求线性方程
3. QR分解

最低要求：
4. DLT最少6对点
5. P3P最少4对点（3求解+1验证）
6. EPnP最少4对点

3D-3D

ICP算法总结

1. 去质心坐标
2. 旋转以匹配最小损失旋转
3. 求解t
   实现：pcl::IterativeCloestPoint

NDT正态分布匹配算法（Normal Distribution Transform）

1. 栅格化
2. 分配点云到栅格
3. 计算各自的概率密度函数
4. 第二帧每个点进行T变换
5. 该点落在某格内时，计算概率分布函数
6. 求所有点的最优值

实现：pcl::NormalDistributionTransform