cartographer_learn_7

续接上一篇

在了解完后Collator类后我们继续往前端算法的那个方向走，回顾本系列的第4篇的前端构造那一节，我们发现这个3d和2d的情况构建的前端是不一样的，我们先来讨论较为简单的2d，这样好理解一点（其实就是作者不学无术。。。）。在map_builder.h文件中可以看到前端的类型是mapping::TrajectoryBuilderInterface，又是***Interface，想必又是一个接口类，进去看看发现果然又是一堆纯虚函数。但是这里定义了两个结构InsertionResult和SensorId，希望读者留意，相信我们以后还有机会讨论它们。前端实际的类型是cartographer::mapping::CollatedTrajectoryBuilder，在文件src/cartographer/cartographer/mapping/internal/collated_trajectory_builder.h中，接下来我们就来瞧一瞧这个类

CollatedTrajectoryBuilder的类成员

class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface {
......//一堆类方法
private:
......//一堆私有方法
  sensor::CollatorInterface* const sensor_collator_;
  const bool collate_landmarks_;
  const bool collate_fixed_frame_;
  const int trajectory_id_;
  std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder_;

  // Time at which we last logged the rates of incoming sensor data.
  std::chrono::steady_clock::time_point last_logging_time_;
  std::map<std::string, common::RateTimer<>> rate_timers_;
}

第一个类成员是耳熟能详的Collator的接口，相信经过第5，6篇的瞎扯，大家对它并不陌生。接下来是两个布尔变量，看起来和Collator有关。再接下来是一个智能指针unique_ptr< TrajectoryBuilderInterface >，据说它就是真正的前端。

CollatedTrajectoryBuilder类方法

先忽略该类的构造函数（有点难），我们看到.h文件中重载了很多个叫AddSensorData的函数，且它们最终都调用了一个叫AddData的函数，那先来看看这个AddData这个函数

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

可以看到其实他是调用Collator中的AddSensorData（具体见第6篇）。
再来看AddSensorData这个函数，由于是重载且大部分操作都类似，我们这里只贴出两个来举例

// 雷达点云数据
  void AddSensorData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data) override {
    AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
  }
  // IMU数据
  void AddSensorData(const std::string& sensor_id,
                     const sensor::ImuData& imu_data) override {
    AddData(sensor::MakeDispatchable(sensor_id, imu_data));
  }

可以看到再调用AddData前还调用了一个sensor::MakeDispatchable，这是什么呢？
这个得回到第5篇，第5篇中得数据队列是使用一个是std::unique_ptr< Data >类型来表示数据，那就奇怪了，各种传感器得到得数据类型得数据结构都一样吗？当然不是，那怎么表示各种类型的数据呢？Google的大神们给出的做法是，先定义一个叫Data的基类

class Data {
 public:
  explicit Data(const std::string &sensor_id) : sensor_id_(sensor_id) {}
  virtual ~Data() {}

  virtual common::Time GetTime() const = 0;
  const std::string &GetSensorId() const { return sensor_id_; }
  virtual void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *trajectory_builder) = 0;

 protected:
  const std::string sensor_id_;
};

再定义一个模板类Dispatchable继承Data，这样又可以用基类指向不同派生类了

template <typename DataType>
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_;
};

//它就是MakeDispatchable
template <typename DataType>
std::unique_ptr<Dispatchable<DataType>> MakeDispatchable(
    const std::string &sensor_id, const DataType &data) {
  return absl::make_unique<Dispatchable<DataType>>(sensor_id, data);
}

再来看看它的构造函数，有点绕，先上代码

CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
    const proto::TrajectoryBuilderOptions& trajectory_options,
    sensor::CollatorInterface* const sensor_collator, const int trajectory_id,
    const std::set<SensorId>& expected_sensor_ids,
    std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
    : sensor_collator_(sensor_collator),
      collate_landmarks_(trajectory_options.collate_landmarks()),
      collate_fixed_frame_(trajectory_options.collate_fixed_frame()),
      trajectory_id_(trajectory_id),
      wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
      last_logging_time_(std::chrono::steady_clock::now()) {
  absl::flat_hash_set<std::string> expected_sensor_id_strings;
  for (const auto& sensor_id : expected_sensor_ids) {
    if (sensor_id.type == SensorId::SensorType::LANDMARK &&
        !collate_landmarks_) {
      continue;
    }
    if (sensor_id.type == SensorId::SensorType::FIXED_FRAME_POSE &&
        !collate_fixed_frame_) {
      continue;
    }
    expected_sensor_id_strings.insert(sensor_id.id);
  }
//关键看那个lambda表达式
  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));
      });
}

一开始是一大堆初始化列表，可以看到再本片上一节中提到的两个bool变量是构建轨迹的一些选项，前面都是正常的遍历所有订阅的话题的名字expected_sensor_ids，并根据两个bool变量选择是否是不接受Landmark和gps这两种话题传过来的数据。并把需要的话题的名字放入 expected_sensor_id_strings中，最后调用了Collator中的AddTrajectory（详细见第6篇）。
注意那个回调函数，在第6篇的AddTrajectory中我们能看到那个回调函数其实就是各种数据的处理函数，话说不一样的数据就应该是不一样的处理函数，这里怎么就一个函数能？
去HandleCollatedSensorData中看看：

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

这个函数前面一点还是有点不太懂，不确定目前的理解是否正确，不正确往后再改。先判断一下是不是第一次处理这种数据，如果是在rate_timers_中插入对应的topic的名字，如果不是就更新一个和频率相关的东西（没看懂。。。），最后调用Data::AddToTrajectoryBuilder。但这个AddToTrajectoryBuilder真正的调用是Dispatchable::AddToTrajectoryBuilder，这样不同的数据对应着不同的模板类，不同的模板类自然调用的函数就不一样了。
好了这个类的讨论就到这，很遗憾，还是没有到具体的算法部分，相信真正的算法很快就来了。