ubuntu20.04+kalibr_相机与imu联合标定

本文使用的相机是 Realsense D435i，imu是轮趣科技的 N100。
基于 ubuntu20.04+kalibr+imu_utils标定相机和imu以及联合标定。

1、安装kalibr

安装依赖

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
   
 sudo apt-get install -y python3-dev python3-pip python3-scipy \
    python3-matplotlib ipython3 python3-wxgtk4.0 python3-tk python3-igraph

建立工作空间

mkdir kalibr_workspace
cd kalibr_workspace
mkdir src
cd src

下载kalibr并编译

git clone https://github.com/ethz-asl/kalibr.git
catkin_make
source devel/setup.bash

或者直接把setup.bash加到~/.bashrc

2、相机标定

2.1 制作标定版

rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.025 --tspace 0.3 --eps

• –type apriltag 标定板类型
• –nx [NUM_COLS] 列个数 6
• –ny [NUM_ROWS] 行个数 6
• –tsize [TAG_WIDTH_M] 二维码方格长度，单位m 0.025
• –tspace [TAG_SPACING_PERCENT] 小方格与二维码方格长度比例 0.3
• –eps 生成eps文件

最终会在kalibr的工作空间下生成target.pdf文件。

2.2 使用ROS启动相机节点

安装相机节点

sudo apt-get install ros-noetic-usb-cam

修改配置文件（主要是相机编号video_device和分辨率，有时也需要修改图像编码pixel_format）

roscd usb_cam
sudo gedit launch/usb_cam-test.launch

可以查询所有相机编号，我用的realsense D435i，RGB相机是video6（一个一个试出来的QAQ）

<launch>
  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
    <param name="video_device" value="/dev/video6" />
    <param name="image_width" value="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="yuyv" />
    <param name="camera_frame_id" value="usb_cam" />
    <param name="io_method" value="mmap"/>
  </node>
  <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
    <remap from="image" to="/usb_cam/image_raw"/>
    <param name="autosize" value="true" />
  </node>
</launch>

启动相机节点

roslaunch usb_cam usb_cam-test.launch 

此时的ros节点如下图所是，后面需要用到的topic是图中红框圈起来的。

降低相机输出频率为4Hz

# rosrun topic_tools throttle messages [相机话题] 4.0 [自定义改变频率的相机话题名字]
# 此时要保证相机节点是启动的
rosrun topic_tools throttle messages /usb_cam/image_raw 4.0 /usb_cam/image_raw_throttle

此时的ros节点

录制bag

# rosbag record [相机话题] -O [保存的bag名字]
rosbag record /usb_cam/image_raw_throttle -O cam_calib.bag

2.3 用kalibr标定相机

设置标定板配置文件，保存为.yaml文件

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

相机标定
支持的相机模型

rosrun kalibr kalibr_calibrate_cameras \
--bag bag/cam_calib.bag  \
--topics /usb_cam/image_raw_throttle \
--models pinhole-radtan \
--target config/apriltag.yaml \
--show-extraction 

• –bag [录制的包]
• –topics [（降频后的）相机话题]
• –models [相机模型-畸变模型，可以参考上面的连接]
• –target [标定板配置文件]
• –show-extraction(设置表示展示角点提取过程)
• –bag-from-to 选择bag的时间段

标定结果一般与bag文件保存在一起，以YAML格式储存的标定结果，它可以直接用来作为相机-IMU校正的输入；txt文件是相机标定的详细参数，重投影误差小于1个像素，该结果可用。


如果生成reporter报错，那么需要修改~/.config/qt5ct/qt5ct.conf配置文件，将style=Fusion改为 style=gtk3

3、imu标定

3.1 使用ROS启动imu节点

**安装imu串口通信 **

sudo apt install ros-noetic-serial

获取imu串口号，图中是ttyACM0，有些教程显示的是ttyUSB0，是一样的。

使用轮趣科技N100_imu，下载官方的ROS_SDK，百度网盘链接：https://pan.baidu.com/s/1TLguF28svpFfcGHnBRzLgQ?pwd=q9dp，提取码：q9dp

建立新的工作空间，把/fdilink_ahrs_ROS1/fdilink_ahrs复制到工作空间/src下，然后catkin_make编译，编译成功后记得把/devel/setup.bash文件source一下，或者加到~/.bashrc中。

修改/home/zxc/imu_workspace/src/fdilink_ahrs/launch目录下的ahrs_data.launch文件，主要修改端口号，与上一步的串口号对应。

<param name="port" value="/dev/ttyACM0"/>

注意：每次重新接IMU时要设置一下权限

sudo chmod 666 /dev/ttyACM0

启动imu节点

roslaunch fdilink_ahrs ahrs_data.launch

可以用rostopic echo /imu打印信息

rviz可视化
打开rviz，添加imu，设置话题，Global Options 固定坐标系设置为gyro_link

静置imu，录制bag，尽量时间长一点，录制2h左右。

rosbag record /imu -O imu.bag

3.2 安装imu_utils

由于imu_utils依赖code_utils，所以必须先编译code_utils，后编译imu_utils，此外code_utils依赖ceres，需要先全局安装ceres

3.2.1 安装ceres-solver

安装依赖

sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3 libgflags-dev libgoogle-glog-dev libgtest-dev

下载编译

wget ceres-solver.org/ceres-solver-1.14.0.tar.gz
tar -zxvf ceres-solver-1.14.0.tar.gz
cd ceres-solver-1.14.0
mkdir build
cd build
cmake ..
make -j8
sudo make install

3.2.2 安装code_utils

安装依赖

sudo apt-get install libdw-dev

下载code_utils

git clone https://github.com/gaowenliang/code_utils.git

编译code_utils
需要修改部分文件

• CMakeLists.txt

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

• src/sumpixel_test.cpp

修改头文件#include "backward.hpp"为 #include “code_utils/backward.hpp”

添加头文件：#include"opencv2/imgcodecs/legacy/constants_c.h"

CV_MINMAX 改为 NORM_MINMAX

• src/mat_io_test.cpp

CV_LOAD_IMAGE_UNCHANGED 改为 cv::IMREAD_UNCHANGED
CV_LOAD_IMAGE_GRAYSCALE 改为cv::IMREAD_GRAYSCALE

由于我把这个包放在之前imu的工作空间下了，所以单独编译

catkin_make -DCATKIN_WHITELIST_PACKAGES="code_utils"

3.2.3 安装imu_utils

下载imu_utils

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

编译imu_utils
需要修改2个文件

• CMakeLists.txt

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

• src/imu_an.cpp

添加头文件：#include <fstream>

也单独编译

catkin_make -DCATKIN_WHITELIST_PACKAGES="imu_utils"

3.3 用imu_utils标定imu

新建一个launch文件
按照如下设置，主要修改IMU话题名称和标定文件保存的位置。

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/imu"/>	<!--IMU话题名-->
        <param name="imu_name" type="string" value= "N100"/>	 <!--相应名字-->
        <param name="data_save_path" type="string" value= "$(find imu_utils)/bag/"/>   <!--存放数据的文件路径可以根据自己需要修改-->
        <param name="max_time_min" type="int" value= "120"/>		<!--设置bag最大时间为120min-->
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

启动标定

roslaunch imu_utils calibrate.launch

出现上述界面后，播放录制的imu包

rosbag play -r 200 imu.bag		// 200 倍速播放rosbag

标定完结果会保存在launch文件设置的存储位置，一堆txt文件和一个yaml文件。

• IMU标定这部分参考博主学无止境的小龟，写的很细，参考链接放在最后面了。

4、imu和相机联合标定

启动相机、imu节点

roslaunch usb_cam usb_cam-test.launch
rosrun topic_tools throttle messages /usb_cam/image_raw 4.0 /cam
roslaunch fdilink_ahrs ahrs_data.launch

录制bag

rosbag record /imu /cam -O cam_imu.bag

充分激励imu六个轴的运动，参考B站搬运视频，俯仰角、偏航角、横滚角、上下平移、左右平移、前后平移，各弄三次，幅度尽量大一点，之后再混合运动三次，录制时间大概一两分钟。

Kalibr标定
需要三个配置文件：标定板配置文件、imu内参文件、相机内参文件；
一个bag文件：imu和相机联合录制的bag

rosrun kalibr kalibr_calibrate_imu_camera \
--imu /home/zxc/kalibr_workspace/config/imu.yaml \
--cam /home/zxc/kalibr_workspace/config/cam_calib-camchain.yaml \
--target /home/zxc/kalibr_workspace/config/apriltag.yaml \
--bag /home/zxc/kalibr_workspace/bag/cam_imu.bag --show-extraction

注意：
（1）imu和相机内参文件包含了imu和相机节点话题，要和录制bag时候的话题对应
（2）相机的配置文件可以直接用相机标定生成的.yaml文件，imu的配置文件需要修改成如下格式👇

#Accelerometers
accelerometer_noise_density: 5.43036e-03   #Noise density (continuous-time)
accelerometer_random_walk:   1.44598e-04   #Bias random walk
#Gyroscopes
gyroscope_noise_density:     4.9700e-03   #Noise density (continuous-time)
gyroscope_random_walk:       6.8522e-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/yangjinghui2019/article/details/130727459
N100在ROS下通信 http://www.jnnr.cn/a/605589.html
ubuntu安装ceres库https://blog.csdn.net/szylight2022/article/details/127313907
IMU标定https://blog.csdn.net/qq_39607707/article/details/125061020#t7
IMU和相机联合标定https://rupingcen.blog.csdn.net/article/details/109592631