【解放双手】Auto Lidar2Cam Calibration——相机雷达自动标定

EchoRouRou

已于 2023-01-07 14:29:59 修改

阅读量1.5k

点赞数 1

分类专栏： Fusion 文章标签：自动驾驶人工智能相机雷达标定

于 2022-07-04 10:56:29 首次发布

本文链接：https://blog.csdn.net/weixin_42396991/article/details/125595231

版权

Fusion 专栏收录该内容

4 篇文章 0 订阅

订阅专栏

文章目录

1.准备工作

这个算法的标定板和之前使用的autoware等其他网上的方法不一样，需要特制的，板子的参数在ros的wiki里面作者有给出参数，我找了一家做木板的厂定制了一块，切掉了中间四个圆孔，然后四个正方形是自己后面一点点量出来再贴上Apriltag，这个木板是合成木，运输过来是容易有断层的，但是凑合着用了。板子的实物图和我最小标定系统如图所示。加工好以后四个角贴上的Apriltag样式需要和论文中给出的一样是不能换的。二维码我放百度网盘吧。

在这里插入图片描述

2.开始使用

2.1 相机选型与标定

如上图可知我就随便找了个实验室能用罗技的720P USB相机，使用ros的usb_cam来启动相机，由于这个程序还需要相机的内参，所以我提前用ros自带的camera_calibration使用棋盘格标定板标定了相机的内参。

2.2 打开调试按钮

打开Debug的话是可以发布相机和雷达检测的结果的方便调试，我的代码是已经打开，需要关闭的话找到velo2cam_calibration/include/velo2cam.utils.h中第30行的

#define DEBUG 1

改为

#define DEBUG 0

改完直接重新编译。

2.3 调试相机

打开一个终端，输入命令

source devel/setup.bash
roslaunch velo2cam_calibration  mono_pattern.launch camera_name:=/camera image_topic:=image_raw  frame_name:=camera_optical_link

成功的话会打开相机检测的画面，通过检测四个AprilTag，估计出四个圆心的位置。我已经打开了Debug这个画面会自动弹出的。

在这里插入图片描述

2.4 调试雷达

新打开一个终端窗口，输入指令（ cloud_topic）写你的点云主题名称。

source devel/setup.bash
roslaunch velo2cam_calibration lidar_pattern.launch cloud_topic:=/sensing/lidar/pointcloud

终端会开始输出检测结果，如下图所示，可以打开rviz来查看检测。雷达检测圆洞的方法是检测断点，如果周围环境太复杂会影响检测结果，因为可以人为过滤一下。

在这里插入图片描述

再新开一个终端窗口运行：

rosrun rqt_reconfigure rqt_reconfigure

这是一个过滤器我们要把点云过滤一下，类似下图的效果图，在/lidar_pattern/zyx_filtered中只留下板子和透过圆洞打过去的点，在lidar_pattern/range_filtered_cloud中只留下板子，这样是检测效果最好的。

激光雷达透过圆心的激光束一定要至少3 ring，不然会检测不到圆心的。雷达这边需要时间慢慢来。过滤完之后，可以把检测结果在RVIZ中显示出来，效果如下，绿色的就是检测出来的圆心。

2.5 标定

确保上面雷达和相机打开检测都没问题了，再新开一个窗口：

source devel/setup.bash
roslaunch velo2cam_calibration  registration.launch sensor1_type:=mono sensor2_type:=lidar

我们用的是单目相机所以参数选择在mono如果双目的选择stereo。

一直输入Y，应该是有三次输入Y，迭代完成后会询问是否需要换一个姿态继续，就是把板子挪挪位置，可以斜起来等重复步骤2.3-2.5直到不想继续了，我做的时候按照作者提出的完成三个姿态。

2.6 检测结果

在完成标定之后会立刻在终端显示标定结果，也就是一个tf，具体的还可以在velo2cam_calibration/launch/calibrated_tf.launch中查看，他给了三个坐标系之间的静态tf，lidar先到一个旋转了一下的相机坐标，旋转的相机坐标再到相机的光心坐标，但我们需要的是lidar到camera的。我写了一个程序用来订阅这个静态tf来转换得到外参矩阵。

roslaunch velo2cam_calibration calibration_tf.launch
roslaunch tf_matrix tf_listen.launch

/*
 * @Description: 
 * @Author: ubuntu
 * @Date: 2022/5/19 下午7:24
 * @LastEditors: ubuntu
 * @LastEditTime: 2022/5/19 下午7:24
 * @Version 1.0
 */

/**
 * @brief: 监听tf消息转换成齐次矩阵
 */

#include <TFtoMatrix/TFtoMatrix.h>

int main (int argc, char** argv) {
    ros::init(argc, argv, "tf_listener");
    ros::NodeHandle nh;

    tf::TransformListener listener;

    ros::Rate rate(10.0);
    while (nh.ok()) {
        tf::StampedTransform transform_lidar;
        tf::StampedTransform transform_cam;

        try {
            listener.lookupTransform("/lidar_link", "/rotated_camera",
                                     ros::Time(0), transform_lidar);
            listener.lookupTransform("/rotated_camera", "/camera_optical_link",
                                     ros::Time(0), transform_cam);
            Eigen::Matrix4f lidar2rotated_cam;
            Eigen::Matrix4f rotated_cam2cam;
            pcl_ros::transformAsMatrix(transform_lidar, lidar2rotated_cam);   //直接得到矩阵
            pcl_ros::transformAsMatrix(transform_cam, rotated_cam2cam);   //直接得到矩阵
            std::cout << "lidar2rotated_cam:"  <<std::endl;
            std::cout << lidar2rotated_cam  <<std::endl;
            std::cout << "rotated_cam2cam:"  <<std::endl;
            std::cout << rotated_cam2cam  <<std::endl;
            std::cout << "final:"  <<std::endl;
            std::cout << lidar2rotated_cam * rotated_cam2cam  <<std::endl;
            // std::cout << "final2:"  <<std::endl;
            // std::cout << (lidar2cam*cam2cam).inverse()  <<std::endl;
        }
        catch (tf::TransformException &ex) {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }
        rate.sleep();
    }
    return 0;
}