描述
主要分析node_main.cc中的Run()函数中的RunFinalOptimization()
node.RunFinalOptimization();
RunFinalOptimization()的基本逻辑
第一层:node.cc
RunFinalOptimization()函数定义在node.cc
void Node::RunFinalOptimization() {
{
for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
const int trajectory_id = entry.first;
if (entry.second == TrajectoryState::ACTIVE) {
LOG(WARNING)
<< "Can't run final optimization if there are one or more active "
"trajectories. Trying to finish trajectory with ID "
<< std::to_string(trajectory_id) << " now.";
CHECK(FinishTrajectory(trajectory_id))
<< "Failed to finish trajectory with ID "
<< std::to_string(trajectory_id) << ".";
}
}
}
// Assuming we are not adding new data anymore, the final optimization
// can be performed without holding the mutex.
map_builder_bridge_.RunFinalOptimization();
}
显然,使用for循环遍历了全部的轨迹状态
for (const auto& entry : map_builder_bridge_.GetTrajectoryStates())
并且使用if语句判断,如果某条轨迹的value是ACTIVE,就要打log并报错
if (entry.second == TrajectoryState::ACTIVE)
如果每条轨迹的value都不是ACTIVE,可能已经完成了建图,我们现在要做的就是优化一下轨迹,执行的也就是核心语句
map_builder_bridge_.RunFinalOptimization();
备注的翻译:假设我们不再添加新的数据,最终的优化可以在不持有互斥锁的情况下执行。
第二层:map_builder_bridge.cc
map_builder_bridge_.RunFinalOptimization();
中的RunFinalOptimization()函数定义在map_builder_bridge.cc
中
void MapBuilderBridge::RunFinalOptimization() {
LOG(INFO) << "Running final trajectory optimization...";
map_builder_->pose_graph()->RunFinalOptimization();
}
map_builder_
是声明在map_builder_bridge.h
中的一个MapBuilderInterface
类型指针
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
而pose_graph()
是MapBuilderInterface
类的一个成员函数,是一个返回值为PoseGraphInterface
类型指针的虚函数,是一个纯虚函数
virtual mapping::PoseGraphInterface* pose_graph() = 0;
RunFinalOptimization()
声明在pose_graph_interface.h
,是PoseGraphInterface
类的一个成员函数,仍是一个纯虚函数
// Waits for all computations to finish and computes optimized poses.
virtual void RunFinalOptimization() = 0;
注释的翻译解释了RunFinalOptimization的功能:等待所有的计算完成并计算优化的姿态。
RunFinalOptimization
的重写函数在哪里呢,我们查看源码发现。pose_graph.h
中声明的PoseGraph
类,继承了pose_graph_interface.h
中的PoseGraphInterface
类。
而在cartographer/cartographer/mapping/internal/2d/pose_graph_2d.h
中,声明的PoseGraph2D
类继承了PoseGraph
类,并对RunFinalOptimization
函数进行了重写;并且在cartographer/cartographer/mapping/internal/3d/pose_graph_3d.h
中,声明的PoseGraph3D
类同样继承了PoseGraph
类,也一样对RunFinalOptimization
函数进行了重写。2D和3D的RunFinalOptimization
函数都定义在了它们对应的cc文件中pose_graph_2d.cc
和pose_graph_3d.cc
因此我们不难看出
PoseGraph2D/PoseGraph3D 继承自-> PoseGraph 继承自 -> PoseGraphInterface