点云：从LaserScan到TimedPointCloudOriginData

• 单一、有序场景。指这么种场景：只有一个传感器，每次采样有一定间隔，且采集新点云帧第一个点的时间一定比旧点云最后一个点的时间晚。
• 到TimedPointCloudOriginData，表示点云中的点用的是tracking_frame坐标系下的3d直角坐标。在rose_ros，tracking_frame="base_footrprint"，即点云中的点格式是base_footprint坐标系下的3d直角坐标。TimedPointCloudOriginData.origins[0]则是laser-->base_footprint的tf中平移部分，像(0, 0, 0.098)。

通过定阅“scan”话题，cartogeapher收到sensor_msgs::LaserScan消息，当中含有点云数据。这里叙述如何处理点云，直到点云数据放在了一个TimedPointCloudOriginData类型变量。示例用的2D SLAM传感器是思岚RPLIDAR A1M8，每次点云采集时有720个点。文中的四张截图，是处理同一次点云时截的，可相互对照。

先给出小结，后详述每个过程。

一、[sensor_msgs::LaserScan](std::vector<float>)ranges

Node::HandleLaserScanMessage()是“scan”话题关联的处理回调，它收到的消息类型是sensor_msgs::LaserScan，点云存放在ranges字段，数据结构是std::vector<float>。每个点的数值是障碍物和laser坐标系原点之间距离R，单位米。如果该点上没障碍物，值是inf（C++数值std::numeric_limits<float>::infinity()），正如图1中的ranges[0]、ranges[1]。

通过angle_increment字段，可以算出每个点的夹角Theta，因而LaserScan是用极坐标存储点云。

time_increment是相邻点之间的到达时间间隔。720个点意味着有719个间隔，这些间隔是同一个值。LaserScan.header.stamp是个绝对时间，它表示第一个点采集时刻，根据时间间隔，便可算出其它各点的采集时间。但实际使用时，往往用一个变量存储最后一个点的绝对时间，其它点存储的是离该点的相对时间。

二、[sensor::PointCloudWithIntensities](std::vector<TimedRangefinderPoint>)points

struct TimedRangefinderPoint {
  // 该点的3D坐标。2D点云时，z总是0，x、y满足sqrt(x*x + y*y)=R，tan(y/x)=Theta。
  Eigen::Vector3f position;
  // 该点的时间戳，单位秒。
  float time;
};

using TimedPointCloud = std::vector<TimedRangefinderPoint>;
struct PointCloudWithIntensities {
  TimedPointCloud points;
  std::vector<float> intensities;
};

在SensorBridge::HandleLaserScanMessage(...)，会调用ToPointCloudWithIntensities(*msg)，把sensor_msgs::LaserScan转成PointCloudWithIntensities类型。转换后，点云存放在points字段，数据结构是std::vector<TimedRangefinderPoint>，该std::vector有一个别名：TimedPointCloud。

template 
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;
  // 留住的点中，初始角度是msg.angle_min。
  float angle = msg.angle_min;
  for (size_t i = 0; i < msg.ranges.size(); ++i) {
    const auto& echoes = msg.ranges[i];
    if (HasEcho(echoes)) {
      // echoes就是一个float值，bool HasEcho(float) { return true; }，这个if总是返回true。
      const float first_echo = GetFirstEcho(echoes);
      // first_echo就是msg.ranges[i]。float GetFirstEcho(float range) { return range; }
      if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
        // 留住r在[msg.range_min, msg.range_max]的点。要没意外，就是剔除那些inf点。
        // angle是该点角度，以该角度作为yaw，生成一个旋转向量rotation。
        const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
        // 极坐标转成3D向量。值是rotation * (first_echo, 0, 0)。该向量z
        const cartographer::sensor::TimedRangefinderPoint point{
            rotation * (first_echo * Eigen::Vector3f::UnitX()),
            i * msg.time_increment};
        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);
        }
      }
    }
    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);
    for (auto& point : point_cloud.points) {
      point.time -= duration;
    }
  }
  return std::make_tuple(point_cloud, timestamp);
}

LaserScanToPointCloudWithIntensities执行的转换要解决两个问题：一是哪些点会留在points，二是点是以什么值存放在points。

哪些点会留在points

ranges[i]值需要落在范围[msg.range_min, msg.range_max]，丢掉的一般有两种情况：1）值是inf的点。2）LaserScan.range_min、range_max设置不正确导致丢掉的点。对应图2，原来720点减少到622，少掉的都因为值是inf。

点是以什么值存放在points

points的每个单元包括两个字段：position和time。

• position是个3D向量。2D点云时，z总是0，x、y满足sqrt(x*x + y*y)=R，tan(y/x) = Threta，换句话说，极坐标变换为在xoy平面的直角坐标。
• time。以秒为单元的相对时间，但相对的基点由#1变成了最后一个#N。#N时的time固定是0，点索引变小时，time值是渐小的负数。

timestamp是个绝对时间，语义是点云中最后那个点的采集时刻。值是消息头的时间戳(msg.header.stamp，点云中第一个点的采集时刻)加上#N相对于#1的生成时间间隔。它会做为LaserScanToPointCloudWithIntensities(...)返回值，对应图2中的变量“time”。这个时刻很重要，称为零时刻。

三、[sensor::TimedPointCloud]points

在SensorBridge::HandleLaserScan()，会把sensor::PointCloudWithIntensities转成sensor::TimedPointCloud类型。转换后，点云存放在points字段。

从PointCloudWithIntensities看到，sensor::TimedPointCloud是它的一个字段points，此次转化是不是把它提取出来单独形成一个独立变量？让进入HandleLaserScan。

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_;
    // num_subdivisions_per_laser_scan_ == 1时，以下这语句等于以points.points复值构造出subdivision。
    carto::sensor::TimedPointCloud subdivision(
        points.points.begin() + start_index, points.points.begin() + end_index);
    if (start_index == end_index) {
      continue;
    }
    // num_subdivisions_per_laser_scan_ == 1时，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.
    // num_subdivisions_per_laser_scan_ == 1时，
    // subdivision_time就是LaserScanToPointCloudWithIntensities算出的timestamp，即零时刻。
    const carto::common::Time subdivision_time =
        time + carto::common::FromSeconds(time_to_subdivision_end);
    ...
    sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
    // num_subdivisions_per_laser_scan_ == 1时，time_to_subdivision_end值是0，for循环不影响point值。
    for (auto& point : subdivision) {
      point.time -= time_to_subdivision_end;
    }
    // num_subdivisions_per_laser_scan_ == 1时，
    // subdivision就是从PointCloudWithIntensities把TimedPointCloud抽出来单独形成一个独立变量：subdivision。
    // subdivision_time则是零时刻。
    HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
  }
}

num_subdivisions_per_laser_scan_是个配置变量，默认值1。值是1时，subdivision就是从PointCloudWithIntensities把TimedPointCloud抽出来单独形成一个独立变量：subdivision。subdivision_time则等于零时刻。

分段处理目的：尽量减小运动对测量匹配的影响，尤其是对于速度/频率比很高的。当然，运动速度很快的就需要高频率的雷达。

四、[sensor::TimedPointCloudData](TimedPointCloud)ranges

struct TimedPointCloudData {
  common::Time time;
  Eigen::Vector3f origin;
  TimedPointCloud ranges;
  // 'intensities' has to be same size as 'ranges', or empty.
  std::vector<float> intensities;
};

在SensorBridge::HandleRangefinder，会调用sensor::TransformTimedPointCloud(...)，把TimedPointCloud中点云进行坐标变换。变换后，点云放在TimedPointCloudData的rangess字段，数据结构仍是std::vector<TimedRangefinderPoint>，该vector另有一个别称：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));
  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>())});
  }
}

首先要理解HandleRangefinder操作的背景。到目前为止，TimedPointCloud中存储的是直角坐标，其坐标系是slam传感器的坐标系，这个坐标系的frame_id来自传感器发自消息中的msg->header.frame_id，由传感器驱动定义，一般叫“laser”。cartographer另定义了一个坐标系，frame_id取自配置变量tracking_frame，HandleRangefinder操作目的就是把坐标值从“laser”坐标系变换到<tracking_frame>坐标系下的坐标。由于要发生坐标变换，点云坐标值可能发生改变，但存储格式不会变，还是Eigen::Vector3f。

在不少场景，像rose_ros，tracking_frame变量值是“base_footprint”，变量sensor_to_tracking存储着laser-->base_footprint的tf。也就是说，如果sensor_to_tracking不是0，这次转换会改变坐标值。

固名思义，base_footprint语义是底盘坐标系、机器人坐标系，这坐标系原点和雷达原点往往有着固定关系。rose_ros用发布静态tf发法让cartographer知道laser-->base_footprint的tf。示例：translation(0, 0, 0.098)、rotation(0, 0, 0)。可这么设想这机器人结构：激光雷达放在机器人中央，但雷达距离地面9.8厘米。有了sensor_to_tracking后，便可用TimedPointCloudData{...}生成各字段。

• time：time，零时刻。
• origin：(tracking_frame_坐标系)“laser”坐标系原点在tracking_frame_坐标系下的坐标。在示例，tracking_frame_是“base_footprint”坐标系时，这个值(0, 0, 0.098)。
• ranges：(tracking_frame_坐标系)点云数据在tracking_frame_坐标系下的坐标。即sensor_to_tracking乘以之前得到的TimedPointCloud类型的ranges。

至此点云存放在一个TimedPointCloudData类型的变量中，作为参数传入LocalTrajectoryBuilder2D::AddRangeData。图3显示了sensor::TimedPointCloudData，高亮的变量“time”就是LaserScanToPointCloudWithIntensities(...)算出的timestamp，即零时刻。

五、[sensor::TimedPointCloudOriginData](std::vector<RangeMeasurement>)ranges

ecstruct TimedPointCloudOriginData {
  struct RangeMeasurement {
    // 该点坐标、时间戳。
    TimedRangefinderPoint point_time;
    float intensity;
    // 该点用了“origins”中哪个原点，对单一、有序场景，既然origins.size()总是1，那它只能是0。
    size_t origin_index;
  };
  common::Time time;
  // ranges中点云涉及哪些坐标系，origins存储着这些坐标系原点在tracking_frame_坐标系下的坐标。
  // 针对单一、有序场景，传感器只涉及一个坐标系，像“laser”，所以origins.size()总是1，值是TimedPointCloudData.origin。
  std::vector<Eigen::Vector3f> origins;
  std::vector<RangeMeasurement> ranges;
};

LocalTrajectoryBuilder2D::AddRangeData，会调用range_data_collator_.AddRangeData(...)，把TimedPointCloudData中点云进行变换。变换后，变量synchronized_data类型是TimedPointCloudOriginData，点云放在TimedPointCloudOriginData的ranges字段，数据结构是std::vector<RangeMeasurement>。

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data) {

  auto synchronized_data =
      range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
  if (synchronized_data.ranges.empty()) {
    LOG(INFO) << "Range data collator filling buffer.";
    return nullptr;
  }
  ...
}

说下这个操作背景。设备可能有N(N > 1)个点云传感器，收到点云时，每个传感器放在自已的TimedPointCloudData，意味着在[current_start_, current_end_]这段间隔内，须要把这N个TimedPointCloudData融合成一个点云，TimedPointCloudOriginData就用于表示这个生成点云的数据结构。这N个传感器有着自个的“laser”坐标系，转换到tracking_frame_坐标系有着自已的原点，正如名称所示，TimedPointCloudData、TimedPointCloudOriginData主要差别在于原点字段。

range_data_collator_是RangeDataCollator类的对象，该类主要功能是执行多传感器时同步融合点云数据。“cartographer 代码思想解读（8）- 多激光传感器同步融合RangeDataCollator”有关于RangeDataCollator::AddRangeData的详细注释。这里只给个结论：对单一、有序场景，其产生的TimedPointCloudOriginData是TimedPointCloudData的原样复制。

怎么复制？因为laser坐标系个数要从1变成N，相比输入TimedPointCloudData，作为输出的TimedPointCloudOriginData，表示laser坐标系原点的字段名从“origin”变成了“origins”，类型从Eigen::Vector3f变成std::vector<Eigen::Vector3f>。针对单一、有序场景，origins.size()总是1，值是TimedPointCloudData.origin，即“laser”坐标系原点在tracking_frame_坐标系下的坐标。在示例，tracking_frame_是“base_footprint”，这个值(0, 0, 0.098)。

两种类型中的“ranges”字段，虽然类型不一样了，但针对每个点，只是后者多了个origin_index字段，作用是指示该点用了“origins”中哪个原点，对单一、有序场景，既然origins.size()总是1，那它只能是0。至于存储坐标值、时间戳，都用了TimedRangefinderPoint，值原样复制。

LocalTrajectoryBuilder2D::AddRangeData得到TimedPointCloudOriginData类型点云数据后，将用它计算位姿，以及建图。