cartographer_learn_9

写在前面

经过前面8篇的讨论，我们终于找到了在2d的情况下究竟是哪个类真正的实现前端的功能，哪个类在正真的负责后端的算法。它们分别是：
GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>中的类成员pose_graph_（类型是LocalTrajectoryBuilder2D）和local_trajectory_builder_（类型是PoseGraph2D）。
从本篇开始我们将正真的接触算法，讨论额模式也不再是谁调用谁了，而是数据A传输到了类B中进行了怎么的处理变成了数据C，或者是数据A来到了类B中后类B维护了怎样的状态这种模式。再提一下作者刚刚进入激光slam，新手有错各位多海涵。
我们从LaserScan数据开始，这里我们要回到前面ros的部分，从数据的话题订阅开始讨论这条线。

ros中订阅LaserScan数据

在Node这个类的构造函数中我们开启了一项名为“start_trajectory”的服务（见第1篇），其功能就是开启一条新的轨迹，回调函数是Node::HandleStartTrajectory，这个回调函数调用了Node::AddTrajectory去具体执行，在这个函数中调用了Node::LaunchSubscribers去订阅各种话题，我们的讨论从这里开始，先上代码：

void Node::LaunchSubscribers(const TrajectoryOptions& options,
                             const int trajectory_id) {
 //订阅LaserScan的数据
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::LaserScan>(
             &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
             this),
         topic});
  }
  .......//还订阅了一堆其他数据
}

可以看到回调函数是Node::HandleLaserScanMessage，就是每当雷达的LaserScan的数据到来都会把数据仍给这个函数处理（ros的知识点）。进入这个函数中

void Node::HandleLaserScanMessage(const int trajectory_id,
                                  const std::string& sensor_id,
                                  const sensor_msgs::LaserScan::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);
  // 判断是否需要跳过该次数据，为了让我们的讨论目标明确，跳过这个，读者可自行查看如何判断
  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
    return;
  }
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLaserScanMessage(sensor_id, msg);
}

可以看到又把数据扔给了SensorBridge::HandleLaserScanMessage处理，这里我们第一次看到LaserScan的数据类型（LaserScan），我们下一节将稍微介绍一下。先计入这个函数：

oid SensorBridge::HandleLaserScanMessage(
    const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
  carto::sensor::PointCloudWithIntensities point_cloud;
  carto::common::Time time;
  //将雷达数据转换成点云并处理每个点的时间，time是点云中最后一个点的时间
  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}

ok来到了本篇简要重点介绍的两个函数ToPointCloudWithIntensities和HandleLaserScan

LaserScan数据介绍及处理

数据格式

LaserScan这个类的类成员如下：
Header :是一个结构体，包含seq、stamp、frame_id。seq目前还没发现其作用，stamp激光数据的时间戳，frame_id在算法中的作用貌似是在tf功能包中找到该数据对应的传感器。
float angle-min：开始扫描的角度
float angle-max：结束扫描的角度
float angle-increment：每次扫描增加的角度
float time-increment： 测量的时间间隔
float scan-time ：扫描的时间间隔
float range-min：测距的最小值
float range-max：测距的最大值
vector< float > ranges：数据，描述每次条激光测量的距离
vector< float >intensities：感觉是描述反射回来的强度的参数

数据处理

首先是ToPointCloudWithIntensities，上代码：

std::tuple<::cartographer::sensor::PointCloudWithIntensities,
           ::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) {
  return LaserScanToPointCloudWithIntensities(msg);
}

看到这个函数也不亲手处理数据，而是调用了别人LaserScanToPointCloudWithIntensities，那为什么要把它贴出来了，因为我们第一此看到了数据LaserScan将要被处理成什么样的数据格式——PointCloudWithIntensities。为了更好的理解处理过程，先来看看这种数据结构

using TimedPointCloud = std::vector< TimedRangefinderPoint >;

struct PointCloudWithIntensities{
TimedPointCloud points;
vector<float> intensities;
}

struct TimedRangefinderPoint {
  Eigen::Vector3f position;
  float time; // 相对点云最后一个点的时间, 最后一个点的时间为0, 其他点的时间都为负的
};

可见本次处理的目标是将数据处理成一个个三维的点，且这些点都带着时间，需要特别指出的是这些点实在传感器坐标系的即Pl(表示在LaserScan坐标系中的点)。
进去看处理过程

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) {
    const auto& echoes = msg.ranges[i];
//HasEcho返回其参数，有点不明白这个函数存在的意义
    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());
          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);
        }
      }
    }
    //更新角度，处理下一个LaserScan的测量
    angle += msg.angle_increment;
  }

  ::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);
}

这里为什么测量值等于最大值时要被忽略呢？这里建议读者阅读《概率机器人》中的第六章关于测距仪的模型这一部分。还有一点就是时间LaserScan的header中的时间戳时获得第一个数据的时间，这里处理过后我们将最后一个点的设为0，其他点的时间时负的。同时tuple中的第二个时间就是最后得到最后一个点时候的时间
接下来看看HandleLaserScan，它其实时按照设置一帧数据分拆成n（n一般为1）帧发送出去

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(points.points.back().time, 0.f);
  // TODO(gaschler): Use per-point time instead of subdivisions.
  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_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_
    HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
  } // for 
}

可见将点云数据分段后调用了HandleRangefinder分发，我们再来看HandleRangefinder

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 坐标系下的坐标, 再传入trajectory_builder_
  if (sensor_to_tracking != nullptr) {
    trajectory_builder_->AddSensorData(
        sensor_id, carto::sensor::TimedPointCloudData{
                       time, 
                       sensor_to_tracking->translation().cast<float>(),
                       carto::sensor::TransformTimedPointCloud(
                           ranges, sensor_to_tracking->cast<float>())} ); 
  }
}

作者习惯称traking_frame为车辆坐标系，这里我们看到这个函数将数据从PointCloudWithIntensities转换成TimedPointCloudData在发出去，具体看看TimedPointCloudData

struct TimedPointCloudData {
  common::Time time;        // 最后一个点的时间
  Eigen::Vector3f origin;   // 从tracking_frame_坐标系原点
  TimedPointCloud ranges; 
  std::vector<float> intensities; // 空的，每个返回点得强度
}

这里的TimedPointCloud上面我们见过。这里的trajectory_builder_的类型是TrajectoryBuilderInterface,在我们讨论的2d情况下，实际上是GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>。
本篇先讨论到这了，虽然还没写数据下一步数据怎么处理，但是困了。。。。。。