02 传感器的格式转换与类型变换

最新推荐文章于 2023-04-12 20:41:08 发布

莫滴感情

最新推荐文章于 2023-04-12 20:41:08 发布

阅读量957

点赞数 1

分类专栏： cartographer 文章标签： c++ 算法

本文链接：https://blog.csdn.net/qq_44178391/article/details/127013375

版权

cartographer 专栏收录该内容

3 篇文章 2 订阅

订阅专栏

SensorBridge.cc

SensorBridge类中会频繁使用到Rigid3这个类，所以首先得了解Rigid类！

01 Rigid3

关于Rigid3的源码，下面博主有详细分析！这里就不再赘述！

(15条消息) cartographer源码分析(13)-transform-rigid_transform.h_slamcode的博客-CSDN博客_rigid transform

简单来说，Rigid3这个类实现了三维刚体的坐标变换（会用接口就行，理论知识涉及到了四元素和矩阵运算）

由于这个类重载了*，可以通过A * B将B坐标系下的点转换到A坐标系下！

这个类存在的意义：因为在无人驾驶中，一个车辆上就存在多个坐标系，比如激光雷达坐标系，gps坐标系，imu坐标系等，最终都必须统一到tracking_frame坐标系下！

02 构造函数

SensorBridge类的构造函数如下：

SensorBridge::SensorBridge(
    const int num_subdivisions_per_laser_scan,
    const std::string& tracking_frame,
    const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
    carto::mapping::TrajectoryBuilderInterface* const trajectory_builder)
    : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
      tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
      trajectory_builder_(trajectory_builder) {}

创建SensorBridge这个类的对象是在之前所分析到的MapBuilderBridge类的成员函数AddTrajectory中的第二步！

//在MapBuilderBridge.h中定义的
//std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
// Step: 2 为这个新轨迹 添加一个SensorBridge
  sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
      trajectory_options.num_subdivisions_per_laser_scan,
      trajectory_options.tracking_frame,
      node_options_.lookup_transform_timeout_sec, 
      tf_buffer_,
      map_builder_->GetTrajectoryBuilder(trajectory_id)); // CollatedTrajectoryBuilder

最后一个参数map_builder_->GetTrajectoryBuilder(trajectory_id)返回的是一个CollatedTrajectoryBuilder的类，这个类尤其重要，后面会专门讲！

03 数据处理

在前面分析到的node.cc中，每个传感器都有对应处理自己相应数据的回调函数！比如imu：

 * @param[in] trajectory_id 轨迹id
 * @param[in] sensor_id imu的topic名字
 * @param[in] msg imu的ros格式的数据
 
void Node::HandleImuMessage(const int trajectory_id,
                            const std::string& sensor_id,
                            const sensor_msgs::Imu::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
    return;
  }
  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
  auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
  // extrapolators_使用里程计数据进行位姿预测
  if (imu_data_ptr != nullptr) {
    extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
  }
  sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}

所有传感器最后都调用了SensorBridge类中相应的成员函数来处理！

因为cartographer_ros是在ROS中通过订阅相应的传感器话题，从而接收从ROS传来的传感器数据！所以传感器数据的时间戳是ROS的时间！传感器数据的类型也是ROS格式下的类型！

而Cartographer中给各个传感器自定义了类型，使用的也是和ROS不同的ICU Universal Time

从协调世界时1970年1月1日0时0分0秒起至现在的总秒数

ICU世界时标的纪元是“0001-01-01 00:00:00.0+0000”，正好比Unix纪元早719162天

因此，接下来的各个传感器的处理都包含了转换时间和格式的操作！

里程计数据处理

`SensorBridge::HandleOdometryMessage`

//trajectory_builder_是sensor_bridge.h的成员变量
//::cartographer::mapping::TrajectoryBuilderInterface* const  trajectory_builder_;

// 调用trajectory_builder_的AddSensorData进行数据的处理
void SensorBridge::HandleOdometryMessage(
    const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
  // 数据类型与数据坐标系的转换
  std::unique_ptr<carto::sensor::OdometryData> odometry_data =
      ToOdometryData(msg);
  if (odometry_data != nullptr) {
    // tag: 这个trajectory_builder_是谁, 讲map_builder时候再揭晓
    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::OdometryData{odometry_data->time, odometry_data->pose});
  }
}

调用SensorBridge类中的成员函数ToOdometryData，作用是将ros格式的里程计数据转成tracking frame的pose, 再转成carto的里程计数据类型

将 ROS的时间转成 ICU Universal Time
找到 tracking坐标系到里程计的child_frame_id 的坐标变换
将里程计的footprint的pose转成tracking_frame的pose, 再转成carto的里程计数据类型

std::unique_ptr<carto::sensor::OdometryData> SensorBridge::ToOdometryData(
    const nav_msgs::Odometry::ConstPtr& msg) {
  //将 ROS的时间 转成 ICU Universal Time
  const carto::common::Time time = FromRos(msg->header.stamp);
  // 找到 tracking坐标系 到 里程计的child_frame_id 的坐标变换, 所以下方要对sensor_to_tracking取逆
  /*比如，imu_link(tracking_frame_) 在 base_link(child_frame_id) 上方 0.1m处，那这里返回的坐标变换的z是 -0.1*/
  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
      time, CheckNoLeadingSlash(msg->child_frame_id));
  if (sensor_to_tracking == nullptr) {
    return nullptr;
  }

  // 将里程计的footprint的pose转成tracking_frame的pose, 再转成carto的里程计数据类型
  return absl::make_unique<carto::sensor::OdometryData>(
      carto::sensor::OdometryData{
          time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
}

然后再使用成员变量 trajectory_builder_调用该类中的成员函数AddSensorData

总结：将ROS传来的里程计数据进行了时间和坐标变换的处理，然后转换成Carto的格式传给CollatedTrajectoryBuilder这个类进行处理，后面的传感器处理都是这个步骤！

Landmark数据处理

`SensorBridge::HandleLandmarkMessage`

// 处理Landmark数据, 先转成carto的格式,再传入trajectory_builder_
void SensorBridge::HandleLandmarkMessage(
    const std::string& sensor_id,
    const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
  // 将在ros中自定义的LandmarkList类型的数据, 转成LandmarkData
  auto landmark_data = ToLandmarkData(*msg);

  // 获取 tracking_frame到landmark的frame 的坐标变换
  auto tracking_from_landmark_sensor = tf_bridge_.LookupToTracking(
      landmark_data.time, CheckNoLeadingSlash(msg->header.frame_id));

  // 将数据转到tracking_frame下
  if (tracking_from_landmark_sensor != nullptr) {
    for (auto& observation : landmark_data.landmark_observations) {
      observation.landmark_to_tracking_transform =
          *tracking_from_landmark_sensor *
          observation.landmark_to_tracking_transform;
    }
  }
  // 调用trajectory_builder_的AddSensorData进行数据的处理
  trajectory_builder_->AddSensorData(sensor_id, landmark_data);
}

同样，首先调用SensorBridge类中的成员函数ToLandmarkData，将在ros中自定义的LandmarkList类型的数据, 转成LandmarkData

创建一个Cartographer自定义类型的landmark_data
将 ROS的时间转成 ICU Universal Time
将在ros中自定义的LandmarkList类型的数据landmark_list赋值给landmark_data并返回

// 将在ros中自定义的LandmarkList类型的数据, 转成LandmarkData
LandmarkData ToLandmarkData(const LandmarkList& landmark_list) {
  LandmarkData landmark_data;
  landmark_data.time = FromRos(landmark_list.header.stamp);
  for (const LandmarkEntry& entry : landmark_list.landmarks) {
    landmark_data.landmark_observations.push_back(
        {entry.id, ToRigid3d(entry.tracking_from_landmark_transform),
         entry.translation_weight, entry.rotation_weight});
  }
  return landmark_data;
}

然后获取 tracking_frame到landmark的frame 的坐标变换，将数据转到tracking_frame下
最后再使用成员变量 trajectory_builder_调用该类中的成员函数AddSensorData

imu数据处理

`SensorBridge::HandleImuMessage`

// 调用trajectory_builder_的AddSensorData进行数据的处理
void SensorBridge::HandleImuMessage(const std::string& sensor_id,
                                    const sensor_msgs::Imu::ConstPtr& msg) {
  std::unique_ptr<carto::sensor::ImuData> imu_data = ToImuData(msg);
  if (imu_data != nullptr) {
    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration,
                               imu_data->angular_velocity});
  }
}

同样，首先调用SensorBridge类中的成员函数ToImuData，将ros格式的imu数据进行数据类型转换与坐标系的转换

首先先检查是否存在线性加速度与角速度，如果没有就会报错！
将 ROS的时间转成 ICU Universal Time
找到 tracking坐标系到 imu的坐标变换sensor_to_tracking
如果sensor_to_tracking 的平移量大于了1e-5m，那么就会报错！

（因为imu的频率很高，如果不以imu的坐标系为tracking_frame，就要频繁的计算imu的数据转到tracking_frame下；相对的，雷达的频率不高，计算量相对偏少）
转换到tracking_frame下后返回carto格式的imu数据

// 进行数据类型转换与坐标系的转换
std::unique_ptr<carto::sensor::ImuData> SensorBridge::ToImuData(
    const sensor_msgs::Imu::ConstPtr& msg) {
  // 检查是否存在线性加速度与角速度
  CHECK_NE(msg->linear_acceleration_covariance[0], -1)
      << "Your IMU data claims to not contain linear acceleration measurements "
         "by setting linear_acceleration_covariance[0] to -1. Cartographer "
         "requires this data to work. See "
         "http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";
  CHECK_NE(msg->angular_velocity_covariance[0], -1)
      << "Your IMU data claims to not contain angular velocity measurements "
         "by setting angular_velocity_covariance[0] to -1. Cartographer "
         "requires this data to work. See "
         "http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";

  const carto::common::Time time = FromRos(msg->header.stamp);
  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
      time, CheckNoLeadingSlash(msg->header.frame_id));
  if (sensor_to_tracking == nullptr) {
    return nullptr;
  }
  // 推荐将imu的坐标系当做tracking frame
  CHECK(sensor_to_tracking->translation().norm() < 1e-5)
      << "The IMU frame must be colocated with the tracking frame. "
         "Transforming linear acceleration into the tracking frame will "
         "otherwise be imprecise.";
  // 进行坐标系的转换
  return absl::make_unique<carto::sensor::ImuData>(carto::sensor::ImuData{
      time, sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
      sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)});
}

使用成员变量 trajectory_builder_调用该类中的成员函数AddSensorData

gps数据处理

`SensorBridge::HandleNavSatFixMessage`

// 将ros格式的gps数据转换成相对坐标系下的坐标,再调用trajectory_builder_的AddSensorData进行数据的处理
void SensorBridge::HandleNavSatFixMessage(
    const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg) {
  const carto::common::Time time = FromRos(msg->header.stamp);
  // 如果不是固定解,就加入一个固定的空位姿
  if (msg->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::FixedFramePoseData{time, absl::optional<Rigid3d>()});
    return;
  }

  // 确定ecef原点到局部坐标系的坐标变换
  if (!ecef_to_local_frame_.has_value()) {
    ecef_to_local_frame_ =
        ComputeLocalFrameFromLatLong(msg->latitude, msg->longitude);
    LOG(INFO) << "Using NavSatFix. Setting ecef_to_local_frame with lat = "
              << msg->latitude << ", long = " << msg->longitude << ".";
  }

  // 通过这个坐标变换 乘以 之后的gps数据,就相当于减去了一个固定的坐标,从而得到了gps数据间的相对坐标变换
  trajectory_builder_->AddSensorData(
      sensor_id, carto::sensor::FixedFramePoseData{
                     time, absl::optional<Rigid3d>(Rigid3d::Translation(
                               ecef_to_local_frame_.value() *
                               LatLongAltToEcef(msg->latitude, msg->longitude,
                                                msg->altitude)))});
}

将 ROS的时间转成 ICU Universal Time
检查gps的状态，如果gps信号不好或者数据波动太大，则传入一个空的数据到AddSensorData
计算第一帧GPS数据指向ECEF坐标系下原点的坐标变换ecef_to_local_frame_,
用ecef_to_local_frame_乘以之后的GPS数据就得到了之后的GPS数据相对于第一帧GPS数据的相对坐标变换，然后转成carto格式的gps数据
最后再使用成员变量 trajectory_builder_调用该类中的成员函数AddSensorData

雷达数据处理

雷达数据的来源有三种，一个是LaserScan、一个是MultiEchoLaserScan、还有一个PointCloud2

所以对应的处理函数也有三个SensorBridge::HandleLaserScanMessage、SensorBridge::HandleMultiEchoLaserScanMessage、SensorBridge::HandlePointCloud2Message！

其中LaserScan和MultiEchoLaserScan的处理类似，采用了函数模板的方法！没有使用重载！

// 处理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);
}

通过代码可以看到，三个处理函数第一步都是先创建carto::sensor::PointCloudWithIntensities和carto::common::Time 类型的变量
然后通过一个重载函数ToPointCloudWithIntensities来给这两个类型的变量赋值
最后LaserScan和MultiEchoLaserScan都调用了一个函数HandleLaserScan

PointCloud2调用的函数是HandleRangefinder

LaserScan和MultiEchoLaserScan的`ToPointCloudWithIntensities`

// 由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);
}

可以看到，两个重载函数最后都调用了一个函数LaserScanToPointCloudWithIntensities，该函数的作用就是为了调用函数LaserScanToPointCloudWithIntensities，而函数LaserScanToPointCloudWithIntensities的作用才是真正实现将LaserScan与MultiEchoLaserScan转成carto格式的点云数据

LaserScan和MultiEchoLaserScan的`LaserScanToPointCloudWithIntensities`

因为LaserScan和MultiEchoLaserScan的ROS数据格式是有一定区别的，所以该函数利用了C++中的模板技术来实现两种不同数据类型的输入，否则就得使用函数重载！

LaserScan和MuLTIEcholaserScan的ROS数据格式对比：

LaserScan的ROS数据格式 MuLTIEcholaserScan的ROS数据格式
std_msgs/Header header std_msgs/Header header
uint32 seq uint32 seq
time stamp time stamp
string frame_id string frame_id
float32 angle_min float32 angle_min
float32 angle_max float32 angle_max
float32 angle_increment float32 angle_increment
float32 time_increment float32 time_increment
float32 scan_time float32 scan_time
float32 range_min float32 range_min
float32 range_max float32 range_max
float32[] ranges sensor_msgs/LaserEcho[] ranges（LaserEcho[]下包含了float32[] echoes）
float32[] intensities sensor_msgs/LaserEcho[] intensities（LaserEcho[]下包含了float32[] echoes)

可以看到，只有最后两行有差别：

LaserScan的ranges[0]、intensities[0]就表示float32类型

而MuLTIEcholaserScan的==ranges[0].echoes[0]、intensities[0].echoes[0]==才表示float32类型

在LaserScanToPointCloudWithIntensities函数中HasEcho和GetFirstEcho函数也利用了这个区别进行函数重载！

LaserScan的ROS数据格式	MuLTIEcholaserScan的ROS数据格式
std_msgs/Header header	std_msgs/Header header
uint32 seq	uint32 seq
time stamp	time stamp
string frame_id	string frame_id
float32 angle_min	float32 angle_min
float32 angle_max	float32 angle_max
float32 angle_increment	float32 angle_increment
float32 time_increment	float32 time_increment
float32 scan_time	float32 scan_time
float32 range_min	float32 range_min
float32 range_max	float32 range_max
float32[] ranges	sensor_msgs/LaserEcho[] ranges（LaserEcho[]下包含了float32[] echoes）
float32[] intensities	sensor_msgs/LaserEcho[] intensities（LaserEcho[]下包含了float32[] echoes)

// 将LaserScan与MultiEchoLaserScan转成carto格式的点云数据
template <typename LaserMessageType>
std::tuple<PointCloudWithIntensities, ::cartographer::common::Time>
LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
  CHECK_GE(msg.range_min, 0.f);
  CHECK_GE(msg.range_max, msg.range_min);
  if (msg.angle_increment > 0.f) {
    CHECK_GT(msg.angle_max, msg.angle_min);
  } else {
    CHECK_GT(msg.angle_min, msg.angle_max);
  }

  PointCloudWithIntensities point_cloud;
  float angle = msg.angle_min;
  for (size_t i = 0; i < msg.ranges.size(); ++i) {
    // c++11: 使用auto可以适应不同的数据类型
    const auto& echoes = msg.ranges[i];
    if (HasEcho(echoes)) {

      const float first_echo = GetFirstEcho(echoes);
      // 满足范围才进行使用
      if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
        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
        // 保存点云坐标与时间信息
        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 {
          point_cloud.intensities.push_back(0.f);
        }
      }
    }
    angle += msg.angle_increment;
  }

  //将msg中的起始时间从ROS格式转换成 ICU Universal Time格式
  ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
  if (!point_cloud.points.empty()) {
    //获取到最后一个点的时间
    const double duration = point_cloud.points.back().time;
    // 先将最后一个点的时间duration转换为时间戳
    // 以点云最后一个点的时间为点云的时间戳
    timestamp += cartographer::common::FromSeconds(duration);

    // 让点云的时间变成相对值, 最后一个点的时间为0
    for (auto& point : point_cloud.points) {
      point.time -= duration;
    }
  }
  return std::make_tuple(point_cloud, timestamp);
}

首先检查各个配置参数是否正确，比如雷达范围的最小值要大于等于0，最大值也要大于等于最小值等等
通过auto以及对函数HasEcho和函数GetFirstEcho的重载，分别将LaserScan与MultiEchoLaserScan转成carto格式的点云数据
其中需要注意的是，cartographer::sensor::TimedRangefinderPoint 的类型是
```
// 带时间戳的单个数据点的坐标
struct TimedRangefinderPoint {
  Eigen::Vector3f position;
  float time; // 相对点云最后一个点的时间, 最后一个点的时间为0, 其他点的时间都为负的
};
```
而最后保存点云数据类型是PointCloudWithIntensities
```
using TimedPointCloud = std::vector<TimedRangefinderPoint>;

// TODO(wohe): Retained for cartographer_ros. To be removed once it is no
// longer used there.
struct PointCloudWithIntensities {
  TimedPointCloud points;
  std::vector<float> intensities;
};
```
由此可见，最后保存的点云数据既有带时间戳的单个数据点的坐标，又有强度信息！

这里需要知道两个概念——时间和时间戳

时间表示以此时此刻的准确时间（是一个数值，在cartographer中用Time）

时间戳表示距离 ICU Universal Time起始时间的时间段（是一个表示范围的数值，在cartographer中用Duration）
point.time保存的是以最后一个点云时间为0，前面的点云时间为负数！

而另一个返回值timestamp则保存的是最后一个点的时间！

LaserScan和MultiEchoLaserScan的`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
  //当设置成1，假设有100个点云，那就一次处理
  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_;
    
    // 生成分段的点云
    //这里的subdivision是一个数组，std::vector<TimedRangefinderPoint>
    carto::sensor::TimedPointCloud subdivision(
        points.points.begin() + start_index, points.points.begin() + end_index);
    if (start_index == end_index) {
      continue;
    }
    //如果是最后一个点，那么time_to_subdivision_end = 0
    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.
    //time为最后一个点的时间
    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 
}

假设传入的是100个点云，num_subdivisions_per_laser_scan_ = 1
- 传入HandleRangefinder的是以subdivision包含的100个点云，点云保存的时间point.time不变，subdivision_time = time，即最后一个点云的时间
假设传入的是100个点云，num_subdivisions_per_laser_scan_ = 2
- 传入HandleRangefinder的是两段点云，第一段subdivision包含前50个点云，点云保存的时间point.time从[-time, -time/2]变成了[-time/2, 0]，subdivision_time = time/2，即最后一个点云时间的一半
- 第二段包含后50个点云，点云保存的时间point.time不变，subdivision_time = time，即最后一个点云的时间

PointCloud2的`ToPointCloudWithIntensities`

// 由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);
}

首先要知道，点云数据可以分为四种情况：

类型	有强度字段, 有时间字段	有强度字段, 没时间字段	没强度字段, 有时间字段	没强度字段, 没时间字段
定义的位置	在pcl库中没有定义在cartographer代码中中定义并在在pcl中注册	pcl库中有定义，直接使用	在pcl库中没有定义在cartographer代码中中定义并在在pcl中注册	pcl库中有定义，直接使用
使用方法	`pcl::PointCloud<PointXYZIT> pcl_point_cloud;`	`pcl::PointCloud<pcl::PointXYZI> pcl_point_cloud;`	`pcl::PointCloud<PointXYZIT> pcl_point_cloud;`	`pcl::PointCloud<pcl::PointXYZI> pcl_point_cloud;`

使用方法的区别就在于定义模板参数时，一个是PointXYZIT，一个是pcl::PointXYZI

Cartographer中定义并在在pcl中注册的方法：

// 自定义的pcl点云格式
struct PointXYZT {
  float x;
  float y;
  float z;
  float time;
};

struct PointXYZIT {
  PCL_ADD_POINT4D;
  float intensity;
  float time;
  float unused_padding[2];
};

}  // namespace

// 将自定义的点云格式在pcl中注册
POINT_CLOUD_REGISTER_POINT_STRUCT(
    PointXYZT, (float, x, x)(float, y, y)(float, z, z)(float, time, time))

POINT_CLOUD_REGISTER_POINT_STRUCT(
    PointXYZIT,
    (float, x, x)(float, y, y)(float, z, z)(float, intensity,
                                            intensity)(float, time, time))

针对四种不同状态的点云，依次将点云的位置、时间戳、强度加入PointCloudWithIntensities类中的point_cloud保存
接着处理保存在点云里的时间point.time，和LaserScan和MultiEchoLaserScan处理相同，依然是以点云最后一个点的时间作为整个点云的时间戳，前面的点云保存的时间都是负数

返回的另一个参数timestamp就是最后一个点云的时间

雷达相关的数据最终的处理函数——`SensorBridge::HandleRangefinder`

/**
 * @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>())} ); // 强度始终为空
  }
}

先通过LookupToTracking 查找从tracking_frame_到点云frame_id的坐标变换sensor_to_tracking
再将雷达坐标系下的TimedPointCloud格式的点云的range和sensor_to_tracking传入到carto::sensor::TransformTimedPointCloud中，将点云从雷达坐标系下转到tracking_frame坐标系系下
```
// 时间同步前的点云
struct TimedPointCloudData {
  common::Time time;        // 点云最后一个点的时间
  Eigen::Vector3f origin;   // 以tracking_frame_到雷达坐标系的坐标变换为原点
  TimedPointCloud ranges;   // 数据点的集合, 每个数据点包含xyz与time, time是负的
  // 'intensities' has to be same size as 'ranges', or empty.
  std::vector<float> intensities; // 空的
};
```
最后将
- 点云的时间戳(最后一个点的时间)的time
- 以tracking_frame_到雷达坐标系的坐标变换为原点的origin
- 将点云从雷达坐标系下转到tracking_frame坐标系系下的数据点集合ranges
- 和没有赋值，强度始终为空的intensities
作为一个结构体TimedPointCloudData，和sensor_id一起传入到trajectory_builder_的AddSensorData函数中

到此为止，所有的传感器数据都传入了这个trajectory_builder_的AddSensorData函数中，而trajectory_builder_正是TrajectoryBuilderInterface的子类CollatedTrajectoryBuilder！