SLAM算法与工程实践系列文章

下面是SLAM算法与工程实践系列文章的总链接，本人发表这个系列的文章链接均收录于此

SLAM算法与工程实践系列文章链接

---

下面是专栏地址：

SLAM算法与工程实践系列专栏

---

文章目录

• SLAM算法与工程实践系列文章
• • SLAM算法与工程实践系列文章链接
  • SLAM算法与工程实践系列专栏
• 前言
• SLAM算法与工程实践——相机篇：RealSense T265相机使用（1）
• • realsense-viewer 不识别 T265
  • • 源码安装
    • 命令安装
  • T265参数
  • • 读取T265内外参数信息
    • 鱼眼相机模型
    • • KANNALA BRANDT模型
      • Opencv API 鱼眼图像去畸变方法
      • • 基础鱼眼图像去畸变
        • 相机主点参数调节
        • 相机f参数调节
  • 相机和IMU标定
  • • 准备工作
    • • 下载并编译ceres
      • 下载并编译code_utils
      • 下载并编译imu_utils
      • 下载并编译kalibr
      • 最后
    • IMU标定
    • • 相关文件的修改
      • 重力加速度自检
      • IMU的校准
      • • 官方rs_imu_calibration.py工具校准
        • imu_utils工具校准
    • 相机的标定
    • Camera-IMU联合标定

---

前言

这个系列的文章是分享SLAM相关技术算法的学习和工程实践

---

SLAM算法与工程实践——相机篇：RealSense T265相机使用（1）

参考：

Z410-4B-T265视觉版教程

Intel Realsense T265使用教程

realsense官方文档：https://dev.intelrealsense.com/docs/code-samples

先修改 rs_t265.launch 的配置文件

将 false 改为 true

修改完后启动相机

roslaunch realsense2_camera rs_t265.launch

可以看到相机的参数开启情况，根据需要去修改配置文件

在 rviz 中添加话题

即可显示相机

启动测试demo，即可看到T265的位姿数据

roslaunch realsense2_camera demo_t265.launch

如果需要查看图像数据，则需要对rs_t265.launch lanunch文件中使能图像输出 ，然后添加topic即可

图像数据发布的Topic 图像数据30Hz

• /camera/fisheye1/camera_info
• /camera/fisheye1/image_raw
• /camera/fisheye2/camera_info
• /camera/fisheye2/image_raw

IMU 数据发布的Topic, 陀螺仪数据200Hz 加速度数据为 63Hz

• /camera/accel/sample

  
  

• /camera/gyro/sample

  

里程计发布的Topic 200Hz

• /camera/odom/sample

如果需要将加速度和陀螺仪的数据整合到一个topic发布的话，则需设置如下参数：

此时IMU的发布频率为200Hz

realsense-viewer 不识别 T265

参考

realsense-viewer 不识别 T265——Realsense SDK 在 v2.54.1 版本以后不再支持T265相机的解决办法

由于T265停产，Intel® RealSense™ SDK 2.0 (v2.54.1) 在该版本中移除了对T265相机的支持，以后的版本也不会支持了。为了继续使用 T265 相机，最好千万不要升级 realsense 相关的 package。

经测试Intel® RealSense™ SDK 2.0 (v2.53.1) 能对 T265 正常支持，因此，本文主要是总结如何安装 v2.53.1 版本的 SDK，经过验证，整理了两种可行方法。

注意：安装时，不要将相机连在设备上。

源码安装

在官网的发行界面，找到 Intel® RealSense™ SDK 2.0 (v2.53.1) 的界面，下载源码 zip 格式或者 tar.gz 都行，然后解压。

安装依赖项

解压好后，进入 librealsense 文件目录，并在该目录下打开终端。

sudo apt-get install libssl-dev libusb-1.0-0-dev libudev-dev pkg-config libgtk-3-dev cmakesudo apt-get install libglfw3-dev libgl1-mesa-dev libglu1-mesa-dev at

安装 librealsense原文件目录下的许可脚本

./scripts/setup_udev_rules.sh

注意：该许可脚本可以通过如下命令移除

./scripts/setup_udev_rules.sh --uninstall

编译

（1） 导航到 librealsense 根目录并运行

mkdir build && cd build

（2）运行CMake

cmake ../ -DCMAKE_BUILD_TYPE=Release
make -jx
sudo make install 

（3）将realsense插到USB3.0插口，测试安装好的librealsense

realsense-viewer

此时会打开realsense软件

（4）重新编译并安装 librealsense 二进制文件

sudo make uninstall && make clean && make && sudo make install

The shared object will be installed in /usr/local/lib, header files in /usr/local/include.

The binary demos, tutorials and test files will be copied into /usr/local/bin.

这个时候就能显示realsense T265了

选择3D可以看到运动轨迹

（5）最后，如果之前已经安装过realsense-ros 功能包，则需要重新对 realsense-ros 功能包 catkin_make 和 catkin_make install 一次。

命令安装

如果之前安装不成功，或者安装了其他版本的 SDK ，最好先全部卸载，然后再依次执行下述命令。使用以下命令删除所有与 RealSense™ SDK 相关的包。

dpkg -l | grep "realsense" | cut -d " " -f 3 | xargs sudo dpkg --purge

（1）注册服务器的公钥

sudo mkdir -p /etc/apt/keyrings
curl -sSf https://librealsense.intel.com/Debian/librealsense.pgp | sudo tee /etc/apt/keyrings/librealsense.pgp > /dev/null

（2） 确保安装了 apt HTTPS 支持

sudo apt-get install apt-transport-https

（3）将服务器添加到存储库列表中

sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u

（4）安装相关库（关键部分）

#========对应原教程命令：sudo apt-get install librealsense2-dkms===========sudo apt-get install librealsense2-udev-rules:amd64=2.53.1-0~realsense0.8250sudo apt-get install librealsense2-dkms=1.3.17-0ubuntu1#========对应原教程命令：sudo apt-get install librealsense2-utils============sudo apt-get install librealsense2=2.53.1-0~realsense0.8250sudo apt-get install librealsense2-gl=2.53.1-0~realsense0.8250sudo apt-get install librealsense2-net=2.53.1-0~realsense0.8250sudo apt-get install librealsense2-utils=2.53.1-0~realsense0.8250

（5）安装开发人员和调试包

与 realsense-ros 的编译过程有关，不装的话会在编译 realsense-ros 时报错： Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases

#==================只在原教程基础上指定了安装版本====================sudo apt-get install librealsense2-dev=2.53.1-0~realsense0.8250sudo apt-get install librealsense2-dbg=2.53.1-0~realsense0.8250

下面的几个包，可以不装。

#============下面的几个好像不安装也没有影响，可以不装==================sudo apt-get install librealsense2-gl-dev=2.53.1-0~realsense0.8250sudo apt-get install librealsense2-gl-dbg=2.53.1-0~realsense0.8250sudo apt-get install librealsense2-net-dev=2.53.1-0~realsense0.8250sudo apt-get install librealsense2-net-dbg=2.53.1-0~realsense0.8250

（6）重新连接英特尔realsense深度摄像头并运行： realsense-viewer 以验证安装。

（7）如果之前安装不成功，或者安装了其他版本的 SDK ，最好先全部卸载，然后再依次执行上述命令。

使用以下命令删除所有与 RealSense™ SDK 相关的包。

dpkg -l | grep "realsense" | cut -d " " -f 3 | xargs sudo dpkg --purge

（8）最后，如果之前已经安装过realsense-ros 功能包，则需要重新对 realsense-ros 功能包 catkin_make 和 catkin_make install 一次。

（9）为了避免系统继续对 realsense 的驱动进行升级，需要关闭系统获取其更新源，可将 软件和更新 中的相关项取消勾选。

T265参数

1、T265采用了Movidius Myriad 2视觉处理单元（VPU），V-SLAM算法都直接在VPU上运行 可直接输出6DOF相机位姿

2、T265使用了双目鱼眼相机 分辨率848X800分辨率 30HZ 单色图像 视场角 163° Fov(±5°)

3、IMU型号为 BMI-055

4、相机与IMU的参数都保存在了传感器中，可通过示例demo直接读取出相机的内参和相机与IMU之间的外参

5、相机外形尺寸 108 x 24.5 x 12.5 mm

坐标系描述
265放置方式： 将摄像头镜头面对准正前方，即摄像头坐标系Z轴指向正前方、Y轴指向地面、X轴指向右。

T265输出的里程计（话题：/camera/odom/sample）的坐标系与官方提供的图片坐标系有不同。实际测量到的坐标系为，正前方为X_odom轴正方向、左边为Y_odom轴正方向、天向为Z_odom轴正方向。

IMU输出值的坐标系为官方图片中的坐标系。

读取T265内外参数信息

参考：

相机的畸变模型

T265在出厂前就对相机进行了标定，每个相机内部都带有相机的内参和外参。在终端中输入以下命令，即可读取到T265的配置信息

rs-enumerate-devices -c

鱼眼相机内参

Intrinsic Parameters:Intrinsic of "Fisheye 1" / 848x800 / {Y8}Width:        848Height:       800PPX:          423.442291259766PPY:          393.319213867188Fx:           286.320709228516Fy:           286.544403076172Distortion:   Kannala Brandt4Coeffs:       -0.00806549098342657    0.041649341583252       -0.0394502282142639     0.00685414113104343   0FOV (deg):    111.9 x 108.8Intrinsic of "Fisheye 2" / 848x800 / {Y8}Width:        848Height:       800PPX:          425.625091552734PPY:          394.598785400391Fx:           286.426513671875Fy:           286.545501708984Distortion:   Kannala Brandt4Coeffs:       -0.0129915298894048     0.0538835003972054      -0.0500907190144062     0.00999459158629179   0FOV (deg):    111.9 x 108.8

IMU参数

Motion Intrinsic Parameters:Motion Intrinsic of "Gyro"        MOTION_XYZ32F
Bias Variances:         0.000000499999999  0.000000499999999  0.000000499999999
Noise Variances:        0.000005148030141  0.000005148030141  0.000005148030141
Sensitivity :1.003607       0.000000       0.000000       0.0004460.000000       1.001347       0.000000      -0.0011870.000000       0.000000       1.000770       0.000148Motion Intrinsic of "Accel"       MOTION_XYZ32F
Bias Variances:         0.000099999997474  0.000099999997474  0.000099999997474
Noise Variances:        0.000066952452471  0.000066952452471  0.000066952452471
Sensitivity :1.020443       0.000000       0.000000       0.1363460.000000       1.036700       0.000000      -0.4084400.000000       0.000000       1.015712      -0.216030

外参

Extrinsic Parameters:Extrinsic from "Fisheye 1"        To      "Fisheye 2" :Rotation Matrix:0.999972        -0.0061182       -0.004207930.00610929       0.999979        -0.002126780.00422086       0.00210101       0.999989Translation Vector: -0.0641016364097595  0.000241093512158841  0.000320313032716513Extrinsic from "Fisheye 2"        To      "Fisheye 1" :Rotation Matrix:0.999972         0.00610929       0.00422086-0.0061182        0.999979         0.00210101-0.00420793      -0.00212678       0.999989Translation Vector: 0.0640970468521118  -0.000633947784081101  -0.000589532195590436

鱼眼相机模型

参考：

AVM环视系统——鱼眼相机去畸变算法

吐血整理：从相机模型（针孔、鱼眼、全景）到OpenCV源码实现

KannalaBrandt8鱼眼相机模型

经过P点的入射光线没有透镜的话，本应交于相机成像平面的e点。然而，经过鱼眼相机的折射，光线会交于相机成像平面的d点，就产生了畸变，因此畸变图像整体上呈现出像素朝图像中心点聚集的态势。

而去畸变，就是将折射到d点的点，重新映射回到e点，因此去畸变之后的图像与原始的鱼眼图像相比，仿佛是把向心聚集的像素又重新向四周铺展开来。下表中的两幅图分别为鱼眼图和去畸变之后的展开图：

KANNALA BRANDT模型

鱼眼相机的投影方式有很多种假设，例如等距投影、等立体角投影、正交投影、体视投影、线性投影。但是真实的鱼眼相机镜头并不完全遵循上述的这些模型假设。因此Kannala-Brandt提出了一种一般形式的估计，适用于不同类型的鱼眼相机，KANNALA BRANDT模型包括4个畸变参数,即 $[k_1{k}_2{k}_3{k}_4]$ ,模型的参数示意图如下：
$$r(\theta )=k_0(\theta )+k_1{\theta }^3+k_2{\theta }^5+k_3{\theta }^7+k_4{\theta }^9$$
公式中的 $\theta$ 为光线入射角，$r(\theta )$ 为上图中 $od$ 的长度。

这个也是纳入opencv中的鱼眼相机畸变模型。现在基本上默认鱼眼相机模型遵循上述公式。

Opencv API 鱼眼图像去畸变方法

Opencv提供了基于Kannala-Brandt数学模型的鱼眼去畸变方法：cv::fisheye::initUndistortRectifyMap，该函数使用相机的内参和畸变参数计算出映射图 mapx 和 mapy。

基础鱼眼图像去畸变

其中入参K为鱼眼相机内参，D为 $k_1,k_2,k_3,k_4$ 畸变参数，R我们一般设置为单位阵，P为去畸变图像的相机内参，size为输出图像的大小；map1，map2为输出的映射图。

/*
@param K Camera intrinsic matrix \f$cameramatrix{K}\f$.
@param D Input vector of distortion coefficients \f$\distcoeffsfisheye\f$.
@param R Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x31-channel or 1x1 3-channel
@param P New camera intrinsic matrix (3x3) or new projection matrix (3x4)
@param size Undistorted image size.
@param m1type Type of the first output map that can be CV_32FC1 or CV_16SC2 . See convertMaps() for details.
@param map1 The first output map.
@param map2 The second output map.
*/CV_EXPORTS_W void initUndistortRectifyMap(InputArray K, InputArray D, InputArray R, InputArray P, const cv::Size& size, int m1type, OutputArray map1, OutputArray map2);void cv::fisheye::initUndistortRectifyMap(const Mat& K,           // 输入参数，鱼眼相机的内参矩阵，3x3的浮点型矩阵const Mat& D,           // 输入参数，鱼眼相机的畸变系数，1x4或1x5的浮点型矩阵const Mat& R,           // 输入参数，立体校正的旋转矩阵，3x3的浮点型矩阵const Mat& P,           // 输入参数，立体校正的投影矩阵，3x3的浮点型矩阵const Size& size,       // 输入参数，输出图像的大小，OpenCV中的Size类型int m1type,             // 输入参数，输出映射矩阵的数据类型，可选值为CV_16SC2、CV_32FC1或CV_32FC2OutputArray map1,       // 输出参数，第一个映射矩阵，CV_16SC2、CV_32FC1或CV_32FC2类型的矩阵OutputArray map2        // 输出参数，第二个映射矩阵，与map1具有相同的类型和大小
)

参数说明如下：

• K：鱼眼相机的内参矩阵，是一个3x3的浮点型矩阵。它包含了相机的焦距、光心和像素宽高比等信息。

• D：鱼眼相机的畸变系数，是一个1x4的浮点型矩阵。畸变系数描述了鱼眼相机镜头的畸变特性，通常包括径向畸变和切向畸变。

  

  注意： 这里如果输入1x5的向量，在校正畸变计算映射矩阵时会报错

  

• R：立体校正的旋转矩阵，是一个3x3的浮点型矩阵。旋转矩阵用于将左右相机的图像进行校正对齐，以便进行立体匹配。一般设为单位阵即可。

• P：立体校正的投影矩阵，是一个3x3的浮点型矩阵。投影矩阵定义了立体校正后的图像的投影方式，通常与相机的内参矩阵有关。

• size：输出图像的大小，是OpenCV中的Size类型，指定了校正后图像的宽度和高度。

• m1type：输出映射矩阵的数据类型，可选值为CV_16SC2、CV_32FC1或CV_32FC2。CV_16SC2表示输出映射矩阵为16位有符号整数型的两通道矩阵，CV_32FC1表示输出映射矩阵为32位浮点型的单通道矩阵，CV_32FC2表示输出映射矩阵为32位浮点型的两通道矩阵。

• map1：输出参数，第一个映射矩阵，是一个CV_16SC2、CV_32FC1或CV_32FC2类型的矩阵。该映射矩阵用于执行畸变校正和立体校正。

• map2：输出参数，第二个映射矩阵，与map1具有相同的类型和大小。该映射矩阵用于执行畸变校正和立体校正请注意，cv::fisheye::initUndistortRectifyMap()函数是用于鱼眼相机的特殊函数，适用于鱼眼相机模型的畸变校正和立体校正。对于普通相机模型，应使用其他函数，如cv::initUndistortRectifyMap()。

相机内参矩阵表示如下，其中 $f_x$ （单位为像素）表示相机焦距 $f$ 与相机cmos参数 $d_x$ 的比值，这个 $d_x$ 的物理意义为每个像素的实际长度，单位可以是mm/像素。 $c_x,c_y$ （单位为像素）表示相机主点，即光心与图像平面相交的坐标，单位为像素。
$$\left(\begin{array}{ccc}{f}_{\mathrm{x}}& 0& c_{\mathrm{x}}\\ 0& f_{\mathrm{y}}& {\mathbf{c}}_{\mathrm{y}}\\ 0& 0& 1\end{array}\right)$$
为什么既需要鱼眼相机的内参，又需要输出图像的相机内参呢，它们之间是什么关系？

最开始的时候，很多同学肯定是把这两个相机内参设置成一样的，即都设置成鱼眼相机的大小，如下图所示。代码中去畸变之后图像的内参是从鱼眼相机内参深拷贝过来的。

cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
cv::Mat mapx_open, mapy_open;
cv::Mat intrinsic_undis;
fish_intrinsic.copyTo(intrinsic_undis);
//intrinsic_undis.at<float>(0,2) *= 2;
//intrinsic_undis.at<float>(1,2) *= 2;
cv::fisheye::initUndistortRectifyMap(fish_intrinsic, m_undis2fish_params, R, intrinsic_undis,cv::Size(intrinsic_undis.at<float>(0, 2) * 2,intrinsic_undis.at<float>(1, 2) * 2),CV_32FC1, mapx_open, mapy_open);
cv::Mat test;
cv::remap(disImg[3], test, mapx_open, mapy_open, cv::INTER_LINEAR);

相机主点参数调节

上图中右侧去畸变之后虽然图像幅面大小与鱼眼图相同都是 1280*960，但是可视范围变得很小。标定所需要的大方格没有包含进来。因此，需要进一步调参，下面代码中将去畸变之后图像相机参数中的主点 $c_x,c_y$ 扩大为原来的两倍，且 initUndistortRectifyMap 函数输出的去畸变图像大小size是与去畸变之后图像相机参数主点相关的，也就是图像大小同样跟着放大了两倍。

记住一点：initUndistortRectifyMap函数中的size参数一般都是与去畸变之后图像的相机参数中主点大小强相关的。这一点在后面C++代码手撕算法流程时候会提到。

cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
cv::Mat mapx_open, mapy_open;
cv::Mat intrinsic_undis;
fish_intrinsic.copyTo(intrinsic_undis);
intrinsic_undis.at<float>(0,2) *= 2;
intrinsic_undis.at<float>(1,2) *= 2;
cv::fisheye::initUndistortRectifyMap(fish_intrinsic, m_undis2fish_params, R, intrinsic_undis,cv::Size(intrinsic_undis.at<float>(0, 2) * 2,intrinsic_undis.at<float>(1, 2) * 2),CV_32FC1, mapx_open, mapy_open);
cv::Mat test;
cv::remap(disImg[3], test, mapx_open, mapy_open, cv::INTER_LINEAR);

去畸变图像相机参数的主点扩大了两倍，同时生成图像大小扩到两倍

从上图中我们依然不能获得到右侧完整的黑色大方格，因此需要进一步扩大去畸变后图像相机主点位置以及生成图像的分辨率：

cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
cv::Mat mapx_open, mapy_open;
cv::Mat intrinsic_undis;
fish_intrinsic.copyTo(intrinsic_undis);
intrinsic_undis.at<float>(0,2) *= 4;
intrinsic_undis.at<float>(1,2) *= 4;
cv::fisheye::initUndistortRectifyMap(fish_intrinsic, m_undis2fish_params, R, intrinsic_undis,cv::Size(intrinsic_undis.at<float>(0, 2) * 2,intrinsic_undis.at<float>(1, 2) * 2),CV_32FC1, mapx_open, mapy_open);
cv::Mat test;
cv::remap(disImg[3], test, mapx_open, mapy_open, cv::INTER_LINEAR);

现在已经把去畸变图像相机内参的主点扩大为fish相机内参的4倍了，生成图像的长宽也放大了4倍，像素数量总体放大16倍，这样才勉强把大方格完全显示出来。

我们知道提取角点需要用到图像处理算法，显然对这么大的图像做处理的效率非常低。

相机f参数调节

上面只讨论了相机参数中主点的调参，想要解决上述问题还需要调整相机的 ，$f_x,f_y$ ，先不说理论，直接看调参结果，这里我们代码中只调整了去畸变图像相机参数中的，$f_x,f_y$ 使它们缩小为原来的1/4。

cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
cv::Mat mapx_open, mapy_open;
cv::Mat intrinsic_undis;
fish_intrinsic.copyTo(intrinsic_undis);
intrinsic_undis.at<float>(0, 0) /= 4;
intrinsic_undis.at<float>(1, 1) /= 4;
/*intrinsic_undis.at<float>(0,2) *= 4;
intrinsic_undis.at<float>(1,2) *= 4;*/
cv::fisheye::initUndistortRectifyMap(fish_intrinsic, m_undis2fish_params, R, intrinsic_undis,cv::Size(intrinsic_undis.at<float>(0, 2) * 2,intrinsic_undis.at<float>(1, 2) * 2),CV_32FC1, mapx_open, mapy_open);
cv::Mat test;
cv::remap(disImg[3], test, mapx_open, mapy_open, cv::INTER_LINEAR);

从图中可以看出，当我们仅将相机焦距缩小时，可以看到更多的东西。虽然去畸变之后的图像很小只有1280*960，但是却可以看到完整的方格。

结论：缩小相机焦距可以使FOV增大，在更小分辨率的图像上呈现出更多的内容，看上去也是更加清晰。

相机和IMU标定

参考：

Inter Realsense D435i标定详细步骤

Intel Realsense D455深度相机的标定及使用（二）——对内置IMU和双目相机进行标定

Ubuntu16.04+RealsenseT265跑通VINS-Fusion

在ros环境下的RealsenceT265标定以及Vins mono运行

Realsense T265标定及运行VINS–kalibr和imu_utils

使用imu_utils工具生成IMU的Allan方差标定曲线

Intel realsense T265双目+IMU传感器使用方法介绍，以及如何快速上手编写realsense的代码，进行简单地开发，积分出位姿，使用opencv显示双目图像

T265的标定包含了相机的标定和IMU的标定

官网说明：How can I get the extrinsic and intrinsic parameter of the T265?

官方使用说明（v2.53.1版本后已删除）： t265.pdf

准备工作

需要注意以下文件编译过程中，可能出现依赖库缺失的报错，这很正常，按照提示的错误信息安装对应依赖库即可

下载并编译ceres

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

下载并编译code_utils

首先，安装依赖库

sudo apt-get install libdw-dev

之后，安装code_utils

cd ~/catkin_ws/src
git clone https://github.com/gaowenliang/code_utils
cd ..
catkin_make
source ~/catkin_ws/devel/setup.bash

下载并编译imu_utils

cd ~/catkin_ws/src
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make
source ~/catkin_ws/devel/setup.bash

注意：先编译code_utils，再编译imu_utils，不能放在一起编译。

下载并编译kalibr

首先安装依赖库

sudo apt-get install python-setuptools
sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen
sudo apt-get install ros-kinetic-vision-opencv ros-kinetic-image-transport-plugins ros-kinetic-cmake-modules python-software-properties 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

接下来安装kalibr

cd ~/catkin_ws/src
git clone https://github.com/ethz-asl/Kalibr.git
cd ..
catkin_make
source ~/catkin_ws/devel/setup.bash

最后

修改工作空间下文件的读写权限，不然后面在运行时会报错

 cd ~/catkin_wssudo chmod 777 src/ -R

IMU标定

相关文件的修改

打开位于realsense-ros/realsense2_camera/launch 目录下的 rs_t265.launch 文件，将原本的代码

<arg name="unite_imu_method" default=""/>

修改为

<arg name="unite_imu_method" default="linear_interpolation"/>

在 ~/catkin_ws/src/imu_utils/launch 中新建t265_imu.launch 如下

<launch><node pkg="imu_utils" type="imu_an" name="imu_an" output="screen"><param name="imu_topic" type="string" value= "/camera/imu"/> <param name="imu_name" type="string" value= "BMI055"/><param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/><param name="max_time_min" type="int" value= "60"/> <param name="max_cluster" type="int" value= "200"/> </node>
</launch>

重力加速度自检

插入相机并静置， 终端输入realsense-viewer，开启realsense-viewer左侧的Motion Module模块，将鼠标放在加速度计（Accel stream）上，观察g_norm读数是否在9.8附近。

我这里显示为9.522，显然误差是很大的

IMU的校准

官方rs_imu_calibration.py工具校准

利用官方的rs_imu_calibration.py工具进行IMU自校准

g_norm 读数正常的，请跳过此步骤，直接进入步骤3 ；g_norm 读数不正常的，利用官方的rs_imu_calibration.py工具进行IMU自校准，详情参考官方网站：https://dev.intelrealsense.com/docs/imu-calibration-tool-for-intel-realsense-depth-camera

（1）下载rs_imu_calibration.py脚本

https://github.com/IntelRealSense/librealsense

通过上面的链接下载，下载后在tools/rs_imu_calibration文件夹里可以找到脚本文件rs_imu_calibration.py。

（2）构建Python环境及其他依赖包

检查Python版本，通过命令：python --version

这里使用的是Python2，如果2和3都安装的，运行如下命令进行手动切换。

sudo update-alternatives --config python

显示如下，键入python3的序号（这里是2），回车。

# 有 2 个候选项可用于替换 python (提供 /usr/bin/python)。选择       路径            优先级  状态
------------------------------------------------------------
* 0            /usr/bin/python3   150       自动模式1            /usr/bin/python2   100       手动模式2            /usr/bin/python3   150       手动模式要维持当前值[*]请按<回车键>，或者键入选择的编号：

注意：此方法不会修改Python的优先级，也就是之后还会切换回Python3。如果想更换Python2为默认环境，需要

sudo update-alternatives --install /usr/bin/python python /usr/bin/python2 150
# 加上
sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 100

命令来修改优先级。两种方法二选一！！！

（3）运行脚本

插上相机，进入 rs_imu_calibration.py 文件所在文件夹（在 librealsense-master/tools/rs-imu-calibration 下），运行

python rs-imu-calibration.py

出现如下界面，并不停地有读数进来。

（4）从6个位置捕获IMU数据

将相机以下图所示位置摆放（螺纹孔朝下，懒得自己照相了，图用的是官方文件中的D435i）

leave the camera still in this position for 3 to 4 seconds. The script will provide a prompt and guide through each of the positions.

放置正确后终端会出现：Status.collect_data[...................]]，期间不要移动相机，直到省略号满格出现Direction data collected。若不小心移动，会出现 Status.collect_dataWARNING: MOVING 的警告，此时摆正位置不要移动直到出现 Direction data collected

出现 Direction data collected 后翻转相机到下一个位置。依次在位置2~6重复操作：

6个位置都捕获完成后，会提示

意思是是否保存原数据，按回车不保存原数据。然后会计算出重力加速度值：

计算完后会提示：Would you like to write the results to the camera? (Y/N)。按Y写入到设备

imu_utils工具校准

将realsenseT265插上电脑后，建议在你喜欢的地方新建一个文件夹，比如在桌面上新建了一个文件夹，然后在其中打开终端，输入以下命令

cd ~/xxx
roslaunch realsense2_camera rs_t265.launch
roslaunch imu_utils t265_imu.launch

注意过程中显示 wait for imu data 是正常情况，等待大约60分钟即可出结果，在你新建的文件夹内生成了BMI055_imu_param.yaml 文件，该文件给出了加速度计和陀螺仪三轴的 noise_density(后缀n) 和 random_walk(后缀w)，同时计算出了平均值，后面IMU+摄像头联合标定的时候需要这些均值。

相机的标定

下载官方给的april_6x6_80x80cm_A0.pdf或者其它标定文件。打印或者在屏幕显示，测量实际的尺寸后，在你之前新建的文件夹中新建 apriltags.yaml，我的文件内容如下：

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.16           #size of apriltag, edge to edge [m]
tagSpacing: 0.3125         #ratio of space between tags to tagSize#example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]

之后，在你新建的文件夹中打开终端，开启realsenseT265

roslaunch realsense2_camera rs_t265.launch

修改话题发布频率

rosrun topic_tools throttle messages /camera/fisheye1/image_raw 10.0 /fisheye1
rosrun topic_tools throttle messages /camera/fisheye2/image_raw 10.0 /fisheye2

录制文件，注意录制过程中要缓慢移动相机，使其能看到完整清晰的标定文件（可以先在录制前打开rviz，调用image的话题进行观察，判断移动的位置）

rosbag record -O cameras_calibration /fisheye1 /fisheye2

调用kalibr的算法计算各个摄像头的内参和外参

kalibr_calibrate_cameras --target ./apriltags.yaml --bag ./cameras_calibration.bag --bag-from-to 2 35 --models omni-radtan omni-radtan --topics /fisheye1 /fisheye2 

因为并不一定要用整个录制视频，2和35是你想要的起始和截止时间，可以修改。

如果在过程中出现 Using the default setup in the initial run leads to an error of Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance. 相机不同步的报错，可以通过修改话题发布的频率，或者在kalibr命令的末尾加上 –approx-sync 0.04 来解决。
最终会生成 camchain-.cameras_calibration.yaml、results-cam-.cameras_calibration.txt 和 report-cam-.cameras_calibration.pdf。

Camera-IMU联合标定

在你之前新建的文件夹中，新建imu.yaml文件如下（根据你之前的BMI055_imu_param.yaml填写参数）：

#Accelerometers
accelerometer_noise_density: 8.003e-02   #Noise density (continuous-time)
accelerometer_random_walk:   5.382e-03   #Bias random walk#Gyroscopes
gyroscope_noise_density:     8.163e-03   #Noise density (continuous-time)
gyroscope_random_walk:       3.470e-05   #Bias random walkrostopic:                    /imu      #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

修改rs_t265.launch其中的两行代码如下：

<arg name="enable_sync"         default="true"/> 
<arg name="unite_imu_method"    default="copy"/>

进入你之前新建的文件夹，打开终端，开启T265

roslaunch realsense2_camera rs_t265.launch

修改发布频率

rosrun topic_tools throttle messages /camera/fisheye1/image_raw 20.0 /fisheye1
rosrun topic_tools throttle messages /camera/fisheye2/image_raw 20.0 /fisheye2
rosrun topic_tools throttle messages /camera/imu 200.0 /imu

录制文件

rosbag record -O imu_cameras_calibration /fisheye1 /fisheye2 /imu

调用kalibr的算法计算IMU和camera外参

kalibr_calibrate_imu_camera --target ./apriltags.yaml --cam ./camchain-.cameras_calibration.yaml --imu ./imu.yaml --bag ./imu_cameras_calibration.bag --bag-from-to 2 35 --max-iter 30 --show-extraction

最终会输出 camchain-imucam-.imu_cameras_calibration.yaml、imu-.imu_cameras_calibration.yaml、results-imucam-.imu_cameras_calibration.txt、report-imucam-.imu_cameras_calibration.pdf 四个文件，你可以通过pdf文件查看你标定的准确性。

至此，我们完成了相机和IMU的标定。