Ubuntu16.04 在ROS中配置RealSence D435 并标定深度相机与rplidar外参

Ubuntu16.04系统，在ROS中配置RealSence D435 ，并标定其与rplidar的外参

---

主要目的是为rplidar标定深度相机外参，为机器人导航的过程提供障碍物识别的功能，因此需要知道两者之间的静态变换。
用到的传感器：
RealSence D435
rplidar A2

主要分为两个步骤：
1 Ubuntu16.04系统下配置RealSence D435的ROS驱动（rplidar A2的ROS驱动比较简单，自行百度）
2 标定深度相机与rplidar外参

---

1. 配置RealSence D435的ROS驱动和对应的SDK

1. 1 安装RealSence D435的SDK

SDK和ROS驱动官方均给出了源码安装和apt-get两种方式。apt-get操作简单，但是题主尝试反复多次安装，SDK都可以正常使用，但是ROS中启动相应节点一直报错（推断ros官方并没有及时将库中提前编译好的驱动文件，更新到与SDK匹配的版本），所以本文只介绍源码安装SDK和ROS包。

首先，从官网打开https://github.com/IntelRealSense/librealsense/releases.下载最新版本的SDK源码。注意，这里的对应版本需要检查：Ubuntu版本、kernel版本、Supported Devices等。题主采用的为Ubuntu16.04，kernel=4.15.0，D435型号（不是D435(i)）。

写本文时的最新版本为Intel® RealSense™ SDK librealsense-2.50.0，建议下载最新的，ROS驱动的版本与SDK必须对应，github的ROS驱动的下面会写明要求，必须是哪一个版本的SDK，否则编译时会出错。与librealsense-2.50.0对应的版本是ROS Wrapper 2.0 for Intel® RealSense™ realsense-ros-2.3.2。

打开https://github.com/IntelRealSense/realsense-ros/releases.下载对应版本的ROS包。

开始安装SDK。先cd到下载的librealsense-2.50.0文件夹下，安装依赖项：

sudo apt-get install git libssl-dev libusb-1.0-0-dev libudev-dev pkg-config libgtk-3-dev
sudo apt-get install libglfw3-dev
sudo apt-get install ros-kinetic-ddynamic-reconfigure*
sudo apt-get install intel-realsense-dfu*

// 运行脚本
 ./scripts/setup_udev_rules.sh
// 创建并运行patched模块
// 官网有这一步，下载巨慢，我直接跳过了没有影响后面，好像和内核版本有关
./scripts/patch-realsense-ubuntu-lts.sh

然后开始安装SDK，编译时间略长（安装时务必拔掉相机）：

mkdir build && cd build
cmake ..
make -j8 //根据你cpu而定
sudo make install
//验证，进入官方启动器，开启左侧设备，有点云和图像安装成功
realsense-viewer 

1. 2 安装RealSence D435的ROS驱动

新建一个ROS的工作空间，将源码放src中，开始编译ros驱动，记得把新devel变量路径写入~/.bashrc文件中：

cd catkin_ws/src/
catkin_init_workspace
cd ..
catkin_make clean
catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release
catkin_make install
source ~/.bashrc

安装配置完成，运行launch文件，记得改一下rviz中的frame id，否则不会显示图像：

roslaunch realsense2_camera rs_camera.launch
rviz

最后添加想要的话题

由于题主是为了标定深度相机的点云坐标系与RPLIDAR坐标系，所以需要用到realsense的左红外相机图像来与雷达的点云进行标定，因此需要开启红外相机的图像话题同时关闭掉红外光辅助深度测量的功能，不关的话会影响成像效果，参考这一篇文章，重写一个launch文件达到上述目的。
链接: https://blog.csdn.net/qq_42928559/article/details/121747781

至此就完成了ROS下的配置安装。效果如图，依次是rgb，左右目红外以及深度图：

深度相机的点云坐标系定义在左目的红外相机，因此下一步标定rplidar与左目相机。

2. 标定D435和RPlidar的外部参数

继续更新了，找到一个可用的开源算法，但是效果不是很理想，也记录一下中间的主要问题。
开源算法是He Yijia在github中给出的，就是那位在深蓝讲VIO的老师，标定算法的原理可以参考：
https://zhuanlan.zhihu.com/p/137501892

github的地址如下：
https://github.com/MegviiRobot/CamLaserCalibraTool
该工具有以下特点（拷贝自github）：

1. 支持多 April tag 格式。 可以选择多 apriltag 的标定板 ( kalibr_tag.pdf ) 或者自己打印一个 apriltag 做标定板(apriltag.pdf)。
2. 支持多种相机模型。radial-tangential (radtan) : (distortion_coeffs: [k1 k2 r1 r2]); equidistant (equi):distortion_coeffs: [k1 k2 k3 k4]). More info please visit kalibr website.
3. 激光线自动检测，无须手标。会自动提取激光线中落在标定板上的激光点，前提是标定板在激光正前方 120 度范围内，并且激光前方 2m 范围内只会存在一个连续的直线线段，所以请你在空旷的地方采集数据，不然激光数据会提取错误。
4. 利用标定板的边界约束，标定结果更加准确。这个是隐藏的功能，暂时未对外开放接口，需要你修改代码。

新建工作空间后拷贝对应代码，直接catkin_make编译，无需其他依赖项和库，April tag的相关库直接集成到了头文件中

mkdir LaserCameraCal_ws
mkdir ../src
cd src
git clone https://github.com/MegviiRobot/CamLaserCalibraTool
cd ..
catkin_make -DCMAKE_BUILD_TYPE=Release

rosbag record录制一段带有/camera和/scan的数据包，保证打印的标定板有充分的旋转和平移（值得强调的是，雷达的x方向必须和相机的z方向大致相同，因为程序中默认提取的是雷达x正前方的连续平面点云）。录制好数据包后记得在config文件中根据标定板的类型和尺寸更改对应的参数，详见config文件，同时更改bag文件的存放路径。随后开始标定程序运行：

roslaunch lasercamcal_ros kalibra_apriltag.launch 

这一launch文件用于识别tag的位姿并将信息对应时间戳保存到一个txt文件中，为后续计算做准备。如图会识别到每一帧图像中标定板的空间位姿，题主封校没地方打印另一种标定板，只能先用这种单个的试试水，但是单个的定位精度真的很一般，也导致了后面标定精度比较差，建议有条件的打印github中提到的第一种标定板。（这一步运行结束后程序并不会自己结束，而是卡在那里需要手动ctrl+c关闭，此时已经生成了一个txt的中间文件，记录位姿）

继续下一步：

roslaunch lasercamcal_ros calibra_offline.launch 

这一launch文件用于离线标定，可以自动提取标定板上的连续直线点云，感觉上构造的是点到平面的距离误差进行优化的，也是两步法先求得初始位姿再非线性优化估计准确外参，感兴趣可以看一下开篇那个知乎的问答。（这一部分也会弹出一个显示点云的窗口，不过我忘记截图了，emmm）
计算结束后会自动生成一个result文件在你设置的config路径下，作者还提供了一个重投影的debug程序帮助你查看标定效果，运行它同时播放bag文件，会得到下图的一个重投影效果：

roslaunch lasercamcal_ros debug.launch 
rosbag play data.bag

题主的标定效果一般，最起码横滚角很明显不对劲，猜测可能是单个的标定板估计位姿精度确实有限，运行过程中有很多帧估计的坐标系都已经偏离图像中的平面了，有条件再打印一个大的标定板测试一下实验结果。