本次閱讀的源碼爲 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
第三篇文章分析瞭如何使用傳感器數據進行位姿的掃描匹配,這篇文章將進行如何使用 匹配後的位姿,以及整體的優化過程。
cartographer/cartographer/mapping/internal/global_trajectory_builder.cc
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
// ...
auto node_id = pose_graph_->AddNode(
matching_result->insertion_result->constant_data, trajectory_id_,
matching_result->insertion_result->insertion_submaps);
//...
}
void AddSensorData(const std::string& sensor_id,
const sensor::ImuData& imu_data) override {
if (local_trajectory_builder_) {
local_trajectory_builder_->AddImuData(imu_data);
}
pose_graph_->AddImuData(trajectory_id_, imu_data);
}
void AddSensorData(const std::string& sensor_id,
const sensor::OdometryData& odometry_data) override {
CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
if (local_trajectory_builder_) {
local_trajectory_builder_->AddOdometryData(odometry_data);
}
pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
}
在使用雷達數據進行掃描匹配之後,將匹配的結果作爲一個node傳入到圖結構中,並調用 pose_graph_->AddNode()
imu 和 odom 數據在傳入到 pose extraloator 中之後,還會再傳入到 pose_graph_中,分別調用 pose_graph_->AddImuData(), pose_graph_->AddOdometryData()方法。
cartographer/mapping/pose_graph_interface.h
Node 即爲
// cartographer/mapping/trajectory_node.h
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.
// local_pose 是相對與 local frame的座標,不是相對於submap的座標
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.
// global_pose 是相對與 global frame的座標,不是相對於submap的座標
transform::Rigid3d global_pose;
};
// cartographer/mapping/internal/2d/local_trajectory_builder_2d.h
class LocalTrajectoryBuilder2D {
public:
struct InsertionResult {
std::shared_ptr<const TrajectoryNode::Data> constant_data;
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
};
struct MatchingResult {
common::Time time;
transform::Rigid3d local_pose;
sensor::RangeData range_data_in_local;
// 'nullptr' if dropped by the motion filter.
std::unique_ptr<const InsertionResult> insertion_result;
};
//...
}
cartographer/mapping/id.h
// Uniquely identifies a trajectory node using a combination of a unique
// trajectory ID and a zero-based index of the node inside that trajectory.
// NodeId: 就是屬於這個軌跡的 Node 的 id,從0開始,即爲(0,0),(0,1)
// 即:軌跡0的,第0個和第一個 Node
struct NodeId {
int trajectory_id;
int node_index;
// ...
}
};
// Uniquely identifies a submap using a combination of a unique trajectory ID
// and a zero-based index of the submap inside that trajectory.
// SubmapId: 就是屬於這個軌跡的 submap 的 id,從0開始,即爲(0,0),(0,1)
// 即:軌跡0的,第0個和第一個submap
struct SubmapId {
int trajectory_id;
int submap_index;
// ...
}
};
// cartographer/mapping/pose_graph_interface.h
struct Constraint {
struct Pose {
transform::Rigid3d zbar_ij;
double translation_weight;
double rotation_weight;
};
SubmapId submap_id; // 'i' in the paper.
NodeId node_id; // 'j' in the paper.
// Pose of the node 'j' relative to submap 'i'.
Pose pose;
// Differentiates between intra-submap (where node 'j' was inserted into
// submap 'i') and inter-submap constraints (where node 'j' was not inserted
// into submap 'i').
// 約束分爲 節點到自身子圖的 子圖內約束(普通約束),和 節點到其他子圖的 子圖間約束(閉環約束)
enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
};
struct SubmapPose {
int version;
transform::Rigid3d pose;
};
struct SubmapData {
std::shared_ptr<const Submap> submap;
transform::Rigid3d pose;
};
struct TrajectoryData {
double gravity_constant = 9.8;
std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
common::optional<transform::Rigid3d> fixed_frame_origin_in_map;
};
座標系
global map frame
這是表示全局SLAM結果的座標系。 它是固定的地圖座標系,包括所有循環閉包和優化結果。 當新的優化結果可用時,此幀與任何其他幀之間的轉換可以跳轉。
local map frame
這是表示本地SLAM結果的座標系。 它是固定的地圖座標系,不包括循環閉包和姿勢圖優化。 對於給定的時間點,此座標系與全局地圖座標系之間的變換可能會發生變化,但此座標系與所有其他座標系之間的變換不會發生變化。
submap frame
每個子圖都有一個單獨的固定座標系。
tracking frame
處理傳感器數據的座標系。 它不是固定的,即它隨時間而變化。 不同的軌跡也有所不同。
座標變換
local_pose
Transforms data from the tracking frame (or a submap frame, depending on context) to the local map frame.
global_pose
Transforms data from the tracking frame (or a submap frame, depending on context) to the global map frame.
local_submap_pose
Transforms data from a submap frame to the local map frame.
global_submap_pose
Transforms data from a submap frame to the global map frame.
cartographer/mapping/internal/2d/pose_graph_2d.h
// It is extended for submapping:
// Each node has been matched against one or more submaps (adding a constraint
// for each match), both poses of nodes and of submaps are to be optimized.
// All constraints are between a submap i and a node j.
//它被擴展爲子地圖:
//每個節點已與一個或多個子圖匹配(爲每個匹配添加約束),節點和子圖的姿勢都要進行優化。
//所有約束都在子圖i和節點j之間。
PoseGraph2D::AddNode() --> PoseGraph2D::ComputeConstraintsForNode() --> PoseGraph2D::ComputeConstraint() --> constraint_builder_.MaybeAddConstraint()
NodeId PoseGraph2D::AddNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data,
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
// ...
// We have to check this here, because it might have changed by the time we
// execute the lambda.
const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForNode(node_id, insertion_submaps,
newly_finished_submap);
});
return node_id;
}
void PoseGraph2D::ComputeConstraintsForNode(
const NodeId& node_id,
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
const bool newly_finished_submap) {
// ...
optimization_problem_->AddTrajectoryNode(
matching_id.trajectory_id,
optimization::NodeSpec2D{constant_data->time, local_pose_2d,
global_pose_2d,
constant_data->gravity_alignment});
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const SubmapId submap_id = submap_ids[i];
// Even if this was the last node added to 'submap_id', the submap will
// only be marked as finished in 'submap_data_' further below.
// ...
constraints_.push_back(Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
options_.matcher_translation_weight(),
options_.matcher_rotation_weight()},
Constraint::INTRA_SUBMAP});
}
for (const auto& submap_id_data : submap_data_) {
if (submap_id_data.data.state == SubmapState::kFinished) {
CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
ComputeConstraint(node_id, submap_id_data.id);
}
}
if (newly_finished_submap) {
// ...
// We have a new completed submap, so we look into adding constraints for
// old nodes.
ComputeConstraintsForOldNodes(finished_submap_id);
}
constraint_builder_.NotifyEndOfNode();
++num_nodes_since_last_loop_closure_;
if (options_.optimize_every_n_nodes() > 0 &&
num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
DispatchOptimization();
}
}
void PoseGraph2D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) {
const auto& submap_data = submap_data_.at(submap_id);
for (const auto& node_id_data : optimization_problem_->node_data()) {
const NodeId& node_id = node_id_data.id;
if (submap_data.node_ids.count(node_id) == 0) {
ComputeConstraint(node_id, submap_id);
}
}
}
void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
const SubmapId& submap_id) {
// lua文件配置了global_constraint_search_after_n_seconds 爲 10秒,也就是10秒進行一次閉環約束查找與添加
if (node_id.trajectory_id == submap_id.trajectory_id ||
node_time <
last_connection_time +
common::FromSeconds(
options_.global_constraint_search_after_n_seconds())) {
//如果節點和子圖屬於同一軌跡,或者如果最近有一個全局約束將該節點的軌跡與子圖的軌跡聯繫起來,
//則只需對局部搜索窗口進行約束即可。
const transform::Rigid2d initial_relative_pose =
optimization_problem_->submap_data()
.at(submap_id)
.global_pose.inverse() *
optimization_problem_->node_data().at(node_id).global_pose_2d;
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get(),
initial_relative_pose);
} else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get());
}
}
cartographer/mapping/internal/constraints/constraint_builder_2d.cc
void ConstraintBuilder2D::MaybeAddConstraint(
const SubmapId& submap_id, const Submap2D* const submap,
const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose) {
if (initial_relative_pose.translation().norm() >
options_.max_constraint_distance()) {
return;
}
// 採樣頻率,POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
// 數值越小,構建普通約束的頻率越低,計算量越小
if (!sampler_.Pulse()) return;
common::MutexLocker locker(&mutex_);
if (when_done_) {
LOG(WARNING)
<< "MaybeAddConstraint was called while WhenDone was scheduled.";
}
constraints_.emplace_back();
kQueueLengthMetric->Set(constraints_.size());
auto* const constraint = &constraints_.back();
const auto* scan_matcher =
DispatchScanMatcherConstruction(submap_id, submap->grid());
auto constraint_task = common::make_unique<common::Task>();
constraint_task->SetWorkItem([=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */
constant_data, initial_relative_pose, *scan_matcher,
constraint);
});
constraint_task->AddDependency(scan_matcher->creation_task_handle);
auto constraint_task_handle =
thread_pool_->Schedule(std::move(constraint_task));
finish_node_task_->AddDependency(constraint_task_handle);
}
void ConstraintBuilder2D::ComputeConstraint(
const SubmapId& submap_id, const Submap2D* const submap,
const NodeId& node_id, bool match_full_submap,
const TrajectoryNode::Data* const constant_data,
const transform::Rigid2d& initial_relative_pose,
const SubmapScanMatcher& submap_scan_matcher,
std::unique_ptr<ConstraintBuilder2D::Constraint>* constraint) {
const transform::Rigid2d initial_pose =
ComputeSubmapPose(*submap) * initial_relative_pose;
// The 'constraint_transform' (submap i <- node j) is computed from:
// - a 'filtered_gravity_aligned_point_cloud' in node j,
// - the initial guess 'initial_pose' for (map <- node j),
// - the result 'pose_estimate' of Match() (map <- node j).
// - the ComputeSubmapPose() (map <- submap i)
float score = 0.;
transform::Rigid2d pose_estimate = transform::Rigid2d::Identity();
// Compute 'pose_estimate' in three stages:
// 1. Fast estimate using the fast correlative scan matcher.
// 2. Prune if the score is too low.
// 3. Refine.
if (match_full_submap) {
kGlobalConstraintsSearchedMetric->Increment();
if (submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap(
constant_data->filtered_gravity_aligned_point_cloud,
options_.global_localization_min_score(), &score, &pose_estimate)) {
CHECK_GT(score, options_.global_localization_min_score());
CHECK_GE(node_id.trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
kGlobalConstraintsFoundMetric->Increment();
kGlobalConstraintScoresMetric->Observe(score);
} else {
return;
}
} else {
kConstraintsSearchedMetric->Increment();
if (submap_scan_matcher.fast_correlative_scan_matcher->Match(
initial_pose, constant_data->filtered_gravity_aligned_point_cloud,
options_.min_score(), &score, &pose_estimate)) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
kConstraintsFoundMetric->Increment();
kConstraintScoresMetric->Observe(score);
} else {
return;
}
}
{
common::MutexLocker locker(&mutex_);
score_histogram_.Add(score);
}
// Use the CSM estimate as both the initial and previous pose. This has the
// effect that, in the absence of better information, we prefer the original
// CSM estimate.
//使用CSM估計值作爲初始和先前姿勢。 這樣做的結果是,在沒有更好的信息的情況下,我們更喜歡原始的CSM估計。
ceres::Solver::Summary unused_summary;
ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
constant_data->filtered_gravity_aligned_point_cloud,
*submap_scan_matcher.grid, &pose_estimate,
&unused_summary);
const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate;
constraint->reset(new Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
options_.loop_closure_translation_weight(),
options_.loop_closure_rotation_weight()},
Constraint::INTER_SUBMAP});
}
總結一下:ConstraintBuilder2D 構建的約束就是當前隊列中的Node與Submap2D中的關係,分爲INTRA_SUBMAP, INTER_SUBMAP,約束就是 當前node與submap的座標系原點間的座標變換,node在submap當中就是intra約束,找到了node與之前submap的約束就是inter約束,也就是閉環約束。
在計算約束時,首先通過 FastCorrelativeScanMatcher2D 類 進行一個粗匹配,之後在通過ceres進行精匹配。
FastCorrelativeScanMatcher2D() 由下篇文章解析。
references
1. https://blog.csdn.net/liuxiaofei823/article/details/70207814
2 https://blog.csdn.net/xiaoma_bk/article/details/81317649 --- cartographer PoseGraph2D
3 https://blog.csdn.net/xiaoma_bk/article/details/83040559 --- cartographer 添加約束 /分支界定法