V-SLAM整理3

2021-09-30

科研

Word count: 2k | Reading time≈ 7 min

第9讲后端1

状态估计问题：从带噪声的数据估计内在的状态

前端：短时间的状态估计问题。从相机的相邻图像中估计相机运动，可以给出一个短时间内的轨迹和地图。

后端：长时间的状态估计问题。希望构建一个尺度、规模更大的优化问题，来获得长时间内的最优轨迹和地图。

主要方案：

渐进：保持当前状态的估计，在加入新信息时，更新已有的估计（滤波器）
批量：给定一定规模的数据，计算该批量数据下的最优估计（优化）【第6讲】
- 根据第6讲的玩法，批量状态估计问题可以转换成最大似然估计问题，再用最小二乘法求解。

9.1 概述

9.1.1 状态估计的概率解释

已知某些运动数据和观测数据z，如何确定状态量x,y的分布？
如果得到了新时刻的数据，x和y的分布又将怎么变化？

假设状态量和噪声项服从高斯分布，问题转变为：已知一些运动数据和观测数据，如何估计状态量的高斯分布？

改变记号——待估计量xk表示k时刻所有未知量，包括位姿和路标点：

改写后的SLAM方程：

目标是用过去0-k时刻的数据直接从x0估计xk：

后续操作有两种方法：

假设马尔可夫性：k时刻状态只与k-1时刻状态有关，而与再之前的状态无关
- 滤波器方法，如EKF
不假设马尔可夫性，k时刻的状态与之前所有状态有关
- 非线性优化

9.1.2 线性系统和KF

有关滤波器的部分先略过，有需要再整理

9.1.3 非线性系统和EKF

EKF：在某段时间内估计某个不确定的量

9.1.4 EKF的讨论

9.2 BA与图优化

Bundle Adjustment：从视觉图像中提炼出最优的3D模型和相机参数（内参+外参），即带有相机位姿和空间点的图优化称为BA。

BA能够精准的优化每个相机位姿与特征点的位置。

形象点解释，就是通过调整相机位姿和特征点的空间位置，使得从任意特征点发出的光线最终收束到相机的光心。

注意：BA只有观测方程

9.2.1 投影模型和BA代价函数

投影过程：

所谓观测数据，指的是相机得到的图像中的像素坐标[u,v]^T

实际观测数据z 与根据投影过程估计的观测点的误差：请参考7.7.3

这里的T是相机位姿x(R,t)对应的李群T，p是路标

整体的代价函数：

对这个最小二乘求解，相当于同时调整位姿和路标，也就是BA。

9.2.2 BA的求解

请参考第6讲的非线性优化的思路。

待优化变量：

目标函数：F是整体代价函数对相机位姿的偏导，E是整体代价函数对路标的偏导：

F和E的推导请参考7.7.3

化简：

增量方程：

J = [F E]

高斯牛顿：H=JJ
阻尼牛顿：H=JJ+λI

高斯牛顿的H：

这个H很庞大，对它求逆看上去非常艰难，但是好在H具有一定特殊结构，会好求一些。下面就说说H的特殊结构。

9.2.3 稀疏性和边缘化

H具有稀疏结构，并且可以用图优化来表示！

由于在Ti看到pj这件事与其他轨迹和路标点无关，这项的误差也与其他无关，因此求导是0。

i在相机位姿中取值，j在路标点中取值：

H11只与相机位姿有关，H22只与路标点有关。

以下事实成立：

Ace. 现在有两个相机位姿和6个路标点：

以e11为例，这是C1处看到P1这件事的误差项，其雅克比矩阵为J11：

邻接矩阵和H矩阵除了对角线元素之外的其余部分结构一致:

H矩阵的非对角部分可以体现约束关系：

一般的H矩阵长这样：

求解这样稀疏结构的H矩阵：边缘化

Schur消元
Cholesky分解

以下是Schur消元：

那么H△x=g变成：

经过一波化简：

此方程系数记为S：

先求出△xc再求△xp：

关于S：S矩阵的非对角线上的非0矩阵块，表示该处对应的两个相机变量之间存在着共同观测的路标点，又名共视。
S矩阵的稀疏性结构取决于实际观测的结果
- ORB-SLAM中的Local Mapping，在做BA的时候刻意选择那些具有共同观测的帧作为关键帧（这帧被多个相机看到），在这种情况下爱，Schur消元后得到的S就是稠密矩阵，计算开销大。但是该模块不是实时进行的，所以也ok。
- 然而，对于别的一些需要实时算BA的方法，S的稠密就变得不可忍受了。比如DSO、OKVIS等采用滑动窗口，该方法对每一帧都要求做一次BA来防止误差累计，因此必须采取一些技巧来保持S矩阵的稀疏性。

9.2.4 鲁棒核函数

如果某个误差项的数据是错误的，意味着突然观测到了一次不可能产生的数据，在图优化中会有一条误差很大的边，会严重影响到其他正确的边。

解决方法：鲁棒核函数——保证每条边的误差不会大的离谱而掩盖其他的边

具体操作是把目标函数中的二范数度量换成其他增长没那么快的函数。

鲁棒核函数：

Huber核
Cauchy核
Tukey核

g2o和Ceres中有提供一些核函数

9.3 编程：Ceres BA

9.3.1 BAL数据集

BAL数据集提供若干个场景

Meshlab软件可以查看ply点云文件

9.3.2 Ceres BA的书写

9.4 编程：g2o求解BA

第10讲后端2

BA能够精准优化每个相机位姿和特征点位置，有效求解大规模定位与建图问题。但是在更大的场景中，大量特征点的存在会导致庞大的计算开销，难以实时化。

在SLAM中，为了保证实时性，需要控制BA的规模。

例如，有一种简化的BA：位姿图。

10.1 滑动窗口滤波和优化

10.1.1 实际环境下的BA结构

控制BA规模的方法：从连续的视频中抽取一部分作为关键帧，仅构造关键帧与路标点之间的BA，非关键帧只用于定位，对建图无贡献。

滑动窗口法：取N个离当前时刻最近的关键帧，去掉更早的关键帧，将BA控制在一个时间窗口内。
共视图法：在BA优化时，按照某种原则在共视图内取一些关键帧和路标进行优化（例如，仅优化与当前帧有20个以上共视路标的关键帧，固定其他部分）
- 共视图是指与现在的相机存在共同观测的关键帧构成的图。
- 例如ORB-SLAM

V-SLAM整理3

第9讲后端1