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: 没有那个文件或目录
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: 没有那个文件或目录
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+双目标定
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
