t265 / 双目相机与imu内参标定及外参联合标定

lul~

已于 2024-07-29 21:08:55 修改

阅读量1.4k

点赞数 23

分类专栏：完整配置过程文章标签：相机无人机图像处理

于 2024-01-28 23:23:07 首次发布

本文链接：https://blog.csdn.net/woaixiaojiang/article/details/135902593

版权

完整配置过程专栏收录该内容

7 篇文章 0 订阅

订阅专栏

一. 标定imu数据

1.安装ceres求解库

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

git clone https://github.com/ceres-solver/ceres-solver.git
cd ceres-solver
mkdir build
cd build
cmake ..
make 
sudo make install

如果cmake版本过低，更新cmake版本到3.22，如果cmake版本不匹配，参考：Ubuntu下Cmake版本更换

2.安装 code_utils

2.1 构建工作空间

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

2.2 下载code_utils功能包，并编译

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

可能会报错

fatal error: backward.hpp: 没有那个文件或目录
 #include "backward.hpp" ^~~~~~~~~~~~~~ compilation terminated.

解决方案：修改code_utils/src/sumpixel_test.cpp文件中的头文件：

#include "backward.hpp"  -->  #include "code_utils/backward.hpp"

或者把两个cpp文件注释了，在code_utils的Cmakelist中，把最后四行注释了：

#add_executable(matIO_test   src/mat_io_test.cpp )
#target_link_libraries(matIO_test dw ${OpenCV_LIBS})

#add_executable(sumpixel_test   src/sumpixel_test.cpp )
#target_link_libraries(sumpixel_test dw ${OpenCV_LIBS})

3.下载imu_utils功能包，并编译

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

又报错了：

imu_cab_ws/src/imu_utils/src/imu_an.cpp:68:19: error: aggregate ‘std::ofstream out_t’ has incomplete type and cannot be defined

在imu_an.cpp前面加上头文件

#include<fstream>

4.录制与标定imu数据

无人机静止情况下采集IMU的数据，并录制为ROS包，我采集的时间为2小时20分钟

rosbag record /mavros/imu/data -O imu_xsens.bag

标定配置xsens.launch文件为如下内容：(指定IMU的topic)，这里有一个max_time_min表示使用bag数据的最大时长，单位是分钟，默认是120分钟

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/imu/data"/>		#话题名称
        <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>

启动 imu_utils 标定IMU

roslaunch imu_utils xsens.launch
rosbag play -r 200 imu.bag

这一步是关键，在执行上上步roslaunch imu_utils xsens.launch之后，程序会进入等待话题的状态。

尽快执行rosbag play -r 200 自己的.bag，程序进入bag读取，并计算allan方差。当bag包加速回放完毕后，执行launch的窗口仍然会显示wait for imu data.，等待一段时间计算，计算完毕后会显示计算结果。

显示done之后，在imu_cab_ws/src/imu_utils/data这个文件夹下会出现一系列的data文件，打开xsens_imu_param.yaml这个文件，会看到计算出来的噪声和随机游走的系数值。

二.kalibr标定相机内参

1.构建工作空间

mkdir -p ~/kalibr/src

2.安装依赖

sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev

sudo apt-get install libopencv-dev ros-melodic-vision-opencv ros-melodic-image-transport-plugins ros-melodic-cmake-modules software-properties-common 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

3.编译kalibr

cd ~/kalibr/src
git clone https://github.com/ethz-asl/Kalibr.git
cd ~/kalibr
catkin build -DCMAKE_BUILD_TYPE=Release -j32
source ~/kalibr/devel/setup.bash

4.录制与标定

4.1 运行相机节点

rosrun sensor_driver stereo_left_node 
rosurn sensor_driver stereo_right_node

启动相机节点，如果频率过高，可以使用topic_tools/throttle对输出的图像话题降频，有的教程推荐4Hz，但是我跑4Hz的结果很差，所以我跑的原本的30Hz。

rosrun topic_tools throttle messages /stereo_left_node/left 20.0 /left 
rosrun topic_tools throttle messages /stereo_right_node/right 20.0 /right 

# topic_tools/throttle工具
rosrun topic_tools throttle messages 修改后的话题名 频率 原始话题名

4.2 录制bag

有棋盘格和aprilgrid两种标定板选择，有条件直接aprilgrid，误差会明显减小

#可以改变频率
rosrun topic_tools throttle messages /camera/fisheye1/image_raw 20.0 /image_raw1_th
rosrun topic_tools throttle messages /camera/fisheye2/image_raw 20.0 /image_raw2_th

#录制
rosbag record -O stereo_camera_calibar.bag /image_raw1_th /image_raw2_th
#or
rosbag record -O checkerboard.bag /image_raw1_th /image_raw2_th

将相机对准标定板，不断移动相机达到获取不同角度图像（三个轴的平移，和三个轴的旋转，还可以加个画8字）

拷到我的电脑

scp stereo_camera_calibar.bag luli@192.168.3.103:bag/

4.2 标定双目相机

填写标定板config，targetCols、targetRows数的是内角点数目

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

checkerboard:

target_type: 'checkerboard' #gridtype
targetCols: 6               #number of internal chessboard corners
targetRows: 7               #number of internal chessboard corners
rowSpacingMeters: 0.06      #size of one chessboard square [m]
colSpacingMeters: 0.06      #size of one chessboard square [m]

开始标定

// 进入到kalibr工具目录下
source devel/setup.bash

rosrun kalibr kalibr_calibrate_cameras --bag /home/luli/bag/checkerboard.bag --topics /image_raw1_th /image_raw2_th --models omni-radtan omni-radtan --target /home/luli/kalibr/config/checkerboard.yaml
//或者
rosrun kalibr kalibr_calibrate_cameras --bag /home/luli/bag/aprilgrid.bag --topics /image_raw1_th /image_raw2_th --models omni-radtan omni-radtan --target /home/luli/kalibr/config/aprilgrid.yaml

解释说明：

rosrun kalibr kalibr_calibrate_cameras --bag xxx/stereo_calibra.bag --topics /left_img /right_img --models omni-radtan omni-radtan --target xxx/april_6x6_80x80cm_A0.yaml –approx-sync 0.05 --bag-from-to 5 105

xxx/stereo_calibra.bag 录制的bag路径
/left_img /right_img 话题名
omni-radtan omni-radtan 后面是相机/畸变模型，有几目相机就要写几个，这里是两个omni-radtan模型相机，所以是两个omni-radtan;相应地，其他支持的模型可以查看https://github.com/ethz-asl/kalibr/wiki/supported-models。
xxx/april_6x6_80x80cm_A0.yaml 标定板配置文件（使用标准的标定板，从官网下载的yaml文件参数不用修改，笔记本中有这个文件可以直接用）
–show-extraction　是在标定过程中的一个显示界面，可以看到图片提取的过程，可以不要；
–approx-sync 0.1　时间戳不对齐问题
–bag-from-to后面是想要使用数据时间段的起始时间和时间，单位：秒(s)，这个参数可以剔除掉刚开始录制和结束时一些出入视野等画面

4.3 标定结果

程序会输出三个文件：

report-cam-%BAGNAME%.pdf:PDF版本的结果报告，包含绘制的图片和标定的参数。
results-cam-%BAGNAME%.txt:以文本文件储存的标定结果。
camchain-%BAGNAME%.yaml:以YAML格式储存的标定结果。它可以直接用来作为相机-IMU校正的输入

4.3 标定结果分析

单目内参数据质量分析
1.看重投影误差：1080p分辨率的相机重投影误差在1pix以内。以此类推
2.看标定板位姿是否丰富、标定板的tag是否均匀分布在相机视野
原文

三. 相机和IMU联合标定

1. 录制数据集

把IMU和相机固定在一起录制ROS bag包, 录制的时候要注意按照官方的说法-充分激励IMU- 绕3个轴旋转和3个方向的平移，这里有个官方视频可以参考。

rosrun topic_tools throttle messages /camera/fisheye1/image_raw 20.0 /image_raw1_th
rosrun topic_tools throttle messages /camera/fisheye2/image_raw 20.0 /image_raw2_th

//可以多录几个，看看谁误差小
rosbag record -O t265_imu_calibra.bag /image_raw1_th  /image_raw2_th /mavros/imu/data /mavros/imu/data_raw /camera/fisheye1/image_raw /camera/fisheye2/image_raw

scp t265_imu_calibra.bag luli@192.168.1.108:bag/

2. 标定

imu的yaml：

#Accelerometers
accelerometer_noise_density: 4.8641695361661035e-03   #Noise density (continuous-time)
accelerometer_random_walk:   2.3944306307068487e-04   #Bias random walk
 
#Gyroscopes
gyroscope_noise_density:     5.4583665041817392e-04   #Noise density (continuous-time)
gyroscope_random_walk:       6.1968037914410386e-06   #Bias random walk
 
rostopic:                    /imu      #the IMU ROS topic
update_rate:                 100.0      #Hz (for discretization of the values above)

t265的yaml：

cam0:
  cam_overlaps: [1]
  camera_model: omni
  distortion_coeffs: [0.020319038393341607, 0.32428592620562313, 0.0057438236623953655, 0.0029316961779618823]
  distortion_model: radtan
  intrinsics: [1.8715796761822414, 762.5197842897883, 763.8534187724779, 419.0206137416464, 393.38957667180983]
  resolution: [848, 800]
  rostopic: /image_raw1_th
cam1:
  T_cn_cnm1:
  - [0.9999350455199729, 0.0017701276339184657, -0.011259280133470662, -0.06433639464298786]
  - [-0.001764891067041942, 0.9999983297620723, 0.0004750079810056064, 0.00019512058621149657]
  - [0.011260102152547483, -0.0004551057241803075, 0.9999364994729859, 0.0005596834348183811]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: omni
  distortion_coeffs: [0.04974633605688235, 0.11691114911730745, 0.007397845263796802, -0.004926442406984831]
  distortion_model: radtan
  intrinsics: [1.8892221137019962, 766.9577452255108, 768.3735484966458, 415.676286506623, 395.8878603088309]
  resolution: [848, 800]
  rostopic: /image_raw2_th

开始标定

rosrun kalibr kalibr_calibrate_imu_camera --target /home/luli/kalibr/config/aprilgrid.yaml --bag /home/luli/bag/aprilgrid.bag  --cam /home/luli/kalibr/config/t265.yaml  --imu /home/luli/kalibr/config/imu.yaml 


//更多参数选择：
rosrun kalibr kalibr_calibrate_imu_camera \
	--target /home/luli/kalibr/config/aprilgrid.yaml \
	--imu /home/luli/kalibr/config/imu.yaml \
	--imu-models scale-misalignment \
	--cam /home/luli/kalibr/config/t265.yaml \
	--bag /home/luli/bag/aprilgrid.bag \
    --timeoffset-padding 0.1 \
	--show-extraction

原文

3. 联合标定结果解释

3.1 外参矩阵解释

Transformation (cam0):
-----------------------
T_ci:  (imu0 to cam0): 
[[-0.00836559 -0.99940138 -0.03356935  0.03700074]
 [-0.46911901  0.03356939 -0.88249671 -0.03425407]
 [ 0.88309533  0.00836542 -0.46911902 -0.11576888]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam0 to imu0): 
[[-0.00836559 -0.46911901  0.88309533  0.08647526]
 [-0.99940138  0.03356939  0.00836542  0.03909694]
 [-0.03356935 -0.88249671 -0.46911902 -0.0832964 ]
 [ 0.          0.          0.          1.        ]]

这里T_ci与T_ic分别是指cam到imu的外参矩阵和imu到cam的外参矩阵，这里的注释：T_ci: (imu0 to cam0): 和T_ic: (cam0 to imu0): ，我感觉是反的，大家使用的时候如果发现外参不对，那俩个外参矩阵都试试。并且因为两个坐标是先旋转再平移的，故而平移矩阵不一样。

另外这里解释一下这个imu和摄像头的坐标系。这里比较容易昏，所以我画出相机和imu的坐标系免得后面我遗忘了。

3.1.1 PX4飞控IMU坐标系

一般px4的飞控imu是FLU（前、左、上）坐标系，如图：
在这里插入图片描述

如果想自己测imu的坐标系，那么可以输出/mavros/imu/data，也可以打开rviz可视化查看imu的方向与大小，但是记得把参考坐标系选择为base_link，测试之前必须要知道的是px4imu加速度测量的是支撑力N对应的加速度大小，例如现在我把imu放桌子上，支撑力向上，箭头向上。测试时可以把imu的pitch和roll转个90度，看看x、y轴的正负，就可以判断出来。
静止的飞控支撑力永远竖直向上，飞控的头朝下，x轴加速度为负说明，x轴正方向朝头。
在这里插入图片描述

3.1.2 相机坐标系

在这里插入图片描述
为了和像素平面对应，x轴对应u轴，y轴对应v轴，然后z轴朝前，这是部分资料的定义方式，在kalibr中定义如图，当然也有其他定义方式，比如t265使用了AR/VR中的定义方式就和我们不一样。

3.1.3 外参矩阵验证

用工具标定出来，还是自己验证一下比较放心。

T_ic:  (cam0 to imu0): 
[[-0.02883653 -0.301973    0.95288024  0.20233882]
 [-0.99790113  0.06399167 -0.00991966  0.05760793]
 [-0.05798093 -0.95116632 -0.3031845  -0.09941433]
 [ 0.          0.          0.          1.        ]]

外参如上，用一个在线的旋转矩阵转欧拉角网站：3D Rotation Converter
在这里插入图片描述
我主要用的就是这几个红框中的数，其他的我没研究过，也看不懂。
输入旋转矩阵，可以看到欧拉角，方便查看可以选成角度模式，一个旋转变换对应一组旋转矩阵、六组欧拉角、一组四元数。欧拉角因为定义的旋转顺序不同导致有六组数据，但是这六组数据都表示的同一变换，所以自己验证的时候需要根据上面的轴顺序来旋转。
同时强调，旋转方向：轴正方向朝向自己，逆时针为正，顺时针为负。

3.2 误差图

标定结果误差要使下图蓝线在红色误差线以内：
在这里插入图片描述

相机重投影误差要在这个虚线圆圈以内，其实也没有一个准确的标准，合理即可。

在这里插入图片描述

四. 报错及解决

1. error:Attempt to unlock mutex that was not locked

Extracting calibration target corners
(Checkerboard corners:19089): GLib-GObject-CRITICAL **: 14:17:09.607: g_object_unref: assertion 'G_IS_OBJECT (object)' failed
(Checkerboard corners:19089): GLib-GObject-CRITICAL **: 14:17:09.612: g_object_unref: assertion 'G_IS_OBJECT (object)' failed
(Checkerboard corners:19089): GLib-GObject-CRITICAL **: 14:17:09.612: g_object_unref: assertion 'G_IS_OBJECT (object)' failed
Attempt to unlock mutex that was not locked
已放弃 (核心已转储)

原因：百度后发现是因为ubuntu自带的opencv与我安装的opencv3.2.0有冲突
于是将kalibr中的以下三个含有 cv::startWindowThread()的cpp的cv::startWindowThread()注释掉
原文

结果没啥用，直接去掉–show-extraction

2. Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance.

在上面的标定命令行加上 --approx-sync 0.1 ，将两帧之间的时间同步容忍度加大到0.1s；

3. 标定结果imu误差是一条平线，显然不符合与其，没有任何波动。

提高imu和camera的频率。

五. 知识点笔记

1. rosbag 指令

    rosbag record -a
    rosbag record /topic1 /topic12
    rosbag play
    rosbag play -r 3 <your bagfile name> # 表示以3倍原始速率发布话题
    rosbag info <your bagfile name>
    rosbag play -l <bagfile> # -l == --loop 循环播放

2. pixhawk 6c 修改imu频率

一次性：在QGC的mavlink命令行中输入，即可

mavlink stream -d /dev/ttyS3 -s HIGHRES_IMU -r 100
mavlink stream -d /dev/ttyS3 -s ATTITUDE_QUATERNION -r 100

永久：通过在飞控的sd卡的根目录下创建 /etc/extras.txt，写入以下内容

mavlink stream -d /dev/ttyS3 -s HIGHRES_IMU -r 100
mavlink stream -d /dev/ttyS3 -s ATTITUDE_QUATERNION -r 100

实际PX4与机载电脑通信的端口可能并非 /dev/ttyS3
可以通过Mavlink操作台输入命令查看当前可用的端口状态：

mavlink status

通过自己设置的波特率可以看到实际通过tel2连接机载电脑的端口号。
查看端口原文

六. 部分参考链接

kalibr
相机与IMU标定教程
 使用imu_utils和kalibr进行相机与IMU标定