C++ Cartographer源碼中關(guān)于Sensor的數(shù)據(jù)走向深扒

更新時間：2023年03月30日 11:21:01 作者：蝦眠不覺曉,

這篇文章主要介紹了C++ Cartographer源碼中關(guān)于Sensor的數(shù)據(jù)走向，整個Cartographer源碼閱讀是很枯燥的, 但絕對是可以學(xué)到東西的，文中通過示例代碼介紹的非常詳細(xì)，對大家的學(xué)習(xí)或者工作具有一定的參考學(xué)習(xí)價值，需要的朋友們下面隨著小編來一起學(xué)習(xí)吧

前言
Node類的HandleLaserScanMessage函數(shù)
SensorBridge類的HandleLaserScanMessage函數(shù)
CollatedTrajectoryBuilder類的AddSensorData函數(shù)
Collator類的AddSensorData函數(shù)
OrderedMultiQueue類的Add函數(shù)
BlockingQueue類的Push函數(shù)
OrderedMultiQueue類的Dispatch函數(shù)
CollatedTrajectoryBuilder類的HandleCollatedSensorData函數(shù)
總結(jié)

前言

整個Cartographer源碼閱讀是很枯燥的, 但絕對是可以學(xué)到東西的! 堅持每天記錄, 加油!

在上一節(jié)我們詳細(xì)了解了MapBuilder類, 發(fā)現(xiàn)其構(gòu)造函數(shù), 以及AddTrajectory中有使用到SensorBridge這個類還有sensor_collator_這個變量, 并且似乎是用這個類進(jìn)行傳感器數(shù)據(jù)的傳遞的. 當(dāng)然啦, 如果想建立一個完整的軌跡(Trajectory)和SLAM功能, 我們肯定需要有傳感器的數(shù)據(jù)灌入的.

在MapBuilder的入口類-MapBuilderBridge中, 可以看到一個變量sensor_bridges_

std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;

sensor_bridges_存儲了一系列SensorBridge類的實例, 并且在MapBuilderBridge::AddTrajectory

的第二步使用, 作用是為當(dāng)前軌跡添加一個SensorBridge.

所以這一節(jié)我們重點看看SensorBridge這個類. 我們以最重要的sensor類-LaserScan為例子

Node類的HandleLaserScanMessage函數(shù)

我們都知道, ros中軟實時的程序數(shù)據(jù)的入口都是從subscriber的回調(diào)函數(shù)開始. 之前已經(jīng)講過了Node的LaunchSubscriber函數(shù), 用來專門啟動所有傳感器的訂閱. 咱們就看看Node類的HandleLaserScanMessage:

// 調(diào)用SensorBridge的傳感器處理函數(shù)進(jìn)行數(shù)據(jù)處理
void Node::HandleLaserScanMessage(const int trajectory_id,
                                  const std::string& sensor_id,
                                  const sensor_msgs::LaserScan::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);
  // 根據(jù)配置,是否將傳感器數(shù)據(jù)跳過
  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
    return;
  }
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLaserScanMessage(sensor_id, msg);
}

我們可以看到, 他最終是調(diào)用了MapBuilderBridge類的sensor_bridge的成員函數(shù)-HandleLaserScanMessage來處理傳入的傳感器的數(shù)據(jù).

SensorBridge類的HandleLaserScanMessage函數(shù)

咱們再去SensorBridge中看看, 以下是主要代碼部分

// 處理LaserScan數(shù)據(jù), 先轉(zhuǎn)成點云,再傳入trajectory_builder_
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);
}
void SensorBridge::HandleRangefinder(
    const std::string& sensor_id, const carto::common::Time time,
    const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
    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>())} ); // 強(qiáng)度始終為空
    }
}

前幾節(jié)也講過這部分SensorBridge::HandleLaserScanMessage調(diào)用了SensorBridge::HandleRangefinder, 把carto::sensor::TimedPointCloudData這個數(shù)據(jù)類型和sensor_id通過trajectory_builder_的AddSensorData把點云類型傳遞給CollatedTrajectoryBuilder的AddSensorData. 為啥是CollatedTrajectoryBuilder的AddSensorData呢?這塊我也想了很久. 咱們先去sensor_bridge.h中看

::cartographer::mapping::TrajectoryBuilderInterface* const
trajectory_builder_;

發(fā)現(xiàn)是TrajectoryBuilderInterface, 這明顯是個父類啊,沒啥意義. 咱們回到SensorBridge的構(gòu)造函數(shù)

/**
 * @brief 構(gòu)造函數(shù), 并且初始化TfBridge
 * 
 * @param[in] num_subdivisions_per_laser_scan 一幀數(shù)據(jù)分成幾次發(fā)送
 * @param[in] tracking_frame 數(shù)據(jù)都轉(zhuǎn)換到tracking_frame
 * @param[in] lookup_transform_timeout_sec 查找tf的超時時間
 * @param[in] tf_buffer tf_buffer
 * @param[in] trajectory_builder 軌跡構(gòu)建器
 */
SensorBridge::SensorBridge(
    const int num_subdivisions_per_laser_scan,
    const std::string& tracking_frame,
    const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
    carto::mapping::TrajectoryBuilderInterface* const trajectory_builder)
    : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
      tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
      trajectory_builder_(trajectory_builder) {}

發(fā)現(xiàn)這個trajectory_builder_是SensorBridge構(gòu)造函數(shù)的最后一個參數(shù), 那么這個SensorBridge是在哪構(gòu)造的呢? 是在map_builder_bridge.cc中, MapBuilderBridge::AddTrajectory的第二步sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>. 我們看到最后一個參數(shù)是

map_builder_->GetTrajectoryBuilder(trajectory_id)

這個map_builder_是MapBuilderBridge構(gòu)造函數(shù)map_builder_(std::move(map_builder)), 而這個map_builder也是父類定義,沒啥參考價值

std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder

再向前回溯, 看MapBuilderBridge是咋構(gòu)造的, 發(fā)現(xiàn)是Node的構(gòu)造函數(shù)就構(gòu)造了map_builder_bridge_

map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer)

又要往前回溯, 在node_main.cc中

 auto map_builder = cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
 Node node(node_options, std::move(map_builder), &tf_buffer, FLAGS_collect_metrics);

發(fā)現(xiàn)map_builder是cartographer::mapping::CreateMapBuilder給的. 咱們再進(jìn)CreateMapBuilder, 發(fā)現(xiàn)其只是一個工廠函數(shù)

std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
    const proto::MapBuilderOptions& options) {
  return absl::make_unique<MapBuilder>(options);
}

而這個工廠函數(shù)實例化了MapBuilder這個類, 所以map_builder_->GetTrajectoryBuilder(trajectory_id)調(diào)用的是MapBuilder的GetTrajectoryBuilder. (有一種峰回路轉(zhuǎn)的感覺), 返回trajectory_builders_的軌跡id為trajectory_id的指針,即:

  mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder(
      int trajectory_id) const override {
    return trajectory_builders_.at(trajectory_id).get();
  }

而trajectory_builders_在map_builder.cc中被壓入absl::make_unique<CollatedTrajectoryBuilder>, 如下

trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        // 將3D前端與3D位姿圖打包在一起, 傳入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder3D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph3D*>(pose_graph_.get()),
            local_slam_result_callback, pose_graph_odometry_motion_filter)));

所以最終SensorBridge的trajectory_builder_實際上是CollatedTrajectoryBuilder的地址, 所以SensorBridge的trajectory_builder_的AddSensorData實際上是把數(shù)據(jù)添加到了CollatedTrajectoryBuilder里面, 而不是GlobalTrajectoryBuilder或者LocalTrajectoryBuilder(這三個TrajectoryBuilder都繼承于TrajectoryBuilderInterface)

這塊地方難就難在子類可以用父類代替, 搞不清到底是調(diào)用的哪個子類的成員函數(shù).

CollatedTrajectoryBuilder類的AddSensorData函數(shù)

既然上面調(diào)用的是CollatedTrajectoryBuilder, 那咱們看看CollatedTrajectoryBuilder這個類的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));
}
void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
    sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}

再看看sensor::MakeDispatchable, 在dispatchable.h文件中

// 根據(jù)傳入的data的數(shù)據(jù)類型,自動推斷DataType, 實現(xiàn)一個函數(shù)處理不同類型的傳感器數(shù)據(jù)
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);
}

這個函數(shù)通過模板, 實現(xiàn)了一個函數(shù)處理多個類型, 也就是說可以用一個函數(shù)去分發(fā)上到激光雷達(dá),下到IMU的數(shù)據(jù), 值得學(xué)習(xí).

CollatedTrajectoryBuilder::AddData又調(diào)用sensor_collator_->AddSensorData, 用std::move(data), 把data移動給AddSensorData, 給某個Trajectory加入傳感器數(shù)據(jù). 而這個sensor_collator_定義如下:

sensor::CollatorInterface* const sensor_collator_;

又是用父類代替子類, 在CollatedTrajectoryBuilder的構(gòu)造函數(shù)中實現(xiàn)實例化, 這個sensor_collator_實際上是sensor::Collator, 原因是在map_builder.cc中的MapBuilder構(gòu)造函數(shù)中有如下一段程序

  // 在 cartographer/configuration_files/map_builder.lua 中設(shè)置
  // param: MAP_BUILDER.collate_by_trajectory 默認(rèn)為false
  if (options.collate_by_trajectory()) {
    sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>();
  } else {
    // sensor_collator_初始化, 實際使用這個
    sensor_collator_ = absl::make_unique<sensor::Collator>();
  }

一般collate_by_trajectory設(shè)置為false, 所以是absl::make_unique<sensor::Collator>, 即sensor的Collator

  // sensor::Collator的初始化
  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)); //傳遞給GlobalTrajectoryBuilder類相應(yīng)的函數(shù)
      });

Collator類的AddSensorData函數(shù)

咱們進(jìn)到collator這個類中看看, 發(fā)現(xiàn)這個類繼承于CollatorInterface, 再看看collator這個類的AddSensorData

// 向數(shù)據(jù)隊列中添加 傳感器數(shù)據(jù) 
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));
}

作用是向隊列中添加傳感器數(shù)據(jù), 啥是隊列?以后將在線程池部分詳細(xì)說說. 現(xiàn)在簡單看看

queue_是Cartographer的任務(wù)隊列, 用于線程池多任務(wù)序列的儲存與處理.

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

也就是說Collator::AddSensorData負(fù)責(zé)把data放在任務(wù)隊列中等待處理并賦一個key, 并不負(fù)責(zé)處理數(shù)據(jù), 所以咱們再往前看看, 看一下OrderedMultiQueue這個類的關(guān)于添加數(shù)據(jù)的成員函數(shù)-Add

OrderedMultiQueue類的Add函數(shù)

OrderedMultiQueue這個類定義在ordered_multi_queue.cc中, 添加數(shù)據(jù)是在Add成員函數(shù)實現(xiàn)的:

// 向數(shù)據(jù)隊列中添加數(shù)據(jù)
void OrderedMultiQueue::Add(const QueueKey& queue_key,
                            std::unique_ptr<Data> data) {
  auto it = queues_.find(queue_key);
  // 如果queue_key不在queues_中, 就忽略data
  if (it == queues_.end()) {
    LOG_EVERY_N(WARNING, 1000)
        << "Ignored data for queue: '" << queue_key << "'";
    return;
  }
  // 向數(shù)據(jù)隊列中添加數(shù)據(jù)
  it->second.queue.Push(std::move(data));
  // 傳感器數(shù)據(jù)的分發(fā)處理
  Dispatch();
}

可以發(fā)現(xiàn)Add就是生產(chǎn)者, 用于生成并傳遞可用數(shù)據(jù).

Dispatch()這個成員函數(shù)負(fù)責(zé)數(shù)據(jù)分發(fā), 將處于數(shù)據(jù)隊列中的數(shù)據(jù)根據(jù)時間依次傳入回調(diào)函數(shù). 這個后面再看, 咱們先看看it->second.queue.Push(std::move(data));這個部分.

it這個變量就是queues_最后一個數(shù)據(jù),可以理解為最新的一個數(shù)據(jù), 而OrderedMultiQueue的queue_和上一小節(jié)提到的Collator是不同的, 在OrderedMultiQueue中的queue_是定義為一個std::map

std::map<QueueKey, Queue> queues_; // 多個數(shù)據(jù)隊列

所以it->second就是Queue, 而Queue是個定義在OrderedMultiQueue的結(jié)構(gòu)體

  struct Queue {
    common::BlockingQueue<std::unique_ptr<Data>> queue;   // 存儲數(shù)據(jù)的隊列
    Callback callback;                                    // 本數(shù)據(jù)隊列對應(yīng)的回調(diào)函數(shù)
    bool finished = false;                                // 這個queue是否finished
  };

Push 也就是把data壓入Queue這個結(jié)構(gòu)體中,然后生成map形成個對列. 而這個Push不是push_back, 這個Push是Cartographer自己定義的一種壓棧方法. 定義在blocking_queue.h中,如下...

BlockingQueue類的Push函數(shù)

  // Pushes a value onto the queue. Blocks if the queue is full.
  // 將值壓入隊列. 如果隊列已滿, 則阻塞
  void Push(T t) {
    // 首先定義判斷函數(shù)
    const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
      return QueueNotFullCondition();
    };
    // absl::Mutex的更多信息可看: https://www.jianshu.com/p/d2834abd6796
    // absl官網(wǎng): https://abseil.io/about/
    // 如果數(shù)據(jù)滿了, 就進(jìn)行等待
    absl::MutexLock lock(&mutex_);
    mutex_.Await(absl::Condition(&predicate));
    // 將數(shù)據(jù)加入隊列, 移動而非拷貝
    deque_.push_back(std::move(t));
  }

發(fā)現(xiàn)Push作用相當(dāng)于阻塞者, 使用了mutex_.Await和鎖用來阻塞數(shù)據(jù)傳入. 看看QueueNotFullCondition這個函數(shù)就一目了然了. 當(dāng)隊列為無限大或者小于queue_size_的時候返回true.

  // Returns true iff the queue is not full.
  // 如果隊列未滿, 則返回true
  bool QueueNotFullCondition() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
    return queue_size_ == kInfiniteQueueSize || deque_.size() < queue_size_;
  }

除了阻塞作用, 最大的所用就是把數(shù)據(jù)壓入deque_, 咱們再看看這個deque_

template <typename T>
...
std::deque<T> deque_ GUARDED_BY(mutex_);

發(fā)現(xiàn)它是std::deque這個基礎(chǔ)類型,類型決定于模板T, GUARDED_BY(mutex_)表示這個數(shù)據(jù)在使用的時候必須要上鎖, 否則就會報錯.

所以Push的作用就是有阻塞作用的push_back, 負(fù)責(zé)把data壓入OrderedMultiQueue的queue_, 并且在隊列滿的時候阻塞.

OrderedMultiQueue類的Dispatch函數(shù)

咱們再回到OrderedMultiQueue類的Add函數(shù)的Dispatch中, 看看Dispatch函數(shù)有關(guān)數(shù)據(jù)分發(fā)的部分

void OrderedMultiQueue::Dispatch() {
    while (true) {
        const Data* next_data = nullptr;
        Queue* next_queue = nullptr;
        QueueKey next_queue_key;
        // 遍歷所有的數(shù)據(jù)隊列, 找到所有數(shù)據(jù)隊列的第一個數(shù)據(jù)中時間最老的一個數(shù)據(jù)
        for (auto it = queues_.begin(); it != queues_.end();) {
            const auto* data = it->second.queue.Peek<Data>();
        } // end for
        // 正常情況, 數(shù)據(jù)時間都超過common_start_time
        if (next_data->GetTime() >= common_start_time) {
            last_dispatched_time_ = next_data->GetTime();
            // 將數(shù)據(jù)傳入 callback() 函數(shù)進(jìn)行處理,并將這個數(shù)據(jù)從數(shù)據(jù)隊列中刪除
            next_queue->callback(next_queue->queue.Pop());
        }
    }
}

Peek是取出隊列最前面的一個數(shù)據(jù). callback定義在頭文件中, 這個是std::function封裝的一個函數(shù)

using Callback = std::function<void(std::unique_ptr<Data>)>;

而Callback這個函數(shù)到底是啥呢? 這個要看OrderedMultiQueue::AddQueue這個成員函數(shù)

void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) {
  CHECK_EQ(queues_.count(queue_key), 0);
  queues_[queue_key].callback = std::move(callback);
}

我們看到這個函數(shù)把參數(shù)傳入的callback傳入queues_的callback. 那么是誰調(diào)用的OrderedMultiQueue的AddQueue這個函數(shù)呢? 是Collator的AddTrajectory調(diào)用的!

我們在回溯到Collator這個類看看Collator的AddTrajectory成員函數(shù)

/**
 * @brief 添加軌跡以生成排序的傳感器輸出, 每個topic設(shè)置一個回調(diào)函數(shù)
 * 
 * @param[in] trajectory_id 新生成的軌跡的id
 * @param[in] expected_sensor_ids 需要排序的topic名字的集合
 * @param[in] callback 2個參數(shù)的回調(diào)函數(shù), 實際是CollatedTrajectoryBuilder::HandleCollatedSensorData()函數(shù)
 */
void Collator::AddTrajectory(
    const int trajectory_id,
    const absl::flat_hash_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,
                    // void(std::unique_ptr<Data> data) 帶了個默認(rèn)參數(shù)sensor_id
                    [callback, sensor_id](std::unique_ptr<Data> data) {
                      callback(sensor_id, std::move(data));
                    });
    queue_keys_[trajectory_id].push_back(queue_key);
  }
}

我們看到它調(diào)用了queue_的AddQueue, 而queue_就是OrderedMultiQueue的實例化 ,所以這里的AddQueue是OrderedMultiQueue的AddQueue. Callback又是個lambda函數(shù)

[callback, sensor_id](std::unique_ptr<Data> data) {
     callback(sensor_id, std::move(data));
}

這個lambda函數(shù)調(diào)用的是傳入的callback函數(shù), 而這個Collator::AddTrajectory是誰調(diào)用的呢?實際上是CollatedTrajectoryBuilder. 在CollatedTrajectoryBuilder的構(gòu)造函數(shù)中就實現(xiàn)了Collator這個類的初始化, 并且調(diào)用了AddTrajectory這個函數(shù)

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_->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)); //傳遞給GlobalTrajectoryBuilder類相應(yīng)的函數(shù)
    });
}

所以傳入的參數(shù)是是HandleCollatedSensorData這個函數(shù).

CollatedTrajectoryBuilder類的HandleCollatedSensorData函數(shù)

這個函數(shù)才是真正的消費者. 看一下HandleCollatedSensorData傳入sensor data的部分:

void CollatedTrajectoryBuilder::HandleCollatedSensorData(
    const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
    // 將排序好的數(shù)據(jù)送入 GlobalTrajectoryBuilder中的AddSensorData()函數(shù)中進(jìn)行使用
    data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
}

這個函數(shù)的作用是處理按照時間順序分發(fā)的傳感器數(shù)據(jù), 在進(jìn)去到Data的AddToTrajectoryBuilder里看看

Data這個類又是個基類, 里面有個純虛函數(shù)

  virtual void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *trajectory_builder) = 0;

這個基類只有一個子類: Dispatchable. 進(jìn)到這個子類中去看看AddToTrajectoryBuilder.

  // 調(diào)用傳入的trajectory_builder的AddSensorData()
  void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
    trajectory_builder->AddSensorData(sensor_id_, data_);
  }

所以這里的trajectory_builder指的就是CollatedTrajectoryBuilder::HandleCollatedSensorData中調(diào)用的wrapped_trajectory_builder_. 而這個wrapped_trajectory_builder_是啥呢?這又要回溯到CollatedTrajectoryBuilder的初始構(gòu)造中去, 在map_builder.cc中實現(xiàn)

trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>( 
        trajectory_options, sensor_collator_.get(), trajectory_id, 
        expected_sensor_ids,
        // 將2D前端與2D位姿圖打包在一起, 傳入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder2D(  //全局軌跡構(gòu)建器
                                          //CreateGlobalTrajectoryBuilder2D是global_trajectory_builderd的方法,
                                          //繼承自TrajectoryBuilderInterface,和CollatedTrajectoryBuilder一個父類
            std::move(local_trajectory_builder), //前端構(gòu)建器
            trajectory_id, //
            static_cast<PoseGraph2D*>(pose_graph_.get()), //后端位姿圖
            local_slam_result_callback, pose_graph_odometry_motion_filter)));

我們看到wrapped_trajectory_builder_實際上是CreateGlobalTrajectoryBuilder2D, 這個在上回也說到就是GlobalTrajectoryBuilder這個類CreateGlobalTrajectoryBuilder2D, 返回Cartographer的前端和后端.

到這里, 整個Cartographer的傳感器數(shù)據(jù)傳遞過程也就明了了.

從GlobalTrajectoryBuilder2D開始, 數(shù)據(jù)才真正走到SLAM的前端與后端部分.

總結(jié)

Cartographer中傳感器數(shù)據(jù)的傳入從Node類的HandleXXXMessage成員函數(shù)開始, 傳遞給SensorBridge類, 然后調(diào)用CollatedTrajectoryBuilder把數(shù)據(jù)給到Collator類, 由Collator進(jìn)行消費這模式的處理, 然后再返回給CollatedTrajectoryBuilder完成Cartographer的整個前后端.

中間的處理用到了很多父子類的互相調(diào)用, 一層套一層, 十分復(fù)雜, 要認(rèn)真看才能懂里面的數(shù)據(jù)流.

到此這篇關(guān)于C++ Cartographer源碼中關(guān)于Sensor的數(shù)據(jù)走向深扒的文章就介紹到這了,更多相關(guān)C++ Sensor數(shù)據(jù)走向內(nèi)容請搜索腳本之家以前的文章或繼續(xù)瀏覽下面的相關(guān)文章希望大家以后多多支持腳本之家！

您可能感興趣的文章: