相机与惯导联合标定

一、基本信息

相机：FLIR Blackfly S
惯性导航：华测CGI-610
系统：Ubuntu 18.04
ROS版本：Melodic

二、过程

（1）编译安装kalibr

1. 安装依赖项

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

2. 创建并初始化kablir编译空间

mkdir -p ~/kalibr_ws/src
cd ~/kalibr_ws
source /opt/ros/melodic/setup.bash
catkin init
catkin config --extend /opt/ros/melodic
catkin config --merge-devel
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release

3. 下载驱动源码

cd ~/kalibr_ws/src
git clone https://github.com/FLIR/flir_adk_ethernet.git

4. 编译源码，-j(根据CPU核数确定)

cd ..
catkin build -DCMAKE_BUILD_TYPE=Release -j16

5. 将驱动添加至环境变量

echo "source ~/kalibr_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

Kalibr编译参考教程：Installation · ethz-asl/kalibr Wiki · GitHub
注意：
如果报错，error: ‘Eigen::MatrixBase＜Derived＞::~MatrixBase()，则打开/usr/include/eigen3/Eigen/src/Core/MatrixBase.h将467行附近的protected修改为public
Kalibr编译报错参考资料：Ubuntu 20.04 + ROS Noetic + OpenCV 4.10编译kalibr相机标定功能包

（2）编译安装IMU分析工具

1. 安装Ceres依赖项

sudo apt-get install libdw-dev
sudo apt-get install cmake
sudo apt-get install libgoogle-glog-dev libgflags-dev
sudo apt-get install libatlas-base-dev
sudo apt-get install libeigen3-dev
sudo apt-get install libsuitesparse-dev

2. 下载Ceres源码

mkdir -p ~/ceres/ceres-bin
cd ceres
git clone https://ceres-solver.googlesource.com/ceres-solver

3. 编译Ceres源码，-j(根据CPU核数确定)

cd ceres-bin
cmake ../ceres-solver
make -j16
make test
sudo make install

Ceres安装参考资料：Installation — Ceres Solver

4. 创建并初始化code_utils编译空间

mkdir -p ~/imu_ws/src
cd ~/imu_ws/src
git clone https://github.com/gaowenliang/code_utils.git

5. 编译code_utils源码

cd ..
catkin_make

注意：

• 如果编译时报错fatal error: backward.hpp: No such file or directory，则打开code_utils/src/sumpixel_test.cp，将第二行#include "backward.hpp"修改为 #include “code_utils/backward.hpp”
  code_util编译错误参考资料：Kalibr使用：imu+camera联合标定过程详解

6. 下载imu_utils源码

cd src
git clone https://github.com/gaowenliang/imu_utils.git

7. 编译imu_utils源码

cd ..
catkin_make

8. 将驱动添加至环境变量

echo "source ~/imu_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

（3）录制标定数据

1. 新建终端，启动FLIR相机驱动

roslaunch flir_adk_ethernet blackfly.launch

2. 新建终端，启动华测组合导航驱动

roslaunch gps_driver driver.launch

3. 新建终端，录制相机与组合导航回传数据

rosbag record /gps/gpchc /gps/fix /gps/imu /gps/vel /gps/pose /flir_adk/image_raw -O img_imu.bag

4. （可选）根据实际情况生成标定板PDF文件（以下为Aprilgrid板生成）

kalibr_create_target_pdf --type apriltag --nx [NUM_COLS] --ny [NUM_ROWS] --tsize [TAG_WIDTH_M（单位：米）] --tspace [TAG_SPACING_PERCENT]

5. 将相机与组合导航同时稳定地进行如下动作（注意：标定板应始终在画面中，但标定板在每次动作中尽量移动至画面边缘）

• 横滚3次
• 俯仰3次
• 偏航3次
• 前进后退3次
• 上下移动3次
• 左右偏移3次
• 随机动作若干

标定动作过程参考教程：DIY Indoor Autonomous Drone! - Part 2 (Kalibr & Calibration)

（4）更改发布图像频率

1. 关闭录制时启动的相机和组合导航驱动
2. 新建终端，启动ROS核心节点

roscore

3. 新建终端，新建转换输出频率输出话题，将相机输出频率降低至4Hz。（通常设备采集的频率为 20-60Hz，这会使得标定的图像过多, 而导致计算量太大。一般将ros topic的频率降低到4Hz左右进行采集.）

rosrun topic_tools throttle messages /flir_adk/image_raw 4.0 /img

4. 新建终端，录制转换后输出数据

rosbag record -O stereo_calibra.bag /img /gps/imu

5. 新建终端，播放相机与组合导航回传数据

rosbag play img_imu.bag

6. 当数据播放完成后，关闭数据录制与数据转换

（5）相机内参计算

1. （可选）根据标定板的实际数据写入对应元数据april_6x6_80x80cm_A0.yaml文件（以下为Aprilgrid板元数据内容）

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[-]

标定板元数据参考资料：Calibration targets

2. 指定数据包与话题，加载对应标定板数据，运行kalibr（根据FLIR Blackfly S成像原理选择pinhole-equi）

rosrun kalibr kalibr_calibrate_cameras --bag stereo_calibra.bag --topics /img --models pinhole-equi --target april_6x6_80x80cm_A0.yaml

3. 生成的相机内参结果在终端运行目录下的camchain-stereo_calibra.yaml（注意：重复运行会覆盖原有结果）
   yaml文件结果解读：Yaml formats

（6）IMU内参计算

1. 在imu_utils/launch文件夹中，新建launch文件

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/gps/imu"/>
        <param name="imu_name" type="string" value= "Huace"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "10"/>
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

参数说明：

- imu_topic：表示录制的imu话题名字
- imu_name：imu名称，可以随便取，最后标定数据的结果会根据这个保存。
- data_save_path：表示存放的目录，默认在imu_utils/data下，找到相应的yaml即可查看标定的结果。
- max_time_min：取数据的时长，一般默认120分钟，程序会在最大时间截断读取数据。（应小于bag包持续时长）

2. 新建终端，启动ROS核心节点

roscore

3. 新建终端，播放相机与组合导航回传数据

rosbag play img_imu.bag

4. 新建终端，运行imu计算程序，并使用自己设置的launch文件

roslaunch imu_utils Huace.launch

imu内参计算参考教程：GitHub - gaowenliang/imu_utils: A ROS package tool to analyze the IMU performance.

（7）相机与IMU联合标定

1. 查看imu内参计算结果yaml文件，默认在imu_utils/data目录中。并根据yaml文件写入新的yaml文件，内容如下：

#Accelerometers
accelerometer_noise_density: 1.86e-03   #对应yaml文件的“Acc->avg-axis->acc_n”
accelerometer_random_walk:   4.33e-04   #对应yaml文件的“Acc->avg-axis->acc_w”

#Gyroscopes
gyroscope_noise_density:     1.87e-04   #对应yaml文件的“Gyr->avg-axis->gyr_n”
gyroscope_random_walk:       2.66e-05   #对应yaml文件的“Gyr->avg-axis->gyr_w”

rostopic:                    /gps/imu   #bag包中imu数据话题
update_rate:                 100.0      #Hz (imu采集频率)

2. 运行kalibr相机与IMU联合标定

kalibr_calibrate_imu_camera --target april_6x6_80x80cm_A0.yaml --cam camchain-stereo_calibra.yaml --imu imu_Huace.yaml --bag stereo_calibra.bag

3. 生成的标定结果在终端运行目录下的camchain-imucam-stereo_calibra.yaml（注意：重复运行会覆盖原有结果）
   yaml文件结果解读：Yaml formats