cartographer 源码解析 (三)

分手后的思念是犯贱 2022-08-31 05:56 301阅读 0赞

## 相关链接： ##

#### [cartographer 源码解析(一)][cartographer] ####

#### [cartographer 源码解析(二)][cartographer 1] ####

#### [cartographer 源码解析(三)][cartographer 2] ####

#### [cartographer 源码解析(四)][cartographer 3] ####

#### [cartographer 源码解析(五)][cartographer 4] ####

#### [cartographer 源码解析(六)][cartographer 5] ####

今天讲一下数据格式转换以及数据的同步的具体的过程，首先以激光数据为例子，看一下在node.cc里面的激光的回调函数。  
**node.cc**

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

逐行讲解一下：

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

最好是把**ros**格式的消息送到数据转换器中，去转换成**cartographer**可以识别的类型。  
让**map\_builder\_bridge**里面的数据格式转化器**sensor\_bridge** 处理**ros**格式的消息。然后我们进一步去看**HandleLaserScanMessage**函数具体如何实现的。  
**sensor\_bridge.cc**

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

下面进行逐行解析

carto::sensor::PointCloudWithIntensities point_cloud;

首先是在另一个项目**cartographer**的命名空间种定义了一个传感器的**PointCloudWithIntensities**类型的**point\_cloud**。我们再去看看carto命名空间的**PointCloudWithIntensities**数据类型是怎么定义的，不过从字面意思上看是带有强度信息的点云。  
**point\_cloud.h**

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

其中`TimedPointCloud points;`中的**TimedPointCloud**可以看到被定义为`using TimedPointCloud = std::vector<TimedRangefinderPoint>;` 另一个是以及每一个点的强度信息的**intensities**。而**TimedPointCloud**是一个带有时间戳的**TimedRangefinderPoint**类型的容器。那么我们再看看**TimedRangefinderPoint**的数据结构是这么定义的吧。定义如下：  
**rangefinder\_point.h**

struct TimedRangefinderPoint {
        
      Eigen::Vector3f position;
      float time;
    };

他是由定义的三维的向量position，以及浮点类型的time。

现在回溯一下，我们知道了**PointCloudWithIntensities**是什么类型的数据格式了，那么我们怎么去把ros消息类型的数据转化成**PointCloudWithIntensities**数据类型呢？我们就要看一下**ToPointCloudWithIntensities**这个函数了。  
**msg\_conversion.cc**

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

他实际上是调用了**LaserScanToPointCloudWithIntensities**这个函数。我们再来看看这个函数的具体实现。  
**msg\_conversion.cc**

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];
        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()),
                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);
    }

逐行解析

CHECK_GE(msg.range_min, 0.f);
    CHECK_GE(msg.range_max, msg.range_min);

这两个检查的意思是 **msg.range\_min** 一定要是大于0的  
而 **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);
      }

如果点与点之间的角度增量是大于0的，那么**msg.angle\_max**一定要大于**msg.angle\_min**。  
小于零，那么就是那么**msg.angle\_min**一定要大于**msg.angle\_max**。

其中，msg.**angle\_min**指的是第一个点的角度，**msg。angle\_max**是最后一个点的角度。

PointCloudWithIntensities point_cloud;

接着是定义了这个变量，一直忘这个变量里面填充东西。最后是把**point\_cloud**带着时间戳给返回出去。

float angle = msg.angle_min;

这是获得第一个点的角度。

for (size_t i = 0; i < msg.ranges.size(); ++i) {
        
       ...
        angle += msg.angle_increment;
      }

这是遍历所有的点，操作完成之后，我们要操作下一个点，所以angle要增加一下。

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());

定义一个旋转向量**rotation**，以Z轴为旋转轴，angle为旋转角度得到的旋转向量。

first_echo * Eigen::Vector3f::UnitX()

首先是一个测距值**first\_echo**乘以一个X轴方向的单位长度的向量。得到一个沿着x轴方向长度为**first\_echo**的向量，那么我们实际的向量可能不是在x轴上，需要一个角度去旋转到该点所在的向量方向上，首先可以确定的是旋转轴是**z轴**。只要乘以旋转向量就能获得旋转的**x,y,z**点的坐标。

rotation * (first_echo * Eigen::Vector3f::UnitX())

const cartographer::sensor::TimedRangefinderPoint point{
        
                rotation * (first_echo * Eigen::Vector3f::UnitX()),
                i * msg.time_increment};

最后获得了带有时间戳的**Point**。

point_cloud.points.push_back(point);

将该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);
            }

最后再将点云的强度信息也放入到cartographer定义的点云的强度信息中。

下一步较为重要，是从msg中获得时间戳

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

他把ros里面的时间戳的格式转化成cartographer里面的时间戳格式。其中ros中的msg的header中的时间戳是以点云的第一个点的时间戳为当前帧的时间戳。而cartographer中是以最后一个点的时间戳为当前帧的时间戳。  
所以这里需要转换一下。

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;

第一个点的时间戳+ 总时长等于最后一个点的时间戳，也就是**cartographer**的时间戳。  
然后每一个点的时间戳都要减去这个总时长，处理之后就获得了cartographer想要的点云数据。

return std::make_tuple(point_cloud, timestamp);

最后把点云给返回回去。

这里我们再回溯一下，这里我们就把ros格式的点云数据，转换成了cartographer想要的点云数据了。  
**sensor\_bridge.cc**

得到了我们想要的点云，再把点云放到这个**HandleLaserScan**函数里面进一步处理

HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);

HandleLaserScan的实现如下：  
**sensor\_bridge.cc**

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);
        HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
      }
    }

逐行讲解如下：

if (points.points.empty()) {
        
        return;
      }

首先判断点云不为空

CHECK_LE(points.points.back().time, 0.f);

**CHECK\_LE**是小于等于，所以是判断points.points.back().time是否是小于等于0  
我们前面说过，每一个都有一个相对时间戳，第一个点相对时间戳是负值，是最小的，最后一个点的相对时间戳是0。所以这个检查是没有问题的，肯定小于等于0。

for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
        
     ....
     }

这个是将激光雷达的一帧点云数据切分成好几块，防止激光点云数据太多无法处理，同时可以方便处激光点云的畸变，方便点云畸变矫正。因为我们用的是`backpack_2d.launch`这个文件，所有我们找到**backpack\_2d.lua**这个文件查看相关的参数设置。  
**backpack\_2d.lua**

include "map_builder.lua"
    include "trajectory_builder.lua"
    
    options = {
        
      map_builder = MAP_BUILDER,
      trajectory_builder = TRAJECTORY_BUILDER,
      map_frame = "map",
      tracking_frame = "base_link",
      published_frame = "base_link",
      odom_frame = "odom",
      provide_odom_frame = true,
      publish_frame_projected_to_2d = false,
      use_pose_extrapolator = true,
      use_odometry = false,
      use_nav_sat = false,
      use_landmarks = false,
      num_laser_scans = 0,
      num_multi_echo_laser_scans = 1,
      num_subdivisions_per_laser_scan = 10,
      num_point_clouds = 0,
      lookup_transform_timeout_sec = 0.2,
      submap_publish_period_sec = 0.3,
      pose_publish_period_sec = 5e-3,
      trajectory_publish_period_sec = 30e-3,
      rangefinder_sampling_ratio = 1.,
      odometry_sampling_ratio = 1.,
      fixed_frame_pose_sampling_ratio = 1.,
      imu_sampling_ratio = 1.,
      landmarks_sampling_ratio = 1.,
    }
    
    MAP_BUILDER.use_trajectory_builder_2d = true
    TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10
    
    return options

我们可以看到，`num_subdivisions_per_laser_scan = 10,`他把一帧激光点云数据分成了十份。

然后我们再看for循环里面的内容  
**sensor\_bridge.cc**

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;

然后我们再逐行解析吧。

const size_t start_index =
            points.points.size() * i / num_subdivisions_per_laser_scan_;

当**i**等于0的时候，那么**start\_index**也是0  
当**i**等于1的时候，那么 **start\_index**是`points.points.size() /10`;  
当**i**等于2的时候，那么 **start\_index**是`（points.points.size() /10）*2`;  
当**i**等9的时候，那么 **start\_index**是`（points.points.size() /10）9`;  
可以明白，其实就是把点分割成了10块，每个**start\_index**都是点云块起始地址。

const size_t end_index =
            points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;

而每一块的结束地址，就是下一块的起始地址，所有就是 `(i + 1)`。

carto::sensor::TimedPointCloud subdivision(
            points.points.begin() + start_index, points.points.begin() + end_index);

然后将该点云块从起始位置到结束位置的那一小块点云分配给**TimedPointCloud**类型的**subdivision**。  
**points.points.begin()** 是整个一帧点云其实的位置。前面说的都是相对位置，有了起始位置，有相对位置，加在一起就是绝对位置。

if (start_index == end_index) {
        
          continue;
        }

如果起始位置和结束位置相同，那么就跳过继续循环。

const double time_to_subdivision_end = subdivision.back().time;

每一小块的点云数据的时间戳就是该小块最后的那个点的时间戳跟整个一帧点云时间戳的差。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70]

const carto::common::Time subdivision_time =
            time + carto::common::FromSeconds(time_to_subdivision_end);

**time**是一帧点云的最后一个点的时间戳，也就是该帧点云的时间戳，加上这个相对时间戳，就是该分块的时间戳。

auto it = sensor_to_previous_subdivision_time_.find(sensor_id);

一般看**sensor\_to\_previous\_subdivision\_time\_**，里有个**to** 一般都是map类型的。我们点进去看看。  
**sensor\_bridge.h**

std::map<std::string, cartographer::common::Time>
          sensor_to_previous_subdivision_time_;

一看传感器，果然是`map`类型的。`string` 是传感器的`id`以及对应的时间`Time`。`previous_subdivision_time_`翻译过来就是上一块点云分块记录的时间戳。

auto it = sensor_to_previous_subdivision_time_.find(sensor_id);

根据传感器的`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;
        }

如果找到了，而且找的时间戳比我们当前子块的时间戳要晚。  
如果你有两个激光雷达，同时发出的激光雷达的话题一样，会来回的交叠。然后我们就需要提示一个**Warning**。

sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;

一切都处理完之后，把当前子块的时间戳记录下来。

for (auto& point : subdivision) {
        
          point.time -= time_to_subdivision_end;
        }

一开始我们的每一帧激光数据的每个点都是相对于那一帧激光最后一个点的时间戳的相对时间戳。而我们现在把激光分成了好多个子块，那么每一个子块的点都要变成相应的，对应于当前子块的最后一个点的时间戳的相对时间戳。  
得到了分块之后，就可以把子块放到这个**HandleRangefinder**函数中去处理

CHECK_EQ(subdivision.back().time, 0.f);
        HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);

然后我们再去检查`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));
      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>())});
      }
    }

逐行解析：

if (!ranges.empty()) {
        
        CHECK_LE(ranges.back().time, 0.f);
      }

首先，要求这个传感器的时间戳要求小于等于0。这个我们分析过了，是没有问题的。

const auto sensor_to_tracking =
          tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));

我们实际要跟踪的坐标系上去。将雷达的坐标系转移到base\_link上去。![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 1]  
我们获得的点的坐标都是以激光雷达为坐标系原点的坐标系。而我们建图定位都是基于车辆坐标系进行的，这里就要求我们变换激光雷达的坐标系变成车体坐标系。

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

如果找到的话，就把数据传送到`trajectory_builder_`中，让他去添加传感器数据。我们回忆一下，我们的**sensor\_bridge**在创建的时候，我们获得了\*\*Collated\_trajectory\_builder\_\*\*的地址,这个是在`cartographer`的源码下，我们可以去看一下。  
**collated\_trajectory\_builder.cc**

void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
        
      sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
    }

**collated\_trajectory\_builder.h**

void AddSensorData(
          const std::string& sensor_id,
          const sensor::TimedPointCloudData& timed_point_cloud_data) override {
        
        AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
      }

把数据传送进来，进行转换你啊，然后再送到**AddData**里面。**MakeDispatchable**是干什么的呢？就是以数据为实参，然后将数据转移到Dispatchable这个类里面。

讲到这里，我们就可以继续研究一下这个类到底是怎么回事。

class Dispatchable : public Data {
        
     public:
      Dispatchable(const std::string &sensor_id, const DataType &data)
          : Data(sensor_id), data_(data) {
        }
    
      common::Time GetTime() const override {
         return data_.time; }
      void AddToTrajectoryBuilder(
          mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
        
        trajectory_builder->AddSensorData(sensor_id_, data_);
      }
      const DataType &data() const {
         return data_; }
    
     private:
      const DataType data_;
    };

这个类除了接收数据本身`data_`，还有就是自带了一个函数`AddToTrajectoryBuilder`。我们将来会把这个数据送到sensorCollected,将这Dispatchable送到数据收集器，数据同步器中。同步之后了干嘛呢？就要分发，分发的英文就是Dispatch。

回溯之前的，就是**collated\_trajectory\_builder.h**的`AddSensorData` 里面的`AddDate`。

**collacted\_trajectory\_builder.cc**

void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
        
      sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
    }

发现果然是`sensor_collator_`里面的。`sensor_collator_`里面有很多个队列，每一个队列对应一种传感器数据。 而且队列里面都有`callback`。开始的时候我们使用`sensor_collaoter` 去管一条轨迹。我们会涉及到该传感器对应的队列。给每一个队列分发一个`callback`。`callback`就是拉姆达表达式。分发完给了谁呢？

sensor_collator_->AddTrajectory(
          trajectory_id, expected_sensor_id_strings,
          [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
        
            HandleCollatedSensorData(sensor_id, std::move(data));
          });

就是给了`HandleCollatedSensorData(sensor_id, std::move(data));` 这里。

然后经过同步之后，数据就给了这里。

void CollatedTrajectoryBuilder::HandleCollatedSensorData(
        const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
        
      auto it = rate_timers_.find(sensor_id);
      if (it == rate_timers_.end()) {
        
        it = rate_timers_
                 .emplace(
                     std::piecewise_construct, std::forward_as_tuple(sensor_id),
                     std::forward_as_tuple(
                         common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)))
                 .first;
      }
      it->second.Pulse(data->GetTime());
    
      if (std::chrono::steady_clock::now() - last_logging_time_ >
          common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
        
        for (const auto& pair : rate_timers_) {
        
          LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();
        }
        last_logging_time_ = std::chrono::steady_clock::now();
      }
    
      data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
    }
    
    }  // namespace mapping

数据data 被封装好了之后，调用它里面的成员函数。

data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());

经过数据同步和数据发送，接下来就是将轨迹数据传给`global_trajector_builder`。也就是`wrapped_trajectory_builder_.get()`。

接下来详细介绍，数据是怎么进行同步，数据收集吧！

我们看到collator.cc类型的**sensor\_collator**  
**collated\_trajectory\_builder.cc**

void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
        
      sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
    }

根据定义的时候，是根据用户的配置去定义的。

MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
        : options_(options), thread_pool_(options.num_background_threads()) {
        
    ......
      if (options.collate_by_trajectory()) {
        
        sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>();
      } else {
        
        sensor_collator_ = absl::make_unique<sensor::Collator>();
      }
    }

我们以通常的例子去讲解，也就是Collator类型去讲解。

sensor_collator_ = absl::make_unique<sensor::Collator>();

我们去`collator.cc`里面去看。

void Collator::AddSensorData(const int trajectory_id,
                                 std::unique_ptr<Data> data) {
        
      QueueKey queue_key{
        trajectory_id, data->GetSensorId()};
      queue_.Add(std::move(queue_key), std::move(data));
    }

怎么去添加的呢，怎么去做数据同步的呢？  
首先我们添加了一个**queue\_key**，这个**queue\_key** 是什么呢？有很多个队列。每一个队列都存储了传感器的一种数据。每个队列都有自己的名字，也就是`queue_key`。将`queue_key`，`data`放到`queue_`里面去。这`queue_`是什么呢？我们可以进去看一下。

// Queue keys are a pair of trajectory ID and sensor identifier.
      OrderedMultiQueue queue_;

可以看到是有序的多个队列。那就是一种传感器一种队列。  
我们再看看`queue_.Add`做了什么？

void OrderedMultiQueue::Add(const QueueKey& queue_key,
                                std::unique_ptr<Data> data) {
        
      auto it = queues_.find(queue_key);
      if (it == queues_.end()) {
        
        LOG_EVERY_N(WARNING, 1000)
            << "Ignored data for queue: '" << queue_key << "'";
        return;
      }
      it->second.queue.Push(std::move(data));
      Dispatch();
    }

再具体看看ordered\_multi\_queue.h  
**ordered\_multi\_queue.h**

class OrderedMultiQueue {
        
     public:
     ...
      std::map<QueueKey, Queue> queues_;
      QueueKey blocker_;
    };

他这里面存储了很多的`queue`。每个queue都有自己的名字`QueueKey`。还有其队列本身`Queue`。

逐行解析，  
**ordered\_muti\_queue.cc**

auto it = queues_.find(queue_key);

我们从队列里面找到对应的队列。例如我们要找到存储scan的队列。如果没有找到的话。

if (it == queues_.end()) {
        
        LOG_EVERY_N(WARNING, 1000)
            << "Ignored data for queue: '" << queue_key << "'";
        return;
      }

如果没有找到`scan`这个队列的话，我们就报告`warning`。然后就return。没问题的话我们就把他push到存储队列里面。

it->second.queue.Push(std::move(data));

添加了之后，我们就开始做分发，这一步非常重要。

Dispatch();

这是做传感器数据同步，最关键的一个函数。  
翠花，上代码！  
**ordered\_multi\_queue.cc**

void OrderedMultiQueue::Dispatch() {
        
      while (true) {
        
        const Data* next_data = nullptr;
        Queue* next_queue = nullptr;
        QueueKey next_queue_key;
        for (auto it = queues_.begin(); it != queues_.end();) {
        
          const auto* data = it->second.queue.Peek<Data>();
          if (data == nullptr) {
        
            if (it->second.finished) {
        
              queues_.erase(it++);
              continue;
            }
            CannotMakeProgress(it->first);
            return;
          }
          if (next_data == nullptr || data->GetTime() < next_data->GetTime()) {
        
            next_data = data;
            next_queue = &it->second;
            next_queue_key = it->first;
          }
          CHECK_LE(last_dispatched_time_, next_data->GetTime())
              << "Non-sorted data added to queue: '" << it->first << "'";
          ++it;
        }
        if (next_data == nullptr) {
        
          CHECK(queues_.empty());
          return;
        }
    
        // If we haven't dispatched any data for this trajectory yet, fast forward
        // all queues of this trajectory until a common start time has been reached.
        const common::Time common_start_time =
            GetCommonStartTime(next_queue_key.trajectory_id);
    
        if (next_data->GetTime() >= common_start_time) {
        
          // Happy case, we are beyond the 'common_start_time' already.
          last_dispatched_time_ = next_data->GetTime();
          next_queue->callback(next_queue->queue.Pop());
        } else if (next_queue->queue.Size() < 2) {
        
          if (!next_queue->finished) {
        
            // We cannot decide whether to drop or dispatch this yet.
            CannotMakeProgress(next_queue_key);
            return;
          }
          last_dispatched_time_ = next_data->GetTime();
          next_queue->callback(next_queue->queue.Pop());
        } else {
        
          // We take a peek at the time after next data. If it also is not beyond
          // 'common_start_time' we drop 'next_data', otherwise we just found the
          // first packet to dispatch from this queue.
          std::unique_ptr<Data> next_data_owner = next_queue->queue.Pop();
          if (next_queue->queue.Peek<Data>()->GetTime() > common_start_time) {
        
            last_dispatched_time_ = next_data->GetTime();
            next_queue->callback(std::move(next_data_owner));
          }
        }
      }
    }

开始逐行讲解

void OrderedMultiQueue::Dispatch() {
      while (true) {
       ......
      }
    }

首先，`while ture` 说明是对这么多序列循环做处理的。

const Data* next_data = nullptr;
        Queue* next_queue = nullptr;
        QueueKey next_queue_key;

然后是定义了几个变量， **next\_data** 不要理解成下一个。就把他理解成头一个，马上要处理的数据。  
比如说我们有很多队列，比如`odom imu scan`。

for (auto it = queues_.begin(); it != queues_.end();) {
        
          const auto* data = it->second.queue.Peek<Data>();
          ...
        }

for (auto it = queues_.begin(); it != queues_.end();) {
        
      ...
     }

遍历所有队列。

首先第一个队列，我们要看他的**Peek**, **Peek**是什么？我们点进去看一下

const R* Peek() {
        
        absl::MutexLock lock(&mutex_);
        if (deque_.empty()) {
        
          return nullptr;
        }
        return deque_.front().get();
      }

通过`return deque_.front().get();`我们知道是把队列最前的数据取出来并返回回去。

if (data == nullptr) {
        
          ...
          }

如果数据`data`是空的，代表最早队列里面什么也没有。

if (it->second.finished) {
        
              queues_.erase(it++);
              continue;
            }
            CannotMakeProgress(it->first);
            return;
          }

这个情况下，我们就不使用这个队列了，也就不适用这个topic了。  
我们就把这个队列给擦除。如果不是呢，我们执行`CannotMakeProgress(it->first);`

void OrderedMultiQueue::CannotMakeProgress(const QueueKey& queue_key) {
        
      blocker_ = queue_key;
      for (auto& entry : queues_) {
        
        if (entry.second.queue.Size() > kMaxQueueSize) {
        
          LOG_EVERY_N(WARNING, 60) << "Queue waiting for data: " << queue_key;
          return;
        }
      }
    }

这个`blocker_`是什么呢？我们带着好奇的心又去一探究竟了。  
发现是

QueueKey blocker_;

那QueueKey具体又长什么样子呢？  
**ordered\_multi\_queue.h**

struct QueueKey {
        
      int trajectory_id;
      std::string sensor_id;
    
      bool operator<(const QueueKey& other) const {
        
        return std::forward_as_tuple(trajectory_id, sensor_id) <
               std::forward_as_tuple(other.trajectory_id, other.sensor_id);
      }
    };

当前堵塞的队列是blocker\_， 那么我们继续遍历其他队列。

for (auto& entry : queues_) {
        
        if (entry.second.queue.Size() > kMaxQueueSize) {
        
          LOG_EVERY_N(WARNING, 60) << "Queue waiting for data: " << queue_key;
          return;
        }
      }

那么存着这个一个队列的大小大于我们设置的这个阈值。那么就告诉这个队列，我们都在等你呢。你不来我们无法进行前进。

if (next_data == nullptr || data->GetTime() < next_data->GetTime()) {
        
            next_data = data;
            next_queue = &it->second;
            next_queue_key = it->first;
    }

如果说，我们第一次进了这个while ，那么next\_data 刚开始就赋值为空，还有就是获得的当前数据的data的时间早于next\_data的时间，那么next\_data 相关的都被赋值为data相关的。

CHECK_LE(last_dispatched_time_, next_data->GetTime())
              << "Non-sorted data added to queue: '" << it->first << "'";
          ++it;

这个CHECK我们暂时先不管。我们继续看下一个队列。再次遇到上面的比时间的时候，谁的时间早，就先处理谁。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 2]  
比如说我们先开始读取`imu`的数据，获得了**imu**最早的数据，然后再读取`odom`的数据，发现**odom**最早的数据比**imu**还要早，那么继续遍历下一个队列的数据，也就是发现`scan`的数据的时间比其他两个都要早，那么最先处理的数据就是**scan**的数据。

继续往下看

if (next_data == nullptr) {
        
          CHECK(queues_.empty());
          return;
        }

如果数据next\_data为空，说明所有队列里面都没有数据，并返回。也只有在queues\_是空的情况下才会出现。  
再往下走的时候，我们就找到了所有队列中最早的数据。  
请允许我引用原生的cartographer的注释

> // If we haven’t dispatched any data for this trajectory yet, fast forward  
> // all queues of this trajectory until a common start time has been reached.

意思是说，如果我们还没有往外发任何数据，那么我们在轨迹里面遍历所有的轨迹，找到一个时间点，这个时间点，所有的数据都有了。

const common::Time common_start_time =
            GetCommonStartTime(next_queue_key.trajectory_id);

我们再看看`GetCommonStartTime`是怎么实现的。  
**order\_multi\_queue.cc**

common::Time OrderedMultiQueue::GetCommonStartTime(const int trajectory_id) {
        
      auto emplace_result = common_start_time_per_trajectory_.emplace(
          trajectory_id, common::Time::min());
      common::Time& common_start_time = emplace_result.first->second;
      if (emplace_result.second) {
        
        for (auto& entry : queues_) {
        
          if (entry.first.trajectory_id == trajectory_id) {
        
            common_start_time = std::max(
                common_start_time, entry.second.queue.Peek<Data>()->GetTime());
          }
        }
        LOG(INFO) << "All sensor data for trajectory " << trajectory_id
                  << " is available starting at '" << common_start_time << "'.";
      }
      return common_start_time;
    }

记录每条轨迹的**common\_start\_time**。

`emplace_result`是获得一个结果，什么结果呢，就是插入数据成功或者失败。  
这个第一句就是往这个容器里面插入数据，这个数据长什么样子啊，看下面。  
**order\_multi\_queue.h**

std::map<int, common::Time> common_start_time_per_trajectory_;

**int** 是轨迹的**id** **common::Time**是轨迹的时间。

common::Time& common_start_time = emplace_result.first->second;

`emplace_result.first`指的是插入那个值的迭代器，而`emplace_result.first->second`是指的第二个值，也就是时间。`common_start_time`初始值我们给的是一个最小值。

if (emplace_result.second) {
        
        for (auto& entry : queues_) {
        
        ...
        }
        LOG(INFO) << "All sensor data for trajectory " << trajectory_id
                  << " is available starting at '" << common_start_time << "'.";
      }

如果我们插入成功了，我们就遍历所有的队列。

if (entry.first.trajectory_id == trajectory_id) {
        
            common_start_time = std::max(
                common_start_time, entry.second.queue.Peek<Data>()->GetTime());
          }

遍历队列干嘛呢，找到所有队列中，时间最早的数据，如果大于`common_start_time`，我们就把他设置为`common_start_time`。  
我们设置common\_start\_time 是一个最小值。  
然后找到一个比如首先找到第一个传感器imu最早时间。然后再找到下一个传感器的最早时间，一直遍历到最后一个传感器为止。找到所有时间中最大的。使得所有的传感器都有这个时间，这个时间，所有的传感器的数据类型都有了。这就是GetCommonStartTime的作用。

if (next_data->GetTime() >= common_start_time) {
        
        ...
        }

接下来判断我们传感器里面的最早的数据是否都大于这个`common_start_time`。在这个时间点上，是否所有的数据都有了。

如果有的话，我们直接送出去用就可以了

// Happy case, we are beyond the 'common_start_time' already.
    last_dispatched_time_ = next_data->GetTime();
    next_queue->callback(next_queue->queue.Pop());

待处理的数据所在的队列都有一个`callback`,这个callback实际上是在`collated_trajectory_builder.cc`的拉姆达表达式里面。  
**collated\_trajectory\_builder.cc**

就是送给上面所在的拉姆达表达式去处理。  
就是这样把数据给`pop`掉。`next_queue->callback(next_queue->queue.Pop());`  
那如果不是呢？如果当时的时间点，不是所有的数据都有，那么你会怎么办？

else if (next_queue->queue.Size() < 2) {
        
          if (!next_queue->finished) {
        
            // We cannot decide whether to drop or dispatch this yet.
            CannotMakeProgress(next_queue_key);
            return;
          }
          last_dispatched_time_ = next_data->GetTime();
          next_queue->callback(next_queue->queue.Pop());
        }

逐行解析  
如果待处理的数据数量很少，`next_queue->queue.Size() < 2`  
而且还没有被标志为结束`!next_queue->finished`  
那么就需要等一等`CannotMakeProgress(next_queue_key);`  
给用户警告一下，  
继续等`return;`  
如果已经标志为结束，那么把仅有的数据，也就是1个数据给用掉

last_dispatched_time_ = next_data->GetTime();
    next_queue->callback(next_queue->queue.Pop());

在这时间戳上，没有集齐七颗龙珠（传感器数据），首先把数据给pop掉。然后pop之后呢，我们再看一下，最早的数据，其实是次早的数据，如果集齐了，就把他放出去。

else {
        
          // We take a peek at the time after next data. If it also is not beyond
          // 'common_start_time' we drop 'next_data', otherwise we just found the
          // first packet to dispatch from this queue.
          std::unique_ptr<Data> next_data_owner = next_queue->queue.Pop();
          if (next_queue->queue.Peek<Data>()->GetTime() > common_start_time) {
        
            last_dispatched_time_ = next_data->GetTime();
            next_queue->callback(std::move(next_data_owner));
          }

[cartographer]: https://dqwang.blog.csdn.net/article/details/118539690
[cartographer 1]: https://dqwang.blog.csdn.net/article/details/118881867
[cartographer 2]: https://dqwang.blog.csdn.net/article/details/119023628
[cartographer 3]: https://dqwang.blog.csdn.net/article/details/119154163
[cartographer 4]: https://dqwang.blog.csdn.net/article/details/119845028
[cartographer 5]: https://dqwang.blog.csdn.net/article/details/120352115
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70]: /images/20220829/ace6953f7bf746369572e5f6f2bb46d1.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 1]: /images/20220829/770c8d62d0fb4c0ba3ad5382c64e8e10.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0NDQ3J1bm5lcg_size_16_color_FFFFFF_t_70 2]: /images/20220829/56c631d8e3d84c119f5cf45039736630.png