Intel RealSense D435i与IMU标定用于vins-fusion

1、标定imu工具

mkdir -p imu_catkin_ws/src
cd  imu_catkin_ws/src
git clone https://github.com/gaowenliang/code_utils.git
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make
source /devel/setup.bash

-----------------------------------------------------------出现的问题--------------------------------------------------

/home/sfann/imu_catkin_ws/src/code_utils-master/include/code_utils/backward.hpp:216:12: fatal error: elfutils/libdw.h: 没有那个文件或目录

include <elfutils/libdw.h>

compilation terminated.

sudo apt-get install libdw-dev

/home/sfann/imu_catkin_ws/src/code_utils-master/src/sumpixel_test.cpp:2:10: fatal error: backward.hpp: 没有那个文件或目录
#include “backward.hpp”
compilation terminated.
code_utils-master/CMakeFiles/sumpixel_test.dir/build.make:62: recipe for target ‘code_utils-master/CMakeFiles/sumpixel_test.dir/src/sumpixel_test.cpp.o’ failed
make[2]: *** [code_utils-master/CMakeFiles/sumpixel_test.dir/src/sumpixel_test.cpp.o] Error 1
CMakeFiles/Makefile2:857: recipe for target ‘code_utils-master/CMakeFiles/sumpixel_test.dir/all’ failed
make[1]: *** [code_utils-master/CMakeFiles/sumpixel_test.dir/all] Error 2
make[1]: *** 正在等待未完成的任务…
In file included from /home/sfann/imu_catkin_ws/src/code_utils-master/src/mat_io_test.cpp:2:0:
/home/sfann/imu_catkin_ws/src/code_utils-master/include/code_utils/backward.hpp:216:12: fatal error: elfutils/libdw.h: 没有那个文件或目录

include <elfutils/libdw.h>

compilation terminated.
code_utils-master/CMakeFiles/matIO_test.dir/build.make:62: recipe for target ‘code_utils-master/CMakeFiles/matIO_test.dir/src/mat_io_test.cpp.o’ failed
make[2]: *** [code_utils-master/CMakeFiles/matIO_test.dir/src/mat_io_test.cpp.o] Error 1
CMakeFiles/Makefile2:719: recipe for target ‘code_utils-master/CMakeFiles/matIO_test.dir/all’ failed
make[1]: *** [code_utils-master/CMakeFiles/matIO_test.dir/all] Error 2
Makefile:140: recipe for target ‘all’ failed
make: *** [all] Error 2
Invoking “make -j4 -l4” failed

将sumpixel_test.cpp中# include "backward.hpp"改为：#include "code_utils/backward.hpp"

******************imu标定
打开/imu_catkin_ws/src/imu_utils/launch，打开终端运行

roslaunch mavros px4.launch
gedit imu_calibration.launch

launch文件中写入如下内容

<launch>

    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
    	<!--TOPIC名称和上面一致-->
        <param name="imu_topic" type="string" value= "/mavros/imu/data"/>
        <!--imu_name 无所谓-->
        <param name="imu_name" type="string" value= "px4"/>
        <!--标定结果存放路径-->
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <!--数据录制时间-min-->
        <param name="max_time_min" type="int" value= "120"/>
        <!--采样频率，即是IMU频率，采样频率可以使用rostopic hz /camera/imu查看，设置为400，为后面的rosbag play播放频率-->
        <param name="max_cluster" type="int" value= "200"/>
    </node>
    
</launch>

插上相机，realsense静止放置，放置时间要稍大于imu_calibration.launch中的录制时间，即大于120分钟

rosbag record -O imu_calibration /mavros/imu/data

等待两小时录制完之后就按下ctrl+c，结束录制。运行校准程序

roslaunch imu_utils imu_calibration.launch
rosbag play -r 400 imu_calibration.bag

其中 -r 400是指400速播放bag数据

标定结束后在imu_catkin_ws/src/imu_utils/data中生成许多文件，其中imu_param.yaml就是我们想要的结果，展示如下：

2、相机标定工具kalibr
依赖环境

sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev
sudo apt-get install ros-melodic-vision-opencv ros-melodic-image-transport-plugins ros-melodic-cmake-modules 
sudo apt-get install software-properties-common software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython 
sudo apt-get install libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev
sudo apt-get install python-igraph 

mkdir -p /kalibr_workspace/src
cd /kalibr_workspace
source /opt/ros/melodic/setup.bash
catkin init
catkin config --extend /opt/ros/melodic
catkin config --merge-devel # Necessary for catkin_tools >= 0.4. 
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
cd /kalibr_workspace/src
git clone https://github.com/ethz-asl/Kalibr.git
git clone https://github.com/ethz-asl/kalibr/tree/fix/18.04.git
catkin build -DCMAKE_BUILD_TYPE=Release -j4
source ~/kalibr_workspace/devel/setup.bash

下载标定板

https://github.com/ethz-asl/kalibr/wiki/downloads

原始pdf的格子参数是：6*6的格子
大格子边长：5.5cm
小格子边长：1.65cm
小格子与大格子边长比例：0.3
打印A4纸的格子参数是：
大格子边长：2.4cm
小格子边长：0.7cm
小格子与大格子边长比例：0.3
生产标定板文件

cd kalibr_workspace   
gedit april_6x6_A4.yaml
kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.024 --tspace 0.3

填写内容

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.024           #size of apriltag, edge to edge [m]
tagSpacing: 0.3          #ratio of space between tags to tagSize

标定板参数定义

–type apriltag 标定板类型

–nx [NUM_COLS] 列个数

–ny [NUM_ROWS] 行个数

–tsize [TAG_WIDTH_M] 二维码方格长度，单位m

–tspace [TAG_SPACING_PERCENT] 小方格与二维码方格长度比例
关闭结构光

roslaunch realsense2_camera rs_camera.launch
rviz
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color & rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left & rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right
rosbag record -O multicameras_calibration /infra_left /infra_right /color
rosrun kalibr kalibr_calibrate_cameras --target april_6x6_A4.yaml --bag  multicameras_calibration.bag --models pinhole-equi pinhole-equi pinhole-equi --topics /infra_left /infra_right /color --bag-from-to 10 100 --show-extraction

打开rviz观察合理的距离，要求摄像头能看到标定棋盘格。
kalibr在处理标定数据的时候要求频率不能太高，一般为4Hz，我们可以使用如下命令来更改topic的频率，实际上是将原来的topic以新的频率转成新的topic，实际测试infra1才是对应左目相机

3、IMU+双目标定

1. 编辑yaml文件
   新建 camchain.yaml，imu.yaml
   官方教程https://github.com/ethz-asl/kalibr/wiki/yaml-formats
   

gedit camchain.yaml

参数参考上面得到的yaml文件，没有的参数可以删除，最终结果示例如下：

cam0:
  camera_model: pinhole
  intrinsics: [422.64066392524313, 424.02986182898763, 433.8337283244407, 239.28229368758497]
  distortion_model: equidistant
  distortion_coeffs: [0.3504343204199696,  0.09942954345832032, 0.06984316113464994, 0.06546342606600335]
  rostopic: /infra_left
  resolution: [848, 480]
cam1:
  camera_model: pinhole
  intrinsics: [422.6846630997528, 424.22648250200166, 432.1283922008165, 239.61231361314714]
  distortion_model: equidistant
  distortion_coeffs: [0.34116697848102345, 0.14997256247725585, -0.035613030583661255, 0.12752761977350466]
  T_cn_cnm1:
  - [0.9999944120461245, -5.1892669972653565e-05, 0.0033426312493865813, -0.04991163289867289]
  - [5.4683759833798546e-05, 0.999999649965876, -0.0008349118544065274, 1.250629277077362e-05]
  - [-0.0033425867535462755, 0.0008350899766020524, 0.9999940648516464, -0.00011162780864553323]
  - [0.0,0.0,0.0,1.0]
  rostopic: /infra_right
  resolution: [848, 480]

gedit imu.yaml

参数使用之前imu标定得到的参数，示例如下：

#Accelerometers
accelerometer_noise_density: 2.0546933436785896e-02   #Noise density (continuous-time)
accelerometer_random_walk:   1.0213093603518382e-03   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     1.9035698029406516e-03   #Noise density (continuous-time)
gyroscope_random_walk:       1.9530927686562762e-05   #Bias random walk

rostopic:                    /imu      #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

参考链接：https://blog.csdn.net/qq_38364548/article/details/124917067

roslaunch mavros px4.launch
roslaunch realsense2_camera rs_camera.launch

关闭IR结构光

rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 20.0 /infra_left & rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 20.0 /infra_right & rosrun topic_tools throttle messages /camera/imu 200.0 /imu
rostopic hz topic名可以查看实际频率。

rosbag record -O imu_stereo.bag /infra_left /infra_right /imu

source  /kalibr_workspace/devel/setup.bash
rosrun kalibr kalibr_calibrate_imu_cameras --target april_6x6_A4.yaml --bag  multicameras_calibration.bag --models pinhole-equi pinhole-equi pinhole-equi --topics /infra_left /infra_right /color --bag-from-to 10 100 --show-extraction --approx-sync 0.04