SLAM学习笔记（二十一）3D雷达与相机的标定方法详细教程

目录

写在前面

环境安装

制作标定版

打印ArUco二维码

裁剪硬纸板

修改文件参数

调整launch文件

第一，remap字段：

第二，marker_size字段：

第三，修改zed_left_uurmi.ini 这个配置文件：

第四，修改其他配置文件：

运行程序

获取外参

发布TF变换

---

写在前面

把3D雷达和相机进行外参标定使用的方法我们采取LiDAR-Camera Calibration using 3D-3D Point correspondences 方法。

这里我使用的相机为realsense D435i 以及Velodyne16雷达。

其实具体的细节github上面已经写过了，但是对于没有跑过的人来说，搞定这个东西还是有一定麻烦的。因此把细节记录在这里。

希望涉及汇总方面的朋友们可以原文引用本链接，码字不易，谢谢！

---

环境安装

其实这块的内容原链接已经写的非常详细了：原github wiki

不过为了本文的完整性，我把它复制在这里。

（以18.04为例，其他的请自行阅读github wiki）

sudo apt-get install -y python-catkin-tools python-catkin-pkg python-rosdep python-wstool ros-melodic-cv-bridge ros-melodic-image-transport
sudo apt-get install -y ros-melodic-nodelet-core ros-melodic-ddynamic-reconfigure
sudo apt-get install -y ros-melodic-velodyne-pointcloud

source /opt/ros/melodic/setup.bash

# Prepare rosdep to install dependencies.
sudo rosdep init
rosdep update --include-eol-distros  # Support EOL distros.

这些内容基本不会有什么报错，当然也有可能会报错：raw.githubusercontent.com无法访问 之类的错误。这种问题主要是Ros melodic安装时候的通用的一些问题，详情可以参考ros 安装遇到 raw.githubusercontent.com无法访问，被墙的问题，尝试有效解决方案_激昂网络的博客-CSDN博客

 然后就是编译里面的内容：

# go to your catkin workspace src
cd ~/catkin_ws/src
git clone https://github.com/ankitdhall/lidar_camera_calibration.git

# move out dependencies
mv lidar_camera_calibration/dependencies/aruco_ros aruco_ros
mv lidar_camera_calibration/dependencies/aruco_mapping aruco_mapping

# checkout corresponding melodic version
cd lidar_camera_calibration
git checkout melodic

# build
cd ../..
rosdep install --from-paths src --ignore-src -r -y
catkin_make -DCATKIN_WHITELIST_PACKAGES="aruco;aruco_ros;aruco_msgs"
catkin_make -DCATKIN_WHITELIST_PACKAGES="aruco_mapping;lidar_camera_calibration"
catkin_make -DCATKIN_WHITELIST_PACKAGES=""

---

制作标定版

这一块的内容涉及到小手工。有一些同学一提到标定就想上淘宝去买标定板。其实完全用不着啊，能节省就节省，这个东西自己动手就可以做一个。

打印ArUco二维码

首先要下载二维码，给一个链接（这个可以收藏一下，我好不容易才找到这样一个在线生成的链接）：

Online ArUco markers generator

 

这两个图案是github里面作者使用的两个图案。一个id是26，看起来像一只站着的小狗。一个id是582，看起来像是一只爬着的蛤蟆。

其实也未必非要用这两个，只要随便打印两个，挂在绳子上的时候保证左边的小于右边的id就好了。

把这两个图片保存下来，用A4纸打印机打印出来就可以了。

裁剪硬纸板

一个纸箱，裁剪出两个正方形或者长方形，把二维码按照图示的方法贴在角上（注意不要颠倒了），然后把它悬挂起来：

就像我这样：

 当然其实我这个位置其实不太合适啊，主要是因为旁边绿色沙发的边缘离这个标定版太近了，没有深度差。最好换一个其他位置，前后距离区分比较明显的，我懒得再挂上去拍照了，翻出来一个以前的照片。

那么现在标定版的制作就搞定了。

---

修改文件参数

调整launch文件

首先，要打开位于src/lidar_camera_calibration/launch 下的find_transform.launch， 把上面的  相关注释掉的地方都给打开。最后这个launch文件大概长这样：

 关于这个文件呢，我要说四点：

第一，remap字段：

要修改成真正相机发送的图像topic，这个根据你的相机的ros驱动决定。

第二，marker_size字段：

要修改成你的二维码的实际大小。 二维码是一个正方形，我的长度是12.5厘米，因此这里写成0.125。

第三，修改zed_left_uurmi.ini 这个配置文件：

（如果不修改，标定出来的结果是完全错误的）。这个里面主要装了相机内参相关的东西。

相机内参，可以通过张正友标定法进行棋盘标定，这块网上的内容已经足够多。这里不再介绍了。我这里给一个投机取巧的方式：一般相机的ros驱动会自动发布一个默认的内参，以realsense为例:

rostopic echo /camera/color/camera_info

可以看到：

 我们这些内容照猫画虎填进去即可（当然如果希望精准一些，最好还是自己去标定一下）：

第四，修改其他配置文件：

打开src/lidar_camera_calibration/conf文件夹：

 这些内容也要进行修改。

第一个，config_file.txt文件： 这点还是建议看一下github原文的介绍，按照以下的格式，把配置文件中的值修改成你对应的值。

image_width image_height
x- x+
y- y+
z- z+
cloud_intensity_threshold
number_of_markers
use_camera_info_topic?
fx 0 cx 0
0 fy cy 0
0 0 1 0

MAX_ITERS

initial_rot_x initial_rot_y initial_rot_z

lidar_type

xyz就是相机视野，只考虑这个视野内的值。这个应该调对，最起码应该估摸一下标定板大概在你雷达的什么位置。（z轴指向雷达正前方）

那么cloud_intensity_threshold，这个需要具体看一会跑起来的效果。一般来讲，会显示出两个板子的边缘，然后后续要求你敲空格，依次顺时针把板子边缘的点圈起来。但是如果弹出的窗口里面，没有看到有板子的边缘，那么这个值就调小一点。比如我这里这个值就是0.0005。

第二个，lidar_camera_calibration.yaml文件，这个没什么好说的，填成对应话题就好了：

lidar_camera_calibration:
  camera_frame_topic: /camera/color/image_raw
  camera_info_topic: /camera/color/camera_info
  velodyne_topic: /velodyne_points

第三个， marker_coordinates.txt文件，格式长这样：

N

length (s1)
breadth (s2)
border_width_along_length (b1)
border_width_along_breadth (b2)
edge_length_of_ArUco_marker (e)

length (s1)
breadth (s2)
border_width_along_length (b1)
border_width_along_breadth (b2)
edge_length_of_ArUco_marker (e)

N代表标定板数目，这里填2。

剩下的两个5行的字段，我也说不清楚，看github的配图即可:

 这个讲的很清楚了，按照这个要求填进去即可。

---

运行程序

roslaunch lidar_camera_calibration find_transform.launch

就这一行代码就行了。

运行完以后，会弹出三个窗口：

第一个窗口长这样：

 原谅我用原github的图像，因为我懒得再挂起来重新拍了。

注意：图片上的两个坐标轴是要自动显示的，如果你的窗口里面没有两个二维码上面的坐标轴，只有灰度图，这是不行的。所以你要把相机和雷达靠近这个二维码，或者打印更大的二维码，或者调整配置文件里的范围，总之是一定要两个坐标轴稳定的显示在二维码上，先显示了但是又消失了的也不行。

第二个窗口：

会显示出来这个板子的边缘，你所要做的就是，把8个边缘按顺时针顺序给框起来，每个边缘用四个点框，每个点点完以后敲空格再点下一个点。框完第一个板子再框第二个板子，结束之后就会开始自动标定程序了。（要是没有显示出来，那么需要调节config_file.txt中的xyz，以及cloud_intensity_threshold。

注意，动鼠标左键和敲空格进行画框的过程中，中间结果会显示在第三个窗口内：

 这个窗口刚开始是黑的，不过随着你框的过程，会显示出来框的结果：

 

 具体我这块没法上传视频，可以看github上面的配套视频。但是缺陷是那个视频需要看youtube，所以没有办法看的同学，可以看看b站有个人也做过同样的事情：

使用lidar_camera_calibration工具标定雷达与相机的过程_哔哩哔哩_bilibili

这块不出意外的话，会标定出结果。但是在框完两个标定板以后，有可能会有两种报错：

第一种：

[FATAL] [1511892092.578293398]: ASSERTION FAILED
file = .../src/lidar_camera_calibration/include/lidar_camera_calibration/Find_RT.h
line = 277
cond = marker_info.size()/7 == num_of_marker_in_config

像这种报错可能有几种原因：

第一，相机那边没有检测到二维码（检测到以后会在二维码上面出现坐标轴）

第二，雷达检测到的板子的边缘点太少了。尝试检查一下config_file.txt中的cloud_intensity_threshold，调小一点。

第三，框的不对。要确保顺时针。并且尽量不要框到重复的点。比如拐角处的点。

第二种：

程序一直在运行，怎么也算不出结果。

这种可能是因为程序在运行开始以后，相机那边又动过了，（但是雷达边缘这块未得到更新），所以及时终止程序重新标定即可。

---

获取外参

最后程序标定完了，会得到这样的结果：

 那么这个结果怎么用，其实也是一个问题。

一方面，有一个Average transformation，显然是根据Average translation 和Average rotation拼起来的。

但是奇妙的是还有一个Final rotation，那么这个东西和Average rotation显然是不一样的。

这里我就直接揭晓答案了：

首先代码订阅的相机的topic是 /camera/color/image_raw，那么这个东西的坐标系名称为camera_color_optical_frame。（这一点如果你用的其他不同的相机，可以rostopic echo看一下frame字段）

这里真正应该使用的是Final rotation。这个东西代表, 也就是说雷达坐标系下的点，经过这个R左乘，就可以变换到相机坐标系下。

不过，可能有朋友要问了，那这里光有旋转，平移用什么？没看到有final translation啊。

事实上平移就上面的用Average translation。

我相信看到这肯定有人要骂我：你特么的到底懂不懂，这玩意哪能这么用？

这可不是我说的啊，这是作者自己在issue里解释的：

In this case, the final and average translation values are the same. This is because we first apply the rotation to match the orientation of the frames, and then the translation. So the final (or total) transformation would be [R_final | t_avg]

参考 After Calibration, I got 'Average rotation' and 'Final rotation'. Which one should be used? · Issue #30 · ankitdhall/lidar_camera_calibration · GitHub

感兴趣的话可以研究一下源码怎么实现的，反正我是没有兴趣。

---

发布TF变换

外参标完了，我们也获得了从雷达坐标系到相机坐标系之间的位姿变换。

如果我们想把两个点云做一个融合，那就有两种方式：一种是获取点云，一股脑乘以外参变换过去。在这里我选择发布一个tf变换：

用三个double来表示translation，然后定义一个tf:Vector3 origin_ori(origin_x,origin_y,origin_z);

用Eigen矩阵保存旋转，然后定义Eigen::Quaterniond，用旋转矩阵构建四元数q，然后用q.matrix().eulerAngles(2,1,0)分别获取yaw，pitch，roll。

然后用这三个角度构建tf::Quaternion origin_q，并用setRPY赋值。

最后把这些内容打包成transform发布静态变换：注意，tf::StampedTransform这个函数里面的两个坐标系，分别为父系和子系，子系左乘发布的变换，可以将子系下的点转换到父系下来。

tf::Transform static_camera_lidar;
static_camera_lidar.setOrigin(origin_ori);
static_camera_lidar.setRotation(origin_q);
ros::Rate r(10.0);
while(nh.ok())
{
    broadcaster.sendTransform(tf::StampedTransform(static_camera_lidar,ros::Time::now(),'camera_color_optical_frame','velodyne'));
    r.sleep();
}

原谅我只能放这些内容，因为根据单位规定自己动手写的代码就有知识产权了，因此不允许放到网上来。看着写吧。嫌麻烦的人也可以直接在launch文件中在启动的时候直接发布静态变换，省去了这个代码的过程。

那么最后我们在rviz里面固定坐标系为velodyne，就可以看到激光和相机的点云重合了：

 顺便提一句，realsense相机发布点云，通过如下方式启动：

roslaunch realsense2_camera rs_camera.launch filters:=pointcloud

 比常规的启动方式多了一个filters的参数。这样就能发布一个话题叫/camera/depth/color/points 可以在rviz里面进行显示。