cartographer代码学习-雷达数据的处理
1、单线雷达数据处理
数据处理函数来自于：
// 处理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);
}
算法首先接受了一个sensor_id与一个LaserScan类型的数据，然后声明了一个point_cloud的数据结构以及一个时间的数据结构。然后通过ToPointCloudWithIntensities这个函数对point_cloud以及 time进行填充，之后将调用HandleLaserScan将sensor_id, time, msg->header.frame_id, point_cloud进行处理。
ToPointCloudWithIntensities(*msg)是一个重载类函数：
// 由ros格式的LaserScan转成carto格式的PointCloudWithIntensities
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) {
return LaserScanToPointCloudWithIntensities(msg);
}
这个函数将ros格式的LaserScan转成carto格式的PointCloudWithIntensities，下面多线超声波雷达以及point_cloud格式的数据调用的也是这个重载类函数，但是输入函数的数据格式是不一样的，对应有不同的函数体实现，在msg_conversion.cc文件中
在这个重载类函数最后是返回了LaserScanToPointCloudWithIntensities(msg)的结果。这个函数实现如下：
// 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);
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) {
//获取msg的第i个值，赋值给echoes
// c++11: 使用auto可以适应不同的数据类型
//laserscan与MultiEchoLaserScan的ranges数据类型是不一样的，通过auto自适应
//laserscan的ranges数据类型位float32，MultiEchoLaserScan的数据类型是sensor_msgs/laserEcho类型
const auto& echoes = msg.ranges[i];
//这里HasEcho也是一个重载类函数，它有两个类型：
//bool HasEcho(float) { return true; }
//或者
//bool HasEcho(const sensor_msgs::LaserEcho& echo) {
// return !echo.echoes.empty();
//}
//这里对于两个数据类型做了区分，上面一种对应激光数据下面对应多线超声波雷达。下面一个要检查laserecho.echoes中的数据，这里面存储的才是真实的值。
if (HasEcho(echoes)) {
//GetFirstEcho也是一个重载类函数
//float GetFirstEcho(float range) { return range; }
// 通过函数重载, 使得函数可以同时适用LaserScan与LaserEcho
//float GetFirstEcho(const sensor_msgs::LaserEcho& echo) {
// return echo.echoes[0];
//}
//用重载区分数据类型，返回最后的range的值
const float first_echo = GetFirstEcho(echoes);
// 满足范围才进行使用
if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
//通过AngleAxisf获得旋转
const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
//TimedRangefinderPoint数据结构为一个带时间戳的单个数据点坐标
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;
}
//通过FromRos函数将ROS下的时间戳转到carto下的时间戳
::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
if (!point_cloud.points.empty()) {
//duration代表点云后面一个时间点，用最后一个点云的时间点作为整个点云的时间戳。
//由于点云的每个点都带有一个时间戳信息，第一个点云时间戳为0，所以最后一个点的时间戳也就是整个点云的时间戳。
const double duration = point_cloud.points.back().time;
// 以点云最后一个点的时间为点云的时间戳
//timestamp一般是第一个点的时间，用第一个点的时间加上duration就变成了最后一个点的时间戳信息
timestamp += cartographer::common::FromSeconds(duration);
// 让点云的时间变成相对值, 最后一个点的时间为0，前面的每一个时间都是负值。
for (auto& point : point_cloud.points) {
point.time -= duration;
}
}
//以最后一个点的时间戳作为这帧点云的时间戳传递出去。注意这是carto的时间戳跟ROS的时间戳是不一样的。
return std::make_tuple(point_cloud, timestamp);
}
注意到这是一个模板类函数，模板参数是LaserMessageType。这里const LaserMessageType& msg会根据传入的不同参数类型进行不同的处理。所以通过这个模板函数实现了传入不同类型的参数的同一个函数的执行（后面多线超声波雷达使用的也是这个函数）。这里其实也可以用重载实现，但是这里用模板的话代码会比重载简洁一点，重载需要写两遍。
在进行完数据个是的转换后，算法调用了：
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
处理。跳到该函数看一下：
// 根据参数配置,将一帧雷达数据分成几段, 再传入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
}
可以看到函数的输入有4个参数：sensor_id，time，frame_id，points。
第一步还是对时间参数进行了检查，判断时间戳是否正常。
第二步是根据num_subdivisions_per_laser_scan_决定对点云进行几次处理
这里用到了一个来自sensor_bridge的参数：num_subdivisions_per_laser_scan_。而sensor_bridge是通过map_builder_bridge构造的。因此这个参数最开始来自于map_builder_bridge.cc中的：
// 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
因此它来自于lx_rs16_2d_outdoor.lua文件中的num_subdivisions_per_laser_scan=1。
所以这里：
// 意为一帧雷达数据被分成几次处理, 一般将这个参数设置为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_;
其实只执行了一次。
第三步是根据第二步中for循环执行的次数对点云进行分段。
第四步是寻找sensor_id的时间戳
这个时间戳是存储在
std::map<std::string, cartographer::common::Time>
sensor_to_previous_subdivision_time_;
中的一个时间。这个时间戳记录了上一个数据的处理时间，这个时间应该要比新进来的数据的时间更早一点，要是时间比最新数据的时间还晚那说明数据时间会有问题。
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);
第六步是将点云传入 trajectory_builder_处理：
// 将分段后的点云 subdivision 传入 trajectory_builder_
HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
看一下最后的处理函数：
// 雷达相关的数据最终的处理函数
// 先将数据转到tracking坐标系下,再调用trajectory_builder_的AddSensorData进行数据的处理
/**
* @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>())} ); // 强度始终为空
}
}
这个函数首先是检查了数据是否为空以及时间戳是否正确，然后查找了tracking_frame到雷达坐标系的坐标变换。 接下来通过调用carto::sensor::TransformTimedPointCloud函数，传入参数为ranges, sensor_to_tracking->cast())。通过这个函数进行一个点云的坐标变换。之前的ranges的坐标系是雷达坐标系的，通过这个函数转到tracking_frame下。
最后将数据与sensor_id,time的等一起传入到AddSensorData中进行处理。
2、多线超声波雷达数据处理
多线超声波雷达数据处理函数如下：
// 处理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);
}
通过两个函数对比，发现他们之间基本是差不多的，后面的处理函数是同一个。
3、点云处理函数
点云处理函数如下：
// 处理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);
}
点云的处理函数与前面的也是相同的，但是也有一些区别。最后的处理函数与前面两个是不同的。
point_cloud的模板类函数实现与前面两个也有所不同：
// 由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);
}
}
}
//通过FromRos函数将ROS下的时间戳转到carto下的时间戳
::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就报错
//参见上面对laserscan的处理，每个点的时间戳应该小于等于0
CHECK_LE(point.time, 0.f)
<< "Encountered a point with a larger stamp than "
"the last point in the cloud.";
}
}
//返回处理后的point_cloud以及时间戳。时间戳为point_cloud中最后一个点的时间。
return std::make_tuple(point_cloud, timestamp);
}
简单来说，它应该分为4种类型：
1、有强度字段，有时间字段：
PointCloud2HasField是一个查找函数，用于查找消息中是否存在某个字段参数。
// 检查点云是否存在 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;
}
对于字段都存在的数据，我们只需要将其的X、Y、Z坐标以及强度信息还有时间信息存储到point_cloud中。
2、有强度字段，没有时间字段：
对于没有时间字段的数据，将X、Y、Z坐标与强度信息存储到point_cloud中并将时间信息设置为0。
3、没有强度字段，有时间字段：
对于没有强度字段的数据，将X、Y、Z坐标与时间信息存储到point_cloud中并将强度信息设置为1。
4、没有强度字段也没有时间字段：
对于没有强度字段也没有时间字段的数据，将X、Y、Z坐标存储到point_cloud中并将强度信息设置为1，时间信息设置为0。