D435i相机与PX4飞控IMU联合标定

Liz-zz

于 2024-05-13 17:04:23 发布

阅读量942

点赞数 14

分类专栏： ROS 运动规划 D435i 文章标签：人工智能机器人

本文链接：https://blog.csdn.net/weixin_67623597/article/details/138785149

版权

ROS 同时被 3 个专栏收录

5 篇文章 1 订阅

订阅专栏

运动规划

3 篇文章 0 订阅

订阅专栏

D435i

1 篇文章 0 订阅

订阅专栏

在复现ego-planner项目时，进行到vins-fusion标定这步，使用vins-fusion在线标定出现发散，位置漂移现象，如下图所时

所以使用kalibr离线标定法

一、IMU内参标定

使用imu_utils包标定IMU内参，可以校准IMU的噪声密度和随机游走噪声

1. 安装IMU内参标定环境

1.1 安装ceres库

该库为必须项，本机在此前使用中已安装，如下记录，具体可参考Installation — Ceres Solver

1.1.1 安装ceres的一些相关依赖

# CMake
sudo apt-get install cmake
# google-glog + gflags
sudo apt-get install libgoogle-glog-dev libgflags-dev
# BLAS & LAPACK
sudo apt-get install libatlas-base-dev
# SuiteSparse and CXSparse (optional)
sudo apt-get install libsuitesparse-dev

1.1.2 下载编译 ceres-solver

git clone https://github.com/ceres-solver/ceres-solver.git
tar zxf ~/Documents/ceres-solver-*.*.*.tar.gz(对应版本号)
cd ceres-solver
mkdir build
cd build
cmake ..
make 
sudo make install

编译ceres过程中出现依赖错误，安装对应库即可，其他问题，自行搜索也可解决

1.2 安装 code_utils

1.2.1 构建工作空间

mkdir -p ~/kalibr_workspace/src
cd ~/kalibr_workspace
catkin_make

1.2.2 下载编译code_utils

sudo apt-get install libdw-dev
cd kalibr_workspace/src
git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_make

编译之前需要修改 code_utils 的 CMakeLists.txt 文件

①（CMAKE_CXX_FLAGS “-std=c++11” ）改为（CMAKE_CXX_STANDARD 14）

② 添加 include_directories(include/code_utils)

1.3 安装 imu_utils

imu_utils依赖code_utils，先编译code_utils 再编译imu_utils

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

编译之前也需要修改 imu_utils 的 CMakeLists.txt 文件

（CMAKE_CXX_FLAGS “-std=c++11” ）改为（CMAKE_CXX_STANDARD 14）

2. 录制IMU数据包及标定

2.1 录制IMU数据包

无人机静置情况下采集飞控IMU数据，使用rosbag录制IMU静置数据，大约两小时

#（确认飞控的串口连接正常，一般是 /dev/ttyACM0）
ls /dev/tty* 
#（为串口附加权限）
sudo chmod 777 /dev/ttyACM0
#（启动mavros）
roslaunch mavros px4.launch 
#录制imu数据包
rosbag record /mavros/imu/data_raw -O imu_xsens.bag

飞控的IMU数据有两个，一个为/mavros/imu/data_raw，一个为/mavros/imu/data，区别可以自行百度。因为这里后续vins-fusion订阅的是/mavros/imu/data_raw，所以直接录制该话题IMU数据。

2.2 标定结果

2.2.1 配置launch文件

配置imu_utils/launch路径下的xsens.launch，相应名称和参数应以自己IMU为准

<launch>

    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/mavros/imu/data_raw"/>
        <param name="imu_name" type="string" value= "xsens"/>
        <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>

2.2.2 运行 imu_utils 标定IMU

cd kalibr_ws
source devel/setup.bash
roslaunch imu_utils xsens.launch
rosbag play -r 200 imu_xsens.bag

在执行roslaunch命令后，会进入等待话题的状态：wait fot imu data，

然后再打开一个终端，尽快执行rosbag命令，程序进入bag读取，

当bag包加速回放完毕后，执行launch的窗口仍然会显示wait for imu data，等待一段时间计算，计算完毕后会显示计算结果，并保存。

可以在imu_utils/data路径下找到与imu_name对应的xxx_imu_param.yaml文件，其中_n代表noise，_w代表random walk。如下图所示，文件后续用到。

二、相机内参标定

1. 安装相机内参标定环境

1.1 安装依赖项

# 安装依赖项
sudo apt-get install python3-setuptools ipython3 libboost-all-dev doxygen libopencv-dev libeigen3-dev
sudo apt-get install libopencv-dev ros-noetic-vision-opencv ros-noetic-image-transport-plugins ros-noetic-cmake-modules python3-software-properties software-properties-common libpoco-dev python3-matplotlib python3-scipy python3-git python3-pip libtbb-dev libblas-dev liblapack-dev python3-catkin-tools libv4l-dev

1.2 安装kalibr包

cd ~/kalibr_workspace/src
git clone https://github.com/ethz-asl/Kalibr.git
cd ~/kalibr_workspace
catkin build -DCMAKE_BUILD_TYPE=Release -j4

这里使用了catkin build命令，因为之前同一工作空间下已有imu_utils相关编译，

如果导致catkin build有问题，可以先把kalibr_workspace下的build和devel文件夹删掉，再使用catkin_build编译。

编译时间相对长，多等一会。

遇到编译缺少依赖或其他原因报错，可自行安装或者根据报错百度。

2. 下载并打印标定板

从网站Downloads · ethz-asl/kalibr Wiki · GitHub下载Aprilgrid 6x6 0.8x0.8 m (A0 page)标定板PDF和对应yaml文件，如下图所示。（二维码Aprilgrid或者棋盘格checkerboard都是可以的，使用时做好相应yaml的修改即可，此处使用Aprilgrid）

下载的PDF如果用A0纸打印下来，二维码标定板尺寸是和下载的yaml文件对应的，如果用其他尺寸纸打印，需要用尺子量出下图示例a和b的长度然后对yaml文件进行修改。

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
codeOffset: 0            #code offset for the first tag in the aprilboard

这里直接使用A0纸打印下来，yaml文件无需修改，制作完的板子如下所示。

3. 录制相机数据包

realsense-viewer #先去关闭结构光
roslaunch realsense2_camera rs_camera.launch   #启动相机
rqt_image_view   #查看图像
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left #左目设置为4hz
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right #右目设置为4hz
rosbag record -O double_cam_1 /infra_left /infra_right #录左右目数据包

首先输入realsense-viewer，打开realsense软件关闭结构光，如下图所示

然后roslaunch启动D435i相机，再打开终端输入rqt_image_view检查话题/camera/infra1/image_rect_raw和/camera/infra2/image_rect_raw图像显示，如下图所示

然后两行rosrun命令输出左右目4hz图，话题名称分别为/infra_left和/infra_right，用rqt_image_view查看是否有输出，如果有输出：

将无人机放置好，然后手拿标定板站在无人机前方相机能够完全拍摄到的地方，

输入rosbag命令开始录制数据包，手拿标定板进行前后左右上下倾斜等全方向动作录制，

动作不要太快，大概两分钟左右，录制完成后Ctrl+C结束保存数据包，

大致如下图所示，录制时不要让板子超过相机视野。

4. 进行标定

cd ~/kalibr_workspace
source devel/setup.bash
rosrun kalibr kalibr_calibrate_cameras --target /home/camera_data/april_6x6_80x80cm.yaml --bag /home/camera_data/double_cam_1.bag --models pinhole-radtan pinhole-radtan --topic /infra_left /infra_right --show-extraction --approx-sync 0.1

命令解释：

--target后面是标定板参数文件的绝对路径

--bag后面是录制的数据包的绝对路径

--models后面是相机/畸变模型，有几目相机就要写几个，这里是两个针孔模型相机，所以是两个pinhole-radtan;相应地，其他支持的模型可以查看Supported models · ethz-asl/kalibr Wiki · GitHub

--topics后面是刚才录制的话题数据

--show-extraction是在标定过程中的一个显示界面，可以看到图片提取的过程，可以不要

--approx-sync 0.1　时间戳不对齐问题

标定完成后会输出下面三个文件。

查看其中的pdf文件，里面的重投影误差（reprojection errors）在1个像素以内标定就是比较好的了，这里标定到了2个像素以内，如果误差比较大，可以多标定几次，其他文件后续用到。

三、相机和飞控IMU联合标定

1.录制相机和飞控IMU数据包

realsense-viewer #关闭结构光
roslaunch realsense2_camera rs_camera.launch  #启动相机
roslaunch mavros px4.launch  #启动mavros
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 20.0 /infra_left #左目设置20hz
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 20.0 /infra_right #右目设置20hz
rosbag record -O imu_cam_1.bag /infra_left /infra_right /mavros/imu/data_raw  #录左右目和imu数据包

与相机内参标定类似，首先检查关闭D435i结构光，

然后启动相机，之后启动mavros包，接收imu话题，

然后这次左右目图像设置为20hz，imu数据直接使用/mavros/imu/data_raw话题（本机之前设置过，频率已经为200hz）。

这次录制将标定板静置，手拿无人机，进行前后左右上下俯仰等全方位运动录制数据包，与相机内参录制一样，

移动过程中，标定板不要超过相机视野，移动不要太快，时间也大致2分钟左右，

演示视频如Kalibr相机及IMU校准教程(Tutorial: IMU-camera calibration)_哔哩哔哩_bilibili所示。

2. 进行标定

rosrun kalibr kalibr_calibrate_imu_camera --target /home/camera_data/april_6x6_80x80cm.yaml --cam /home/camera_data/double_cam_2-camchain.yaml --imu /home/imu/imu.yaml --bag /home/imu_cam_data/imu_cam_1.bag

命令解释：

--target后面依旧是标定板参数文件的绝对路径

--cam后面是相机内参yaml（相机内参标定生成的yaml文件）的绝对路径

--imu后面是对应格式的imu内参yaml的绝对路径

--bag后面是录制的数据包的绝对路径

imu格式需设置为如下所示：

#Accelerometers
accelerometer_noise_density: 1.4909699009847377e-02   #Noise density (continuous-time)
accelerometer_random_walk:   4.8545209750110478e-04   #Bias random walk
 
#Gyroscopes
gyroscope_noise_density:     9.4975779621881166e-04   #Noise density (continuous-time)
gyroscope_random_walk:       1.7041508754582521e-05   #Bias random walk
 
rostopic:                    /mavros/imu/data_raw      #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

上面数据来自IMU内参标生成的yaml文件，如下图所示：