多目三角化

1、闭式求解
- 1.1 DLT 最小二乘方法求解
- 1.2 最小化3D距离
2、构建优化方法求解
3、构建高斯深度滤波器 LSD_SLAM
4、构建Beta分布滤波器-- SVO
5、EKF-SLAM
参考资料

在能实现双目计算特征点的深度基础上，考虑，如果这个3D点有大于两个以上的观测值该怎么样估计出最准确的深度。

1、闭式求解

1.1 DLT 最小二乘方法求解

设特征点齐次坐标 x x x ，在第 j 个相机的图像观测点齐次坐标为 z j = ( u j , v j , 1 ) z_j=(u_j,v_j,1) zj=(uj,vj,1)，投影矩阵为 P j = K [ R j t j ] P_j=K[R_j t_j] Pj=K[Rjtj] ，投影模型如下：

从上一篇博客中，我们可以得到DLT方法中每次观测可以提供两个约束条件，即每个相机观测有两个线性方程。

将M个相机观测的约束方程合并得到 2M个线性方程：

因此有形式AX=0。

当存在外点（错误的匹配点），则采用RANSAC（随机一致性采样）剔除外点重复上诉步骤。

1.2 最小化3D距离

该方法是沈老师在论文中使用的方法：《Shaojie Shen, Yash Mulgaonkar, Vision-Based State Estimation and Trajectory Control Towards High-Speed Flight with a Quadrotor》

这里的优化目标是：最小化3D特征点到所有观测射线之间的距离。

将图像观测转为相机坐标系下的单位观测向量 u i j u_{ij} uij （注意：这里的 u i j u_{ij} uij是单位向量，不是normalize 平面的向量）：

z i j z_{ij} zij是观测值在归一化平面的其次坐标， u i j u_{ij} uij是归一化坐标点和相机中心连线的方向的单位向量。将向量 u i j u_{ij} uij转到世界坐标系下，得到世界坐标系下观测射线的方程为：

特征点到射线的距离为：

目标函数定义为特征点 w P i ^wP_i wPi到所有观测距离的平方和最小（最小二乘）：

为了简化求解，令 u j = w c j R T u i j u_j= ^{c_j}_wR^Tu_{ij} uj=wcjRTuij，去掉部分上下标，目标函数变形为

因此对于M个观测值下，优化问题等价于求线性方程组的最小二乘解：

对于超定方程组Ax=b其最小二乘解为： x = ( A T A ) − 1 A T b x=(A^TA)^{-1}A^Tb x=(ATA)−1ATb，对于上述方程组：

2、构建优化方法求解

前面介绍的两种方法都是得到最小二乘意义下的闭式解，但它们的目标函数不同使得解的形式确不一样。而优化方法基本都是最小化重投影误差（图像平面或Normalize平面），优化重投影误差是个非线性优化问题。

先思考一个问题：既然已经有了闭式解法，为什么还要做优化迭代呢？闭式求解它不香吗？

（答案来源于博客[1]） 实践证明，闭式求解确实不香，至少没有优化方法香。其实很好理解，我们把最小二乘解理解为将误差项建模为高斯分布情况下的最大似然估计，闭式方法将高斯误差加在特征点到观测射线的距离上，而实际误差来源于图像观测，所以将高斯误差加在图像平面的距离上更符合实际。因此一般做三角化都是先用闭式解得到初始值，然后在此基础上优化重投影误差，以获得比闭式解更高的精度。

优化方法的目标函数为：

得到Jacobian之后，就是常规的非线性最小二乘求解了，常用的方法有高斯牛顿法、LM法、Dogleg法；

该方法在SLAM的后端优化中使用。

3、构建高斯深度滤波器 LSD_SLAM

通过匹配算法和三角化可以得到一个符合高斯分布的观测模型，同时如果估计值也有一个初始观测值的话

m u mu mu 和 s i g m a sigma sigma 分别是3D点上一次的深度平均值和方差。 d d d 和是最新计算得到的深度观测值和观测值的方差，该方差由观测特征点移动一个像素值引起的深度变化确定。因此更新以后的深度平均值和方差为：

4、构建Beta分布滤波器-- SVO

SVO假设参考帧的观测坐标 z_ij 没有误差，这样特征点只取决与深度 d，我们只需要利用其他帧观测来估计深度 d 就好，从而将三维的特征点估计问题转变的一维的深度估计问题。出于一系列考虑，Depth Filter估计的是逆深度 1/d。
逆深度的概率分布建模为一个高斯分布和一个 [d_min,d_max]区间均匀分布的加权

其中，逆深度为Z, 内点率为π, 观测为x，已知观测x需要估计Z和π, 根据Bayes公式得到后验概率:

由于计算复杂，无法直接用上式来迭代计算概率分布，SVO中用Gaussian-Beta分布来近似

5、EKF-SLAM

该形式为早期的SLAM系统,将特征点表示成一个概率分布进行估计，通常是Gaussian分布 N ( μ ， Σ ) N(\mu，\Sigma) N(μ，Σ)，通过相机观测来估计特征的均值和协方差。与优化方法类似，滤波方法也是推导重投影误差对特征点的Jacobian，但通常相机位姿也是不准的，所以会把相机位姿和特征点放到特征向量中一起估计，也就是EKF-SLAM，这是残差模型中的Jacobian包含两块：重投影误差对相机状态的Jacobian以及对特征状态的Jacobian：

注意：该形式为早期的SLAM系统所采用的，当前的EKF SLAM 系统都通过左零空间投影消除 J f J_f Jf 项以减小系统的维度。

参考资料

[1] https://zhuanlan.zhihu.com/p/103694374
[2] https://www.semanticscholar.org/paper/Vision-Based-State-Estimation-and-Trajectory-Flight-Shen-Mulgaonkar/7b9e7b5c372692f6dedad0bfba4771e9e278e2a5?p2df

欢迎大家点赞在评论区交流讨论（cenruping@vip.qq.com） O(∩_∩)O

或者加群水一波（1149897304）

SLAM中多目三角化相关推荐

SLAM基础 ICP PnP 三角化 F和H（ORB例子）
一.预备知识点 3D-3D ICP与SE3/Sim3 3D-2D PnP 2D-3D Triangulation 2D-2D 对极几何和单应变换作用: ICP:回环的时候用,检测两个帧足够相似的特征 ...
SLAM中的三角测量
[来源]视觉SLAM十四讲,作者高翔三角测量是SLAM中,利用相机运动估计特征点空间位置的过程. 本节旨在解决以下问题: 三角测量的概念三角测量的过程及代码实现: 三角测量有哪些不确定性如何提 ...
贪婪投影三角化算法中的参数设置
无序点云的快速三角化本小节描述了怎样使用贪婪投影三角化算法对有向点云进行三角化,具体方法是先将有向点云投影到某一局部二维坐标平面内,再在坐标平面内进行平面内的三角化,再根据平面内三位点的拓扑连接关系 ...
SLAM中双目三角化
双目三角化形式1:在等式左边同时乘x1x_1x1 和Rx2Rx_2Rx2 形式2:直接变形形式3:等式两边同时叉乘x1 形式4:Direct Linear Transform 参考资料在SL ...
VSLAM中的特征点三角化
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:紫薯萝卜 https://zhuanlan.zhihu.com/p/103694374 本文仅做 ...
SLAM机器人开发（三）SLAM中常见的里程计
SLAM机器人开发(三)SLAM中常见的里程计里程计分类车轮里程计惯性里程计电磁波(光学)雷达超声波雷达视觉里程计里程计对比图里程计分类里程计(Odometry)这个词是由希腊单词o ...
摄影测量(计算机视觉)中的三角化方法
提到三角化大家都十分熟悉,在CV 领域中,由像点计算物点的过程称为三角化,但在摄影测量领域,其称作为前方交会.值得注意的是单张影像是无法恢复像点的三维坐标,至少需要两张影像才能得到像素点的真实坐标(这 ...
视觉SLAM中的数学基础第三篇李群与李代数
转自:http://www.cnblogs.com/gaoxiang12/p/5137454.html 致敬原作者,请移步原作者博文详细浏览前言在SLAM中,除了表达3D旋转与位移之外,我们还要对 ...
PCL中GreedyProjection三角化算法简介与示例
文章目录前言一.PCL点云三角化 1.1 Delaunay三角剖分 1.2 贪婪三角化二.程序示例总结前言 Delaunay三角剖分最初应用于2维领域,而与Greedy三角化算法的结合,使之 ...

SLAM中多目三角化