Ubuntu 18.04 ———（Intel RealSense D435i）安装kalibr + 双目视觉与IMU标定（2022年）
Ubuntu 18.04 ———（Intel RealSense D435i）安装kalibr + 双目视觉与IMU标定（2022年）
- 一、安装标定工具
-
- 1. 下载编译code_utils
- 2. 下载编译imu_utils
- 3. 安装kalibr
-
- 解决：kalibr_calibrate_cameras：未找到命令
- 二、imu标定
-
- 1. 写标定参数文件
- 2. 然后运行启动文件
- 3. 编写启动文件
- 4. 录制imu数据包
- 5. 运行校准程序
- 6. 回放数据包
- 7. 标定结果
- 三、双目视觉标定
-
- 1. 下载打印标定板或购买标定板（方式一）
- 2. 自己生成标定板（方式二）
- 3. 启动关闭结构光
- 4. 确定适合距离
- 5. 修改相机帧数（官方推荐是4Hz，尽管实际频率不完全准确，但是不影响结果）
- 6. 录制ROS数据包
- 7. 使用Kalibr标定
- 8. 视觉标定总结
- 四、imu+双目标定
-
- 1. 编辑yaml文件
- 2. rs_imu_stereo.launch
- 3. 关闭IR结构光，参考上面
- 4. imu+双目标定
- 5. 结果
- 参考文献
一、安装标定工具
1. 下载编译code_utils
创建ros工作空间，用于code_utils以及后面的imu_utils，工作空间名和路径可以自己更改，code_utils和imu_utils都是imu标定需要用到的，用于标定imu噪声密度以及随机游走系数
mkdir -p ~/imu_catkin_ws/src
cd ~/imu_catkin_ws/src
catkin_init_workspace
cd ..
catkin_make
source ~/imu_catkin_ws/devel/setup.bash
下载编译code_utils
cd ~/imu_catkin_ws/src
git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_make
- 报错
/home/sfann/imu_catkin_ws/src/code_utils-master/include/code_utils/backward.hpp:216:12: fatal error: elfutils/libdw.h: 没有那个文件或目录
# include 解决：
sudo apt-get install libdw-dev
- 报错
/home/sfann/imu_catkin_ws/src/code_utils-master/src/sumpixel_test.cpp:2:10: fatal error: backward.hpp: 没有那个文件或目录
#include "backward.hpp"
^~~~~~~~~~~~~~
compilation terminated.
code_utils-master/CMakeFiles/sumpixel_test.dir/build.make:62: recipe for target 'code_utils-master/CMakeFiles/sumpixel_test.dir/src/sumpixel_test.cpp.o' failed
make[2]: *** [code_utils-master/CMakeFiles/sumpixel_test.dir/src/sumpixel_test.cpp.o] Error 1
CMakeFiles/Makefile2:857: recipe for target 'code_utils-master/CMakeFiles/sumpixel_test.dir/all' failed
make[1]: *** [code_utils-master/CMakeFiles/sumpixel_test.dir/all] Error 2
make[1]: *** 正在等待未完成的任务....
In file included from /home/sfann/imu_catkin_ws/src/code_utils-master/src/mat_io_test.cpp:2:0:
/home/sfann/imu_catkin_ws/src/code_utils-master/include/code_utils/backward.hpp:216:12: fatal error: elfutils/libdw.h: 没有那个文件或目录
# include 解决：
解决方法：将
sumpixel_test.cpp中#include "backward.hpp"改为：#include "code_utils/backward.hpp"
2. 下载编译imu_utils
cd ~/imu_catkin_ws/src/
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make
3. 安装kalibr
- 安装依赖环境
sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev
sudo apt-get install ros-melodic-vision-opencv ros-melodic-image-transport-plugins ros-melodic-cmake-modules
sudo apt-get install software-properties-common software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython
sudo apt-get install libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev
sudo pip install python-igraph --upgrade
报错：
Exception:
Traceback (most recent call last):
File "/usr/lib/python2.7/dist-packages/pip/basecommand.py", line 215, in main
status = self.run(options, args)
File "/usr/lib/python2.7/dist-packages/pip/commands/install.py", line 342, in run
requirement_set.prepare_files(finder)
File "/usr/lib/python2.7/dist-packages/pip/req/req_set.py", line 380, in prepare_files
ignore_dependencies=self.ignore_dependencies))
File "/usr/lib/python2.7/dist-packages/pip/req/req_set.py", line 620, in _prepare_file
session=self.session, hashes=hashes)
File "/usr/lib/python2.7/dist-packages/pip/download.py", line 821, in unpack_url
hashes=hashes
File "/usr/lib/python2.7/dist-packages/pip/download.py", line 659, in unpack_http_url
hashes)
File "/usr/lib/python2.7/dist-packages/pip/download.py", line 876, in _download_http_url
stream=True,
File "/usr/share/python-wheels/requests-2.18.4-py2.py3-none-any.whl/requests/sessions.py", line 533, in get
return self.request('GET', url, **kwargs)
File "/usr/lib/python2.7/dist-packages/pip/download.py", line 386, in request
return super(PipSession, self).request(method, url, *args, **kwargs)
File "/usr/share/python-wheels/requests-2.18.4-py2.py3-none-any.whl/requests/sessions.py", line 520, in request
resp = self.send(prep, **send_kwargs)
File "/usr/share/python-wheels/requests-2.18.4-py2.py3-none-any.whl/requests/sessions.py", line 630, in send
r = adapter.send(request, **kwargs)
File "/usr/share/python-wheels/CacheControl-0.11.7-py2.py3-none-any.whl/cachecontrol/adapter.py", line 47, in send
resp = super(CacheControlAdapter, self).send(request, **kw)
File "/usr/share/python-wheels/requests-2.18.4-py2.py3-none-any.whl/requests/adapters.py", line 508, in send
raise ConnectionError(e, request=request)
ConnectionError: HTTPSConnectionPool(host='files.pythonhosted.org', port=443): Max retries exceeded with url: /packages/8c/96/0c027d017a334d22237ef29263d314724bd0f345e8ff122e4133c85cec91/python-igraph-0.9.6.tar.gz (Caused by NewConnectionError('<urllib3.connection.VerifiedHTTPSConnection object at 0x7f48d86b1c10>: Failed to establish a new connection: [Errno 101] \xe7\xbd\x91\xe7\xbb\x9c\xe4\xb8\x8d\xe5\x8f\xaf\xe8\xbe\xbe',))
解决：
- 关掉网络代理，有说是因为网络不好，建议多次暴力尝试的，还有建议说使用easy_install --upgrade pip 然而结果总是让人失望，可以看看这个https://blog.csdn.net/learner_ZF/article/details/110875792
- 本步骤如果不成功，则可以直接安装：
sudo apt-get install python-igraph
- 创建工作空间
mkdir -p ~/kalibr_workspace/src
cd ~/kalibr_workspace
source /opt/ros/melodic/setup.bash
catkin init
catkin config --extend /opt/ros/melodic
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
git clone https://github.com/ethz-asl/kalibr/tree/fix/18.04.git
//后续出现报错：kalibr_calibrate_cameras：未找到命令
解决：kalibr_calibrate_cameras：未找到命令
18.04版报错：
解决办法：
修改文件名为Kalibr
报错：
解决办法：
//下载suitesparse文件
https://github.com/jluttine/suitesparse/archive/v4.2.1.tar.gz
//重命名SuiteSparse后放入下面文件夹里
/home/sfann/kalibr_workspace/build/suitesparse/suitesparse_src-prefix/src/suitesparse_src
修改CMakeLists.txt：
将CMakeLists.txt的如下内容：
ExternalProject_Add(suitesparse_src
CMAKE_ARGS -DCMAKE_C_COMPILER=${CMAKE_C_COMPILER} -DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER}
DOWNLOAD_COMMAND rm -f SuiteSparse-${VERSION}.tar.gz && wget http://faculty.cse.tamu.edu/davis/SuiteSparse/SuiteSparse-${VERSION}.tar.gz
PATCH_COMMAND tar -xzf ../SuiteSparse-${VERSION}.tar.gz && rm -rf ../suitesparse_src-build/SuiteSparse && sed -i.bu "s/\\/usr\\/local\\/lib/..\\/lib/g" SuiteSparse/SuiteSparse_config/SuiteSparse_config.mk && sed -i.bu "s/\\/usr\\/local\\/include/..\\/include/g" SuiteSparse/SuiteSparse_config/SuiteSparse_config.mk && mv SuiteSparse ../suitesparse_src-build/
CONFIGURE_COMMAND ""
BUILD_COMMAND cd SuiteSparse && make library -j8 -l8
INSTALL_COMMAND cd SuiteSparse && mkdir -p lib && mkdir -p include && make install && cd lib && cp libamd.2.3.1.a libcamd.2.3.1.a libcholmod.2.1.2.a libcxsparse.3.1.2.a libldl.2.1.0.a libspqr.1.3.1.a libumfpack.5.6.2.a libamd.a libcamd.a libcholmod.a libcxsparse.a libldl.a libspqr.a libumfpack.a libbtf.1.2.0.a libccolamd.2.8.0.a libcolamd.2.8.0.a libklu.1.2.1.a librbio.2.1.1.a libsuitesparseconfig.4.2.1.a libbtf.a libccolamd.a libcolamd.a libklu.a librbio.a libsuitesparseconfig.a ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/ && cd .. && mkdir -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/suitesparse && cd include && cp amd.h cholmod_matrixops.h SuiteSparseQR_definitions.h umfpack_load_symbolic.h umfpack_save_symbolic.h btf.h cholmod_modify.h SuiteSparseQR.hpp umfpack_numeric.h umfpack_scale.h camd.h cholmod_partition.h umfpack_col_to_triplet.h umfpack_qsymbolic.h umfpack_solve.h ccolamd.h cholmod_supernodal.h umfpack_defaults.h umfpack_report_control.h umfpack_symbolic.h cholmod_blas.h cholmod_template.h umfpack_free_numeric.h umfpack_report_info.h umfpack_tictoc.h cholmod_camd.h colamd.h umfpack_free_symbolic.h umfpack_report_matrix.h umfpack_timer.h cholmod_check.h cs.h umfpack_get_determinant.h umfpack_report_numeric.h umfpack_transpose.h cholmod_cholesky.h klu.h umfpack_get_lunz.h umfpack_report_perm.h umfpack_triplet_to_col.h cholmod_complexity.h ldl.h umfpack_get_numeric.h umfpack_report_status.h umfpack_wsolve.h cholmod_config.h RBio.h umfpack_get_symbolic.h umfpack_report_symbolic.h cholmod_core.h spqr.hpp umfpack_global.h umfpack_report_triplet.h cholmod.h SuiteSparse_config.h umfpack.h umfpack_report_vector.h cholmod_io64.h SuiteSparseQR_C.h umfpack_load_numeric.h umfpack_save_numeric.h ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/suitesparse
)
修改为:
ExternalProject_Add(suitesparse_src
CMAKE_ARGS -DCMAKE_C_COMPILER=${CMAKE_C_COMPILER} -DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER}
DOWNLOAD_COMMAND rm -f SuiteSparse-${VERSION}.tar.gz && wget -O SuiteSparse-${VERSION}.tar.gz "https://github.com/jluttine/suitesparse/archive/v${VERSION}.tar.gz"
#DOWNLOAD_COMMAND wget "https://www.baidu.com"
PATCH_COMMAND tar -xzf ../SuiteSparse-${VERSION}.tar.gz && mv suitesparse-${VERSION} ./SuiteSparse && rm -rf ../suitesparse_src-build/SuiteSparse && sed -i.bu "s/\\/usr\\/local\\/lib/..\\/lib/g" SuiteSparse/SuiteSparse_config/SuiteSparse_config.mk && sed -i.bu "s/\\/usr\\/local\\/include/..\\/include/g" SuiteSparse/SuiteSparse_config/SuiteSparse_config.mk && mv SuiteSparse ../suitesparse_src-build/
CONFIGURE_COMMAND ""
BUILD_COMMAND cd SuiteSparse && make library -j8 -l8
INSTALL_COMMAND cd SuiteSparse && mkdir -p lib && mkdir -p include && make install && cd lib && cp libamd.2.3.1.a libcamd.2.3.1.a libcholmod.2.1.2.a libcxsparse.3.1.2.a libldl.2.1.0.a libspqr.1.3.1.a libumfpack.5.6.2.a libamd.a libcamd.a libcholmod.a libcxsparse.a libldl.a libspqr.a libumfpack.a libbtf.1.2.0.a libccolamd.2.8.0.a libcolamd.2.8.0.a libklu.1.2.1.a librbio.2.1.1.a libsuitesparseconfig.4.2.1.a libbtf.a libccolamd.a libcolamd.a libklu.a librbio.a libsuitesparseconfig.a ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/ && cd .. && mkdir -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/suitesparse && cd include && cp amd.h cholmod_matrixops.h SuiteSparseQR_definitions.h umfpack_load_symbolic.h umfpack_save_symbolic.h btf.h cholmod_modify.h SuiteSparseQR.hpp umfpack_numeric.h umfpack_scale.h camd.h cholmod_partition.h umfpack_col_to_triplet.h umfpack_qsymbolic.h umfpack_solve.h ccolamd.h cholmod_supernodal.h umfpack_defaults.h umfpack_report_control.h umfpack_symbolic.h cholmod_blas.h cholmod_template.h umfpack_free_numeric.h umfpack_report_info.h umfpack_tictoc.h cholmod_camd.h colamd.h umfpack_free_symbolic.h umfpack_report_matrix.h umfpack_timer.h cholmod_check.h cs.h umfpack_get_determinant.h umfpack_report_numeric.h umfpack_transpose.h cholmod_cholesky.h klu.h umfpack_get_lunz.h umfpack_report_perm.h umfpack_triplet_to_col.h cholmod_complexity.h ldl.h umfpack_get_numeric.h umfpack_report_status.h umfpack_wsolve.h cholmod_config.h RBio.h umfpack_get_symbolic.h umfpack_report_symbolic.h cholmod_core.h spqr.hpp umfpack_global.h umfpack_report_triplet.h cholmod.h SuiteSparse_config.h umfpack.h umfpack_report_vector.h cholmod_io64.h SuiteSparseQR_C.h umfpack_load_numeric.h umfpack_save_numeric.h ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/suitesparse
)
- 编译源码
cd ~/kalibr_workspace
catkin build -DCMAKE_BUILD_TYPE=Release -j4
source ~/kalibr_workspace/devel/setup.bash
二、imu标定
1. 写标定参数文件
找到realsense-ros包，进入/catkin_ws/src/realsense2_camera/launch（路径仅供参考），复制其中的rs_camera.launch，并重命名为rs_imu_calibration.launch(命名随意)，并对里面的内容做如下更改
将
<arg name="unite_imu_method" default=""/>
// ########### 改为#############
<arg name="unite_imu_method" default="linear_interpolation"/>
这样做的目的是将accel和gyro的数据合并得到imu话题，如何不这样做发布的topic中只要加速计和陀螺仪分开的topic，没有合并的camera/imu topic。
2. 然后运行启动文件
roslaunch realsense2_camera rs_imu_calibration.launch
rostopic list -v
3. 编写启动文件
打开/imu_catkin_ws/src/imu_utils/launch（路径仅供参考），打开终端运行
gedit d435i_imu_calibration.launch
文件名可以自行更改，在其中写入如下内容
<launch>
<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
<!--TOPIC名称和上面一致-->
<param name="imu_topic" type="string" value= "/camera/imu"/>
<!--imu_name 无所谓-->
<param name="imu_name" type="string" value= "d435i"/>
<!--标定结果存放路径-->
<param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
<!--数据录制时间-min-->
<param name="max_time_min" type="int" value= "120"/>
<!--采样频率，即是IMU频率，采样频率可以使用rostopic hz /camera/imu查看，设置为400，为后面的rosbag play播放频率-->
<param name="max_cluster" type="int" value= "400"/>
</node>
</launch>
4. 录制imu数据包
插上相机，realsense静止放置，放置时间要稍大于d435i_imu_calibration.launch中的录制时间，即大于120分钟
roscore
roslaunch realsense2_camera rs_imu_calibration.launch
cd ~/imu_catkin_ws //等下录制到这个文件夹上
rosbag record -O imu_calibration /camera/imu
等待两小时录制完之后就按下ctrl+c，结束录制。
你会发现当前目录有一个名为 imu_calibration.bag的文件，其中imu_calibration是bag包的名字，可以更改，/camera/imu是发布的IMU topic，可以通过以下命令查看
rostopic list -v
5. 运行校准程序
首先激活imu_util工作空间的setup.bash，前面路径仅供参考
source ~/imu_catkin_ws/devel/setup.bash
roslaunch imu_utils d435i_imu_calibration.launch
6. 回放数据包
打开新的终端
cd 存放imu_calibration.bag的路径
source ~/imu_catkin_ws/devel/setup.sh
cd ~/imu_catkin_ws //数据包在这个文件夹下
rosbag play -r 400 imu_calibration.bag
其中 -r 400是指400速播放bag数据
7. 标定结果
标定结束后在imu_catkin_ws/src/imu_utils/data中生成许多文件，其中d435i_imu_param.yaml就是我们想要的结果，展示如下：
自己标定的结果：
%YAML:1.0
---
type: IMU
name: d435i
Gyr:
unit: " rad/s"
avg-axis:
gyr_n: 1.9035698029406516e-03
gyr_w: 1.9530927686562762e-05
x-axis:
gyr_n: 1.4892362024027585e-03
gyr_w: 1.2193835487072606e-05
y-axis:
gyr_n: 2.2147467482919636e-03
gyr_w: 3.1132672413782294e-05
z-axis:
gyr_n: 2.0067264581272322e-03
gyr_w: 1.5266275158833387e-05
Acc:
unit: " m/s^2"
avg-axis:
acc_n: 2.0546933436785896e-02
acc_w: 1.0213093603518382e-03
x-axis:
acc_n: 3.0836008609834702e-02
acc_w: 2.1500692642280059e-03
y-axis:
acc_n: 1.5226156894860206e-02
acc_w: 2.6409911360802290e-04
z-axis:
acc_n: 1.5578634805662779e-02
acc_w: 6.4975970321948598e-04
别的博主标定后的结果：
%YAML:1.0
---
type: IMU
name: d435i
Gyr:
unit: " rad/s"
avg-axis:
gyr_n: 2.3539521240749008e-03
gyr_w: 2.2003805724014335e-05
x-axis:
gyr_n: 2.6188870753537648e-03
gyr_w: 1.9633429257398280e-05
y-axis:
gyr_n: 3.2575261188632675e-03
gyr_w: 3.5268522097346084e-05
z-axis:
gyr_n: 1.1854431780076705e-03
gyr_w: 1.1109465817298644e-05
Acc:
unit: " m/s^2"
avg-axis:
acc_n: 2.8250053766610776e-02
acc_w: 7.8925155899657628e-04
x-axis:
acc_n: 2.2612064059723894e-02
acc_w: 4.9228531499070764e-04
y-axis:
acc_n: 2.4653311434561623e-02
acc_w: 7.6221527154486917e-04
z-axis:
acc_n: 3.7484785805546816e-02
acc_w: 1.1132540904541519e-03
realsense自带的参数：
roslaunch realsense2_camera rs_camera.launch
rostopic echo /camera/accel/imu_info
rostopic echo /camera/gyro/imu_info
三、双目视觉标定
1. 下载打印标定板或购买标定板（方式一）
到https://github.com/ethz-asl/kalibr/wiki/downloads选择
下载，然后缩放到40%，用A4纸就可以打印出来
原始pdf的格子参数是：6*6的格子
大格子边长：5.5cm
小格子边长：1.65cm
小格子与大格子边长比例：0.3
调整后的格子参数是：
大格子边长：2.2cm
小格子边长：0.66cm
小格子与大格子边长比例：0.3
但这只是理想情况，实际情况还得实际测量。
2. 自己生成标定板（方式二）
1、进入calibr_workspace工程目录
cd ~/kalibr_workspace //你的工程路径
source ./devel/setup.sh
kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.022 --tspace 0.3
2、标定板参数定义
--type apriltag 标定板类型
--nx [NUM_COLS] 列个数
--ny [NUM_ROWS] 行个数
--tsize [TAG_WIDTH_M] 二维码方格长度，单位m
--tspace [TAG_SPACING_PERCENT] 小方格与二维码方格长度比例
新建april_6x6_A4.yaml文件，格式参考上图的yaml，内容展示如下：
target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags
tagRows: 6 #number of apriltags
tagSize: 0.022 #size of apriltag, edge to edge [m]
tagSpacing: 0.3 #ratio of space between tags to tagSize
测量实际长度后，请自行更改。
3. 启动关闭结构光
默认开始结构光时，双目图像会有很多点，这些点可能对标定有影响，所以使用时需要关闭结构光。
先启动
roslaunch realsense2_camera rs_camera.launch//注意不是：rs_imu_calibration.launch
新打开终端，运行
rosrun rqt_reconfigure rqt_reconfigure
打开后将camera->stereo_module中的emitter_enabled设置为off(0) ，展示如下：
4. 确定适合距离
打开rviz观察合理的距离，要求摄像头能看到标定棋盘格。
roscore
roslaunch realsense2_camera rs_camera.launch
rviz
左侧 Fixed Frame 选择camera_link
左下角 add --> By topic --> /camera/color/image_raw/ --> 双击Camera ，找一个适合的能拍到棋盘格的距离
关闭
之后在里面add rgb和双目对应的topic，/camera/color/image_raw、/camera/infra1/image_rect_raw、/camera/infra2/image_rect_raw
注意：如果找不到双目对应的topic，则需要找到launch文件进行修改！！！
5. 修改相机帧数（官方推荐是4Hz，尽管实际频率不完全准确，但是不影响结果）
kalibr在处理标定数据的时候要求频率不能太高，一般为4Hz，我们可以使用如下命令来更改topic的频率，实际上是将原来的topic以新的频率转成新的topic，实际测试infra1才是对应左目相机
roscore
roslaunch realsense2_camera rs_camera.launch
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right
可以一起降频：
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color & rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left & rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right
等待。。。。成功
注意：这种方式可能导致不同摄像头的时间不同步，如果出现这个问题，可以尝试不做这个操作，不这样做意味着需要更多的处理时间，这样的话后面也要相应的更改。
6. 录制ROS数据包
rosbag record -O multicameras_calibration /infra_left /infra_right /color
bag文件属性信息：
rosbag info multicameras_calibration.bag
后面三个topic就是转换频率后的topic
可以打开
roscore
roslaunch realsense2_camera rs_camera.launch
rviz
rosbag info指令可以显示数据包中的信息:
rosbag info filename.bag
这些信息包括 topic 的名称、类型和 message 数量。 接下来回放数据包中的 topic。 首先在turtle_teleop_key所在的终端窗口中按Ctrl+C退出该键盘控制节点。保留turtlesim节点继续运行。在终端中bag文件所在目录下运行以下命令:rosbag play 就能够回放出 bag 中包含的 topic 内容了。 如果想改变消息的发布速率，可以用下面的命令：
rosbag play -r 2
这时的轨迹相当于以两倍的速度通过按键发布控制命令时产生的轨迹。-r后面的数字对应播放速率。
7. 使用Kalibr标定
先激活环境变量
source devel/setup.bash
//kalibr_calibrate_cameras --target /位置/文件名.yaml --bag /位置/camd435i.bag --bag-from-to 26 100 --models pinhole-radtan --topics /color --show-extraction
kalibr_calibrate_cameras --target april_6x6_A4.yaml --bag multicameras_calibration.bag --models pinhole-equi pinhole-equi pinhole-equi --topics /infra_left /infra_right /color --bag-from-to 10 100 --show-extraction
其中–target …/yaml/april_6x6_A4.yaml是标定板的配置文件，注意如果选择棋格盘，注意targetCols和targetRows表示的是内侧角点的数量，不是格子数量。–bag multicameras_calibration.bag是录制的数据包，models pinhole-equi pinhole-equi pinhole-equi表示三个摄像头的相机模型和畸变模型（解释参考https://github.com/ethz-asl/kalibr/wiki/supported-models，根据需要选取）， --topics /infra_left /infra_right /color表示三个摄像头对应的拍摄的数据话题，–bag-from-to 10 100表示处理bag中10-100秒的数据。–show-extraction表示显示检测特征点的过程，这些参数可以相应的调整。
报错：
kalibr_calibrate_cameras：未找到命令
解决：
重新装18.04版本！！！！！
报错：
ImportError: cannot import name NavigationToolbar2Wx
解决：
把 matplotlib 中的 NavigationToolbar2Wx 换成 NavigationToolbar2WxAgg
报错：
ImportError: No module named wx
解决：
pip install -U wxPython
生成文件：
.yaml 主要作用为了后期IMU+相机联合标定
.pdf 以图的方式显示效果
.txt 含有相机的内参
Calibration results
====================
Camera-system parameters:
cam0 (/infra_left):
type: <class 'aslam_cv.libaslam_cv_python.EquidistantDistortedPinholeCameraGeometry'>
distortion: [0.35043432 0.09942954 0.06984316 0.06546343] +- [0.00781519 0.04741581 0.11274296 0.08942823]
projection: [422.64066393 424.02986183 433.83372832 239.28229369] +- [0.20221793 0.20514435 0.14661001 0.18238583]
reprojection error: [-0.000012, -0.000001] +- [0.132899, 0.137670]
cam1 (/infra_right):
type: <class 'aslam_cv.libaslam_cv_python.EquidistantDistortedPinholeCameraGeometry'>
distortion: [ 0.34116698 0.14997256 -0.03561303 0.12752762] +- [0.00795709 0.0515372 0.12997253 0.10955421]
projection: [422.6846631 424.2264825 432.1283922 239.61231361] +- [0.18937895 0.19610187 0.14878201 0.1664231 ]
reprojection error: [0.000003, -0.000000] +- [0.140687, 0.133449]
cam2 (/color):
type: <class 'aslam_cv.libaslam_cv_python.EquidistantDistortedPinholeCameraGeometry'>
distortion: [ 0.45699434 0.29814143 -2.39416573 3.86503647] +- [0.00479866 0.02820847 0.08063053 0.11523948]
projection: [892.58112899 894.8405036 644.21848141 372.01450162] +- [0.14415508 0.12922398 0.13908751 0.32740773]
reprojection error: [0.000007, 0.000000] +- [0.528433, 0.633812]
baseline T_1_0:
q: [-0.0004175 -0.00167131 -0.00002664 0.99999852] +- [0.00036468 0.00030905 0.00008834]
t: [-0.04991163 0.00001251 -0.00011163] +- [0.00003675 0.00003242 0.00011497]
baseline T_2_1:
q: [-0.00279635 -0.00483828 -0.00002549 0.99998439] +- [0.00038545 0.00036808 0.0001018 ]
t: [ 0.06486191 0.00022212 -0.00036701] +- [0.00003264 0.00003088 0.00011151]
Target configuration
====================
Type: aprilgrid
Tags:
Rows: 6
Cols: 6
Size: 0.022 [m]
Spacing 0.0066 [m]
intrinsic 是内参
distortion_coeffs 是畸变参数
cam0:
cam_overlaps:
- 1
- 2
camera_model: pinhole
distortion_coeffs:
- 0.3504343204199696
- 0.09942954345832032
- 0.06984316113464994
- 0.06546342606600335
distortion_model: equidistant
intrinsics:
- 422.64066392524313
- 424.02986182898763
- 433.8337283244407
- 239.28229368758497
resolution:
- 848
- 480
rostopic: /infra_left
cam1:
T_cn_cnm1:
- - 0.9999944120461245
- -5.1892669972653565e-05
- 0.0033426312493865813
- -0.04991163289867289
- - 5.4683759833798546e-05
- 0.999999649965876
- -0.0008349118544065274
- 1.250629277077362e-05
- - -0.0033425867535462755
- 0.0008350899766020524
- 0.9999940648516464
- -0.00011162780864553323
- - 0.0
- 0.0
- 0.0
- 1.0
cam_overlaps:
- 0
- 2
camera_model: pinhole
distortion_coeffs:
- 0.34116697848102345
- 0.14997256247725585
- -0.035613030583661255
- 0.12752761977350466
distortion_model: equidistant
intrinsics:
- 422.6846630997528
- 424.22648250200166
- 432.1283922008165
- 239.61231361314714
resolution:
- 848
- 480
rostopic: /infra_right
cam2:
T_cn_cnm1:
- - 0.9999531808683079
- -2.391133562855248e-05
- 0.009676543782981287
- 0.0648619072192395
- - 7.802943217049163e-05
- 0.9999843595299047
- -0.00559237042376466
- 0.00022212473620736555
- - -0.009676258716241545
- 0.005592863649054125
- 0.9999375429962823
- -0.00036701478820695867
- - 0.0
- 0.0
- 0.0
- 1.0
cam_overlaps:
- 0
- 1
camera_model: pinhole
distortion_coeffs:
- 0.45699433605212914
- 0.29814143046464464
- -2.3941657257388846
- 3.8650364650694873
distortion_model: equidistant
intrinsics:
- 892.5811289930502
- 894.8405035959634
- 644.2184814063597
- 372.014501622761
resolution:
- 1280
- 720
rostopic: /color
8. 视觉标定总结
在kalibr_workspace下打开多个终端分别输入如下指令：
//终端1
roscore
//终端2
roslaunch realsense2_camera rs_camera.launch
//终端3
rviz
//终端4
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color & rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left & rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right
//终端5
rosbag record -O multicameras_calibration /infra_left /infra_right /color
rosbag info multicameras_calibration.bag
source devel/setup.bash
kalibr_calibrate_cameras --target april_6x6_A4.yaml --bag multicameras_calibration.bag --models pinhole-equi pinhole-equi pinhole-equi --topics /infra_left /infra_right /color --bag-from-to 10 100 --show-extraction
四、imu+双目标定
1. 编辑yaml文件
新建 camchain.yaml，imu.yaml
官方教程https://github.com/ethz-asl/kalibr/wiki/yaml-formats
gedit camchain.yaml
参数参考上面得到的yaml文件，没有的参数可以删除，最终结果示例如下：
cam0:
camera_model: pinhole
intrinsics: [422.64066392524313, 424.02986182898763, 433.8337283244407, 239.28229368758497]
distortion_model: equidistant
distortion_coeffs: [0.3504343204199696, 0.09942954345832032, 0.06984316113464994, 0.06546342606600335]
rostopic: /infra_left
resolution: [848, 480]
cam1:
camera_model: pinhole
intrinsics: [422.6846630997528, 424.22648250200166, 432.1283922008165, 239.61231361314714]
distortion_model: equidistant
distortion_coeffs: [0.34116697848102345, 0.14997256247725585, -0.035613030583661255, 0.12752761977350466]
T_cn_cnm1:
- [0.9999944120461245, -5.1892669972653565e-05, 0.0033426312493865813, -0.04991163289867289]
- [5.4683759833798546e-05, 0.999999649965876, -0.0008349118544065274, 1.250629277077362e-05]
- [-0.0033425867535462755, 0.0008350899766020524, 0.9999940648516464, -0.00011162780864553323]
- [0.0,0.0,0.0,1.0]
rostopic: /infra_right
resolution: [848, 480]
gedit imu.yaml
参数使用之前imu标定得到的参数，示例如下：
#Accelerometers
accelerometer_noise_density: 2.0546933436785896e-02 #Noise density (continuous-time)
accelerometer_random_walk: 1.0213093603518382e-03 #Bias random walk
#Gyroscopes
gyroscope_noise_density: 1.9035698029406516e-03 #Noise density (continuous-time)
gyroscope_random_walk: 1.9530927686562762e-05 #Bias random walk
rostopic: /imu #the IMU ROS topic
update_rate: 200.0 #Hz (for discretization of the values above)
2. rs_imu_stereo.launch
复制realsense-ros包中rs_camera.launch，重命名为rs_imu_stereo.launch，更改内容为
<arg name="enable_sync" default="false"/>
//改为：
<arg name="enable_sync" default="true"/>
这样来使imu和双目数据时间对齐
<arg name="unite_imu_method" default=""/>
//改为
<arg name="unite_imu_method" default="linear_interpolation"/>
这样来保证会有imu话题
3. 关闭IR结构光，参考上面
rosrun rqt_reconfigure rqt_reconfigure
4. imu+双目标定
在kalibr_workspace下打开多个终端分别输入如下指令：
//终端1
roscore
//终端2
roslaunch realsense2_camera rs_imu_stereo.launch
//终端3
rviz
//终端4
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 20.0 /infra_left & rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 20.0 /infra_right & rosrun topic_tools throttle messages /camera/imu 200.0 /imu
rostopic hz topic名可以查看实际频率。
//终端5录包：
rosbag record -O imu_stereo.bag /infra_left /infra_right /imu
//终端6标定：
source ~/kalibr_workspace/devel/setup.bash
kalibr_calibrate_imu_camera --bag imu_stereo.bag --cam camchain.yaml --imu imu.yaml --target april_6x6_A4.yaml --bag-from-to 15 115 --show-extraction
5. 结果
cam0:
T_cam_imu:
- - 0.9999397258788518
- -0.004683976226100742
- -0.009930003828888262
- 0.00740103501939206
- - 0.004719594270289592
- 0.9999825025383232
- 0.0035665175056139383
- -0.006330740676123676
- - 0.00991312459582054
- -0.0036131681260804997
- 0.9999443359391772
- -0.01893316608480435
- - 0.0
- 0.0
- 0.0
- 1.0
camera_model: pinhole
distortion_coeffs:
- 0.3504343204199696
- 0.09942954345832032
- 0.06984316113464994
- 0.06546342606600335
distortion_model: equidistant
intrinsics:
- 422.64066392524313
- 424.02986182898763
- 433.8337283244407
- 239.28229368758497
resolution:
- 848
- 480
rostopic: /infra_left
timeshift_cam_imu: 0.005100358375965326
cam1:
T_cam_imu:
- - 0.9999670292694932
- -0.004747919302927823
- -0.006587688231643368
- -0.04257359730949143
- - 0.0047659964968495335
- 0.9999849130497962
- 0.002731107867444307
- -0.006302019926150486
- - 0.006574621763756843
- -0.002762414719857174
- 0.9999745713834822
- -0.019074706862001883
- - 0.0
- 0.0
- 0.0
- 1.0
T_cn_cnm1:
- - 0.999994412046126
- -5.189266997265358e-05
- 0.0033426312493865817
- -0.04991163289867289
- - 5.4683759833798546e-05
- 0.9999996499658775
- -0.0008349118544065274
- 1.250629277077362e-05
- - -0.003342586753546276
- 0.0008350899766020524
- 0.999994064851648
- -0.00011162780864553323
- - 0.0
- 0.0
- 0.0
- 1.0
camera_model: pinhole
distortion_coeffs:
- 0.34116697848102345
- 0.14997256247725585
- -0.035613030583661255
- 0.12752761977350466
distortion_model: equidistant
intrinsics:
- 422.6846630997528
- 424.22648250200166
- 432.1283922008165
- 239.61231361314714
resolution:
- 848
- 480
rostopic: /infra_right
timeshift_cam_imu: 0.005063952809659112
Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0): mean 0.114291730438, median 0.102916532522, std: 0.0667627153792
Reprojection error (cam1): mean 0.105048543509, median 0.0936183682567, std: 0.063397637115
Gyroscope error (imu0): mean 0.0570492572985, median 0.0473407884672, std: 0.0480612749683
Accelerometer error (imu0): mean 0.0796799558747, median 0.078217672501, std: 0.0499522767362
Residuals
----------------------------
Reprojection error (cam0) [px]: mean 0.114291730438, median 0.102916532522, std: 0.0667627153792
Reprojection error (cam1) [px]: mean 0.105048543509, median 0.0936183682567, std: 0.063397637115
Gyroscope error (imu0) [rad/s]: mean 0.00153579694557, median 0.00127443969951, std: 0.00129383558685
Accelerometer error (imu0) [m/s^2]: mean 0.0231532039172, median 0.0227282972419, std: 0.0145150086581
Transformation (cam0):
-----------------------
T_ci: (imu0 to cam0):
[[ 0.99993973 -0.00468398 -0.00993 0.00740104]
[ 0.00471959 0.9999825 0.00356652 -0.00633074]
[ 0.00991312 -0.00361317 0.99994434 -0.01893317]
[ 0. 0. 0. 1. ]]
T_ic: (cam0 to imu0):
[[ 0.99993973 0.00471959 0.00991312 -0.00718302]
[-0.00468398 0.9999825 -0.00361317 0.00629689]
[-0.00993 0.00356652 0.99994434 0.01902818]
[ 0. 0. 0. 1. ]]
timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
0.005100358375965326
Transformation (cam1):
-----------------------
T_ci: (imu0 to cam1):
[[ 0.99996703 -0.00474792 -0.00658769 -0.0425736 ]
[ 0.004766 0.99998491 0.00273111 -0.00630202]
[ 0.00657462 -0.00276241 0.99997457 -0.01907471]
[ 0. 0. 0. 1. ]]
T_ic: (cam1 to imu0):
[[ 0.99996703 0.004766 0.00657462 0.04272764]
[-0.00474792 0.99998491 -0.00276241 0.0060471 ]
[-0.00658769 0.00273111 0.99997457 0.01881097]
[ 0. 0. 0. 1. ]]
timeshift cam1 to imu0: [s] (t_imu = t_cam + shift)
0.005063952809659112
Baselines:
----------
Baseline (cam0 to cam1):
[[ 0.99999441 -0.00005189 0.00334263 -0.04991163]
[ 0.00005468 0.99999965 -0.00083491 0.00001251]
[-0.00334259 0.00083509 0.99999406 -0.00011163]
[ 0. 0. 0. 1. ]]
baseline norm: 0.0499117592936465 [m]
Gravity vector in target coords: [m/s^2]
[-0.09537361 -9.80053477 -0.32991678]
Calibration configuration
=========================
cam0
-----
Camera model: pinhole
Focal length: [422.64066392524313, 424.02986182898763]
Principal point: [433.8337283244407, 239.28229368758497]
Distortion model: equidistant
Distortion coefficients: [0.3504343204199696, 0.09942954345832032, 0.06984316113464994, 0.06546342606600335]
Type: aprilgrid
Tags:
Rows: 6
Cols: 6
Size: 0.022 [m]
Spacing 0.0066 [m]
cam1
-----
Camera model: pinhole
Focal length: [422.6846630997528, 424.22648250200166]
Principal point: [432.1283922008165, 239.61231361314714]
Distortion model: equidistant
Distortion coefficients: [0.34116697848102345, 0.14997256247725585, -0.035613030583661255, 0.12752761977350466]
Type: aprilgrid
Tags:
Rows: 6
Cols: 6
Size: 0.022 [m]
Spacing 0.0066 [m]
IMU configuration
=================
IMU0:
----------------------------
Model: calibrated
Update rate: 200.0
Accelerometer:
Noise density: 0.0205469334368
Noise density (discrete): 0.290577519315
Random walk: 0.00102130936035
Gyroscope:
Noise density: 0.00190356980294
Noise density (discrete): 0.0269205423224
Random walk: 1.95309276866e-05
T_i_b
[[1. 0. 0. 0.]
[0. 1. 0. 0.]
[0. 0. 1. 0.]
[0. 0. 0. 1.]]
time offset with respect to IMU0: 0.0 [s]
参考文献
https://github.com/ethz-asl/kalibr/wiki/camera-imu-calibration
https://blog.csdn.net/Hanghang_/article/details/103546033
https://blog.csdn.net/ndwonduosk_/article/details/124718186
https://blog.csdn.net/SLCnoon/article/details/122083955
https://blog.csdn.net/xiaoxiaoyikesu/article/details/105646064