使用Kalibr和imu_utils标定相机和IMU的内外参

摄像头和IMU内参标定以及其外参标定

使用工具：Kalibr，imu_utils

标定目的：

飞机上使用的是realsense双目摄像头，以及飞控内置的IMU。不同传感器的数据需要被准确地整合。因此需要确定IMU、摄像头两目分别的内参，两目之间的外参以及IMU和相机之间的外参。

操作思路：

1. 飞机上安装好传感器，机载电脑需安装ROS以及IMU, realsense的驱动；工作站上安装好ROS
2. 在工作站上安装imu_utils用于标定IMU的内参，安装Kalibr用于标定摄像头内参以及IMU和摄像头的外参
3. 在飞机上录制IMU和摄像头的数据，并传输到工作站上
4. 在工作站上使用imu_utils标定IMU的内参，使用Kalibr标定摄像头内参以及IMU和摄像头的外参

0. Environment

介绍本标定实验使用的环境。其它软硬件当然也可以，但是需要做相应调整

Drone’s onboard computer

Hardware

• Core board: Jetson (ARM Architecture)
• Carrier Board: Damiao v1.1 nx carrier board
• Camera: Intel Realsense D430 (without IMU)
• IMU: pixhawk mini 6c’s built-in IMU
• FCU-Computer connection: USB c2c (instead of telem2uart)

Software preparation

• Ubuntu 20.04
• ROS Noetic (ROS1)
• librealsense installed (sdk2.0)
• realsense-ros installed ros1-legacy
• mavros installed
• Terminal: bash

Workstation

Software

• Ubuntu 20.04
• ROS Noetic (ROS1)
• Terminal: zsh

1. Install Kalibr and imu_utils (in workstation)

Install Kalibr

按照官方教程 Installation — Building Kalibr from Source来就行

sudo apt-get install -y \
    git wget autoconf automake nano \
    libeigen3-dev libboost-all-dev libsuitesparse-dev \
    doxygen libopencv-dev \
    libpoco-dev libtbb-dev libblas-dev liblapack-dev libv4l-dev
  
# Ubuntu 20.04
sudo apt-get install -y python3-dev python3-pip python3-scipy \
    python3-matplotlib ipython3 python3-wxgtk4.0 python3-tk python3-igraph python3-pyx

mkdir -p ~/kalibr_workspace/src
cd ~/kalibr_workspace
export ROS1_DISTRO=noetic # kinetic=16.04, melodic=18.04, noetic=20.04
source /opt/ros/$ROS1_DISTRO/setup.bash
catkin init
catkin config --extend /opt/ros/$ROS1_DISTRO
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

cd ~/kalibr_workspace/
catkin build -DCMAKE_BUILD_TYPE=Release -j16

Install imu_utils

1. 首先安装好依赖的库
2. 创建catkin_ws/src
3. 将code_utils仓库克隆到catkin_ws/src进行编译
4. code_utils编译之后再将code_utils放在src路径下，再编译

安装依赖

sudo apt-get install libdw-dev

sudo apt-get install libceres-dev

编译安装code_utils

code_utils仓库：点击跳转

将源码克隆下来之后，需要修改一堆东西：

1. CMakeLists.txt

set(CMAKE_CXX_FLAGS "-std=c++11")修改为 set(CMAKE_CXX_FLAGS "-std=c++14")

2. sumpixel_test.cpp

#include "backward.hpp"修改为 #include "code_utils/backward.hpp"

添加头文件： #include <opencv2/imgcodecs.hpp>

然后就可以使用catkin工具编译了

3. opencv宏的修改

需要修改的地方还挺多，可以使用搜索工具查找

所有 CV_LOAD_IMAGE_UNCHANGED 改为 cv::IMREAD_UNCHANGED

所有 CV_LOAD_IMAGE_GRAYSCALE 改为 cv::IMREAD_GRAYSCALE

编译安装imu_utils

imu_utils仓库：点击跳转

1. CMakeLists.txt

set(CMAKE_CXX_FLAGS "-std=c++11")修改为 set(CMAKE_CXX_FLAGS "-std=c++14")

2. imu_an.cpp

添加头文件： #include <fstream>

然后使用catkin工具编译即可

2. Record rosbag (on plane)

Record IMU data

1. 启动px4节点

sudo chmod 777 /dev/ttyACM0

roslaunch mavros px4.launch

2. 调整IMU频率

rosrun mavros mavcmd long 511 105 5000 0 0 0 0 0

rosrun mavros mavcmd long 511 31 5000 0 0 0 0 0

第一条命令调整/mavros/imu/data_raw的话题频率为200Hz，第二条调整/mavros/imu/data的话题频率为200Hz

提高频率之后使用如下命令检查IMU频率，应该是200Hz左右

rostopic hz /mavros/imu/data_raw

rostopic hz /mavros/imu/data

3. 录制数据包

需要静止录制，官方推荐两小时，至少需要一小时

可以把data和data_raw的数据都录下来

rosbag record --duration=120m -o ~/Desktop/imu_calibration/imu_calibration /mavros/imu/data_raw /mavros/imu/data

Calibration targets preparation

摄像头标定以及摄像头-IMU联合标定都需要一个标定板

点击跳转官方文档 Calibration targets

可以在kalibr/wiki/downloads直接下载Aprilgrid 6x6 0.8x0.8 m (A0 page)的pdf文件和yaml文件。使用A4纸打印出来即可。但是yaml文件里的参数需要修改。

图片来自官方wiki。参数文件中tagSize和tagSpacing两个参数的含义如图所示

标定板yaml文件 aprilgrid_A4.yaml：

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.0205          #size of apriltag, edge to edge [m]
tagSpacing: 0.2927       #ratio of space between tags to tagSize
                         #example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]

以上参数是用A4纸自适应大小打印Aprilgrid 6x6 0.8x0.8 m (A0 page)的参数。具体参数用尺测量即可

Record camera data

1. 启动realsense摄像头节点

roslaunch realsense2_camera rs_d430.launch

2. 另开两个终端，调整两个摄像头的话题频率

rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra1_image_rect_raw

rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra2_image_rect_raw

两个新的话题将以4Hz的频率发布图像数据，话题名称分别为infra1_image_rect_raw和infra2_image_rect_raw

3. 再开两个终端，使用rqt_image_view监测摄像头画面

rqt_image_view /infra1_image_rect_raw

rqt_image_view /infra2_image_rect_raw

4. 录制rosbag

录制时的手法演示：Youtube-DIY Indoor Autonomous Drone! - Part 2 (Kalibr & Calibration) 第57秒开始

• 录制150秒
• 保存到 ~/Desktop/camera_calibration/路径下，文件名为”camera_calibration+时间戳+.bag”
• 记录 infra1_image_rect_raw和 infra2_image_rect_raw两个话题

rosbag record \
  --duration=150 \
  -o ~/Desktop/camera_calibration/camera_calibration \
  /infra1_image_rect_raw \
  /infra2_image_rect_raw

整理为自动化脚本

#!/bin/bash

# 启动第一个终端 启动realsense节点 
gnome-terminal -- bash -c "echo 'The first terminal to launch realsense node'; roslaunch realsense2_camera rs_d430.launch; exec bash"
# 等待10秒钟 等摄像头节点完全启动
sleep 10

# 启动第二个终端 调整第一个摄像头的话题频率
gnome-terminal -- bash -c "echo 'The second terminal to modify the frame rate'; rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra1_image_rect_raw; exec bash"
# 启动第三个终端 调整第二个摄像头的话题频率
gnome-terminal -- bash -c "echo 'The third  terminal to modify the frame rate'; rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra2_image_rect_raw; exec bash"

# 启动两个终端，使用rqt_image监测两个摄像头的内容
gnome-terminal -- bash -c "echo 'The fourth terminal to monitor the first camera'; rqt_image_view /infra1_image_rect_raw; exec bash"
gnome-terminal -- bash -c "echo 'The fifth terminal to monitor the second camera'; rqt_image_view /infra2_image_rect_raw; exec bash"

# 留15秒钟时间做录制准备
# 调整rqt窗口位置，调整摄像头位置
sleep 15

# 启动录制脚本
gnome-terminal -- bash -c "echo 'Start recording'; rosbag record --duration=150 -o ~/Desktop/camera_calibration/camera_calibration /infra1_image_rect_raw /infra2_image_rect_raw; exec bash"

Record camera-IMU data

和摄像头数据录制类似，增加启动IMU节点和的步骤以及录包时录上IMU的数据

自动化脚本如下

#!/bin/bash

# Starting the imu
gnome-terminal -- bash -c "echo 'Terminal for starting imu'; 
PASSWORD='1'; 
echo $PASSWORD | sudo -S chmod 777 /dev/ttyACM0;
roslaunch mavros px4.launch;
exec bash"

sleep 10

gnome-terminal -- bash -c "echo 'Terminal for changing the rate of imu topic';
rosrun mavros mavcmd long 511 105 5000 0 0 0 0 0;
rosrun mavros mavcmd long 511 31 5000 0 0 0 0 0;
exec bash"
sleep 2

# Starting the cameras
gnome-terminal -- bash -c "echo 'The first terminal to launch realsense node'; roslaunch realsense2_camera rs_d430.launch; exec bash"
sleep 10

gnome-terminal -- bash -c "echo 'The fourth terminal to monitor the first camera'; rqt_image_view /camera/infra1/image_rect_raw; exec bash"
sleep 2
gnome-terminal -- bash -c "echo 'The fifth terminal to monitor the second camera'; rqt_image_view /camera/infra2/image_rect_raw; exec bash"

# Ensure directory exists
mkdir -p ~/Desktop/camera_imu_calibration
# 等待15秒钟 准备开启录制窗口
sleep 15

gnome-terminal -- bash -c "echo 'Start recording'; rosbag record --duration=120 -o ~/Desktop/camera_imu_calibration/camera_imu_calibration /camera/infra1/image_rect_raw /camera/infra2/image_rect_raw /mavros/imu/data /mavros/imu/data_raw; exec bash"

3. Calibration (on workstation)

IMU calibration

1. 编写launch文件

参考官方REAEDME to run 部分

px4_calibration.launch文件内容如下:

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

• imu_topic的参数值，修改为录制时话题的名称
• max_time的参数值，需要小于录包时长

2. 启动刚编写好的launch文件

source devel/setup.zsh
roslaunch imu_utils px4_calibration.launch

3. 播放录制的IMU数据包

rosbag play -r 200 imu.bag

Camera calibration (Using Kalibr)

官方文档： Kalibr - Multiple camera calibration - wiki page

文件准备

1. 刚才录制的摄像头数据包
2. 标定板yaml参数文件

运行标定

启动roscore, 并运行如下脚本

source ~/kalibr_workspace/devel/setup.zsh

rosrun kalibr kalibr_calibrate_cameras \
  --bag camera_calibration_2024-09-29-00-43-00.bag \
  --topics /infra1_image_rect_raw /infra2_image_rect_raw \
  --models pinhole-radtan pinhole-radtan \
  --target aprilgrid_A4.yaml

• bag后面跟包名，注意相对路径
• topics后面跟话题名，注意和录制的话题名一致
• models后面跟相机模型，realsense d430用的是pinhole-radtan。两个摄像头就需要写两次

Camera-IMU calibration (Using Kalibr)

文件准备

1. 刚才录制的摄像头+IMU数据包
2. 标定板yaml文件（和摄像头内参标定是用的一样）
3. IMU yaml文件 （需从IMU标定生成的yaml文件中提取）
4. camera yaml文件 （摄像头标定后生成的）

参数文件标准格式示例网站：Kalibr - Yaml formats - wiki page

IMU yaml文件迁移示例（从imu_utils标定的参数结果文件迁移）：

运行标定

启动roscore, 并运行如下脚本

source ~/kalibr_workspace/devel/setup.zsh

rosrun kalibr kalibr_calibrate_imu_camera \
    --target aprilgrid_A4.yaml \
    --cam camera_calibration_2024-09-29-00-43-00-camchain.yaml \
    --imu imu.yaml \
    --bag camera_imu_calibration_2024-09-29-01-32-19.bag \
    --bag-from-to 10 110

• target 后面加标定板参数文件
• cam 后面加摄像头参数文件。注意参数文件中的话题名称要调整为camera_imu联合录制的包里面对应的话题名称
• imu 后面加IMU参数文件
• bag 后面加数据包文件
• bag-from-to 后面加数据包开始和结束时间（单位：秒）（这里选择10~110秒弃用开始和结尾的一小段数据）

References & Navigation

[0] 首先感谢浩哥和师兄泽总的帮助🤩🤩🤩

[1] Kalibr github repository

[2] imu_utils github repository

[3] code_utils github repository

[4] 使用kalibr进行Camera-IMU联合标定

[5] Ubuntu20.04编译并运行imu_utils，并且标定IMU