(02)Cartographer源码无死角解析-(19) SensorBridge→雷达点云数据预处理(函数重载)
本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
文 末 正 下 方 中 心 提 供 了 本 人 联 系 方 式 ， 点 击 本 人 照 片 即 可 显 示 W X → 官 方 认 证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证
一、前言
再前面的博客中，已经对 Odome、landmark、Imu、GPS 的预处理进行了讲解，有的朋友或许会觉得GPS的处理挺复杂的，那么再来看看稍微复杂一些的点云处理，如下：
//所有单线雷达topic回调函数
SensorBridge::HandleLaserScanMessage()
//所有回声波雷达topoc回调函数
SensorBridge::HandleMultiEchoLaserScanMessage()
//所有多线雷达topic回调函数
SensorBridge::HandlePointCloud2Message()
其上函数都实现于 src/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc 文件中。另外，这里所谓的复杂，仅仅是代码逻辑上的复杂，而非算法，那么就开始讲解吧。
二、雷达重要指标
1.视野范围→数据的最大最小距离以及扫描的角度范围
激光雷达的视野范围决定了这款雷达的应用场景，墙距离20m的屋子里不能可能使用视野范围为4米的雷达，根本扫不到东西。
2.角度分辨率或点数→2个数据点间的角度
雷达的角度分辨率决定了激光雷达每帧数据点的个数。如果是0.25度的分辨率，360度视角的激光雷达，那其一帧最多有1441个点。每帧数据的点数越多，越能更好的表征环境的细节信息。当然，点数也不是越多越好，点数越多，计算成本也越高。
3.频率：发出数据的频率，如10HZ，就可以理解为1秒10帧数据
雷达的频率是一个十分重要的指标，雷达的频率越高，雷达2帧数据的间隔就越小。假设雷达的频率是20Hz，也就是50ms来一次数据。当我进行定位的时候，这50ms时间内的机器人的运动，只能通过估计来获得，所以雷达的频率越高，我们通过估计的距离也就越小，定位也就越准确。
4.强度：发出数据的能量强度，主要物体材质相关
激光雷达的激光点是有能量的，不同品牌激光点的能量也不同。当能量太小时，远距离情况下可能存在返回不了数据的情况。
5.精度：点的跳动程度（精度）等等
这是最重要的一个指标，如果一个激光雷达的数据跳动特别大，那这个雷达就没法用了。现在一般厂商的雷达的精度都是2%。也就是100m的情况下，点的跳动幅度为2cm。但是，实际感觉能达到这个精度的雷达不是很多。
雷 达 数 据 帧 概 念 ： \color{blue}雷达数据帧概念： 雷达数据帧概念：
明白了基本概念之后，这里再讲解以下雷达帧的概念，假设雷达为20HZ，那么他的帧率可以理解为20，也就是每间隔50ms发送一帧数据，一帧数据中包含了雷达视野范围的所有点云数据。如20HZ单线180度视角的雷达，说明其1秒内对180度视野范围扫描了20次(帧)，每次的数据为180/0.25=720个点云数据。
三、雷达消息
讲解代码之前，先来看消息类型。
1、单线雷达
在终端执行指令：
rosmsg show LaserScan
std_msgs/Header header //消息头
uint32 seq //消息
time stamp //包含了开始扫描的时间和与开始扫描的时间差
string frame_id //数据是基于该坐标系的，本人为scan
float32 angle_min //可检测范围的起始角度(弧度制)
float32 angle_max //可检测范围的终止角度，与angle_min组成激光雷达的可检测范围。像从-180度到+180度就是360度的范围。
float32 angle_increment //雷达数据的角度分辨率（角度增量）
float32 time_increment //雷达数据每个数据点的时间间隔
float32 scan_time //当前帧数据与下一帧数据的时间间隔
float32 range_min //最近可检测深度的阈值
float32 range_max //最远可检测深度的阈值
float32[] ranges //一帧深度数据的存储数组
float32[] intensities //雷达数据每个点对应的强度值
2、多回声雷达
在终端执行指令：
rosmsg show MultiEchoLaserScan
可以看到与 LaserScan 的结构基本一致的，不同点如下所示
sensor_msgs/LaserEcho[] ranges
float32[] echoes
sensor_msgs/LaserEcho[] intensities
float32[] echoes
普通的激光扫描消息(LaserScan)表述的是：每个激光脉冲的单个返回深度和强度值，如果返回了多个，通常只会选取其中强度最强的一个。
多回波传感器(MultiEchoLaserScan)不同的是，它能够为每个激光脉冲接收多个回波。例如，如果您扫描窗户，则通常会从玻璃以及玻璃后面的墙壁中接收到回波；如果在两个物体的边界处，则会收取到不同深度的回波。在创建地图和定位机器人的位置时，这些数据可以为您提供更多附加信息。因此，使用 MultiEchoLaserScan 的节点可以充分利用这种类型的传感器。
理解其为高级传感器，简单的说，每次激光打出，可以接受多个反馈信息，也就是多少深度值。
2、多线雷达
在终端执行指令：
rosmsg show PointCloud2
std_msgs/Header header //消息头
uint32 seq //消息序列
time stamp //时间戳
string frame_id
uint32 height //点云的高度，如果是无序点云，则为1
uint32 width //每行点云的宽度
sensor_msgs/PointField[] fields //表示一个点的结构，x、y、z代表三维坐标， intensity代表反射强度，
uint8 INT8=1 //INT8为一个字节
uint8 UINT8=2 //UINT8为两个字节
uint8 INT16=3 //INT16为3个字节
uint8 UINT16=4 //UINT16为四个字节
uint8 INT32=5 //INT32为五个字节
uint8 UINT32=6 //UINT32为六个字节
uint8 FLOAT32=7 //FLOAT32为七个字节
uint8 FLOAT64=8 //FLOAT64为八个字节
string name //一般填写为 X,Y,Z，intensity
uint32 offset //表一个点结构内的起始地址
uint8 datatype //上面的八种之一
uint32 count //count代表field的个数
bool is_bigendian //是否大端存储，计算机一般是小端存储，所以默认设置为flase即可
uint32 point_step //每个点占用的比特数，1个字节对应8个比特数
uint32 row_step //每一行占用的beye数
uint8[] data //为序列化后的数据，直接获得不了信息，序列化是为了方便信息传输和交换，使用时需要反序列化
bool is_dense //是否有非法数据点，true表示没有
比如下面来看一帧多线点云的数据，再终端执行如下指令：
//查看消息，本人可以看到 /rslidar_points
rostopic list
//打印话题内容，并且输入到 rslidar_points.txt 文件中
rostopic echo /rslidar_points > rslidar_points.txt
header:
seq: 7390
stamp:
secs: 1606808670
nsecs: 787610000
frame_id: "front_laser_link"
height: 16
width: 1032
fields:
-
name: "x"
offset: 0
datatype: 7
count: 1
-
name: "y"
offset: 4
datatype: 7
count: 1
-
name: "z"
offset: 8
datatype: 7
count: 1
-
name: "intensity"
offset: 16
datatype: 7
count: 1
is_bigendian: False
point_step: 32
row_step: 33024
data: [181, 159, 96, 64, 101, 243, 14, 191, 239, 125, 113, ......]
is_dense: False
根据上面的内容，可以知道每点云数据包含 x,y,z,intensity信息，其数据类型为 FLOAT32，也就是x,y,z,intensity各占用4个字节，共16个字节，共32(point_step)个beye数。使用小端存储，不含非法数据。height=16，可以知道其为16线雷达，每线(行) width=1032点云数据，共16x1032=16512个点云数据，共占用beyes数16512x32=528384。执行代码 msg->data.size() 可以直接获得共占用的beyes数。
四、函数重载
关于 HandleLaserScanMessage()、HandleMultiEchoLaserScanMessage()、HandlePointCloud2Message() 函数，注释如下：
// 处理LaserScan数据, 先转成点云,再传入trajectory_builder_
void SensorBridge::HandleLaserScanMessage(
const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
carto::sensor::PointCloudWithIntensities point_cloud;
carto::common::Time time;
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}
// 处理MultiEchoLaserScan数据, 先转成点云,再传入trajectory_builder_
void SensorBridge::HandleMultiEchoLaserScanMessage(
const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
carto::sensor::PointCloudWithIntensities point_cloud;
carto::common::Time time;
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}
// 处理ros格式的PointCloud2, 先转成点云,再传入trajectory_builder_
void SensorBridge::HandlePointCloud2Message(
const std::string& sensor_id,
const sensor_msgs::PointCloud2::ConstPtr& msg) {
carto::sensor::PointCloudWithIntensities point_cloud;
carto::common::Time time;
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
HandleRangefinder(sensor_id, time, msg->header.frame_id, point_cloud.points);
}
可以看到其处理过程都是非常类似的，其核心都是调用 ToPointCloudWithIntensities() 函数把 msg 转换成 carto 算法需要的
carto::sensor::PointCloudWithIntensities 类型。但是这里注意，虽然其调用函数的的函数名都是 ToPointCloudWithIntensities()，但是其处理过程都是不一样的，因为其有三个重载函数，根据输入的数据类型，分别调用不同的函数，重载函数如下(src/cartographer_ros/cartographer_ros/cartographer_ros/msg_conversion.cc)：
// 由ros格式的LaserScan转成carto格式的PointCloudWithIntensities
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) {
return LaserScanToPointCloudWithIntensities(msg);
}
// 由ros格式的MultiEchoLaserScan转成carto格式的PointCloudWithIntensities
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg) {
return LaserScanToPointCloudWithIntensities(msg);
}
// 由ros格式的PointCloud2转成carto格式的PointCloudWithIntensities
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg){
......
......
}
这里需要注意，LaserScanToPointCloudWithIntensities()为模板函数(理解为高级重载)，那么就先对齐进行讲解，然后再分析 ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) 吧
五、LaserScanToPointCloudWithIntensities()
该函数为模板函数，大家这里可能会觉得奇怪，上面代码再调用的时候，明明没有指定模板参数，却可以调用模板函数，这主要来自于编译器的自动模板参数推导。
这里举一个例子，假设 first_echo=19.6152687，angle= -3.0089736
那么该点在雷达坐标系的占位置为 (-19.4430275，-2.59373975, 0)