设特征点齐次坐标
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（随机一致性采样）剔除外点重复上诉步骤。
该方法是沈老师在论文中使用的方法：《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，对于上述方程组：
前面介绍的两种方法都是得到最小二乘意义下的闭式解，但它们的目标函数不同使得解的形式确不一样。而优化方法基本都是最小化重投影误差（图像平面或Normalize平面），优化重投影误差是个非线性优化问题。
先思考一个问题：既然已经有了闭式解法，为什么还要做优化迭代呢？闭式求解它不香吗？
（答案来源于博客[1]） 实践证明，闭式求解确实不香，至少没有优化方法香。其实很好理解，我们把最小二乘解理解为将误差项建模为高斯分布情况下的最大似然估计，闭式方法将高斯误差加在特征点到观测射线的距离上，而实际误差来源于图像观测，所以将高斯误差加在图像平面的距离上更符合实际。因此一般做三角化都是先用闭式解得到初始值，然后在此基础上优化重投影误差，以获得比闭式解更高的精度。
优化方法的目标函数为：
得到Jacobian之后，就是常规的非线性最小二乘求解了，常用的方法有高斯牛顿法、LM法、Dogleg法；
该方法在SLAM的后端优化中使用。
通过匹配算法和三角化可以得到一个符合高斯分布的观测模型 ，同时如果估计值也有一个初始观测值的话
m
u
mu
mu 和
s
i
g
m
a
sigma
sigma 分别是3D点上一次的深度平均值和方差。
d
d
d 和 是最新计算得到的深度观测值和观测值的方差，该方差由观测特征点移动一个像素值引起的深度变化确定。因此更新以后的深度平均值和方差为：
SVO假设参考帧的观测坐标 z_ij 没有误差，这样特征点只取决与深度 d，我们只需要利用其他帧观测来估计深度 d 就好，从而将三维的特征点估计问题转变的一维的深度估计问题。出于一系列考虑，Depth Filter估计的是逆深度 1/d。
逆深度的概率分布建模为一个高斯分布和一个 [d_min,d_max]区间均匀分布的加权
其中，逆深度为Z, 内点率为π, 观测为x，已知观测x需要估计Z和π, 根据Bayes公式得到后验概率:
由于计算复杂，无法直接用上式来迭代计算概率分布，SVO中用Gaussian-Beta分布来近似
该形式为早期的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）
