C++?Cartographer源碼中關(guān)于MapBuilder的聲明與構(gòu)造

更新時(shí)間：2023年03月31日 08:31:42 作者：蝦眠不覺(jué)曉,

這篇文章主要介紹了C++?Cartographer源碼中關(guān)于MapBuilder的聲明與構(gòu)造，前面已經(jīng)談到了Cartographer中添加軌跡的方法和傳感器的數(shù)據(jù)流動(dòng)走向。在添加軌跡的時(shí)候，除了添加位姿估計(jì)器還有采樣器，訂閱回調(diào)函數(shù)之外，最重要的是通過(guò)map_builder_bridge添加了一條軌跡

前面已經(jīng)談到了Cartographer中添加軌跡的方法和傳感器的數(shù)據(jù)流動(dòng)走向。我們注意到，在添加軌跡的時(shí)候，除了添加位姿估計(jì)器還有采樣器，訂閱回調(diào)函數(shù)之外，最重要的是通過(guò)map_builder_bridge添加了一條軌跡，其他的都是為它服務(wù)的。咱們這節(jié)詳細(xì)看看MapBuilder這個(gè)Cartographer的核心。

開(kāi)始一條軌跡

添加軌跡是開(kāi)啟Cartographer的大門(mén). 顧名思義, 添加軌跡就是AddTrajectory. 我們最先接觸到的AddTrajectory函數(shù)是Node類里面的. 這個(gè)函數(shù)除了我們之前詳細(xì)提到的添加傳感器等功能之外,還有一個(gè)核心函數(shù):

  // 調(diào)用map_builder_bridge的AddTrajectory, 添加一個(gè)軌跡
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);

這一行調(diào)用了Map_builder_bridge_的AddTrajectory添加一條軌跡. map_builder_bridge_從何而來(lái)呢?在Node類的構(gòu)造函數(shù)中有個(gè)初始化列表,用于構(gòu)造MapBuilderBridge這個(gè)類.

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

我們看到MapBuilderBridge構(gòu)造函數(shù)有三個(gè)參數(shù),其中一個(gè)(中間一個(gè))就是std::unique_ptr<cartographer::mapping::MapBuilderInterface> ,也就是MapBuilderInterface這個(gè)類的指針. MapBuilderInterface這個(gè)類和MapBuilder是父子關(guān)系, 所以這個(gè)地方實(shí)際上構(gòu)造的是MapBuilder這個(gè)類. 這個(gè)map_builder又是那來(lái)的呢? 他在Cartographer的入口程序: node_main.cc中, 就出現(xiàn)了,

  // MapBuilder類是完整的SLAM算法類
  // 包含前端(TrajectoryBuilders,scan to submap) 與 后端(用于查找回環(huán)的PoseGraph) 
  auto map_builder =
      cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);//在map_builder.cc中實(shí)現(xiàn)，工廠函數(shù)
                                                                                //在這里,實(shí)例化一個(gè)MapBuilder, 而MapBuilder是MapBuilderInterface的子類
                                                                                //MapBuilder的AddTrajectoryBuilder實(shí)例化了CollatedTrajectoryBuilder

CreateMapBuilder這個(gè)函數(shù)是在map_builder.cc中實(shí)現(xiàn):

// 工廠函數(shù),生成接口API
std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
    const proto::MapBuilderOptions& options) {
  return absl::make_unique<MapBuilder>(options);
}

可見(jiàn)CreateMapBuilder就是返回了構(gòu)造好了的MapBuilder類. 而cpp允許子類用父類代替實(shí)現(xiàn)多態(tài), 所以上面用MapBuilderInterface也不會(huì)出錯(cuò).

再回到node_main.cc中的CreateMapBuilder, 這個(gè)意思就是用傳入的選項(xiàng)構(gòu)造一個(gè)MapBuilder類的對(duì)象并返回這個(gè)對(duì)象,然后再到node.cc中, 把node_main.cc中的CreateMapBuilder作為Node的構(gòu)造函數(shù)的變量傳給Node構(gòu)造出map_builder_bridge_這個(gè)變量(MapBuilderBridge類). 在調(diào)用Node的AddTrajectory的時(shí)候, 通過(guò)調(diào)用map_builder_bridge_.AddTrajectory完成一條軌跡的添加.

MapBuilderBridge類的AddTrajectory函數(shù)

回到map_builder_bridge_.AddTrajectory, 下面摘取了 MapBuilderBridge::AddTrajectory主要的部分:

int MapBuilderBridge::AddTrajectory(
    const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
        expected_sensor_ids,
    const TrajectoryOptions& trajectory_options) 
{
    // Step: 1 開(kāi)始一條新的軌跡, 返回新軌跡的id,需要傳入一個(gè)函數(shù)
    const int trajectory_id = map_builder_->AddTrajectoryBuilder(
    expected_sensor_ids, trajectory_options.trajectory_builder_options,
    [this]() {
    OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
    });
    // Step: 2 為這個(gè)新軌跡 添加一個(gè)SensorBridge
    sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
    trajectory_options.num_subdivisions_per_laser_scan,
    trajectory_options.tracking_frame,
    node_options_.lookup_transform_timeout_sec,
    tf_buffer_,
    map_builder_->GetTrajectoryBuilder(trajectory_id)); // CollatedTrajectoryBuilder
    ......
    return trajectory_id;
}

傳入的參數(shù)有一個(gè)std::set<...Sensor_id>類型的變量,std::set是一個(gè)容器,可以簡(jiǎn)單理解為鍵值對(duì),而鍵就是值,值就是鍵.(比較基礎(chǔ)不細(xì)說(shuō)啦).另一個(gè)就是從node_main.cc就跟著我們的TrajectoryOptions. 也就是配置文件讀取的內(nèi)容. 返回值很簡(jiǎn)單,就是新建軌跡的編號(hào). Cartographer允許有多個(gè)軌跡同時(shí)維護(hù),而且后面我們會(huì)發(fā)現(xiàn), Cartographer定位其實(shí)就是把建好的地圖和定位作為兩個(gè)不同的軌跡實(shí)現(xiàn)的, 這個(gè)我們?cè)谶@個(gè)系列的最后會(huì)說(shuō).

咱們看一下expected_sensor_ids的面目,他是一個(gè)結(jié)構(gòu)體, 在Cartographer部分的trajectory_builder_interface.h中實(shí)現(xiàn):

  struct SensorId {
    // c++11: 限域枚舉 enum class 
    enum class SensorType {
      RANGE = 0,
      IMU,
      ODOMETRY,
      FIXED_FRAME_POSE,
      LANDMARK,
      LOCAL_SLAM_RESULT
    };
    SensorType type;  // 傳感器類型
    std::string id;   // topic的名字
    bool operator==(const SensorId& other) const {
      return std::forward_as_tuple(type, id) ==
             std::forward_as_tuple(other.type, other.id);
    }
    bool operator<(const SensorId& other) const {
      return std::forward_as_tuple(type, id) <
             std::forward_as_tuple(other.type, other.id);
    }
  };

其中比較有用的是它規(guī)定了一個(gè)傳感器的類型與一個(gè)對(duì)應(yīng)的topic的名字. 傳感器的類型是一個(gè)限域枚舉(枚舉類). 總之作用就是聯(lián)系topic與topic對(duì)應(yīng)的傳感器類型, 以便后續(xù)維護(hù).

Node的AddTrajectory實(shí)際上是調(diào)用的MapBuilderBridge的AddTrajectory. 咱們看看map_builder_bridge中的AddTrajectory. 這個(gè)函數(shù)分為三個(gè)步驟:

開(kāi)啟一條新軌跡
給新軌跡添加傳感器
保存軌跡的參數(shù)配置

第一步程序如下:

  // Step: 1 開(kāi)始一條新的軌跡, 返回新軌跡的id,需要傳入一個(gè)函數(shù)
  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
      expected_sensor_ids, trajectory_options.trajectory_builder_options,
      // lambda表達(dá)式 local_slam_result_callback_
      [this](const int trajectory_id, 
             const ::cartographer::common::Time time,
             const Rigid3d local_pose,
             ::cartographer::sensor::RangeData range_data_in_local,
             const std::unique_ptr<
                 const ::cartographer::mapping::TrajectoryBuilderInterface::
                     InsertionResult>) {
        // 保存local slam 的結(jié)果數(shù)據(jù) 5個(gè)參數(shù)實(shí)際只用了4個(gè)
        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
      });

這一塊很復(fù)雜,一開(kāi)始看很可能看不透,所以值得仔細(xì)剖析. 在第三部分剖析.

第二部分程序如下:

  // Step: 2 為這個(gè)新軌跡 添加一個(gè)SensorBridge
  sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>( //sensor_bridges_是一個(gè)unorderedmap
      trajectory_options.num_subdivisions_per_laser_scan,
      trajectory_options.tracking_frame,
      node_options_.lookup_transform_timeout_sec, 
      tf_buffer_,
      map_builder_->GetTrajectoryBuilder(trajectory_id)); // map_builder是MapBuilder的實(shí)例化,  在map_builder.h中實(shí)現(xiàn),
                                                          // 返回當(dāng)前軌跡的CollatedTrajectoryBuilder的指針.
                                                          // CollatedTrajectoryBuilder就是前端后端綁在一起的,傳給了SensorBridge

作用是為第一步添加的軌跡, 聯(lián)合一個(gè)傳感器相關(guān)處理與維護(hù)器. 這一塊將在傳感器的數(shù)據(jù)分發(fā)器部分詳細(xì)解讀. 咱們這解主要挖掘MapBuilder相關(guān)部分,而不是Sensor部分.

MapBuilder類的AddTrajectoryBuilder函數(shù)

新建軌跡通過(guò)調(diào)用map_builder的AddTrajectoryBuilder方法, 依舊只摘取重要的部分程序:

/**
 * @brief 創(chuàng)建一個(gè)新的 TrajectoryBuilder 并返回它的 trajectory_id
 * 
 * @param[in] expected_sensor_ids 所有需要的topic的名字的集合
 * @param[in] trajectory_options 軌跡的參數(shù)配置
 * @param[in] local_slam_result_callback 需要傳入的回調(diào)函數(shù)
 *        實(shí)際上是map_builder_bridge.cc 中的 OnLocalSlamResult() 函數(shù)
 * @return int 新生成的軌跡的id
 */
int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback) 
{
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      // local_trajectory_builder(前端)的初始化
      local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>( //建立一個(gè)local_trajectory_builder
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }
    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
    // CollatedTrajectoryBuilder初始化
    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>( //NOTE:MapBuilder::AddTrajectoryBuilder使用的是CollatedTrajectoryBuilder
        trajectory_options, sensor_collator_.get(), trajectory_id, 
        expected_sensor_ids,
        // 將2D前端與2D位姿圖打包在一起, 傳入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder2D(  //全局軌跡構(gòu)建器
                                          //CreateGlobalTrajectoryBuilder2D是global_trajectory_builderd的方法,
                                          //繼承自TrajectoryBuilderInterface,和CollatedTrajectoryBuilder一個(gè)父類
            std::move(local_trajectory_builder), //前端構(gòu)建器
            trajectory_id, //
            static_cast<PoseGraph2D*>(pose_graph_.get()), //后端位姿圖
            local_slam_result_callback, pose_graph_odometry_motion_filter)));
}

先看傳入的參數(shù). 傳入的有三個(gè)參數(shù), 前面兩個(gè)為Sensor_id和配置參數(shù), 咱們之前都詳細(xì)說(shuō)到過(guò).

重點(diǎn)要看的是最后一個(gè)參數(shù). 最后一個(gè)傳入的參數(shù)是函數(shù)指針,也就是一個(gè)回調(diào)函數(shù)的地址(c++的基礎(chǔ)內(nèi)容), 在上級(jí)直接使用的是一個(gè)lambda函數(shù), lambda表達(dá)式我覺(jué)得可以定義為"一次性函數(shù)", 這里不深入討論了. 這個(gè)函數(shù)的類型的具體實(shí)現(xiàn)還是在trajectory_builder_interface中:

  // 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, //返回值為空, 5個(gè)參數(shù)列表
                         transform::Rigid3d /* local pose estimate */,
                         sensor::RangeData /* in local frame */,
                         std::unique_ptr<const InsertionResult>)>; //InsertionResult保存了地圖的具體數(shù)據(jù),柵格值

咱們一一把這個(gè)函數(shù)和他的lambda表達(dá)式對(duì)照著看看

LocalSlamResultCallback有5個(gè)參數(shù), 分別是: 1. 軌跡的id, 時(shí)間戳, cartographer自己定義的位姿結(jié)構(gòu)體(類似eigen但是擁有更多方法), 激光雷達(dá)給的結(jié)果, 還有地圖的柵格值.

咱么看一下這幾個(gè)傳入的參數(shù):

前三個(gè)比較明確,

RangeData定義在range_data.h中:

/**
 * @brief local_slam_data中存儲(chǔ)所有雷達(dá)點(diǎn)云的數(shù)據(jù)結(jié)構(gòu)
 * 
 * @param origin  點(diǎn)云的原點(diǎn)在local坐標(biāo)系下的坐標(biāo)
 * @param returns 所有雷達(dá)數(shù)據(jù)點(diǎn)在local坐標(biāo)系下的坐標(biāo), 記為returns, 也就是hit
 * @param misses  是在光線方向上未檢測(cè)到返回的點(diǎn)(nan, inf等等)或超過(guò)最大配置距離的點(diǎn)
 */
struct RangeData {
  Eigen::Vector3f origin;
  PointCloud returns;
  PointCloud misses; // local坐標(biāo)系下的坐標(biāo)
};

這個(gè)結(jié)構(gòu)體表示一幀激光點(diǎn)云的信息: 1. 當(dāng)前雷達(dá)在哪里掃的(相對(duì)于local坐標(biāo)系), 2. 有效激光點(diǎn)和無(wú)效激光點(diǎn).

這里涉及到Cartographer中坐標(biāo)的關(guān)系(我被繞了好久才搞懂~.~), 這里引用一下大佬的圖

這里要注意的是global coordinate和 local coordinate之間的關(guān)系, 就是沒(méi)有直接關(guān)系. 經(jīng)過(guò)我的實(shí)驗(yàn), 發(fā)現(xiàn)Cartographer一開(kāi)始的時(shí)候這倆(local和global)坐標(biāo)系是重合的, 只有在經(jīng)歷回環(huán)之后他們才會(huì)產(chǎn)生偏移. 而且local coordinate永遠(yuǎn)是固定的, 只有g(shù)lobal才會(huì)變. 這只是我的實(shí)驗(yàn)得到的, 如果有錯(cuò)請(qǐng)大家一定要指出.

PointCloud類在point_cloud.h, 定義了點(diǎn)云結(jié)構(gòu), 包含了雷達(dá)一幀數(shù)據(jù)的所有數(shù)據(jù)點(diǎn) 與數(shù)據(jù)點(diǎn)對(duì)應(yīng)的強(qiáng)度值, 比較簡(jiǎn)單, 就不細(xì)說(shuō)了.

咱們?cè)倏吹诙€(gè)參數(shù):InsertionResult

  struct InsertionResult {
    NodeId node_id;
    std::shared_ptr<const TrajectoryNode::Data> constant_data;
    std::vector<std::shared_ptr<const Submap>> insertion_submaps;
  };

這就是一個(gè)簡(jiǎn)單的結(jié)構(gòu)體: 1. 節(jié)點(diǎn)的id, 2. 某個(gè)數(shù)據(jù)(下面會(huì)說(shuō)), 3. 子圖(比較復(fù)雜, 后面說(shuō)...)

看一下這個(gè)結(jié)構(gòu)體第二個(gè)參數(shù), 在trajectory_node.h中

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

這個(gè)Data結(jié)構(gòu)體就是Cartographer重要的數(shù)據(jù)結(jié)構(gòu)之一, 包含著: 前端匹配所用的數(shù)據(jù)(去重力后的點(diǎn)云)與計(jì)算出的local坐標(biāo)系下的位姿.

再看第二個(gè)參數(shù):submap

也就是子圖, 可以先認(rèn)為是很多個(gè)單幀點(diǎn)云形成的,也是Cartographer主要的數(shù)據(jù)類型之一, 主要有三個(gè)功能:1. 保存在local坐標(biāo)系下的子圖的坐標(biāo), 2. 記錄插入到子圖中雷達(dá)數(shù)據(jù)的個(gè)數(shù), 3. 標(biāo)記這個(gè)子圖是否是完成狀態(tài).

講submap需要的篇幅比較長(zhǎng), 之后單獨(dú)拿出來(lái)說(shuō).

咱們?cè)诜祷氐阶钋懊?去看看map_builder_bridge的AddTrajectory調(diào)用的map_builder_的AddTrajectoryBuilder的參數(shù)的lambda函數(shù)

      // lambda表達(dá)式 local_slam_result_callback_
      [this](const int trajectory_id, 
             const ::cartographer::common::Time time,
             const Rigid3d local_pose,
             ::cartographer::sensor::RangeData range_data_in_local,
             const std::unique_ptr<const ::cartographer::mapping::TrajectoryBuilderInterface::InsertionResult>) 
        // 保存local slam 的結(jié)果數(shù)據(jù) 5個(gè)參數(shù)實(shí)際只用了4個(gè)
        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
      }

有關(guān)lambda函數(shù)的知識(shí)請(qǐng)參閱知識(shí)

這里我們看到,這個(gè)lambda表達(dá)式捕獲了自己MapBuilderBridge這個(gè)類, 讓我們能用其變量與方法.捕獲，其實(shí)就是將局部自動(dòng)變量保存到 Lambda 表達(dá)式內(nèi)部.

傳入的參數(shù)有5個(gè): 1. 軌跡id, 2. 時(shí)間戳, 3. 掃描匹配計(jì)算出的在local坐標(biāo)系下的位姿, 4. 掃描匹配使用的雷達(dá)數(shù)據(jù),還有一個(gè)沒(méi)有用上的InsertionResult. 然后把前四個(gè)參數(shù)傳給OnLocalSlamResult.

所以咱們?cè)倏纯碠nLocalSlamResult這個(gè)函數(shù),這個(gè)函數(shù)就定義在map_builder_bridge.cc下面, 作用是保存local SLAM的結(jié)果

/**
 * @brief 保存local slam 的結(jié)果
 * 
 * @param[in] trajectory_id 當(dāng)前軌跡id
 * @param[in] time 掃描匹配的時(shí)間
 * @param[in] local_pose 掃描匹配計(jì)算出的在local坐標(biāo)系下的位姿
 * @param[in] range_data_in_local 掃描匹配使用的雷達(dá)數(shù)據(jù)
 */
void MapBuilderBridge::OnLocalSlamResult(
    const int trajectory_id, const ::cartographer::common::Time time,
    const Rigid3d local_pose,
    ::cartographer::sensor::RangeData range_data_in_local) {
  std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data =
      std::make_shared<LocalTrajectoryData::LocalSlamData>(
          LocalTrajectoryData::LocalSlamData{time, local_pose,
                                             std::move(range_data_in_local)});
  // 保存結(jié)果數(shù)據(jù)
  absl::MutexLock lock(&mutex_);
  local_slam_data_[trajectory_id] = std::move(local_slam_data);
  // todo: local_slam_data_[trajectory_id].loc
}

其中LocalSlamData結(jié)構(gòu)體定義在map_builder_bridge.h中, 包含了時(shí)間,位姿與雷達(dá)數(shù)據(jù)

    // LocalSlamData中包含了local slam的一些數(shù)據(jù), 包含當(dāng)前時(shí)間, 當(dāng)前估計(jì)的位姿, 以及累計(jì)的所有雷達(dá)數(shù)據(jù)
    struct LocalSlamData {
      ::cartographer::common::Time time;
      ::cartographer::transform::Rigid3d local_pose;
      ::cartographer::sensor::RangeData range_data_in_local;
    };

代碼數(shù)據(jù)類型的深挖告一段落, 咱們必須要回頭去看看MapBuilder類的AddTrajectoryBuilder函數(shù)了. 咱們以2D地圖為例:

    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      // local_trajectory_builder(前端)的初始化
      local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>( //建立一個(gè)local_trajectory_builder
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }
    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
    // CollatedTrajectoryBuilder初始化
    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>( //NOTE:MapBuilder::AddTrajectoryBuilder使用的是CollatedTrajectoryBuilder
        trajectory_options, sensor_collator_.get(), trajectory_id, 
        expected_sensor_ids,
        // 將2D前端與2D位姿圖打包在一起, 傳入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder2D(  //全局軌跡構(gòu)建器
                                          //CreateGlobalTrajectoryBuilder2D是global_trajectory_builderd的方法,
                                          //繼承自TrajectoryBuilderInterface,和CollatedTrajectoryBuilder一個(gè)父類
            std::move(local_trajectory_builder), //前端構(gòu)建器
            trajectory_id, //
            static_cast<PoseGraph2D*>(pose_graph_.get()), //后端位姿圖
            local_slam_result_callback, pose_graph_odometry_motion_filter)));

MapBuilder的AddTrajectoryBuilder開(kāi)啟了Cartographer的前端和后端! 可以從上面的程序看出.

第一部分, 首先通過(guò)absl::make_unique<LocalTrajectoryBuilder2D>構(gòu)建了一個(gè)2D的local SLAM, 啥是local SLAM? 在Cartographer中就是所謂的前端. 傳入的參數(shù)有配置參數(shù), 以及SelectRangeSensorIds的返回值. 我們?nèi)タ纯碨electRangeSensorIds干了些啥

// 只返回傳感器類型是RANGE的topic的集合
std::vector<std::string> SelectRangeSensorIds(
    const std::set<MapBuilder::SensorId>& expected_sensor_ids) {
  std::vector<std::string> range_sensor_ids;
  for (const MapBuilder::SensorId& sensor_id : expected_sensor_ids) {
    if (sensor_id.type == MapBuilder::SensorId::SensorType::RANGE) {
      range_sensor_ids.push_back(sensor_id.id);
    }
  }
  return range_sensor_ids;
}

發(fā)現(xiàn)SelectRangeSensorIds返回了傳感器類型是range的topic集合, 也就是各種激光雷達(dá)的所有topic. LocalTrajectoryBuilder2D具體內(nèi)容將在前端部分細(xì)講.

第二部分, 為CollatedTrajectoryBuilder初始化. 這一部分一層套一層, 咱們從最底層分析.

里面有個(gè)函數(shù)CreateGlobalTrajectoryBuilder2D, 顧名思義, Global就是全局的意思, 就是前端加后端的意思. 咱們看看傳入的參數(shù)就知道為啥是前端加后端了.

第一個(gè)參數(shù)std::move(local_trajectory_builder), 通過(guò)std::move把local_trajectory_builder, 也就是前端的構(gòu)建器, 當(dāng)成參數(shù)傳遞給了CreateGlobalTrajectoryBuilder2D函數(shù).

第二個(gè)參數(shù), 軌跡id就不多說(shuō)了

第三個(gè)參數(shù)是pose_graph_, 也就是位姿圖, 在Cartographer中也就是后端. 把整個(gè)后端的構(gòu)建器的指針傳給了CreateGlobalTrajectoryBuilder2D函數(shù).

第四個(gè)參數(shù), local_slam_result_callback, 也就是前面大費(fèi)周章寫(xiě)的MapBuilderBridge的AddTrajectory的lambda表達(dá)式(作用是保存local SLAM的結(jié)果).

第五個(gè)參數(shù)是pose_graph_odometry_motion_filter, 既運(yùn)動(dòng)過(guò)濾器, 用來(lái)把太小的運(yùn)動(dòng)給過(guò)濾掉. (可能后端沒(méi)有使用, 還沒(méi)深究).

看完參數(shù),咱們找找CreateGlobalTrajectoryBuilder2D在哪,干了啥. 在global_trajectory_builder.cc中我們可以找到CreateGlobalTrajectoryBuilder2D的具體實(shí)現(xiàn):

// 2d的完整的slam
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
    const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
    const TrajectoryBuilderInterface::LocalSlamResultCallback&
        local_slam_result_callback,
    const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter) {
  return absl::make_unique<
      GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
      std::move(local_trajectory_builder), trajectory_id, pose_graph,
      local_slam_result_callback, pose_graph_odometry_motion_filter);
}

這個(gè)函數(shù)沒(méi)處理啥, 直接返回了由Local(前端)和PoseGraph(后端)組合出來(lái)的GlobalTrajectoryBuilder. 而GlobalTrajectoryBuilder有著自己個(gè)構(gòu)造函數(shù), 起到了初始化賦值的作用. 所以我們可以說(shuō)GlobalTrajectoryBuilder才是完整的SLAM, 鏈接了前端和后端.

另外提一嘴, 由于LocalTrajectoryBuilder和GlobalTrajectoryBuilder都繼承自mapping::TrajectoryBuilderInterface, 都有相同的父類, 所以他們之間是可以通過(guò)多態(tài)互相調(diào)用彼此的成員函數(shù)的. (CreateGlobalTrajectoryBuilder2D在map_builder.cc中使用的時(shí)候并沒(méi)有添加命名空間)

GlobalTrajectoryBuilder返回了整個(gè)前端和后端, 聯(lián)合trajectory_options, sensor_collator_.get(), trajectory_id, expected_sensor_ids, 這些參數(shù), 一起傳給了CollatedTrajectoryBuilder.

對(duì)于CollatedTrajectoryBuilder這個(gè)類, 源程序中注釋已經(jīng)很明確(如下), 他就是聯(lián)合和傳感器的前端后端SLAM, 所以他叫Collated(收集并綜合). 他同local與global, 依然繼承于TrajectoryBuilderInterface

// Collates sensor data using a sensor::CollatorInterface, then passes it on to
// a mapping::TrajectoryBuilderInterface which is common for 2D and 3D.
// 使用 sensor::CollatorInterface 整理傳感器數(shù)據(jù), 
// 然后將其傳遞到2D和3D通用的 mapping::TrajectoryBuilderInterface
// 處理傳感器數(shù)據(jù), 使其按照時(shí)間排列, 然后傳入GlobalTrajectoryBuilder

總結(jié)一下: CollatedTrajectory = GlobalTrajectory + Sensor = LocalTrajectory + PoseGraph + Sensor

總結(jié)

MapBuilder通過(guò)聯(lián)合LocalTrajectory, PoseGraph, 還有Sensor, 開(kāi)啟了整個(gè)Cartographer的前端與后端.

到此這篇關(guān)于C++ Cartographer源碼中關(guān)于MapBuilder的聲明與構(gòu)造的文章就介紹到這了,更多相關(guān)C++ MapBuilder的聲明與構(gòu)造內(nèi)容請(qǐng)搜索腳本之家以前的文章或繼續(xù)瀏覽下面的相關(guān)文章希望大家以后多多支持腳本之家！

您可能感興趣的文章: