cartographer 代码思想解读（17）- PoseGraph2D整体处理流程
前面分析了位姿图结构，优化的原理，约束的形成以及优化求解等具体实现，同时也提前介绍了PoseGraph2D内的成语变量。本节作为最后一节将整个后端的优化的流程（包括如何调用约束以及求解器）进行分析。可看下如下整个后端的结构图。
从图上可看出，后端PoseGraph2D类主要外部接口类包括：前端匹配结果InsertionResult、里程计、imu和fixed Frame Pose。其中InsertionResult为必需元素，而其他传感器信息可以无。
位姿图增加节点顶层接口AddNode
在global_trajectory_builder即顶层调用接口可以看到，前端和后端的主要联系则是经过pose_graph_->AddNode接口调用。每次经过一次前端有效匹配后，将前端匹配的结果传递于后端接口。
回忆下global_trajectory_builder中的调用。
// 将匹配后的结果加入图优化节点中
auto node_id = pose_graph_->AddNode(
matching_result->insertion_result->constant_data, trajectory_id_,
matching_result->insertion_result->insertion_submaps);
可以看出后端增加一个位姿图节点需传递匹配后的结果包括submap和点云数据，最后返回一个位姿图节点ID。其中前端处理后的点云数据，包括水平投影的点云，重力方向和在local submap的位置。
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) {
}
以上为函数接口定义。
// 将点云的原点坐标的local的位置转换为全局位置
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
将本帧点云的原点坐标转换为全局绝对坐标，其转移矩阵由最新的优化结果决定。
// 添加一个节点
const NodeId node_id = AppendNode(constant_data, trajectory_id,
insertion_submaps, optimized_pose);
添加一个新的节点，并生成新的node id，全局位置
// 查看维护的两个submap的front是否插入完成（两个submap，front用于匹配和更新的）
// 如果一个submap finished，则需要进行闭环检测
const bool newly_finished_submap =
insertion_submaps.front()->insertion_finished();
// 把计算约束的工作较为workitem完成，是个多线程工作组
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
return ComputeConstraintsForNode(node_id, insertion_submaps,
newly_finished_submap);
});
return node_id;
根据当前submap的是否完成状态执行计算约束，上节计算约束具体内容已分析，可在多线程后台运行。
增加新的节点具体实现AppendNode
NodeId PoseGraph2D::AppendNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data,
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps,
const transform::Rigid3d& optimized_pose)
与顶层接口基本一致，增加了该节点在整个全局地图的全局绝对位置，也是后期需要优化的位姿。
absl::MutexLock locker(&mutex_);
// 判断是否需要添加trajectory id
AddTrajectoryIfNeeded(trajectory_id);
// 此 trajectory id的轨迹是否存在或更改，暂时只是判断无用
if (!CanAddWorkItemModifying(trajectory_id)) {
LOG(WARNING) << "AddNode was called for finished or deleted trajectory.";
}
由于多线程操作，为安全增加线程锁保护。其中AddTrajectoryIfNeeded是判断对轨迹进行的操作，包括增加，删除或者轨迹之间的关系操作，没有进一步研究，仍然假设仅有一个轨迹。
// 添加一个scan id， 返回trajectoryid和对应的scan idex
const NodeId node_id = data_.trajectory_nodes.Append(
trajectory_id, TrajectoryNode{constant_data, optimized_pose});
// 记录轨迹节点个数
++data_.num_trajectory_nodes;
将全局数据data_中增加一个位姿图轨迹节点即点云数据帧，同时也保存对应的轨迹ID信息，同时记录其节点个数。
if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))
->data.submap != insertion_submaps.back()) {
// 添加submap id，
const SubmapId submap_id =
data_.submap_data.Append(trajectory_id, InternalSubmapData());
// 闭环中submap节点采用维护的两个中后一个submap
data_.submap_data.at(submap_id).submap = insertion_submaps.back();
LOG(INFO) << "Inserted submap " << submap_id << ".";
// 被处理的submap个数自加
kActiveSubmapsMetric->Increment();
}
在全局数据data_中添加submap信息，但是由于submap是不停地更新，但是不一定更换，因此添加时只考虑新增加的submap。在submap维护一节中分析过，前端输出的submap除初始时刻时有一个submap，而其他情况下都会存在两个submap。而insertion_submaps.back()则为前端结果中最新的submap，当更换时即与当前data_最后一个不一致时才会增加（简单来说一个submap仅增加一次）。
return node_id;
最后返回的位姿图ID为data_存储的轨迹节点ID。
计算node内的约束ComputeConstraintsForNode
WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
const NodeId& node_id,
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
const bool newly_finished_submap) {
std::vector<SubmapId> submap_ids;
std::vector<SubmapId> finished_submap_ids;
std::set<NodeId> newly_finished_submap_node_ids;
需要计算的约束为轨迹节点与submap的相对关系，输入为轨迹节点id和submap信息，同时也增加当前submap是否为新完成的submap。同时定义了临时变量用于缓存中间过程。
absl::MutexLock locker(&mutex_);
// 获取该nodeid的点云数据和位置信息
const auto& constant_data =
data_.trajectory_nodes.at(node_id).constant_data;
// 获取submap对应的submapid vector
// 如果第一个则为index 为0， 否则会返回最新的两个连续的submap id
submap_ids = InitializeGlobalSubmapPoses(
node_id.trajectory_id, constant_data->time, insertion_submaps);
根据节点ID获取对应的点云相关数据，同时根据对应id的轨迹获取最新的两个submap（如果初始时刻仅返回一个submap）。
const SubmapId matching_id = submap_ids.front();
//获取scan 的在submap的local pose
const transform::Rigid2d local_pose_2d =
transform::Project2D(constant_data->local_pose *
transform::Rigid3d::Rotation(
constant_data->gravity_alignment.inverse()));
// 获取全局绝对位置，通过优化后的submap 全局位置和submap的local位置求出转移矩阵
const transform::Rigid2d global_pose_2d =
optimization_problem_->submap_data().at(matching_id).global_pose *
constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
local_pose_2d;
与submap更新维护一节分析可知，submap队列包括两个，front主要用于匹配，back主要用于更新。根据重力方向将轨迹节点水平投影成2d pose；根据优化后的结果对轨迹节点的全局位置进行修正。
// 优化器中增加轨迹节点信息
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 'data_.submap_data' further below.
CHECK(data_.submap_data.at(submap_id).state ==
SubmapState::kNoConstraintSearch);
// submap_id中的对应的添加nodeid
data_.submap_data.at(submap_id).node_ids.emplace(node_id);
//nodeid的scan pose在submap的相对位置
const transform::Rigid2d constraint_transform =
constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
local_pose_2d;
// 增加一个约束条件，为submap插入的scan id， submapid和nodeid ，约束为相对位置
data_.constraints.push_back(
Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
options_.matcher_translation_weight(),
options_.matcher_rotation_weight()},
Constraint::INTRA_SUBMAP});
}
遍历matching结果的所有submap，其新增加的轨迹节点必定组成了其submap，故其约束为INTRA_SUBMAP类型约束，可直接将相对位置加入约束队列中。
for (const auto& submap_id_data : data_.submap_data) {
// 判断当前graph中所有的finished状态并存储
if (submap_id_data.data.state == SubmapState::kFinished) {
CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
finished_submap_ids.emplace_back(submap_id_data.id);
}
}
遍历全局submap队列中并记录已finished状态的submap，即已经完成过约束计算的submap。
if (newly_finished_submap) {
const SubmapId newly_finished_submap_id = submap_ids.front();
InternalSubmapData& finished_submap_data =
data_.submap_data.at(newly_finished_submap_id);
CHECK(finished_submap_data.state == SubmapState::kNoConstraintSearch);
// 将新的submap设置为finished，表明已经增加约束条件了
finished_submap_data.state = SubmapState::kFinished;
newly_finished_submap_node_ids = finished_submap_data.node_ids;
}
如果前端匹配输出的submap是刚完成的一个，即维护的两个submap中front表明不再更新。则修改全局变量中该submap的状态为finished状态。
for (const auto& submap_id : finished_submap_ids) {
ComputeConstraint(node_id, submap_id);
}
新加入的轨迹节点需要与之前所有完成的submap均计算一次约束。
if (newly_finished_submap) {
const SubmapId newly_finished_submap_id = submap_ids.front();
// We have a new completed submap, so we look into adding constraints for
// old nodes.
// 当出现一个新的submap完成时，需要将graph存储的所有节点与当前submap进行计算约束
for (const auto& node_id_data : optimization_problem_->node_data()) {
const NodeId& node_id = node_id_data.id;
if (newly_finished_submap_node_ids.count(node_id) == 0) {
ComputeConstraint(node_id, newly_finished_submap_id);
}
}
}
每次新的submap完成不再更新即不再添加新的轨迹节点时，则需要计算此submap与所有优化位姿图node之间的约束。
// 此次创建约束结束
constraint_builder_.NotifyEndOfNode();
每次新添加一个节点时，均需执行，通知约束构建器。
absl::MutexLock locker(&mutex_);
// 记录距离上次闭环处理增加的节点个数
++num_nodes_since_last_loop_closure_;
// 当新增加节点个数达到一定时，则调用优化处理结果
if (options_.optimize_every_n_nodes() > 0 &&
num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
return WorkItem::Result::kRunOptimization;
}
return WorkItem::Result::kDoNotRunOptimization;
每增加一个新的位姿节点时进行计数，通过一定的周期实现是否需要执行后端优化处理，添加节点和约束是实时的，但是优化不需要，更不需要每个节点都进行。
计算指定node和submap的约束ComputeConstraint
// 计算指定的nodeid和submapid计算其约束
void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
const SubmapId& submap_id) {
bool maybe_add_local_constraint = false; // 声明默认不添加局部约束条件
bool maybe_add_global_constraint = false; // 声明默认不添加全局约束条件
const TrajectoryNode::Data* constant_data;
const Submap2D* submap;
指定的node和submap计算约束主要为INTER_SUBMAP类型约束，即需要重新扫描匹配获取更加准确的约束，即闭环。声明了局部和全局是否需要计算约束的标志位。
CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
if (!data_.submap_data.at(submap_id).submap->insertion_finished()) {
// Uplink server only receives grids when they are finished, so skip
// constraint search before that.
return;
}
如果需要计算的submap的还未完成，即为结束更新，则无需计算约束。
// 获取两个id最新的那个时刻
const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
// 最近轨迹链接的时间
const common::Time last_connection_time =
data_.trajectory_connectivity_state.LastConnectionTime(
node_id.trajectory_id, submap_id.trajectory_id);
//scanid所在的轨迹与submap所在的轨迹为同一条，则进入局部约束
if (node_id.trajectory_id == submap_id.trajectory_id ||
node_time <
last_connection_time +
common::FromSeconds(
options_.global_constraint_search_after_n_seconds())) {
maybe_add_local_constraint = true;
} else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
// 一条轨迹全局采样一定时间间隔时，可添加全局约束
maybe_add_global_constraint = true;
}
上段代码主要是确定指定的两个node和submap应该计算局部约束还是全局约束，如果节点与submap在同一轨迹内或者距离上次全局约束时间较短时，则均是计算局部约束；如果不在同一轨迹内一定间隔计算全局约束。
// 获取node id的相关data
constant_data = data_.trajectory_nodes.at(node_id).constant_data.get();
// 获取submapid的相关data
submap = static_cast<const Submap2D*>(
data_.submap_data.at(submap_id).submap.get());
if (maybe_add_local_constraint) {
// 获取相对位置
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, node_id, constant_data, initial_relative_pose);
} else if (maybe_add_global_constraint) {
// 添加全局约束
constraint_builder_.MaybeAddGlobalConstraint(submap_id, submap, node_id,
constant_data);
}
根据前面的预处理，调用约束规划器计算其约束，约束器计算可看constraint_builder_中的详细介绍。说明下，正常情况下仅有一条轨迹，全局约束一般不用，非常耗时间；大部分是局部约束，即给出初始位置，在其附近进行相关匹配。
辅助函数
计算约束时获取的的submap初始化InitializeGlobalSubmapPoses
// 初始化正在维护的submap为一个submap id
std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
const int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
CHECK(!insertion_submaps.empty());
// 获取优化器中的submap地址
const auto& submap_data = optimization_problem_->submap_data();
// 表明为第一个submap
if (insertion_submaps.size() == 1) {
// If we don't already have an entry for the first submap, add one.
// 如果optimization_problem_的submap data中还没有存储任何id，则添加一个新的
// 获取存储的submap id的个数
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
// trajectory_id初始位置的个数如果大于0，即存在初始位置
if (data_.initial_trajectory_poses.count(trajectory_id) > 0) {
// 创建两个trajectory_id的链接， 即有相对位置的trajectory id
data_.trajectory_connectivity_state.Connect(
trajectory_id,
data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id,
time);
}
// 添加submap的全局位置
optimization_problem_->AddSubmap(
trajectory_id, transform::Project2D(
ComputeLocalToGlobalTransform(
data_.global_submap_poses_2d, trajectory_id) *
insertion_submaps[0]->local_pose()));
}
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
// submapid 为0 添加到集合中
const SubmapId submap_id{trajectory_id, 0};
CHECK(data_.submap_data.at(submap_id).submap == insertion_submaps.front());
return {submap_id};
}
CHECK_EQ(2, insertion_submaps.size());
// 获取trajectory_id的最后一个submap id
const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
// 检测是否为空（理论上不为空，因为上面为空时，已经初始化添加了一个）
CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
// 取出前一个submap id
const SubmapId last_submap_id = std::prev(end_it)->id;
//如果submap data中上一个id为正在维护的submap中的front，说明back为新的还未处理
if (data_.submap_data.at(last_submap_id).submap ==
insertion_submaps.front()) {
// In this case, 'last_submap_id' is the ID of
// 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
// 获取上个submap ID的全局位置
const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
// 根据上一个submap推出相邻的下一个submap的全局位置
optimization_problem_->AddSubmap(
trajectory_id,
first_submap_pose *
constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
constraints::ComputeSubmapPose(*insertion_submaps[1]));
// 返回上一个和当前新的submap id集合
// 新的id为上个id+1
return {last_submap_id,
SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
}
// 表明存储的上一个submapid 为 正在维护的back submap， 则说明已经处理过
CHECK(data_.submap_data.at(last_submap_id).submap ==
insertion_submaps.back());
// 获取上上个submapid
const SubmapId front_submap_id{trajectory_id,
last_submap_id.submap_index - 1};
CHECK(data_.submap_data.at(front_submap_id).submap ==
insertion_submaps.front());
// 返回前前和前一个的submap
return {front_submap_id, last_submap_id};
}
根据优化器里存储的submap id和正在维护的submap进行判断，如果一个都没有，则创建一个0 index的submap。如果上个submapid为正在维护的front， 则返回上一个和新增加的submap id。如果上个submapid为正在维护的back，说明都已经处理过了，则返回上上个和上个的submap id。
GetLocalToGlobalTransform坐标转换函数
此函数主要是获取激光点云当前local下的pose转换为全局绝对pose的转移矩阵。
// trajectory_id轨迹从local转换到global pose的转移矩阵
transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform(
const int trajectory_id) const {
absl::MutexLock locker(&mutex_);
// 实际是submap的全局位置转换矩阵
return ComputeLocalToGlobalTransform(data_.global_submap_poses_2d,
trajectory_id);
}
同时调用ComputeLocalToGlobalTransform函数具体实现，绝对位置会根据优化的结果进行变动，当前全局位置与最新优化结果有关。
// 计算trajectory_id轨迹中pose到全局位置的转移矩阵
transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses,
const int trajectory_id) const {
// 查找已经优化过的trajectory id中的submap的位置，如果有可返回存储的值
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
if (begin_it == end_it) {
const auto it = data_.initial_trajectory_poses.find(trajectory_id);
if (it != data_.initial_trajectory_poses.end()) {
return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id,
it->second.time) *
it->second.relative_pose; // 初始为单位矩阵，后续通过相对位置推导每个位置
} else {
return transform::Rigid3d::Identity(); // 仅是唯一个，表明为初始submap，转移矩阵为单位矩阵
}
}
// 如果不存在，则返回由上个优化后的submapid的位置的相对位置换算出的位置
const SubmapId last_optimized_submap_id = std::prev(end_it)->id;
// Accessing 'local_pose' in Submap is okay, since the member is const.
return transform::Embed3D(
global_submap_poses.at(last_optimized_submap_id).global_pose) *
data_.submap_data.at(last_optimized_submap_id)
.submap->local_pose()
.inverse();
}
扫一扫
1418
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
