C++?Cartographer源碼中關于MapBuilder的聲明與構造
前面已經談到了Cartographer中添加軌跡的方法和傳感器的數(shù)據流動走向。 我們注意到,在添加軌跡的時候,除了添加位姿估計器還有采樣器,訂閱回調函數(shù)之外,最重要的是通過map_builder_bridge添加了一條軌跡,其他的都是為它服務的。咱們這節(jié)詳細看看MapBuilder這個Cartographer的核心。
開始一條軌跡
添加軌跡是開啟Cartographer的大門. 顧名思義, 添加軌跡就是AddTrajectory. 我們最先接觸到的AddTrajectory函數(shù)是Node類里面的. 這個函數(shù)除了我們之前詳細提到的添加傳感器等功能之外,還有一個核心函數(shù):
// 調用map_builder_bridge的AddTrajectory, 添加一個軌跡
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);這一行調用了Map_builder_bridge_的AddTrajectory添加一條軌跡. map_builder_bridge_從何而來呢?在Node類的構造函數(shù)中有個初始化列表,用于構造MapBuilderBridge這個類.
map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer)
我們看到MapBuilderBridge構造函數(shù)有三個參數(shù),其中一個(中間一個)就是std::unique_ptr<cartographer::mapping::MapBuilderInterface> ,也就是MapBuilderInterface這個類的指針. MapBuilderInterface這個類和MapBuilder是父子關系, 所以這個地方實際上構造的是MapBuilder這個類. 這個map_builder又是那來的呢? 他在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中實現(xiàn),工廠函數(shù)
//在這里,實例化一個MapBuilder, 而MapBuilder是MapBuilderInterface的子類
//MapBuilder的AddTrajectoryBuilder實例化了CollatedTrajectoryBuilder CreateMapBuilder這個函數(shù)是在map_builder.cc中實現(xiàn):
// 工廠函數(shù),生成接口API
std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
const proto::MapBuilderOptions& options) {
return absl::make_unique<MapBuilder>(options);
}可見CreateMapBuilder就是返回了構造好了的MapBuilder類. 而cpp允許子類用父類代替實現(xiàn)多態(tài), 所以上面用MapBuilderInterface也不會出錯.
再回到node_main.cc中的CreateMapBuilder, 這個意思就是用傳入的選項構造一個MapBuilder類的對象并返回這個對象,然后再到node.cc中, 把node_main.cc中的CreateMapBuilder作為Node的構造函數(shù)的變量傳給Node構造出map_builder_bridge_這個變量(MapBuilderBridge類). 在調用Node的AddTrajectory的時候, 通過調用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 開始一條新的軌跡, 返回新軌跡的id,需要傳入一個函數(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 為這個新軌跡 添加一個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ù)有一個std::set<...Sensor_id>類型的變量,std::set是一個容器,可以簡單理解為鍵值對,而鍵就是值,值就是鍵.(比較基礎不細說啦).另一個就是從node_main.cc就跟著我們的TrajectoryOptions. 也就是配置文件讀取的內容. 返回值很簡單,就是新建軌跡的編號. Cartographer允許有多個軌跡同時維護,而且后面我們會發(fā)現(xiàn), Cartographer定位其實就是把建好的地圖和定位作為兩個不同的軌跡實現(xiàn)的, 這個我們在這個系列的最后會說.
咱們看一下expected_sensor_ids的面目,他是一個結構體, 在Cartographer部分的trajectory_builder_interface.h中實現(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ī)定了一個傳感器的類型與一個對應的topic的名字. 傳感器的類型是一個限域枚舉(枚舉類). 總之作用就是聯(lián)系topic與topic對應的傳感器類型, 以便后續(xù)維護.
Node的AddTrajectory實際上是調用的MapBuilderBridge的AddTrajectory. 咱們看看map_builder_bridge中的AddTrajectory. 這個函數(shù)分為三個步驟:
- 開啟一條新軌跡
- 給新軌跡添加傳感器
- 保存軌跡的參數(shù)配置
第一步程序如下:
// Step: 1 開始一條新的軌跡, 返回新軌跡的id,需要傳入一個函數(shù)
const int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_options.trajectory_builder_options,
// lambda表達式 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 的結果數(shù)據 5個參數(shù)實際只用了4個
OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
});這一塊很復雜,一開始看很可能看不透,所以值得仔細剖析. 在第三部分剖析.
第二部分程序如下:
// Step: 2 為這個新軌跡 添加一個SensorBridge
sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>( //sensor_bridges_是一個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的實例化, 在map_builder.h中實現(xiàn),
// 返回當前軌跡的CollatedTrajectoryBuilder的指針.
// CollatedTrajectoryBuilder就是前端后端綁在一起的,傳給了SensorBridge作用是為第一步添加的軌跡, 聯(lián)合一個傳感器相關處理與維護器. 這一塊將在傳感器的數(shù)據分發(fā)器部分詳細解讀. 咱們這解主要挖掘MapBuilder相關部分,而不是Sensor部分.
MapBuilder類的AddTrajectoryBuilder函數(shù)
新建軌跡通過調用map_builder的AddTrajectoryBuilder方法, 依舊只摘取重要的部分程序:
/**
* @brief 創(chuàng)建一個新的 TrajectoryBuilder 并返回它的 trajectory_id
*
* @param[in] expected_sensor_ids 所有需要的topic的名字的集合
* @param[in] trajectory_options 軌跡的參數(shù)配置
* @param[in] local_slam_result_callback 需要傳入的回調函數(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>( //建立一個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( //全局軌跡構建器
//CreateGlobalTrajectoryBuilder2D是global_trajectory_builderd的方法,
//繼承自TrajectoryBuilderInterface,和CollatedTrajectoryBuilder一個父類
std::move(local_trajectory_builder), //前端構建器
trajectory_id, //
static_cast<PoseGraph2D*>(pose_graph_.get()), //后端位姿圖
local_slam_result_callback, pose_graph_odometry_motion_filter)));
}先看傳入的參數(shù). 傳入的有三個參數(shù), 前面兩個為Sensor_id和配置參數(shù), 咱們之前都詳細說到過.
重點要看的是最后一個參數(shù). 最后一個傳入的參數(shù)是函數(shù)指針,也就是一個回調函數(shù)的地址(c++的基礎內容), 在上級直接使用的是一個lambda函數(shù), lambda表達式我覺得可以定義為"一次性函數(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個參數(shù)列表
transform::Rigid3d /* local pose estimate */,
sensor::RangeData /* in local frame */,
std::unique_ptr<const InsertionResult>)>; //InsertionResult保存了地圖的具體數(shù)據,柵格值咱們一一把這個函數(shù)和他的lambda表達式對照著看看
LocalSlamResultCallback有5個參數(shù), 分別是: 1. 軌跡的id, 時間戳, cartographer自己定義的位姿結構體(類似eigen但是擁有更多方法), 激光雷達給的結果, 還有地圖的柵格值.
咱么看一下這幾個傳入的參數(shù):
前三個比較明確,
RangeData定義在range_data.h中:
/**
* @brief local_slam_data中存儲所有雷達點云的數(shù)據結構
*
* @param origin 點云的原點在local坐標系下的坐標
* @param returns 所有雷達數(shù)據點在local坐標系下的坐標, 記為returns, 也就是hit
* @param misses 是在光線方向上未檢測到返回的點(nan, inf等等)或超過最大配置距離的點
*/
struct RangeData {
Eigen::Vector3f origin;
PointCloud returns;
PointCloud misses; // local坐標系下的坐標
};這個結構體表示一幀激光點云的信息: 1. 當前雷達在哪里掃的(相對于local坐標系), 2. 有效激光點和無效激光點.
這里涉及到Cartographer中坐標的關系(我被繞了好久才搞懂~.~), 這里引用一下大佬的圖

這里要注意的是global coordinate和 local coordinate之間的關系, 就是沒有直接關系. 經過我的實驗, 發(fā)現(xiàn)Cartographer一開始的時候這倆(local和global)坐標系是重合的, 只有在經歷回環(huán)之后他們才會產生偏移. 而且local coordinate永遠是固定的, 只有global才會變. 這只是我的實驗得到的, 如果有錯請大家一定要指出.
PointCloud類在point_cloud.h, 定義了點云結構, 包含了雷達一幀數(shù)據的所有數(shù)據點 與 數(shù)據點對應的強度值, 比較簡單, 就不細說了.
咱們再看第二個參數(shù):InsertionResult
struct InsertionResult {
NodeId node_id;
std::shared_ptr<const TrajectoryNode::Data> constant_data;
std::vector<std::shared_ptr<const Submap>> insertion_submaps;
};這就是一個簡單的結構體: 1. 節(jié)點的id, 2. 某個數(shù)據(下面會說), 3. 子圖(比較復雜, 后面說...)
看一下這個結構體第二個參數(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;
};這個Data結構體就是Cartographer重要的數(shù)據結構之一, 包含著: 前端匹配所用的數(shù)據(去重力后的點云)與計算出的local坐標系下的位姿.
再看第二個參數(shù):submap
也就是子圖, 可以先認為是很多個單幀點云形成的,也是Cartographer主要的數(shù)據類型之一, 主要有三個功能:1. 保存在local坐標系下的子圖的坐標, 2. 記錄插入到子圖中雷達數(shù)據的個數(shù), 3. 標記這個子圖是否是完成狀態(tài).
講submap需要的篇幅比較長, 之后單獨拿出來說.
咱們在返回到最前面,去看看map_builder_bridge的AddTrajectory調用的map_builder_的AddTrajectoryBuilder的參數(shù)的lambda函數(shù)
// lambda表達式 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 的結果數(shù)據 5個參數(shù)實際只用了4個
OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
}有關lambda函數(shù)的知識請參閱知識
這里我們看到,這個lambda表達式捕獲了自己MapBuilderBridge這個類, 讓我們能用其變量與方法.捕獲,其實就是將局部自動變量保存到 Lambda 表達式內部.
傳入的參數(shù)有5個: 1. 軌跡id, 2. 時間戳, 3. 掃描匹配計算出的在local坐標系下的位姿, 4. 掃描匹配使用的雷達數(shù)據,還有一個沒有用上的InsertionResult. 然后把前四個參數(shù)傳給OnLocalSlamResult.
所以咱們再看看OnLocalSlamResult這個函數(shù),這個函數(shù)就定義在map_builder_bridge.cc下面, 作用是保存local SLAM的結果
/**
* @brief 保存local slam 的結果
*
* @param[in] trajectory_id 當前軌跡id
* @param[in] time 掃描匹配的時間
* @param[in] local_pose 掃描匹配計算出的在local坐標系下的位姿
* @param[in] range_data_in_local 掃描匹配使用的雷達數(shù)據
*/
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)});
// 保存結果數(shù)據
absl::MutexLock lock(&mutex_);
local_slam_data_[trajectory_id] = std::move(local_slam_data);
// todo: local_slam_data_[trajectory_id].loc
}其中LocalSlamData結構體定義在map_builder_bridge.h中, 包含了時間,位姿與雷達數(shù)據
// LocalSlamData中包含了local slam的一些數(shù)據, 包含當前時間, 當前估計的位姿, 以及累計的所有雷達數(shù)據
struct LocalSlamData {
::cartographer::common::Time time;
::cartographer::transform::Rigid3d local_pose;
::cartographer::sensor::RangeData range_data_in_local;
};代碼數(shù)據類型的深挖告一段落, 咱們必須要回頭去看看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>( //建立一個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( //全局軌跡構建器
//CreateGlobalTrajectoryBuilder2D是global_trajectory_builderd的方法,
//繼承自TrajectoryBuilderInterface,和CollatedTrajectoryBuilder一個父類
std::move(local_trajectory_builder), //前端構建器
trajectory_id, //
static_cast<PoseGraph2D*>(pose_graph_.get()), //后端位姿圖
local_slam_result_callback, pose_graph_odometry_motion_filter)));MapBuilder的AddTrajectoryBuilder開啟了Cartographer的前端和后端! 可以從上面的程序看出.
第一部分, 首先通過absl::make_unique<LocalTrajectoryBuilder2D>構建了一個2D的local SLAM, 啥是local SLAM? 在Cartographer中就是所謂的前端. 傳入的參數(shù)有配置參數(shù), 以及SelectRangeSensorIds的返回值. 我們去看看SelectRangeSensorIds干了些啥
// 只返回傳感器類型是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集合, 也就是各種激光雷達的所有topic. LocalTrajectoryBuilder2D具體內容將在前端部分細講.
第二部分, 為CollatedTrajectoryBuilder初始化. 這一部分一層套一層, 咱們從最底層分析.
里面有個函數(shù)CreateGlobalTrajectoryBuilder2D, 顧名思義, Global就是全局的意思, 就是前端加后端的意思. 咱們看看傳入的參數(shù)就知道為啥是前端加后端了.
第一個參數(shù)std::move(local_trajectory_builder), 通過std::move把local_trajectory_builder, 也就是前端的構建器, 當成參數(shù)傳遞給了CreateGlobalTrajectoryBuilder2D函數(shù).
第二個參數(shù), 軌跡id就不多說了
第三個參數(shù)是pose_graph_, 也就是位姿圖, 在Cartographer中也就是后端. 把整個后端的構建器的指針傳給了CreateGlobalTrajectoryBuilder2D函數(shù).
第四個參數(shù), local_slam_result_callback, 也就是前面大費周章寫的MapBuilderBridge的AddTrajectory的lambda表達式(作用是保存local SLAM的結果).
第五個參數(shù)是pose_graph_odometry_motion_filter, 既運動過濾器, 用來把太小的運動給過濾掉. (可能后端沒有使用, 還沒深究).
看完參數(shù),咱們找找CreateGlobalTrajectoryBuilder2D在哪,干了啥. 在global_trajectory_builder.cc中我們可以找到CreateGlobalTrajectoryBuilder2D的具體實現(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);
}這個函數(shù)沒處理啥, 直接返回了由Local(前端)和PoseGraph(后端)組合出來的GlobalTrajectoryBuilder. 而GlobalTrajectoryBuilder有著自己個構造函數(shù), 起到了初始化賦值的作用. 所以我們可以說GlobalTrajectoryBuilder才是完整的SLAM, 鏈接了前端和后端.
另外提一嘴, 由于LocalTrajectoryBuilder和GlobalTrajectoryBuilder都繼承自mapping::TrajectoryBuilderInterface, 都有相同的父類, 所以他們之間是可以通過多態(tài)互相調用彼此的成員函數(shù)的. (CreateGlobalTrajectoryBuilder2D在map_builder.cc中使用的時候并沒有添加命名空間)
GlobalTrajectoryBuilder返回了整個前端和后端, 聯(lián)合trajectory_options, sensor_collator_.get(), trajectory_id, expected_sensor_ids, 這些參數(shù), 一起傳給了CollatedTrajectoryBuilder.
對于CollatedTrajectoryBuilder這個類, 源程序中注釋已經很明確(如下), 他就是聯(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ù)據, // 然后將其傳遞到2D和3D通用的 mapping::TrajectoryBuilderInterface // 處理傳感器數(shù)據, 使其按照時間排列, 然后傳入GlobalTrajectoryBuilder
總結一下: CollatedTrajectory = GlobalTrajectory + Sensor = LocalTrajectory + PoseGraph + Sensor
總結
MapBuilder通過聯(lián)合LocalTrajectory, PoseGraph, 還有Sensor, 開啟了整個Cartographer的前端與后端.
到此這篇關于C++ Cartographer源碼中關于MapBuilder的聲明與構造的文章就介紹到這了,更多相關C++ MapBuilder的聲明與構造內容請搜索腳本之家以前的文章或繼續(xù)瀏覽下面的相關文章希望大家以后多多支持腳本之家!

