"国君含诟"
- 0. 引入
- 1. 状态估计
- 2. 具体求解状态估计==>最小二乘引出
注意： 本节及下一节，我们将会围绕该问题展开---- 噪声中恢复数据进行视觉 S L A M \color {red}噪声中恢复数据进行视觉SLAM 噪声中恢复数据进行视觉SLAM
0. 引入
\;\;\; 至此，(视觉)理想且经典的SLAM问题已经弄清楚了
\;\;\; 之前SLAM的运动和观测都是基于理想情况下的，实际中我们要做的是在噪声数据中恢复理想情况的数据，由于同一时刻观测到不同的点 且 同一点会被观测多次，这为我们带来了更多约束，便于我们进行下来的任务—在实际的噪声环境中考虑SLAM任务。
1. 状态估计
- 带有顺序的推导，请顺序观看。
\quad 只有图像没有运动传感器时，仅考虑观测方程。
z k , j = h ( y j , x k ) + v k , j 这里设噪声 v 符合高斯分布 v k ∈ N ( 0 , Q k , j ) x = { x 1 , . . . , x N , y 1 , . . . , y M } “状态变量”：要估计的量即位姿和实际点坐标 z_{k,j} = h(y_j, x_k)+v_{k,j}\qquad 这里设噪声v符合高斯分布 \;v_k \in N(0,Q_{k,j}) \\\quad\\x=\begin{Bmatrix}x_1,...,x_N,y_1,...,y_M\end{Bmatrix}\qquad “状态变量”：要估计的量即位姿和实际点坐标 zk,j=h(yj,xk)+vk,j这里设噪声v符合高斯分布vk∈N(0,Qk,j)x={x1,...,xN,y1,...,yM}“状态变量”：要估计的量即位姿和实际点坐标
构成一个三维重建中的SFM(Structrue from motion)问题,我们要做的是实时求解SFM问题而已(在多张知道时间先后顺序的图片中恢复结构(这里就是恢复地图点(当然，你知道我说的是世界坐标系下)))，问题转换为如下：
P ( x ∣ z , u ) = 无运动传感器 , 无 u P ( x ∣ z ) = 贝叶斯 P ( z ∣ x ) P ( x ) P ( z ) ∝ 分母和和待估计状态无关 P ( z ∣ x ) P ( x ) P(x|z, u)\overset{无运动传感器,无u}{=}\color{red}P(x|z) \overset{贝叶斯}{=}\frac{P(z|x)P(x)}{P(z)} \color{b}\overset{分母和和待估计状态无关}{\propto}P(z|x)P(x) \\ P(x∣z,u)=无运动传感器,无uP(x∣z)=贝叶斯P(z)P(z∣x)P(x)∝分母和和待估计状态无关P(z∣x)P(x)
红色部分就是贝叶斯公式，它描述的是 “用数据来修正先验得到后验概率的过程”。简单说一下:
- P ( x ∣ z ) P(x|z) P(x∣z)：后验概率， P ( z ∣ x ) \quad P(z|x) P(z∣x):似然， P ( x ) \quad P(x) P(x)：先验
我们要求的问题则为(观测数据已知)：
x M A P ∗ = a r g m a x P ( x ∣ z ) = a r g m a x P ( x ∣ z ) P ( x ) \color{red}x^*_{MAP} \color{b}= argmax\;P(x|z) = argmax\;P(x|z)P(x) xMAP∗=argmaxP(x∣z)=argmaxP(x∣z)P(x)
\quad 这就是最大后验概率：求一个状态最优估计(等式右边)，使得在该状态下，后验概率(等式左)最大化 (Maximize a Posterior, MAP)
而实际中，机器人的位姿大概位置也不知道(没有先验)，则上式变为求解：
x M L E ∗ = a r g m a x P ( z ∣ x ) \color{red}x^*_{MLE}\color{b}=argmax \; P(z|x) xMLE∗=argmaxP(z∣x)
\quad 这就是最大似然估计(Maximize Likelihood Estimation, MLE): 没有先验的时候，即 x \;x x 也未知。
\quad 直观上看该问题：观测数据已知的时候，在什么样的状态下，最有可能产生现在观测到的数据
这篇文章讲贝叶斯和最大似然这些东西比较通俗易懂，不会的可以看一下，本身也比较简单，只是讲清楚比较麻烦。
2. 具体求解状态估计==>最小二乘引出
前边假设误差符合高斯，则实际上运动方程符合高维高斯分布，那么上边的最大似然也符合，即:
P ( z j , k ∣ x k , y j ) = 符合高斯分布 N ( h ( y j , x k ) , Q k , j ) P(z_{j,k}|x_k,y_j) \overset{符合高斯分布}{=} N(h(y_j,x_k),Q_{k,j})\qquad P(zj,k∣xk,yj)=符合高斯分布N(h(yj,xk),Qk,j)
- 其中的 h ( y j , x k ) \;h(y_j,x_k) h(yj,xk) 是运动方程中的理想情况即高斯理想均值， Q k , j \;Q_{k,j} Qk,j 是运动方程中的误差项，但是高维。
首先回忆一下高维高斯分布吧：概率密度函数如下( Σ 称为协方差矩阵 \Sigma称为协方差矩阵 Σ称为协方差矩阵)：
P ( x ) = 1 ( 2 π ) N ⋅ d e t ( Σ ) e x p ( − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) ) P(x) = \frac{1}{\sqrt {(2\pi)^N \cdot det(\Sigma)}}exp(-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)) P(x)=(2π)N⋅det(Σ)1exp(−21(x−μ)TΣ−1(x−μ))
求解该高斯问题的最大值，一般最小化负对数
− l n ( P ( x ) ) = 1 2 l n ( ( 2 π ) N ⋅ d e t ( Σ ) ) + 1 2 ( x − μ ) T Σ − 1 ( x − μ ) -ln(P(x)) = \frac{1}{2}ln((2\pi)^N \cdot det(\Sigma))+\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu) −ln(P(x))=21ln((2π)N⋅det(Σ))+21(x−μ)TΣ−1(x−μ)
二次型 \color{red}二次型 二次型一般这样：
x T A x x^TAx xTAx
将我们上边的推理的状态分布带入该高维高斯分布的最小化负对数等式中：
x ∗ = a r g m i n ( ( z k , j − h ( x k , y j ) ) T ⋅ Q k , j − 1 ⋅ ( z k , j − h ( x k , y j ) ) ) ⇓ 因为第一项与 x 无关，所以只要最小化等式右侧二次型项即可 ⇓ 其实这件事等价于最小化误差的平方 ( 范数意义下 ) 这个括号里的 z k , j 恰好是观测， h ( x k , y j ) 刚好是真实值，印证了上述的误差 x^* = argmin\;((z_{k,j}-h(x_k,y_j))^T\cdot Q^{-1}_{k,j}\cdot (z_{k,j}-h(x_k,y_j))) \\\quad \\\Downarrow 因为第一项与x无关，所以只要最小化等式右侧二次型项即可 \\\Downarrow 其实这件事等价于最小化误差的平方(范数意义下) \\\quad \\ 这个括号里的 \;z_{k,j}恰好是观测，h(x_k,y_j)刚好是真实值，印证了上述的误差 x∗=argmin((zk,j−h(xk,yj))T⋅Qk,j−1⋅(zk,j−h(xk,yj)))⇓因为第一项与x无关，所以只要最小化等式右侧二次型项即可⇓其实这件事等价于最小化误差的平方(范数意义下)这个括号里的zk,j恰好是观测，h(xk,yj)刚好是真实值，印证了上述的误差
\quad 那么对于任意运动和观测方程，定义它们分别的误差：
{ e v , k = x k − f ( x k − 1 , v k ) 运动误差，一帧图像相机只有一个位姿 e y , j , k = z k , j − h ( x k , y j ) 观测误差，一帧图像有多个点 ⇓ 求误差的平方和（ a s w e a l l k n o w : A t A = A 的平方） J ( x ) = ∑ k e v , k T R k − 1 e v , k + ∑ k ∑ j e y , k , j T Q k , j − 1 e y , k , j 其中的 R k 代表运动方程的误差所符合的高斯分布的误差项 同理 Q k , j 代表运动方程的误差所符合的高斯分布的误差项 \begin{cases} e_{v,k} = x_k - f(x_{k-1}, v_k)\quad 运动误差，一帧图像相机只有一个位姿 \\e_{y,j,k} = z_{k,j} - h(x_k,y_j) \quad 观测误差，一帧图像有多个点 \end{cases} \\ \quad \\ \Downarrow 求误差的平方和（as\;we\;all\;know:A^tA=A的平方） \\ \quad \\J(x) = \sum_ke^T_{v,k}R_k^{-1}e_{v,k}+\sum_k\sum_je_{y,k,j}^TQ^{-1}_{k,j}e_{y,k,j} \\其中的R_{k}代表运动方程的误差所符合的高斯分布的误差项 \\同理Q_{k,j}代表运动方程的误差所符合的高斯分布的误差项 {ev,k=xk−f(xk−1,vk)运动误差，一帧图像相机只有一个位姿ey,j,k=zk,j−h(xk,yj)观测误差，一帧图像有多个点⇓求误差的平方和（asweallknow:AtA=A的平方）J(x)=k∑ev,kTRk−1ev,k+k∑j∑ey,k,jTQk,j−1ey,k,j其中的Rk代表运动方程的误差所符合的高斯分布的误差项同理Qk,j代表运动方程的误差所符合的高斯分布的误差项
这是SLAM中通用的最小二乘问题(Least Square Problem)。根据以上推导， J ( x ) J(x) J(x) 的最优解就等价于状态的最大似然估计。
即一个非线性优化问题：微调状态估计值，使得误差下降到一个极小值的过程。
由于它的每个误差项仅于一两个状态有关，采取参数块的方式进行分块求各个误差的雅克比矩阵然后再带入整体的雅克比矩阵中。
- 对于变换的表述，李代数的使用可以使它没有约束。
至此，我们的问题转为研究一个 无约束的，非线性的最小二乘问题
下一节推理非线性最小二乘问题的求解，下一节再更吧，我要去写论文了，等我再更新的时候，就是我论文写完的时候。我先随便扔个链接哈哈。
