SLAM学习（一）

一.VINS学习(1)

0.基础

(1) IMU

IMU: 通常6轴陀螺仪，包括3轴线加速度和3轴角速度。9轴陀螺仪增加3轴地磁信息。
MEMS 陀螺仪没有姿态的输出，所有的MEMS陀螺都是非旋转装置，通过获取一个振动机械元件上的哥氏加速度效应，实现角速率检测。即一个验证质量在一个平面内做正弦振动，如果此平面以角速率Ω旋转，那么哥氏力就会使该质量以垂直于该平面的方向做正弦振动，其幅值正比于Ω。对哥氏力所引起的运动进行测量，就可获得Ω的信号，这就是微机械陀螺的基本工作原理。
市面上IMU可以输出姿态四元数或者欧拉角，是通过3轴线加速度和3轴角加速度或者增加3轴地磁信息融合求解出来。
加速度a能够提供重力g在俯仰角和翻滚角平面的投影，但是由于IMU本身在不断运动变化，a不等于g，因此不能很好的反应翻滚角和俯仰角，但是长期来看，这种方法不会产生大的漂移。
角速度w能够预测三个角度的变化量，在短期来看，积分能够预测两个时间段内角度的变化，长期运用会造成角度的漂移。
因此通常的做法是融合两者计算结果（卡尔曼滤波），能够得到精确的俯仰角和翻滚角，由于g在偏航角的平面上没有投影，因此偏航角是不准确的（因为相当于只能依靠角速度的积分获取），会有很大的漂移。融合地磁信息之后，可以提高偏航角的准确度，但是同样存在偏移。而且地磁的信息很容易受到环境的干扰，大型设备用电，电机转动都可能产生磁场，干扰地磁信息。

1.系统框架

第一部分是Measuremen Preprocessing：观测值数据预处理，包含图像特征提取跟踪和IMU数据预积分；
第二部分是Initialization：初始化，包含单纯的视觉初始化和视觉惯性联合初始化；
第三部分Local Visual-Inertia BA and Relocalization：基于滑动窗口的单目视觉紧耦合后端优化模型，局部BA联合优化和重定位；
第四部分Global Pose Graph Optimization：全局图优化，只对全局的位姿进行优化；
第五部分Loop detection：回环检测

（1）Measuremen Preprocessing

（2）Initialization

初始化有三个环节：求取相机与IMU之间的相对旋转、相机初始化(局部滑窗内的SFM，包括没有尺度的BA)、IMU与视觉的对齐（修正陀螺仪的bias，初始化速度、重力向量 g和尺度因子，改进重力向量 g的量值）。

（3）Local Visual-Inertia BA and Relocalization

优化中的三个残差项依次是边缘化的先验信息、IMU测量残差以及视觉的观测残差。三种残差都是用马氏距离来表示

a. 边缘化的先验信息

边缘化(marginalization)的过程就是将滑窗内的某些较旧或者不满足要求的视觉帧剔除的过程，所以边缘化也被描述为将联合概率分布分解为边缘概率分布和条件概率分布的过程(说白了，就是利用shur补减少优化参数的过程)。利用Sliding Window做优化的过程中，边缘化的目的主要有两个：

• 滑窗内的pose和feature个数是有限的，在系统优化的过程中，势必要不断将一些pose和feature移除滑窗。
• 如果当前帧图像和上一帧添加到滑窗的图像帧视差很小，则测量的协方差(重投影误差)会很大，进而会恶化优化结果。LIFO导致了协方差的增大，而恶化优化结果。

直接进行边缘化而不加入先验条件的后果：

• 无故地移除这些pose和feature会丢弃帧间约束，会降低了优化器的精度，所以在移除pose和feature的时候需要将相关联的约束转变为一个先验的约束条件作为prior放到优化问题中

在边缘化的过程中，不加先验的边缘化会导致系统尺度的缺失，尤其是系统在进行退化运动时(如无人机的悬停和恒速运动)。一般来说只有两个轴向的加速度不为0的时候，才能保证尺度可观，而退化运动对于无人机或者机器人来说是不可避免的。所以在系统处于退化运动的时候，要加入先验信息保证尺度的可观性。
以上就可以描述为边缘化的目的以及在边缘化中加入先验约束的原因。

b. IMU测量残差

c. 视觉的观测残差

（4）Global Pose Graph Optimization

（5）Loop detection

闭环检测的步骤如下：
a)在滑动窗口后，把新关键帧加入集合中，再提取500个FAST特征，计算BRIEF描述子
b)通过BoW去搜索匹配到的闭环候选帧
c)在小窗口范围进行特征匹配（这里只对之前的Harris特征进行基础矩阵求解，因为只有这些特征点具有深度），用fundamentalmatrix+RANSAC去outliers
d)利用共视的特征点，将得到的闭环帧放入当前的滑动窗口进行优化。这里闭环帧的位姿是被固定的。

2.代码框架

• vins_mono代码结构：
  • ar_demo:AR上层应用
  • benchmark_publisher：发布数据集中参考值
  • camera_mode：各类相机模型
  • config：硬件、系统配置文件
  • support_files
  • feature_trackers ：图像接收和特征点提取与跟踪
  • pose_graph
  • vins_estimator

(a)feature_trackers:
对于每一帧新读取的图像，用KLT稀疏光流对特征点进行跟踪（cv::calcOpticalFlowPyrLK），然后用OpenCV的goodFeaturesToTrack()函数检测新的特征点，从而保证每一帧有100到300个特征点。通过设置最小分布距离，检测到的特征点会有一个相对均匀的分布。检测到的2D特征首先会根据相机内参进行去畸变处理，然后投影到归一化相机坐标系下。相邻帧间用cv::findFundamentalMat()函数计算基本矩阵，这一步中函数算法设置为cv::FM_RANSAC，主要目的是根据函数输出的status变量来剔除光流跟踪中的误匹配点。VINS对图像设置了一定的发布频率，默认值为10Hz(euroc demo)。如果当前帧是发布帧，则把当前帧特征点的归一化坐标、图像中的像素坐标、特征点速度发布给vins_estimator节点。

(b)vins_estimator

• factor 实现IMU、camera等残差模型，涉及了ceres优化库，Jacobian矩阵。
• initial 系统初始化，外参标定，SFM
  初始化基于松耦合的方式，先将视觉Structure from Motion与IMU预积分分开进行，然后进行Visual-Inertial Alignment计算尺度、重力向量和IMU偏置，然后将滑窗中各帧位姿的z轴旋转至重力向量的方向。
  VINS中默认滑动窗口大小为WINDOW_SIZE = 10，实际进行非线性优化时会多一帧，也就是11帧。滑窗中的图像信息和IMU信息保存在all_image_frame变量中，当all_image_frame中的图像数达到WINDOW_SIZE（+1）时，进行初始化。进行初始化有两个条件：
  （1）足够的IMU激励，即运动速度有明显变化，VINS中通过计算加速度的方差来计算，如果方差小于0.25，则会给出IMU激励不够的警告。
  （2）相机有足够的运动，从而保证特征点有足够的视差，对应relativePose()函数。
  Structure from Motion:对滑动窗口中的图像从第0帧到WINDOW_SIZE-1帧遍历，当遍历的某一帧图像满足：
  （1）与最新帧之间的视差大于阈值。
  （2）能够成功求解相对变换且匹配到的特征点数够大。
  则返回真，同时记录该帧与最新帧之间的相对变换relative_R, relative_T, 和该帧在滑窗中的索引。relativePose()返回真后用solve pnp计算滑窗中所有图像帧的位姿并三角化特征点。然后把得到的特征点和位姿作为初值进行全局Bundle Adjustment。
• utility 相机显示，四元数等数据转换
• estimator_node(main()函数)
  首先定义了类estimator,类成员用于滑动窗口内变量的记录，即11帧图片的位姿（ Ps,Rs,与里面的para_Pose等价），IMU偏移(Bas,Bgs)等。里面的estimator.f_manager.feature:记录的是全局特征点，f_manager代码特别用了两个独立的文件来定义。
  类的初始化即整个系统的初始化。
  初始化包括以下几个函数：

initialStructure() //如果返回TRUE，则初始化完成

定义了三个队列 imu_buf,feature_buf,和relo_buf, 定义了三个变量tmp_P、tmp_Q和tmp_V代表最新的里程计信息

• parameters.cpp 外部系统设置参数输入

3.细节

（1）故障检测（failure detection）

存在于VINS_ESTIMATOR/estimator.cpp/failureDetection()
故障检测的有这么一些场景：
（1）当前帧的特征点太少
（2）非线性求解器中的解有大跳动
（3）相机IMU的外参矩阵或IMU偏移（bias）求解出大跳动

（2）td估计

命令行窗口在同步的VI设备中有类似

td -0.0004
td -0.0004

的输出，比较麻烦，找到VINS_ESTIMATOR/utility/visalization/printStatistics(),注释掉输出

二.OKVIS学习(1)

0.安装

根据：
https://github.com/ethz-asl/okvis_ros
提示下载到ros工作环境中。
正常安装会提示： ceres-solver链接无法连接，导致安装不成功,
将在okvis目录下CMklist.txt的151行链接修改为

     GIT_REPOSITORY  https://github.com/ceres-solver/ceres-solver

catkin_make后即可完成安装。
下载数据集 http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets. 下载MH_01_easy.bag，该数据集有对应的校正配置在 okvis/config 里面.
接着运行：
rosrun okvis_ros okvis_node_synchronous ~/okvis_ws/src/okvis_ros/okvis/config/config_fpga_p2_euroc.yaml ~/Downloads/MH_01_easy.bag
打开rviz，file -->open–>…/okvis_ros/config/rviz.rviz
即可看到okvis输出的结果。

三.Cartographer学习(1)