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时配置文件调用即可。
    vins-mono视惯融合_第1张图片

理论知识

在实现视觉惯性融合之前,需要对相机、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预积分理论

你可能感兴趣的:(VIO,SLAM)