前端优化告一段落,接下来我们对回环检测和后端优化的代码进行阅读。
pose_graph实际是SPA的一个实现,拓展成了submaping版本,每一个节点都会和一个或者多个子图进行匹配.每一个匹配都会形成一个约束,进行优化的时候节点的位姿和子图的位姿会一起进行优化. 所有的约束都是子图和节点之间的约束.
cartographer\mapping\internal\2d\pose_graph_2d.cc
1.AddNode
回顾源码阅读的第一篇文章,我们可以看到,在前端匹配完成后,调用InsertToSubmap函数,将激光数据插入匹配完成的子图之后,就开始调用pose_graph的AddNode函数来进行回环检测和后端优化。
AddNode的主要工作是把一个TrajectoryNode的数据加入到PoseGraph维护的trajectory_nodes_这个容器中。并计算跟这个节点相关的约束.然后返回node_id.
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)
{
//该节点的全局位姿.
//这个Pose是由trajectory相对于世界坐标系的Pose乘以
//节点数据中包含的该节点相对于该trajectory的LocalTrajectoryBuilder
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
//加入节点,并得到节点的id.
const NodeId node_id = AppendNode(constant_data, trajectory_id,
insertion_submaps, optimized_pose);
// We have to check this here, because it might have changed by the time we
// execute the lambda.
// 该子图是否完成.即插入的帧数足够了.
//当插入帧数到达一定数量时,子图就不会在接受激光数据的加入
// 如果被finished,那么我们需要计算一下约束并且进行一下全局优化了。
// 这里是把这个工作交到了workItem中等待处理
const bool newly_finished_submap =
insertion_submaps.front()->insertion_finished();
//进行约束计算.--多线程.后台会调用ComputeConstraintsForNode()来计算约束.
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
return ComputeConstraintsForNode(node_id, insertion_submaps,
newly_finished_submap);
});
return node_id;
}
- 计算关键帧全局位姿,并将此关键帧加入node队列,得到node_id,调用了AppendNode
- 查看插入子图的关键帧数量有没有达到上限,如果达到上限停止插入,开始计算约束,约束计算调用AddWorkItem
- 返回node_id
2.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_);
AddTrajectoryIfNeeded(trajectory_id);
if (!CanAddWorkItemModifying(trajectory_id))
{
LOG(WARNING) << "AddNode was called for finished or deleted trajectory.";
}
//加入当前轨迹中.
const NodeId node_id = data_.trajectory_nodes.Append(
trajectory_id, TrajectoryNode{constant_data, optimized_pose});
++data_.num_trajectory_nodes;
// Test if the 'insertion_submap.back()' is one we never saw before.
// 如果是新的submap,则需要加入到submap_中.
if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))
->data.submap != insertion_submaps.back())
{
// We grow 'data_.submap_data' as needed. This code assumes that the first
// time we see a new submap is as 'insertion_submaps.back()'.
const SubmapId submap_id =
data_.submap_data.Append(trajectory_id, InternalSubmapData());
data_.submap_data.at(submap_id).submap = insertion_submaps.back();
LOG(INFO) << "Inserted submap " << submap_id << ".";
kActiveSubmapsMetric->Increment();
}
return node_id;
}
直接加入节点队列,如果生成新的子图,同样将子图加入队列
3.后端优化
3.1AddWorkItem
将一个函数的地址加入工作队列,相当于把一个函数加入工作队列,在后端等待调用。
void PoseGraph2D::AddWorkItem(
const std::function<WorkItem::Result()>& work_item)
{
absl::MutexLock locker(&work_queue_mutex_);
//如果队列里没有,则直接开始执行.这里将这个函数启动
if (work_queue_ == nullptr)
{
work_queue_ = absl::make_unique<WorkQueue>();
auto task = absl::make_unique<common::Task>();
//DrainWorkQueue()将指定的函数的地址依次取出
task->SetWorkItem([this]() { DrainWorkQueue(); });
thread_pool_->Schedule(std::move(task));
}
//将函数加入队列中.work_item相当于指向函数的指针
const auto now = std::chrono::steady_clock::now();
work_queue_->push_back({now, work_item});
kWorkQueueDelayMetric->Set(
std::chrono::duration_cast<std::chrono::duration<double>>(
now - work_queue_->front().time)
.count());
}
重要步骤:调用DrainWorkQueue将指定函数取出,然后将压入工作队列
3.2 DrainWorkQueue
从work_queue_中不断的取函数ComputeConstraintsForNode()出来进行计算,至少返回的结果是进行WorkItem::Result::kRunOptimization;为止
.这里的work_item()就是调用ComputeConstraintsForNode()函数.
void PoseGraph2D::DrainWorkQueue()
{
bool process_work_queue = true;
size_t work_queue_size;
//当工作队列为空时,则直接返回
while (process_work_queue)
{
std::function<WorkItem::Result()> work_item;
{
absl::MutexLock locker(&work_queue_mutex_);
if (work_queue_->empty())
{
work_queue_.reset();
return;
}
//不为空,则开始取出函数ComputeConstraintsForNode
work_item = work_queue_->front().task;
work_queue_->pop_front();
work_queue_size = work_queue_->size();
}
// work_item实际是函数指针,指向ComputeConstraintsForNode
//当work_item()时,可以直接理解为实际就是ComputeConstraintsForNode()
//ComputeConstraintsForNode()返回两个结果,一个是需要优化,一个是不需要优化
process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization;
}
LOG(INFO) << "Remaining work items in queue: " << work_queue_size;
//到了这里说明需要进行优化了,ComputeConstraintsForNode()返回了需要进行优化的结果.
// We have to optimize again.
constraint_builder_.WhenDone(
[this](const constraints::ConstraintBuilder2D::Result& result) {
HandleWorkQueue(result);
});
}
当需要优化时,最后调用了HandleWorkQueue()函数来进行优化
3.3HandleWorkQueue
当ComputeConstraintsForNode()返回需要优化的结果后,调用本函数.
本函数会进行pose-graph的优化.在DrainWorkQueue()里面进行调用.
代码中对理清思路最重要的是:
- 进行优化:RunOptimization()
- 优化完成之后,继续处理剩下的workitem:DrainWorkQueue();
void PoseGraph2D::HandleWorkQueue(
const constraints::ConstraintBuilder2D::Result& result)
{
{
absl::MutexLock locker(&mutex_);
data_.constraints.insert(data_.constraints.end(), result.begin(),
result.end());
}
//进行优化.
RunOptimization();
if (global_slam_optimization_callback_)
{
std::map<int, NodeId> trajectory_id_to_last_optimized_node_id;
std::map<int, SubmapId> trajectory_id_to_last_optimized_submap_id;
{
absl::MutexLock locker(&mutex_);
const auto& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_->node_data();
for (const int trajectory_id : node_data.trajectory_ids())
{
if (node_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0)
{
continue;
}
trajectory_id_to_last_optimized_node_id.emplace(
trajectory_id,
std::prev(node_data.EndOfTrajectory(trajectory_id))->id);
trajectory_id_to_last_optimized_submap_id.emplace(
trajectory_id,
std::prev(submap_data.EndOfTrajectory(trajectory_id))->id);
}
}
global_slam_optimization_callback_(
trajectory_id_to_last_optimized_submap_id,
trajectory_id_to_last_optimized_node_id);
}
{
//更新图的联通性.
absl::MutexLock locker(&mutex_);
for (const Constraint& constraint : result)
{
UpdateTrajectoryConnectivity(constraint);
}
//对轨迹进行删除
DeleteTrajectoriesIfNeeded();
TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_)
{
trimmer->Trim(&trimming_handle);
}
trimmers_.erase(
std::remove_if(trimmers_.begin(), trimmers_.end(),
[](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
return trimmer->IsFinished();
}),
trimmers_.end());
num_nodes_since_last_loop_closure_ = 0;
// Update the gauges that count the current number of constraints.
// 统计约束的数量.
double inter_constraints_same_trajectory = 0;
double inter_constraints_different_trajectory = 0;
for (const auto& constraint : data_.constraints)
{
if (constraint.tag ==
cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP)
{
continue;
}
if (constraint.node_id.trajectory_id ==
constraint.submap_id.trajectory_id)
{
++inter_constraints_same_trajectory;
}
else
{
++inter_constraints_different_trajectory;
}
}
kConstraintsSameTrajectoryMetric->Set(inter_constraints_same_trajectory);
kConstraintsDifferentTrajectoryMetric->Set(
inter_constraints_different_trajectory);
}
//继续处理剩下的workitem.
DrainWorkQueue();
}
3.4RunOptimization
正式进行优化的函数.被HandleWorkQueue()调用.
void PoseGraph2D::RunOptimization()
{
if (optimization_problem_->submap_data().empty())
{
return;
}
// No other thread is accessing the optimization_problem_,
// data_.constraints, data_.frozen_trajectories and data_.landmark_nodes
// when executing the Solve. Solve is time consuming, so not taking the mutex
// before Solve to avoid blocking foreground processing.
// 正式进行求解.
//当误差函数构造完成之后,直接调用ceres库求解
optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
data_.landmark_nodes);
absl::MutexLock locker(&mutex_);
const auto& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_->node_data();
for (const int trajectory_id : node_data.trajectory_ids())
{
for (const auto& node : node_data.trajectory(trajectory_id))
{
auto& mutable_trajectory_node = data_.trajectory_nodes.at(node.id);
mutable_trajectory_node.global_pose =
transform::Embed3D(node.data.global_pose_2d) *
transform::Rigid3d::Rotation(
mutable_trajectory_node.constant_data->gravity_alignment);
}
// Extrapolate all point cloud poses that were not included in the
// 'optimization_problem_' yet.
const auto local_to_new_global =
ComputeLocalToGlobalTransform(submap_data, trajectory_id);
const auto local_to_old_global = ComputeLocalToGlobalTransform(
data_.global_submap_poses_2d, trajectory_id);
const transform::Rigid3d old_global_to_new_global =
local_to_new_global * local_to_old_global.inverse();
const NodeId last_optimized_node_id =
std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
auto node_it =
std::next(data_.trajectory_nodes.find(last_optimized_node_id));
for (; node_it != data_.trajectory_nodes.EndOfTrajectory(trajectory_id);
++node_it)
{
auto& mutable_trajectory_node = data_.trajectory_nodes.at(node_it->id);
mutable_trajectory_node.global_pose =
old_global_to_new_global * mutable_trajectory_node.global_pose;
}
}
for (const auto& landmark : optimization_problem_->landmark_data())
{
data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;
}
data_.global_submap_poses_2d = submap_data;
}
小节
这部分的代码实现的功能就是将节点加入,并且计算约束(回环检测)以及后端优化。本篇文章大家看了后端优化的内容,就下一篇将介绍回环检测,也就是ComputeConstraintsForNode的内容。