cartographer探祕第四章之代碼解析（二） --- 傳感器數據處理過程

本次閱讀的源碼爲 release-1.0 版本的代碼

https://github.com/googlecartographer/cartographer_ros/tree/release-1.0

https://github.com/googlecartographer/cartographer/tree/release-1.0

也可以下載我上傳的 全套工作空間的代碼，包括 protobuf, cartographer, cartographer_ros, ceres,

https://download.csdn.net/download/tiancailx/11224156

 

第一篇文章簡要分析了下 carto的代碼是如何使用的，以及如何進行SLAM的。這篇文章將進雷達數據處理流程的分析。

 

在看 trajectory_builder_interface.h 之前需要了解幾個數據類型。

cartographer/mapping/submaps.h

// An individual submap, which has a 'local_pose' in the local map frame, keeps
// track of how many range data were inserted into it, and sets the
// 'finished_probability_grid' to be used for loop closing once the map no
// longer changes.
// 一個單獨的子圖，在local map frame中有一個'local_pose'，跟蹤插入到其中的範圍數據的數量，
// 並設置'finished_probability_grid'用於在地圖不再更改時 loop closing。
class Submap {
 public:
  Submap(const transform::Rigid3d& local_submap_pose)
      : local_pose_(local_submap_pose) {}

// ...

 private:
  const transform::Rigid3d local_pose_;
  int num_range_data_ = 0;
  bool finished_ = false;
};

cartographer/mapping/trajectory_node.h

struct TrajectoryNodePose {
  struct ConstantPoseData {
    common::Time time;
    transform::Rigid3d local_pose;
  };
  // The node pose in the global SLAM frame.
  transform::Rigid3d global_pose;

  common::optional<ConstantPoseData> constant_pose_data;
};

struct TrajectoryNode {
  struct Data {
    common::Time time;

    // Transform to approximately gravity align the tracking frame as
    // determined by local SLAM.
    Eigen::Quaterniond gravity_alignment;

    // Used for loop closure in 2D: voxel filtered returns in the
    // 'gravity_alignment' frame.
    sensor::PointCloud filtered_gravity_aligned_point_cloud;

    // Used for loop closure in 3D.
    sensor::PointCloud high_resolution_point_cloud;
    sensor::PointCloud low_resolution_point_cloud;
    Eigen::VectorXf rotational_scan_matcher_histogram;

    // The node pose in the local SLAM frame.
    transform::Rigid3d local_pose;
  };

  common::Time time() const { return constant_data->time; }

  // This must be a shared_ptr. If the data is used for visualization while the
  // node is being trimmed, it must survive until all use finishes.
  std::shared_ptr<const Data> constant_data;

  // The node pose in the global SLAM frame.
  transform::Rigid3d global_pose;
};

cartographer/mapping/trajectory_builder_interface.h

// This interface is used for both 2D and 3D SLAM. Implementations wire up a
// global SLAM stack, i.e. local SLAM for initial pose estimates, scan matching
// to detect loop closure, and a sparse pose graph optimization to compute
// optimized pose estimates.
class TrajectoryBuilderInterface {
 public:
  struct InsertionResult {
    NodeId node_id;
    std::shared_ptr<const TrajectoryNode::Data> constant_data;
    std::vector<std::shared_ptr<const Submap>> insertion_submaps;
  };

  // A callback which is called after local SLAM processes an accumulated
  // 'sensor::RangeData'. If the data was inserted into a submap, reports the
  // assigned 'NodeId', otherwise 'nullptr' if the data was filtered out.
  using LocalSlamResultCallback =
      std::function<void(int /* trajectory ID */, common::Time,
                         transform::Rigid3d /* local pose estimate */,
                         sensor::RangeData /* in local frame */,
                         std::unique_ptr<const InsertionResult>)>;

SensorId

type 是傳感器類型，id 是就是ros 的　話題，多個相同傳感器時在　話題　名字後加　_1。

  struct SensorId {
    enum class SensorType {
      RANGE = 0,
      IMU,
      ODOMETRY,
      FIXED_FRAME_POSE,
      LANDMARK,
      LOCAL_SLAM_RESULT
    };

    SensorType type;
    std::string id;
｝

數據處理過程　AddSensorData()：

trajectory_builder->AddSensorData(kRangeSensorId.id, measurement);

cartographer/mapping/internal/collated_trajectory_builder.h

// cartographer/mapping/internal/collated_trajectory_builder.cc
class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface {
// ...
}

CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
    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),
      trajectory_id_(trajectory_id),
      wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
      last_logging_time_(std::chrono::steady_clock::now()) {
  std::unordered_set<std::string> expected_sensor_id_strings;
  for (const auto& sensor_id : expected_sensor_ids) {
    expected_sensor_id_strings.insert(sensor_id.id);
  }
  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));
      });

// cartographer/sensor/collator_interface.h
using Callback =
      std::function<void(const std::string&, std::unique_ptr<Data>)>;

  // Adds a trajectory to produce sorted sensor output for. Calls 'callback'
  // for each collated sensor data.
  virtual void AddTrajectory(
      int trajectory_id,
      const std::unordered_set<std::string>& expected_sensor_ids,
      const Callback& callback) = 0;


// cartographer/sensor/internal/collator.cc

void Collator::AddTrajectory(
    const int trajectory_id,
    const std::unordered_set<std::string>& expected_sensor_ids,
    const Callback& callback) {
  for (const auto& sensor_id : expected_sensor_ids) {
    const auto queue_key = QueueKey{trajectory_id, sensor_id};
    queue_.AddQueue(queue_key,
                    [callback, sensor_id](std::unique_ptr<Data> data) {
                      callback(sensor_id, std::move(data));
                    });
    queue_keys_[trajectory_id].push_back(queue_key);
  }
}

// cartographer/sensor/internal/ordered_multi_queue.cc
// 添加一個關鍵詞是key的隊列,並用比較函數Callback排序
void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) {
  CHECK_EQ(queues_.count(queue_key), 0);
  queues_[queue_key].callback = std::move(callback);
}

sensor_collator->AddTrajectory() 的參數爲：1 一個數據包就是一個trajectory_id，跑定位時會有２個id，多機器人SLAM會有很多個id， 2 expected_sensor_ids_strings 就是所有的　topic ，3 傳入回調函數的函數名 - HandleCollatedSensorData() 。

調用了trajectory_builder_interface.h的AddSensorData()方法，這個方法是在collated_trajectory_builder.h　進行了具體的實現，它將　trajectory_id　和　傳感器的數據　傳入到隊列中，cartographer/sensor/internal/ordered_multi_queue.h　這個文件聲明瞭隊列的定義. ordered_multi_queue.h 具體的說明請去　參考文獻３進行查看，我就不多說了。

// cartographer/mapping/trajectory_builder_interface.h
  virtual void AddSensorData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data) = 0;
  virtual void AddSensorData(const std::string& sensor_id,
                             const sensor::ImuData& imu_data) = 0;
  virtual void AddSensorData(const std::string& sensor_id,
                             const sensor::OdometryData& odometry_data) = 0;
  virtual void AddSensorData(
      const std::string& sensor_id,
      const sensor::FixedFramePoseData& fixed_frame_pose) = 0;
  virtual void AddSensorData(const std::string& sensor_id,
                             const sensor::LandmarkData& landmark_data) = 0;

// cartographer/mapping/internal/collated_trajectory_builder.h
// 傳感器的數據　data 是通過一個模板類　cartographer/sensor/internal/dispatchable.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));
  }

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

// cartographer/sensor/internal/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));
}

// cartographer/sensor/internal/ordered_multi_queue.cc
// 根據key找到隊列,並添加data元素, 調用一次Add()就要調用一次Dispatch()
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();
}

/*
OrderedMultiQueue，用於管理多個有序的傳感器數據，
是有序的多隊列類,每個隊列有一個key,並且有一個自定義排序函數
queues_的形式爲：
key1:Queue
key2：Queue
key3：Queue

Queue的形式爲
  struct Queue {
    common::BlockingQueue<std::unique_ptr<Data>> queue;
    Callback callback;
    bool finished = false;
  };

OrderedMultiQueue的數據成員有
1,common_start_time_per_trajectory_:軌跡id及對應創建軌跡時間
2,last_dispatched_time_
3,std::map<QueueKey, Queue> queues_;按照key排序的map
4,QueueKey blocker_;

--------------------- 
作者：slamcode 
來源：CSDN 
原文：https://blog.csdn.net/learnmoreonce/article/details/76218106 
版權聲明：本文爲博主原創文章，轉載請附上博文鏈接！
*/

也就是說，隊列的個數爲：每個軌跡所訂閱的話題數。如下，２個軌跡，軌跡１訂閱２個話題，軌跡２訂閱１個話題，那我的隊列的map就只有３個key，也就是３個隊列。

  // These are keys are chosen so that they sort first, second, third.
  const QueueKey kFirst{1, "a"};
  const QueueKey kSecond{1, "b"};
  const QueueKey kThird{2, "b"};

 

總結：每次數據到來，根據，調用trajectory_builder_interface.h的AddSensorData()，再調用ordered_multi_queue.cc的Add()方法，再調用OrderedMultiQueue::Dispatch()方法，Dispatch()方法使用　回調函數CollatedTrajectoryBuilder::HandleCollatedSensorData()　處理　根據數據的時間已經排好序的數據隊列。

// cartographer/mapping/internal/collated_trajectory_builder.cc
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());
}

// 也就是跑carto時候的消息：
// [ INFO]: collated_trajectory_builder.cc:72] imu rate: 10.00 Hz 1.00e-01 s +/- 4.35e-05 s (pulsed at 100.44% real time)
// [ INFO]: collated_trajectory_builder.cc:72] scan rate: 19.83 Hz 5.04e-02 s +/- 4.27e-05 s (pulsed at 99.82% real time)



// cartographer/sensor/internal/dispatchable.h
  void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
    trajectory_builder->AddSensorData(sensor_id_, data_);
  }

之後在通過CollatedTrajectoryBuilder::HandleCollatedSensorData() 方法將這個　已經排好序的數據隊列集合　傳回　Local SLAM.　那這是怎麼傳的呢？又看了好久纔看懂這個轉換。

首先，先進入到 map_builder.cc　中。這是整體SLAM的處理過程。

cartographer/mapping/map_builder.cc

// cartographer/mapping/map_builder.cc
int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback) {
  const int trajectory_id = trajectory_builders_.size();
  
  if (options_.use_trajectory_builder_3d()) {
...
} else {
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder2D>(
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }
    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
    trajectory_builders_.push_back(
        // 看這裏
        common::make_unique<CollatedTrajectoryBuilder>(
            sensor_collator_.get(), trajectory_id, expected_sensor_ids,
            CreateGlobalTrajectoryBuilder2D(
                std::move(local_trajectory_builder), trajectory_id,
                static_cast<PoseGraph2D*>(pose_graph_.get()),
                local_slam_result_callback)));
...}

AddTrajectoryBuilder()　方法就是一個整體SLAM的初始化方法。

首先我們這裏只看2d SLAM 的部分，也就是在上述代碼中標記的　看這裏　。　這塊代碼執行了如下功能，首先，將CollatedTrajectoryBuilder() 進行初始化，並且傳入　sensor_collator_.get(), trajectory_id　和　expected_sensor_ids　參數。expected_sensor_ids是調用AddTrajectoryBuilder()時的參數輸入，代表着所有傳感器話題的集合。同時，也傳入了一個　CreateGlobalTrajectoryBuilder2D() 的函數名，這是GlobalTrajectoryBuilder類的構造函數，直接傳入函數名就是傳入函數的地址，而正好CollatedTrajectoryBuilder()構造函數的第四個參數是個指針 std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder .  之後又使用wrapped_trajectory_builder進行右值轉換後對 wrapped_trajectory_builder_進行賦值。

之後使用HandleCollatedSensorData()將這個指針指向的數據傳入到 data的AddToTrajectoryBuilder()　方法中，也就是將GlobalTrajectoryBuilder類的函數指針（？？？對不對，是傳入地址指針還是傳入什麼）傳入進去。

由於GlobalTrajectoryBuilder類是繼承於mapping::TrajectoryBuilderInterface的，所以指針的類型是 std::unique_ptr<TrajectoryBuilderInterface> 。

之後在dispatchable.h　文件中的　AddToTrajectoryBuilder() 方法　調用　GlobalTrajectoryBuilder類下的AddSensorData().

// cartographer/mapping/internal/collated_trajectory_builder.cc
void CollatedTrajectoryBuilder::HandleCollatedSensorData() {
  //  ...
  data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
}

// cartographer/sensor/internal/dispatchable.h
  void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
    trajectory_builder->AddSensorData(sensor_id_, data_);
  }

GlobalTrajectoryBuilder類下的AddSensorData() 將進行　local slam　的掃描匹配，下篇文章再講。

references

1. https://blog.csdn.net/liuxiaofei823/article/details/70207814

2. https://blog.csdn.net/roadseek_zw/article/details/66968762

3. https://blog.csdn.net/u012209790/article/details/82735923

4. https://blog.csdn.net/learnmoreonce/article/details/76218106

5. https://blog.csdn.net/learnmoreonce/article/details/73718535