开发: C++知识库 Java知识库 JavaScript Python PHP知识库人工智能区块链大数据移动开发嵌入式开发工具数据结构与算法开发测试游戏开发网络协议系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑笔记本显卡显示器固态硬盘硬盘耳机手机 iphone vivo oppo 小米华为单反装机图拉丁

-> 系统运维 -> Ubuntu 18.04 ———（Intel RealSense D435i）安装kalibr + 双目视觉与IMU标定（2022年） -> 正文阅读

[系统运维]Ubuntu 18.04 ———（Intel RealSense D435i）安装kalibr + 双目视觉与IMU标定（2022年）

Ubuntu 18.04 ———（Intel RealSense D435i）安装kalibr + 双目视觉与IMU标定（2022年）

一、安装标定工具
二、imu标定
三、双目视觉标定
四、imu+双目标定
参考文献

一、安装标定工具

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 <elfutils/libdw.h>
            ^~~~~~~~~~~~~~~~~~
compilation terminated.

解决：

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 <elfutils/libdw.h>
            ^~~~~~~~~~~~~~~~~~
compilation terminated.
code_utils-master/CMakeFiles/matIO_test.dir/build.make:62: recipe for target 'code_utils-master/CMakeFiles/matIO_test.dir/src/mat_io_test.cpp.o' failed
make[2]: *** [code_utils-master/CMakeFiles/matIO_test.dir/src/mat_io_test.cpp.o] Error 1
CMakeFiles/Makefile2:719: recipe for target 'code_utils-master/CMakeFiles/matIO_test.dir/all' failed
make[1]: *** [code_utils-master/CMakeFiles/matIO_test.dir/all] Error 2
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j4 -l4" failed

解决：

解决方法：将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]