目录
4.1ToPointCloudWithIntensities函数
4.1.1LaserScanToPointCloudWithIntensities函数
4.2(const sensor_msgs::PointCloud2& msg)
在Sensor_bridge头文件中定义了许多回调函数，前一篇文章提到了HandleOdometryMessage函数，这篇文章将对剩余几类函数一一介绍：
//处理单线雷达数据的回调函数
Node::HandleLaserScanMessage() = SensorBridge::HandleLaserScanMessage()
//处理多回声波雷达数据的回调函数
Node::HandleMultiEchoLaserScanMessage() = SensorBridge::HandleMultiEchoLaserScanMessage()
//处理多线雷达数据的回调函数
Node::HandlePointCloud2Message() = SensorBridge::HandlePointCloud2Message()
//处理IMU数据的回调函数
Node::HandleImuMessage() = SensorBridge::HandleImuMessage()
//处理里程计数据的回调函数
Node::HandleOdometryMessage() = SensorBridge::HandleOdometryMessage()
//处理GPS数据的回调函数
Node::HandleNavSatFixMessage() = SensorBridge::HandleNavSatFixMessage()
//处理Landmar数据的回调函数
Node::HandleLandmarkMessage() = SensorBridge::HandleLandmarkMessage()
一.HandleNavSatFixMessage函数
首先需要传入两个参数，前者 sensor_id为kNavSatFixTopic，没有进行重映射就为默认为fix，后者是一个经过转换传入Cartographer的时间time：
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)))});
}接着进行判断GPS的数据是否为固定解，如果不是，这些数据无法保证精度（可能是周围信号不好，周遭环境影响比较大），源码中会丢弃这些数据，然后加入一个固定的空位姿。由于初始数据为经纬度(时分)坐标，无法直接进行定位，所以需要将地心坐标系(ecef)转成机器人所在坐标系。通过ComputeLocalFrameFromLatLong函数(这个会单独开一篇文章讲解)对经纬度信息进行计算，并打印经纬度信息。
二.HandleLandmarkMessage函数
同理也是传入两个相同的函数，sensor_id以node_constants.h为准，msg为ros中自定义的LandmarkList数据类型：
// 处理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);
}对传入的msg进行处理需要转换为Cartographer格式，调用了ToLandmarkData函数：
2.1ToLandmarkData函数
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;
}
首先将landmark列表中的时间戳转换成Cartographer中的时间，接着对landmark_list这个数组进行遍历，把平移，旋转权重以及tracking_from_landmark_transform(获取tracking_frame到landmark的frame的坐标变换)传入，然后返回Carto格式的landmark。
回到HandleLandmarkMessage函数：
// 获取 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);通过LookupToTracking函数求出tracking_frame坐标系到landmark转换矩阵，并检查sensor_id情况，前一篇文章详细介绍了，就不再过多解释。
landmark_to_tracking_transform以Cartographer提供的数据包，实现camera坐标系到landmark坐标系的一个变换，乘上tracking_from_landmark_sensor代表的tracking到camera间的变换矩阵，就实现了tracking_frame坐标系到landmark坐标变换。再通过trajectory_builder_->AddSensorData传入数据进行处理。
三.HandleImuMessage函数
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});
}
}首先也是通过ToImuData函数将ros格式的imu数据转换成Carto格式的，如果imu_data不为空，则把传感器id，imu中的运行时间，线加速度，角速度等数据传入。下面来看下ToImuData函数，代码如下：
3.1ToImuData函数
// 进行数据类型转换与坐标系的转换
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)});
}
传入数据后首先检测线性加速度与角速度是否为-1，如果是则默认为没有数据。接着就将时间戳格式进行转换，调用了 tf_bridge_.LookupToTracking() 函数，得出一个坐标变换，如果平移分量大于
便会报错，imu频率很高，如果不以它为tracking_frame，还需要将imu数据进行转换，计算量大大增加，最后考虑旋转向量，如果tracking_frame就为“imu_link”，则相当于没有旋转。转到imu_data数据：
struct ImuData {
common::Time time;
Eigen::Vector3d linear_acceleration;
Eigen::Vector3d angular_velocity;
};有着时间，线加速度以及角速度三个成员变量。
四.处理雷达数据
三个函数都需要先传入sensor_id以及msg，再通过ToPointCloudWithIntensities函数对时间以及点云数据进行填充，单线激光雷达和多回声波雷达都是调用的HandleLaserScan，而多线激光雷达则是调用的HandleRangefinder函数：
// 处理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这个重载函数：
4.1ToPointCloudWithIntensities函数
根据输入的数据类型，分别调用不同的函数：
// 由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) {
PointCloudWithIntensities point_cloud;
// We check for intensity field here to avoid run-time warnings if we pass in
// a PointCloud2 without intensity.
对于单线激光雷达以及多回声波雷达都返回了LaserScanToPointCloudWithIntensities函数，但是数据类型不一样，代码如下：
4.1.1LaserScanToPointCloudWithIntensities函数
该函数为模板函数，首先需要判断最小的距离是否大于等于0.f以及最大距离是否大于等于最小距离。接着根据角度的增量的两种情况去判断，最大角度与最小角度的关系。对所有的数据点进行遍历，获取msg中距离的值，其中msg包括了LaserScan以及MultiEchoLaserScan两种类型的数据：
// 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) {
CHECK_GT(msg.angle_max, msg.angle_min);
} else {
CHECK_GT(msg.angle_min, msg.angle_max);
}
//用于存储一帧带强度的点云数据
PointCloudWithIntensities point_cloud;
//注意angle值随着循环在变化，使用弧度制
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);
// 满足范围才进行使用,即第一个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;
}
4.1.2 HasEcho函数
然后调用了一个HasEcho函数，先看一下LaserScan以及MultiEchoLaserScan的数据类型:
前面数据基本一致，区别在于LaserScan的距离信息ranges是一个float32[]的数组，而MultiEchoLaserScan的ranges数据类型为sensor_msgs/LaserEcho[]，sensor_msgs/LaserEcho[]这个数据类型只包括echoes这一项，强度信息也是同理。前者通过索引返回一个float32的值，后者通过LaserEcho数据类型中的索引值再返回float32的值。
HasEcho同样是重载函数，如果传入的是float32型的单线雷达数据，返回 true；如果为多回声雷达，则需要echoes.echoes不为空才返回true：
bool HasEcho(float) { return true; }
// For sensor_msgs::MultiEchoLaserScan.
bool HasEcho(const sensor_msgs::LaserEcho& echo) {
return !echo.echoes.empty();
}
4.1.3 GetFirstEcho函数
根据传入类型不同，又引入了一个新的重载函数GetFirstEcho分别进行处理：
float GetFirstEcho(float range) { return range; }
// 通过函数重载, 使得函数可以同时适用LaserScan与LaserEcho
float GetFirstEcho(const sensor_msgs::LaserEcho& echo) {
return echo.echoes[0];
}上文代码所提到的时间增量 i * msg.time_increment，i从0开始，第一个点就是一个时间增量如此往复，最后一个点n的时间就是n-1再乘上时间的增量。duration代表的是从第一个点到最后一个点的这段时间，而timestamp一般代表第一个点的时间，故第一个点时间加上第一个点到最后一个点的时间便可以得到在carto下的时间戳（以最后一个点的时间为点云的时间戳）。point.time -= duration这一布将每个点的时间减去最后一个点的时间，使前面的时间为负，让最后一个点的时间为0。
::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);
// 让点云的时间变成相对值, 最后一个点的时间为0
for (auto& point : point_cloud.points) {
point.time -= duration;
}
}
return std::make_tuple(point_cloud, timestamp);
}最后来到多线雷达的数据处理
4.2(const sensor_msgs::PointCloud2& msg)
首先通过PointCloud2HasField对函数字段进行查找看是否有强度以及时间字段：
// 由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);
}
}
}
4.2.1PointCloud2HasField函数，
代码如下，对传入的字段进行查找如果匹配到了就返回true，否则返回false。
// 检查点云是否存在 field_name 字段
bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2,
const std::string& field_name) {
for (const auto& field : pc2.fields) {
if (field.name == field_name) {
return true;
}
}
return false;
}两种字段分为了四种可能性：
// 有强度字段, 有时间字段
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);
}
} 首先生成了pcl格式的一个点云PointXYZIT，这是carto自定义的一个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];
};
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))
再通过fromROSmsg函数对pcl_point_cloud进行时间戳等的赋值，并对返回的点云信息中的数组大小进行重置。然后对pcl_point_cloud进行遍历，填入x,y,z,time信息以及强度信息，如果没有时间字段便填入0.f即可，如果没有强度信息将point_cloud.intensities.push_back(1.0f)设置为1倍即可。
4.3 HandleLaserScan函数
首先传入sensor_id,frame_id,time以及points，time为上述提及的最后一个点的时间，参数num_subdivisions_per_laser_scan_用来判断雷达数据分为几次处理：
假设num_subdivisions_per_laser_scan_=1 ，size =100
则有开始索引start_index为100*0/1=0,结束索引end_index为100*（0+1）/1=100
当num_subdivisions_per_laser_scan_=2 ，size =100时
第一次循环start_index=0 end_index=50；
第二次循环start_index=50 end_index=100；雷达数据就分为了两部分进行处理。
// 根据参数配置,将一帧雷达数据分成几段, 再传入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
}再通过subdivision生成分段的点云，如果分为一次处理，可得到所有的点云，分两次处理就只能得到一半的点云，可能是0-50区间的也可能是50-100区间的。令time_to_subdivision_end为分段后的最后一个点的时间，假设为0-50段的，所得时间为负再加上整个的时间，可以得到前五十个点云的时间，若雷达数据只分了一次进行处理，即最后subdivision_time为0，subdivision_time就为time代表一帧点云的时间。
接着在map中sensor_to_previous_subdivision_time_查找sensor_id，如果查询到了，就将其中保存的时间信息与当前计算的时间进行比较，记录的时间（上一帧）如果比当前的时间还要大则会报错。然后再更新一下时间戳，检查每个点的时间减去最后一个点的时间是否等于0，并将参数传入HandleRangefinder函数。
4.3.1 HandleRangefinder函数
这个函数首先检查一下ranges是否为空，且要求最后一个点的时间小于0否则将会报错。接着查找了从tracking_frame到雷达坐标系的变换，如果查询不为空，则把基于雷达的传感器的点云数据坐标系通过TransformTimedPointCloud函数变换到 tracking_frame坐标系下，将三数据传入到TimedPointCloudData这个数据结构中，再与sensor_id一起传入AddSensorData中。
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>())} ); // 强度始终为空
}
}TransformTimedPointCloud这个函数主要是通过变换矩阵对雷达格式下的点云数据进行转换：
TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud,
const transform::Rigid3f& transform) {
TimedPointCloud result;
result.reserve(point_cloud.size());
for (const TimedRangefinderPoint& point : point_cloud) {
result.push_back(transform * point);
}
return result;
}