vins-mono视惯融合
vins-mono的安装
配置ros运行环境
- 安装kinectic版本
ROS kinectic - 安装ceres solver
Ceres Installation - 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融合的理论。
一下罗列相关论文
- VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimate
- Online Temporal Calibration for Monocular Visual-Inertial Systems
- On-Manifold Preintegration for Real-Time Visual-Inertial Odometry
- Keyframe-Based Visual-Inertial SLAM Using Nonlinear Optimization
- A Robust and Easy to Implement Method for IMU Calibration without External Equipments
- Extending Kalibr: Calibrating the Extrinsics of Multiple IMUs and of Individual Axes
- 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方差将是各项方差之和。