c++中射线表示_VSLAM中的特征点三角化

特征点三角化（Triangulation）是VSLAM中一个非常基础的问题，它是根据特征点在多个相机下的投影恢复出特征点的3D坐标。

特征点在某个相机中被观测到，根据相机位姿和观测向量可以得到3D空间中的一条从相机中心出发的观测“射线”，多个相机位姿观测会产生多条观测射线，理想情况下这些观测射线相交于空间中一点，求所有观测射线的交点就是特征点在3D空间的位置，这就是三角化最朴素的思想。

实际中由于噪声的存在，这些观测射线不可能刚好相交于一点，那如何求特征点坐标呢？有几种思路：

1. 最直觉的思路是：找一个3D点与所有观测射线的距离都很近，以此作为特征点。
2. 另一种思路觉得：误差来源于2D图像观测，所以把特征点投影到每个2D相机平面，最小化所有2D投影点到对应观测点之间的距离（重投影误差）。
3. 第三种思路是：很多时候三角化是在线（Online）运行的，并不是全部观测都拿到以后再进行求解，而是一边获取观测一边估计特征点，所以用一个滤波器来估计特征点的概率分布（通常是Gaussian），旧的观测信息被隐式的存放在概率分布中，来了新的观测后，用新的观测来更新特征点的概率分布，这样能够节省很大的计算量，这就是滤波方法的朴素思想。

0. 问题定义

设3D特征点

被

个相机观测到，已知：相机姿态

，相机内参

，特征点在各相机的图像观测

，求：特征点的3D坐标

：

不考虑误差的情况有如下投影关系（接下来的方法基本上都会用到该投影关系）：

1. 闭式求解

1.1 最小化3D距离

Shen Shaojie. Vision-Based State Estimation and Trajectory Control Towards High-Speed Flight with a Quadrotor​pdfs.semanticscholar.org

最小化3D距离的目标是：在世界坐标系下求一个3D点离所有观测射线的3D距离最近。

先求观测射线的数学方程：将图像观测转为相机坐标系下的单位观测向量

（注意：这里的