基于Autoware的二维激光雷达与相机的标定下，雷达图像显示不正确的问题处理

AlexashB

已于 2023-11-30 15:46:49 修改

阅读量1.1k

点赞数 20

文章标签： c++ 机器人计算机视觉 linux

于 2023-11-28 21:09:48 首次发布

本文链接：https://blog.csdn.net/AlexashB/article/details/134676025

版权

项目场景：

使用Autoware中的 Calibration Toolkit 进行二维激光雷达和相机的标定。目标是获取坐标系转换的外参矩阵，将相机的坐标系变换到雷达坐标系上。

实验设备

Sick Tim561 一个，realsense D435i 一个，设备摆放如下图所示
在这里插入图片描述

实验环境

Ubuntu18.04，ROS melodic
Calibration Toolkit
sick Tim561驱动

问题描述

问题一

雷达数据的显示不符合预期，整体的雷达数据在Calibration Toolkit 中的显示顺时针旋转了90度。
在这里插入图片描述

问题二

标定后的外参矩阵不符合预期，旋转矩阵数据存在问题。

原因分析：

原因一：传感器的坐标系定义

需要搞清楚realsense和sick雷达的坐标系是以何种形式的定义的，两者都采用右手坐标系

Realsense坐标系

根据Realsense ROS的官方文档中提到的 “All data published in our wrapper topics is optical data taken directly from our camera sensors” 。可知相机传感器发出的数据，是基于Optical坐标系。
也就是说标定过程中拍摄的标定板的位姿，是基于Optical坐标系下的坐标定义的。

在这里插入图片描述

Sick雷达坐标系

Sick 二维激光雷达本身并没有定义XYZ坐标系，而是采用的极坐标系。如下图所示，Sick的扫描角度范围为 $-45<\theta<225$ 。雷达扫描到的一个物体采用角度 $\theta$ 和距离range 表示。

Sick激光雷达扫描角度
而XYZ坐标系是在Calibration Toolkit中人为定义的，源程序中将极坐标系，转换成了XY坐标系。具体如何转换的会在后面说明。

//selectionwidget.cpp:380
void PointsExtractor::updateLaserScan(sensor_msgs::LaserScanConstPtr lidarPoints)
{
    pointsptr=lidarPoints;
    int i,pointsnum=(pointsptr->angle_max-pointsptr->angle_min)/pointsptr->angle_increment+1;
    points.resize(pointsnum);
    for(i=0;i<pointsnum;i++)
    {
        float angle=pointsptr->angle_min+i*pointsptr->angle_increment;
        points[i]=QPointF(pointsptr->ranges[i]*cos(angle）,pointsptr->ranges[i]*sin(angle)); //极坐标转换成XY坐标
    }
    drawPoints();
}

将坐标系总结画在下图中
在这里插入图片描述

原因二：Sick激光雷达传入的原始数据

激光雷达在ROS中的数据为Scan，数据格式如下所示。

# 测量的激光扫描角度，逆时针为正
# 设备坐标帧的0度面向前（沿着X轴方向）
Header header            # Header也是一个结构体,包含了seq,stamp,frame_id,其中seq指的是扫描顺序增加的id,stamp包含了开始扫描的时间和与开始扫描的时间差,frame_id是扫描的参考系名称.注意扫描是逆时针从正前方开始扫描的.
 
float32 angle_min        # 开始扫描的角度(角度)
float32 angle_max        # 结束扫描的角度(角度)
float32 angle_increment  # 每一次扫描增加的角度(角度)
 
float32 time_increment   # 测量的时间间隔(s)
float32 scan_time        # 扫描的时间间隔(s)
 
float32 range_min        # 距离最小值(m)
float32 range_max        # 距离最大值(m)
 
float32[] ranges         # 距离数组(长度360)(注意: 值 < range_min 或 > range_max 应当被丢弃)
float32[] intensities    # 与设备有关,强度数组(长度360)

所以Sick激光雷达将极坐标转换成XY坐标的时候，选取angle_min作为起始角度，每次遍历一帧中其他的点的时候就加一次angle_increment。再结合点的距离range，就可以获得在XY坐标系下的点。

    for(i=0;i<pointsnum;i++)
    {
        float angle=pointsptr->angle_min+i*pointsptr->angle_increment;
        points[i]=QPointF(pointsptr->ranges[i]*cos(angle）,pointsptr->ranges[i]*sin(angle)); //极坐标转换成XY坐标
    }

这里有个坑，sick雷达传过来的数据中，angle_min和angle_max并不是-45和225,利用rostopic查看数据。发现实际的angle_min是-2.35rad(-135度)，angle_max是2.35rad(135度)。
在这里插入图片描述
通过下面的图可以发现，这样计算出来的点的坐标是有问题的。假定的坐标系应该是0°沿x轴正方向，90°沿y轴正方向(如下右图所示)

解决方案：

解决方法就很简单了，只要将原先的angle加上90度就可以实现所有点在XY坐标系下逆时针旋转90度。

//selectionwidget.cpp:387
points[i]=QPointF(pointsptr->ranges[i]*cos(angle + PI/2）,pointsptr->ranges[i]*sin(angle + PI/2));