BA求解

最新推荐文章于 2024-05-27 21:24:13 发布

珈若

最新推荐文章于 2024-05-27 21:24:13 发布

阅读量847

点赞数

本文链接：https://blog.csdn.net/kr0920/article/details/99580191

版权

在之前接触SfM和SLAM时,对于后端优化的实操不多,对于图优化的理解也仅局限于"顶点为待优化变量,边为约束"这种基础理论层面,没有关注过图优化的求解.因为最近在做BA的优化,所以第一步必须要搞清楚BA问题的常规求解方法.

这里主要参考贺博的资料:https://blog.csdn.net/heyijia0327/article/details/47686523,简单记录一下自己的理解以及疑问.

图优化SfM和SLAM中,顶点是传感器位姿,边是位姿间关系.

1.信息矩阵

图优化中非常重要的概念.参考高博资料:https://www.cnblogs.com/gaoxiang12/p/5244828.html.

信息矩阵是协方差矩阵的逆,是一个对称矩阵.矩阵元素的大小代表对误差项的信任程度.如果比较相信由某个传感器或者某种计算方式所得的误差,则赋予这项误差较大的权重.

2.为什么说"多元高斯分布的协方差矩阵的某一维越大，高斯曲线越矮胖，表示在这个方向上越不确定"??

(1)方差:度量各维度偏离其均值的程度. $var(X)=\sum (X_i-\bar{X})(X_i-\bar{X})/(n-1)$

(2)协方差:度量元素间的相关性,处理二维问题. $cov(X,Y)=\sum (X_I-\bar{X})(Y_i-\bar{Y})/(n-1)$

1.cov(X,X)=var(X) 2.cov(X,Y)=cov(Y,X)

(3)协方差矩阵:处理多变量的多维问题.对称矩阵,对角线是各维度的方差.

$C=\begin{pmatrix} cov(x,x) & cov(x,y) & cov(x,z) \\ cov(y,x) & cov(y,y) & cov(y,z) \\ cov(z,x) & cov(z,y) & cov(z,z) \end{pmatrix}=\begin{pmatrix} var(x) &cov(x,y) & cov(x,z)\\ cov(x,y) & var(y) & cov(y,z)\\ cov(x,z) & cov(y,z) & var(z) \end{pmatrix}$

(4)高斯分布密度函数:

一元: $f(x)=(1/\sqrt{2\pi }\sigma )exp(-(x-\mu )^2/2\sigma ^2)$

二元: $f(x,y)=\frac{1}{2\pi \sigma _1\sigma _2\sqrt{1-\rho ^2}}exp({\frac{-1}{2(1-\rho ^2)}[\frac{(x-\mu _1)^2}{\sigma _1^2}-2\rho \frac{(x-\mu _1)(y-\mu _2)}{\sigma _1\sigma _2}+\frac{(y-\mu _2)^2}{\sigma _2^2}])}$ (rho为相关系数)

变换形式:令 $x=(x,y)^T,\mu =(\mu _1,\mu _2)^T,C=\begin{pmatrix} c_{11} &c_{12} \\ c_{12} & c_{22} \end{pmatrix}$ ,其中,C为协方差矩阵, $C=\begin{pmatrix} var(x) & cov(x,y) \\ cov(x,y) & var(y) \end{pmatrix}=\begin{pmatrix} \sigma _1^2 & \rho \sigma _1\sigma _2\\ \rho \sigma _1\sigma _2 & \sigma _2^2 \end{pmatrix}$

将 $x,\mu ,C$ 代入 f(x,y) 整理可得:

$f(x,y)=\frac{1}{(2\pi )^{\frac{2}{2}}(|C|)^{\frac{1}{2}}}exp(-\frac{1}{2}(x-\mu )^TC^{-1}(x-\mu ))$

多元: $f(x_1,x_2,x_3,......,x_n)=\frac{1}{(2\pi )^{\frac{n}{2}}(|\Sigma |)^{\frac{1}{2}}}exp(-\frac{1}{2}(x-\mu )^T\Sigma ^{-1}(x-\mu ))$ ,其中 $\Sigma$ 为协方差矩阵

(5)博客https://blog.csdn.net/xfijun/article/details/53822490绘制了协方差矩阵对二维高斯分布形状的影响.

对于一元高斯分布,我们在中学数学中学过其特性:小sigma瘦高,大sigma矮胖.上述博客的结果与此特性相符.如下图所示,左图中var(x)=5,var(y)=1,cov(x,y)=0,明显可看出,x轴方向比较胖,y轴方向比较瘦.同样对于右图,var(x)=10,var(y)=50,cov(x,y)=0,y轴方向比较瘦,x轴方向比较胖.

(左) $\begin{bmatrix} 5 &0 \\ 0 & 1 \end{bmatrix}$ (右) $\begin{bmatrix} 10 & 0\\ 0 & 50 \end{bmatrix}$

所以说,"高斯分布的协方差矩阵的某一维越大，高斯曲线越矮胖",这一结论推广到高维高斯分布仍然适用.协方差矩阵的每一维包含某一相关的方差以及其与其他变量的相关系数,我们在BA问题中考虑不同时刻观测误差相互独立,则变量间相关系数为0,那么这一维越大,意味着方差越大,"在这个方向上越不确定".

珈若

关注

0
点赞
踩
2

收藏

觉得还不错? 一键收藏
0
评论
BA求解

在之前接触SfM和SLAM时,对于后端优化的实操不多,对于图优化的理解也仅局限于"顶点为待优化变量,边为约束"这种基础理论层面,没有关注过图优化的求解.因为最近在做BA的优化,所以第一步必须要搞清楚BA问题的常规求解方法. 这里主要参考贺博的资料:https://blog.csdn.net/heyijia0327/article/details/47686523,简单记...
复制链接

扫一扫