vins-mono视惯融合

vins-mono的安装

配置ros运行环境

1. 安装kinectic版本
   ROS kinectic
2. 安装ceres solver
   Ceres Installation
3. Opencv 3.3.1安装
   可以省略该步骤，ros自带3.3.1版本cv,在opencv官网下载对应版本opencv3.3.1 以及 opencv_contrib-3.3.1
   输入以下指令

cmake \
-D CMAKE_BUILD_TYPE=Release \
-D CMAKE_INSTALL_PREFIX=/usr/local/opencv-3.3.1 \
-D BUILD_PNG=OFF \
-D BUILD_TIFF=OFF \
-D BUILD_TBB=OFF \
-D BUILD_JPEG=OFF \
-D BUILD_JASPER=OFF \
-D BUILD_ZLIB=OFF \
-D BUILD_EXAMPLES=ON \
-D BUILD_opencv_java=OFF \
-D BUILD_opencv_python2=ON \
-D BUILD_opencv_python3=ON \
-D ENABLE_PRECOMPILED_HEADERS=OFF \
-D WITH_OPENCL=OFF \
-D WITH_OPENMP=OFF \
-D WITH_FFMPEG=ON \
-D WITH_GSTREAMER=OFF \
-D WITH_GSTREAMER_0_10=OFF \
-D WITH_CUDA=ON \
-D WITH_GTK=ON \
-D WITH_VTK=ON \
-D WITH_TBB=ON \
-D WITH_1394=OFF \
-D WITH_OPENEXR=OFF \
-D OPENCV_EXTRA_MODULES_PATH=/home/ly/package/OpenCV-3.3.1/opencv_contrib-3.3.1/modules \ 
-D CUDA_TOOLKIT_ROOT_DIR=/usr/local/cuda-10.0 \
-D CUDA_ARCH_BIN=6.1 \
-D INSTALL_C_EXAMPLES=ON \
-D INSTALL_TESTS=OFF \
..

成功编译后，在安装eigen库以及其他依赖，完成环境配置

vins-mono测试（demo测试）

• 下载数据集
  下载Euroc数据集进行测试
  下载地址
• 测试程序
  按照README.md进行测试，开启三个终端，运行如下命令：
  1.roslaunch vins_estimator euroc.launch
  2.roslaunch vins_estimator vins_rviz.launch
  3.rosbag play YOUR_PATH_TO_DATASET/MH_01_easy.bag
  运行结束后，保存运行的结果，euroc.launch终端中，输入s，enter，将结果保存至output中
• 结果分析
  此处学习利用SLAM的评价工具对VINS的结果进行分析，其中包含对轨迹的对齐，ATE、RPE等误差的分析。
  采用分析工具之前，需要提供GroundTruth 与 估计轨迹的数据，将其转换特定数据格式，如TUM格式** timestamp tx ty tz qx qy qz qw**，数据中的时间戳在生成中存在单位问题，统一转换为相同格式以s为单位。
  evo评价工具
  1.安装
  采用pip 安装即可pip install evo --uograde --np-binary evo --user
  2.测试

evo_traj euroc data.csv --save_as_tum

将GroundTruth转换为tum格式

evo_traj tum FrameTrajectory_TUM_Format.txt --ref=data.tum -p

画出轨迹，后添加–align，可以将轨迹对齐

evo_ape tum data.tum FrameTrajectory_TUM_Format.txt -va --plot

计算绝对轨迹误差
。。。

rpg_trajectory_evaluation评价工具
ToolBox
按照README即可，无需在ros下编译。

vins-mono设备测试

设备要求

• 相机数据输出频率大于20Hz
• IMU数据输出频率大于100Hz
• 发布对应的节点名称/imu0 /cam0/image_raw 或更改对应配置文件

IMU自标定

对于廉价的IMU设备，需要对IMU的一些自身参数进行矫正：三轴的对齐、比例因子、固有的零偏。采用imu_tk安装包进行标定，将得到的校正矩阵、校正向量对发布的IMU数据进行修改。

• 安装imu_tk安装包
  找到imu_tk ros封装的安装包，按照README进行安装。
  安装后，进行测试，可能会报错，将缺失的工具gnuplot对应安装即可解决。

• 录制数据包
  imu_tk对数据包的录制具有有特殊的要求，必须满足其中算法的需要。
  首先静置50s左右
  接着每隔1～2s换一次IMU位置，过程中平滑的旋转IMU
  最后结束记录

• 进行标定
  执行命令

rosrun imu_tk imu_calib_node imu_tk.bag /imu

结果保存至对应路径的文件夹内

对imu传感器的噪声数据、随机游走进行标定，IMU需要包含重力加速度。使用imu_utis包进行标定

• 首先安装code_utils包
  code_utils安装
  其中会报错：1.安装多个Eigen库会产生关于EIgen的错误，需要卸载掉其中一个
  2.头文件找不的错误，找到对应头文件前增加code_utils/
• 安装imu_utils包
  imu_utils安装
  IMU噪声自标定：
• 播放录制的imu数据

rosbag play imu.bag

• 运行imu_utils节点对imu进行标定，修改对应的launch文件

roslaunch imu_utils my_imu.launch

结果保存至对应的yaml文件以及txt文件，获取imu的噪声以及随机游走

相机自标定

使用camera calibration 标定
下载对应的ros功能包ros camera calibration

• 编译文件
• 打印标准棋盘格
• 启动标定程序

rosrun camera_calibration cameracalibrator.py --size 11x8 --square 0.03 image:=/zed_node/left/image_rect_color

其中参数size 11x8 注意是字母x，他是棋盘内部角点个数，如下图所示。square为棋盘正方形边长，注意刚刚开始标定的时候，CALIBRATE按钮是灰色的。

使用kalibr工具实现单目标定

• 录制bag文件

rosbag record -O camera.bag /zed_node/left/image_rect_color

• 解压bag文件

kalibr_bagextractor --image-topics /zed_node/left/image_rect_color --output-folder /home/ly/catkin_kalibr_ws/src/Kalibr/bags --bag /home/ly/catkin_kalibr_ws/src/Kalibr/bags/camera.bag

• 生成标准bag文件

kalibr_bagcreater --folder /home/ly/catkin_kalibr_ws/src/Kalibr/bags --output-bag test.bag

• 相机标定

 kalibr_calibrate_cameras --bag /home/ly/catkin_kalibr_ws/test.bag --topic /cam0/image_raw --models pinhole-radtan --target /home/ly/catkin_kalibr_ws/src/Kalibr/config/april_6×6.yaml

在标定过程中，可能会报关于“focal length”初始化错误的错误，目前参照解决办法标定完成后，在文件夹内生成yaml文件

IMU-相机联合标定

采用Kalibr工具进行联合标定

• 准备标定板
  使用aprilgrid标定板，使用kalibr自带工具生成对应的pdf，其命令如下：

 kalibr_create_target_pdf --type apriltag --nx [NUM_COLS] --ny [NUM_ROWS] --tsize [TAG_WIDTH_M] --tspace [TAG_SPACING_PERCENT]

方括号内填入对应的标定板尺寸，官方给出的尺寸及对应的配置文件，链接标定板设置
若产生：ImportError：No module named pyx的错误，需要安装python对应的模块即可

sudo apt-get install python-pyx

同时保存对应的配置文件aprilgrid.yaml，采用gedit生成对应文件，填入如下数据，否则脚本可能打不开

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.088           #size of apriltag, edge to edge [m]
tagSpacing: 0.3          #ratio of space between tags to tagSize
                         #example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]

• 数据采集
  为标定采集IMU以及相机数据，制作相应的bag包之前，需要记录下相应的图像数据以及imu数据。两种方案对IMU数据进行采集：
  1.采用ros工具rosbag对IMU话题进行数据采集

rosbag record /cam0/image_raw /cam1/image_raw /imu0 -O dynamic.bag

rosbag能同时采集多个节点对应的数据，后续采用rosbag 的play模式播放记录的数据，或者在脚本文件读取bag中的数据。利用Kalibr_bagextractor进行bag数据的解析，并生成对应的imu.csv文件以及将图片数据保存至制定的目录。脚本实现代码如下：kalibr_bagextractor --image-topics /zed_node/left/image_rect_color --imu-topics /imu --output-folder /home/ly/catkin_kalibr_ws/src/Kalibr/bags --bag /home/ly/catkin_kalibr_ws/src/Kalibr/bags/combination.bag
`在解析到指定的文件夹后，再次生成对应的bag文件，其代码如下：

 kalibr_bagcreater --folder /home/ly/catkin_kalibr_ws/src/Kalibr/bags --output-bag test.bag

2.改写脚本，在节点运行中实时保存对应传感器的数据，并将对应数据存储到对应路径，以一定格式存储

+-- dataset-dir 
    +-- cam0 
    │   +-- 1385030208726607500.png 
    │   +--      ... 
    │   -- 1385030212176607500.png 
    +-- cam1 
    │   +-- 1385030208726607500.png 
    │   +--      ... 
    │   -- 1385030212176607500.png 
    -- imu0.csv

为了实现标定，相机节点以及imu节点写了数据爬取脚本，为相机IMU联合标定做准备。

• 进行标定
  采用kalibr工具包和rosbag进行数据的采集，之后进行联合标定：

kalibr_calibrate_imu_camera --target april.yaml --cam src/Kalibr/config/camchain-homelycatkin_kalibr_wstest.yaml --imu src/Kalibr/config/imu_config.yaml --bag src/Kalibr/bags/calibration.bag

得到相机与IMU之间的变换矩阵以及时间偏移等参数。

运行vins-mono程序

• 初始化设备
  以ros为例，在ros系统中开启Camera节点、IMU节点，同时开启VINS-mono的两个节点：roslaunch vins_estimator euroc.launch以及roslaunch vins_estimator vins_rviz.launch。在运行之前需要考虑配置/home/××/vins-mono-catkin_ws/src/VINS-Mono/config/euroc/euroc_config.yaml配置文件的设置。其中包含：
  1.传感器节点名称
  2.相机模型：PINHOLE 等
  3.图片属性：大小、内参
  4.IMU与相机的外参、时移（设备可在线估计）
  5.特征点个数、最小特征距离
  6.IMU的内参：noise、random walk等
  7.其他

• 运行程序
  对于标定好的IMU与相机的设备来说，VINS程序运行不会出现太大问题，做一些适当的平移与旋转即可满足激励，完成初始化工作，实现位姿实时估计。而对于一些低成本传感器设备，而且没有经过精确校准来说，VINS的初始化困难。用一些廉价IMU与自己的Camera相机刚性连接后测试VINS后，发现一直无法初始化设备，错误显示在Gyro的偏置估计太大导致程序崩溃，后边一直没调通，猜测是Gyro传感器的问题（标定该传感器时，发现传感器的噪声密度过大），亦或是时间偏移过大，没有验证。
  运用视觉惯导传感器在正确配置下应该没有太大问题。

• 保存与重用
  VINS-Mono提供了PoseGraph的保存与重用，在运行结束时，可以将关键帧数据保存，同样在启动VINS时配置文件调用即可。
  

理论知识

在实现视觉惯性融合之前，需要对相机、IMU储备一些知识，之后再去看vins融合的理论。
一下罗列相关论文

1. VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimate
2. Online Temporal Calibration for Monocular Visual-Inertial Systems
3. On-Manifold Preintegration for Real-Time Visual-Inertial Odometry
4. Keyframe-Based Visual-Inertial SLAM Using Nonlinear Optimization
5. A Robust and Easy to Implement Method for IMU Calibration without External Equipments
6. Extending Kalibr: Calibrating the Extrinsics of Multiple IMUs and of Individual Axes
7. A Tutorial on Quantitative Trajectory Evaluation for Visual(-Inertial) Odometry

相机

IMU

IMU(Inertial measurement unit,简称IMU）惯性测量单元，测量物体的三轴角速度和加速度，有的配有地磁以及气压的测量。IMU属于捷联式惯导link.对于IMU，我们关心IMU的噪声，Allan方差分析噪声的问题link.

• ALLAN方差
  Allan方差是一种时域分析技术，能较容易对各种误差源的统计特征进行表征和辨识，可作为频域分析技术的补充，可以用于任何仪器噪声的研究，是光学陀螺仪参数分析的标准方法。用不同相关时间内方差所表现的不同特征来描述各种噪声。陀螺零漂数据包括五项噪声源：量化噪声、角速度随机游走、零偏差不稳定性、速率随机游走、速度斜坡。大量实验表明不同的随机误差项出现在不同的相关时间上，符合统计意义上的独立，Allan方差将是各项方差之和。

VIO结果评估

IMU预积分理论