(02)Cartographer源码无死角解析-(19) SensorBridge→雷达点云数据预处理(函数重载)

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

再前面的博客中，已经对 Odome、landmark、Imu、GPS 的预处理进行了讲解，有的朋友或许会觉得GPS的处理挺复杂的，那么再来看看稍微复杂一些的点云处理，如下：

//所有单线雷达topic回调函数
SensorBridge::HandleLaserScanMessage() 
//所有回声波雷达topoc回调函数
SensorBridge::HandleMultiEchoLaserScanMessage()
//所有多线雷达topic回调函数
SensorBridge::HandlePointCloud2Message()
1
2
3
4
5
6

其上函数都实现于 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。但是，实际感觉能达到这个精度的雷达不是很多。
 

$雷达数据帧概念：$

明白了基本概念之后，这里再讲解以下雷达帧的概念，假设雷达为20HZ，那么他的帧率可以理解为20，也就是每间隔50ms发送一帧数据，一帧数据中包含了雷达视野范围的所有点云数据。如20HZ单线180度视角的雷达，说明其1秒内对180度视野范围扫描了20次(帧)，每次的数据为180/0.25=720个点云数据。
 

三、雷达消息

讲解代码之前，先来看消息类型。

1、单线雷达

在终端执行指令：

rosmsg show LaserScan
1

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 //雷达数据每个点对应的强度值
1
2
3
4
5
6
7
8
9
10
11
12
13

2、多回声雷达

在终端执行指令：

rosmsg show MultiEchoLaserScan
1

可以看到与 LaserScan 的结构基本一致的，不同点如下所示

sensor_msgs/LaserEcho[] ranges
  float32[] echoes
sensor_msgs/LaserEcho[] intensities
  float32[] echoes
1
2
3
4

普通的激光扫描消息(LaserScan)表述的是：每个激光脉冲的单个返回深度和强度值，如果返回了多个，通常只会选取其中强度最强的一个。

多回波传感器(MultiEchoLaserScan)不同的是，它能够为每个激光脉冲接收多个回波。例如，如果您扫描窗户，则通常会从玻璃以及玻璃后面的墙壁中接收到回波；如果在两个物体的边界处，则会收取到不同深度的回波。在创建地图和定位机器人的位置时，这些数据可以为您提供更多附加信息。因此，使用 MultiEchoLaserScan 的节点可以充分利用这种类型的传感器。

理解其为高级传感器，简单的说，每次激光打出，可以接受多个反馈信息，也就是多少深度值。

2、多线雷达

在终端执行指令：

rosmsg show PointCloud2
1

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表示没有
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

比如下面来看一帧多线点云的数据，再终端执行如下指令：

//查看消息，本人可以看到 /rslidar_points
rostopic  list
//打印话题内容，并且输入到 rslidar_points.txt 文件中
rostopic  echo /rslidar_points > rslidar_points.txt
1
2
3
4

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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

根据上面的内容，可以知道每点云数据包含 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);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

可以看到其处理过程都是非常类似的，其核心都是调用 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){
	......
	......
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

这里需要注意，LaserScanToPointCloudWithIntensities()为模板函数(理解为高级重载)，那么就先对齐进行讲解，然后再分析 ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) 吧
 

五、LaserScanToPointCloudWithIntensities()

该函数为模板函数，大家这里可能会觉得奇怪，上面代码再调用的时候，明明没有指定模板参数，却可以调用模板函数，这主要来自于编译器的自动模板参数推导。先来看一下代码注释，然后再对其中的重难点进行分析：

// For sensor_msgs::LaserScan and sensor_msgs::MultiEchoLaserScan.
// 将LaserScan与MultiEchoLaserScan转成carto格式的点云数据
template <typename LaserMessageType>
std::tuple<PointCloudWithIntensities, ::cartographer::common::Time>
LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
  CHECK_GE(msg.range_min, 0.f); //最小距离大于等于0.f
  CHECK_GE(msg.range_max, msg.range_min); //最大距离大于等于最小距离

  //针对与点云的两种情况
  if (msg.angle_increment > 0.f) { //如果后一个数据点与前一个数据点之间的角度应该大于0.f
    CHECK_GT(msg.angle_max, msg.angle_min);//那么最大角度大于最小角度
  } else {//如果后一个数据点小于前一个数据点之间的角度应该大于0.f
    CHECK_GT(msg.angle_min, msg.angle_max);//那么最小角度应该大于最大于角度
  }

  //用于存储一帧带强度的点云数据
  PointCloudWithIntensities point_cloud;
  //注意angle值随着循环在变化，使用弧度制
  float angle = msg.angle_min; 
  //对所有的数据点进行遍历，每个数据点可能存在多个echoe
  for (size_t i = 0; i < msg.ranges.size(); ++i) {
    // c++11: 使用auto可以适应不同的数据类型
    const auto& echoes = msg.ranges[i];
    //HasEcho其有两个重载函数，会根据输入类型分别进行不同处理
    //如果为单线雷达，直接返回 true；如果为多回声雷达，则需要echoes.echoes不为空才执行
    if (HasEcho(echoes)) {
      //仍然有两个重载分别对单线雷达与多回声雷达进行处理
      const float first_echo = GetFirstEcho(echoes); 
      // 满足范围才进行使用,即第一个echo其数值在合理范围内才进行处理，为inf不会进行处理
      if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
        //first_echo仅仅是一个三维点到传感器的距离，需要根据把其变换成雷达坐标系下的点云数据
        //假设雷达与first_echo连线新坐标的x轴,则在这个新坐标系中first_echo的位置是(x,0,0)
        //绕Z轴旋转angle弧度,即回到了雷达坐标系。
        const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
        const cartographer::sensor::TimedRangefinderPoint point{
            rotation * (first_echo * Eigen::Vector3f::UnitX()), // position
            i * msg.time_increment};//根据time_increment参数记录时间戳time          
        // 保存点云坐标与时间信息
        point_cloud.points.push_back(point);
        // 如果存在强度信息
        if (msg.intensities.size() > 0) {
          CHECK_EQ(msg.intensities.size(), msg.ranges.size());
          // 使用auto可以适应不同的数据类型
          const auto& echo_intensities = msg.intensities[i];
          CHECK(HasEcho(echo_intensities));
          //存储真实强度信息
          point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));
        } else {//不过不存在强度信息,则把强度设置为0.f
          point_cloud.intensities.push_back(0.f);
        }
      }
    }
    //每处理完一个数据，增加一个角度分辨率单位
    angle += msg.angle_increment;
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55

相信大部分注释都是没有难度的，可能只有如下代码不太好理解而已：

        const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
        const cartographer::sensor::TimedRangefinderPoint point{
            rotation * (first_echo * Eigen::Vector3f::UnitX()), // position
            i * msg.time_increment};//根据time_increment参数记录时间戳time    
1
2
3
4

首先需要了解到的是， 再ROS中，使用的通常都是右手坐标系，如下所示：

$右手坐标系：$也就是一般来说，雷达正前方为X+方向，左边为Y+方向，上面为Z+反向，逆时针旋转为正旋转，且通常以弧度为单位，范围在[-Π,Π]之间。

这里举一个例子，假设 first_echo=19.6152687，angle= -3.0089736，那么该点在雷达坐标系的占位置为 $\mathbf{P}$ = (-19.4430275，-2.59373975, 0)。从俯视视角来看，那么如下图所示：

很明显的知道，$\mathbf{P}$ 其应该在的位置为第四象限(逆时针方向计算)，可以简单通过$x=-|\mathbf{P}|\cos (\Pi -\theta )$，$y=-|\mathbf{P}|\sin (\Pi -\theta )$ 求得坐标，但是源码的实现是通过对 ${\mathbf{P}}^{\prime }$ 进行旋转，从而获得 $\mathbf{P}$ 的坐标，大家可以使用勾股定理验证一下结果。
 

六、ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg)

最后，在来了解处理多线雷达 sensor_msgs::PointCloud2 类型数据的 ToPointCloudWithIntensities() 函数。
该函数根据 msg 中的字段，分成了四种情况进行处理,如下所示：

$(1)有强度，有时间：$ 调用pcl中的fromROSMsg()接口，先把msg中的点云数据转换成 PointXYZIT(I表示强度,T表示有时间)类型全部存储于 pcl_point_cloud 变量中，然后对齐进行遍历：①利用点云的 x,y,z 加 time 构建 TimedRangefinderPoint 数据存放于 point_cloud.points 变量之中。②提取点云的强度信息存储于 point_cloud.intensities 变量之中。

$(2)有强度，无时间：$ 调用pcl中的fromROSMsg()接口，先把msg中的点云数据转换成 PointXYZI(I表示强度)类型全部存储于 pcl_point_cloud 变量中，然后对其进行遍历：①利用点云的 x,y,z 加 time 构建 TimedRangefinderPoint 数据存放于 point_cloud.points 变量之中，其中 time 都设置为 0.f。②提取点云的强度信息存储于 point_cloud.intensities 变量之中。

$(3)无强度，有时间：$ 调用pcl中的fromROSMsg()接口，先把msg中的点云数据转换成 PointXYZT(T表示时间)类型全部存储于 pcl_point_cloud 变量中，然后对其进行遍历：①利用点云的 x,y,z 加 time 构建 TimedRangefinderPoint 数据存放于 point_cloud.points 变量之中。②点云强度设置为 1.0f 存储于 point_cloud.intensities 变量之中。

$(4)无强度，无时间：$ 调用pcl中的fromROSMsg()接口，先把msg中的点云数据转换成 PointXYZ类型全部存储于 pcl_point_cloud 变量中，然后对其进行遍历：时间设置为0.f，强度设置为 1.0f。

$总结：$总的来说，没有时间，则时间设置为0.f，如果没有强度，则强度设置为1.0f，xyz保持不变，存储于 point_cloud 变量中。

另外，点云第一个数据时间为0，即最后一个数据的时间戳可以认为是点云帧的持续时间，这里把点云最后一个数据的时间作为整个点云的时间戳，所以其他的点时间戳应该是目前的时间戳，加上点云帧的持续时间。然后再验证一下点云时间是否正确，正常情况下每个点的时间应该都是小于最后一个数据点的时间(帧持续时间)。代码注释如下：

// 由ros格式的PointCloud2转成carto格式的PointCloudWithIntensities
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
           ::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
  PointCloudWithIntensities point_cloud;
  // We check for intensity field here to avoid run-time warnings if we pass in
  // a PointCloud2 without intensity.

  // 有强度数据
  if (PointCloud2HasField(msg, "intensity")) {

    // 有强度字段, 有时间字段
    if (PointCloud2HasField(msg, "time")) {
      pcl::PointCloud<PointXYZIT> pcl_point_cloud;
      pcl::fromROSMsg(msg, pcl_point_cloud);
      point_cloud.points.reserve(pcl_point_cloud.size());
      point_cloud.intensities.reserve(pcl_point_cloud.size());
      for (const auto& point : pcl_point_cloud) {
        point_cloud.points.push_back(
            {Eigen::Vector3f{point.x, point.y, point.z}, point.time});
        point_cloud.intensities.push_back(point.intensity);
      }
    } 
    // 有强度字段, 没时间字段
    else {
      pcl::PointCloud<pcl::PointXYZI> pcl_point_cloud;
      pcl::fromROSMsg(msg, pcl_point_cloud);
      point_cloud.points.reserve(pcl_point_cloud.size());
      point_cloud.intensities.reserve(pcl_point_cloud.size());
      for (const auto& point : pcl_point_cloud) {
        point_cloud.points.push_back(
            {Eigen::Vector3f{point.x, point.y, point.z}, 0.f}); // 没有时间信息就把时间填0
        point_cloud.intensities.push_back(point.intensity);
      }
    }
  } 
  // 没有强度数据
  else {
    // If we don't have an intensity field, just copy XYZ and fill in 1.0f.
    // 没强度字段, 有时间字段
    if (PointCloud2HasField(msg, "time")) {
      pcl::PointCloud<PointXYZT> pcl_point_cloud;
      pcl::fromROSMsg(msg, pcl_point_cloud);
      point_cloud.points.reserve(pcl_point_cloud.size());
      point_cloud.intensities.reserve(pcl_point_cloud.size());
      for (const auto& point : pcl_point_cloud) {
        point_cloud.points.push_back(
            {Eigen::Vector3f{point.x, point.y, point.z}, point.time});
        point_cloud.intensities.push_back(1.0f);
      }
    } 
    // 没强度字段, 没时间字段
    else {
      pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
      pcl::fromROSMsg(msg, pcl_point_cloud);
      point_cloud.points.reserve(pcl_point_cloud.size());
      point_cloud.intensities.reserve(pcl_point_cloud.size());
      for (const auto& point : pcl_point_cloud) {
        point_cloud.points.push_back(
            {Eigen::Vector3f{point.x, point.y, point.z}, 0.f}); // 没有时间信息就把时间填0
        point_cloud.intensities.push_back(1.0f);
      }
    }
  }

  ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
  if (!point_cloud.points.empty()) {
    const double duration = point_cloud.points.back().time;
    // 点云开始的时间 加上第一个点到最后一个点的时间
    // 点云最后一个点的时间 作为整个点云的时间戳
    timestamp += cartographer::common::FromSeconds(duration);

    for (auto& point : point_cloud.points) {
      // 将每个点的时间减去整个点云的时间, 所以每个点的时间都应该小于0
      point.time -= duration;

      // 对每个点进行时间检查, 看是否有数据点的时间大于0, 大于0就报错
      CHECK_LE(point.time, 0.f)
          << "Encountered a point with a larger stamp than "
             "the last point in the cloud.";
    }
  }

  return std::make_tuple(point_cloud, timestamp);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85

 

七、HandleLaserScan

通过该篇博客，已经对雷达预处理过程进行了详细的分析，也就是如下三个函数：

//所有单线雷达topic回调函数
SensorBridge::HandleLaserScanMessage() 
//所有回声波雷达topoc回调函数
SensorBridge::HandleMultiEchoLaserScanMessage()
//所有多线雷达topic回调函数
SensorBridge::HandlePointCloud2Message()
1
2
3
4
5
6

他们有一个共同点，那就是调用：

std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);//已经完成讲解
1

函数，把点云数据都转换成 cartographer::sensor::PointCloudWithIntensities 数据，以及一个时间戳的格式。共同表示为 std::tie(point_cloud, time)，但是接下来有一小部分没有讲解，如下所示

HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
HandleRangefinder(sensor_id, time, msg->header.frame_id, point_cloud.points);
1
2

该函数就比较简单了，就是把雷达点云数据转换到 tracking_frame=“img” 坐标系下。先看HandleLaserScan() 函数：

// 根据参数配置,将一帧雷达数据分成几段, 再传入trajectory_builder_
void SensorBridge::HandleLaserScan(
    const std::string& sensor_id, const carto::common::Time time,
    const std::string& frame_id,
    const carto::sensor::PointCloudWithIntensities& points) {
  if (points.points.empty()) {
    return;
  }
  // CHECK_LE: 小于等于
  CHECK_LE(points.points.back().time, 0.f);
  // TODO(gaschler): Use per-point time instead of subdivisions.

  // 意为一帧雷达数据被分成几次处理, 一般将这个参数设置为1
  for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
    const size_t start_index =
        points.points.size() * i / num_subdivisions_per_laser_scan_;
    const size_t end_index =
        points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
    
    // 生成分段的点云
    carto::sensor::TimedPointCloud subdivision(
        points.points.begin() + start_index, points.points.begin() + end_index);
    if (start_index == end_index) {
      continue;
    }
    const double time_to_subdivision_end = subdivision.back().time;
    // `subdivision_time` is the end of the measurement so sensor::Collator will
    // send all other sensor data first.
    const carto::common::Time subdivision_time =
        time + carto::common::FromSeconds(time_to_subdivision_end);
    
    auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
    if (it != sensor_to_previous_subdivision_time_.end() &&
        // 上一段点云的时间不应该大于等于这一段点云的时间
        it->second >= subdivision_time) {
      LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
                   << sensor_id << " because previous subdivision time "
                   << it->second << " is not before current subdivision time "
                   << subdivision_time;
      continue;
    }
    // 更新对应sensor_id的时间戳
    sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
    
    // 检查点云的时间
    for (auto& point : subdivision) {
      point.time -= time_to_subdivision_end;
    }
    CHECK_EQ(subdivision.back().time, 0.f);
    // 将分段后的点云 subdivision 传入 trajectory_builder_
    HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
  } // for 
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53

其原理函数比较简单的，即使根据 .lua 文件中 num_subdivisions_per_laser_scan 参数，把一帧点云分成几段数据，同时更新每段点数据的时间戳。然后调用 HandleRangefinder() 函数。
 

八、结语

对于 HandleRangefinder() 函数，其把基于雷达的传感器的点云数据坐标系变换到 tracking_frame=“imu” 坐标系下，代码如下

/**
 * @brief 
 * 
 * @param[in] sensor_id 数据的话题
 * @param[in] time 点云的时间戳(最后一个点的时间)
 * @param[in] frame_id 点云的frame
 * @param[in] ranges 雷达坐标系下的TimedPointCloud格式的点云
 */
void SensorBridge::HandleRangefinder(
    const std::string& sensor_id, const carto::common::Time time,
    const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
  if (!ranges.empty()) {
    CHECK_LE(ranges.back().time, 0.f);
  }
  const auto sensor_to_tracking =
      tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));

  // 以 tracking 到 sensor_frame 的坐标变换为TimedPointCloudData 的 origin
  // 将点云的坐标转成 tracking 坐标系下的坐标, 再传入trajectory_builder_
  if (sensor_to_tracking != nullptr) {
    trajectory_builder_->AddSensorData(
        sensor_id, carto::sensor::TimedPointCloudData{
                       time, 
                       sensor_to_tracking->translation().cast<float>(),
                       // 将点云从雷达坐标系下转到tracking_frame坐标系系下
                       carto::sensor::TransformTimedPointCloud(
                           ranges, sensor_to_tracking->cast<float>())} ); // 强度始终为空
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

那么到这里位置，可以说对于雷达点云数据预处理的预处理都讲解完成了。大家再回过头来看 sensor_bridge.h 这个文件，可以发现其包含的函数于变量都讲解完毕了，