讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证
一、前言
通过上一篇博客的小结，可以知道下面的目标就是对 PoseGraph2D::RunOptimization() 函数进行分析了，该函数是累计计算节点的约束达到一定数量时，才会在 PoseGraph2D::DrainWorkQueue() 函数中被调用。当然，还有前面遗留的一个疑问也不能落下：
疑问 1 : \color{red}疑问1: 疑问1: global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。
说到 PoseGraph2D::data_ 就不得不提及到 PoseGraph2D::optimization_problem_，他们存在一定的相似性，所以不要搞混，首先看：
struct PoseGraphData {
// Submaps get assigned an ID and state as soon as they are seen, even
// before they take part in the background computations.
// submap_data_ 里面,包含了所有的submap
MapById<SubmapId, InternalSubmapData> submap_data;
// Global submap poses currently used for displaying data.
// submap 在 global 坐标系下的坐标
MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d;
MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_3d;
// Data that are currently being shown.
// 所有的轨迹节点的id与 节点的在global坐标系下的坐标, 在local map 下的坐标与时间
MapById<NodeId, TrajectoryNode> trajectory_nodes;
}
然后再来看：
class OptimizationProblem2D
: public OptimizationProblemInterface<NodeSpec2D, SubmapSpec2D,
transform::Rigid2d> {
......
// 优化的目标就是节点的位姿与子图的位姿
MapById<NodeId, NodeSpec2D> node_data_; // 节点坐标列表
MapById<SubmapId, SubmapSpec2D> submap_data_; // submap原点坐标列表
}
可以看到 PoseGraphData 与 OptimizationProblem2D 中都包含了节点数据，子图数据等。具体细节后面再讨论，这里需要记住的一点时 PoseGraphData::global_submap_poses_2d 中的全局位姿，是优化过后的位姿。OptimizationProblem2D 的主要目的就是节点的位姿与子图的位姿。
如果有心的朋友，可以查看一下 src/cartographer/cartographer/mapping/pose_graph_interface.h 这个头文件，可以看到预定义的如下接口：
// Returns the global poses for all submaps.
virtual MapById<SubmapId, SubmapPose> GetAllSubmapPoses() const = 0;
// Returns the transform converting data in the local map frame (i.e. the
// continuous, non-loop-closed frame) into the global map frame (i.e. the
// discontinuous, loop-closed frame).
virtual transform::Rigid3d GetLocalToGlobalTransform(
int trajectory_id) const = 0;
// Returns the current optimized trajectories.
virtual MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const = 0;
// Returns the current optimized trajectory poses.
virtual MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses()
const = 0;
// Returns the states of trajectories.
virtual std::map<int, TrajectoryState> GetTrajectoryStates() const = 0;
// Returns the current optimized landmark poses.
virtual std::map<std::string, transform::Rigid3d> GetLandmarkPoses()
const = 0;
......
这些接口都是对外的，也就是说 PoseGraph2D::data_ 中应该存储相应的数据，这些才方便外部获取。后面可以知道，PoseGraph2D::data_ 与 PoseGraph2D::optimization_problem_ 并没有太多的重叠之处，如计算出来的约束只存储在前者之中，并没有同时加载到后者之中。
二、OptimizationProblem2D数据添加
首先在 pose_graph_2d.cc 文件中查看如下几个函数：
// 将 把里程计数据加入到优化问题中 这个任务放入到任务队列中
void PoseGraph2D::AddOdometryData(const int trajectory_id,
const sensor::OdometryData& odometry_data) {
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
}
return WorkItem::Result::kDoNotRunOptimization;
});
}
// 将 把gps数据加入到优化问题中 这个任务放入到任务队列中
void PoseGraph2D::AddFixedFramePoseData(
const int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) {
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddFixedFramePoseData(trajectory_id,
fixed_frame_pose_data);
}
return WorkItem::Result::kDoNotRunOptimization;
});
}
// 将 把landmark数据加入到data_.landmark_nodes中 这个任务放入到任务队列中
void PoseGraph2D::AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) {
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) {
for (const auto& observation : landmark_data.landmark_observations) {
data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
PoseGraphInterface::LandmarkNode::LandmarkObservation{
trajectory_id, landmark_data.time,
observation.landmark_to_tracking_transform,
observation.translation_weight, observation.rotation_weight});
}
}
return WorkItem::Result::kDoNotRunOptimization;
});
}
这几个函数都由一些共同点，首先判断一下轨迹是否能够添加任务，如果可以添加，则构建添加数据的任务，可以明显的看到这些数据都是添加到 PoseGraph2D::optimization_problem_ 这个实例中的。其上添加的数据分别有 ImuData、OdometryData、FixedFramePoseData。但是对于 LandmarkData 数据的添加，有一些不一样，如下：
// 将 把landmark数据加入到data_.landmark_nodes中 这个任务放入到任务队列中
void PoseGraph2D::AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) {
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) {
for (const auto& observation : landmark_data.landmark_observations) {
data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
PoseGraphInterface::LandmarkNode::LandmarkObservation{
trajectory_id, landmark_data.time,
observation.landmark_to_tracking_transform,
observation.translation_weight, observation.rotation_weight});
}
}
return WorkItem::Result::kDoNotRunOptimization;
});
}
由上面的代码可以知道，关于 LandmarkData 的数据，是添加到 PoseGraph2D::data_ 之中的，因为 LandmarkData 是不需要进行优化的。另外传入的参数 landmark_data 中，还包含了每个 observation 平移与旋转的权重。
PoseGraph2D::optimization_problem_ 除了添加 ImuData、OdometryData、FixedFramePoseData 数据之外，还有前面的已经分析过的代码如下(都可以在)pose_graph_2d.cc 文件中搜索到):
optimization_problem_->AddSubmap(......)
optimization_problem_->AddTrajectoryNode(......)
也就是说，子图与节点数据也会被添加到 optimization_problem_ 这个实例之中。optimization_problem_ 与外界的交互基本讲解完了，下面来分析一下 PoseGraph2D::RunOptimization() 这个函数。
三、PoseGraph2D::RunOptimization()-整体注释
大家可以先大致浏览一下该函数的整体注释，然后再分析其具体细节(大致看一下即可，后面有详细注释)：
// 进行优化处理, 并使用优化结果对保存的数据进行更新
void PoseGraph2D::RunOptimization() {
// 如果submap为空直接退出
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.
// Solve 比较耗时, 所以在执行 Solve 之前不要加互斥锁, 以免阻塞其他的任务处理
// landmark直接参与优化问题
optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
data_.landmark_nodes);
absl::MutexLock locker(&mutex_);
// 获取优化后的结果
// submap_data的类型是 MapById<SubmapId, optimization::SubmapSpec2D>
const auto& submap_data = optimization_problem_->submap_data();
// node_data的类型是 MapById<NodeId, NodeSpec2D>
// node_data是优化后的所有节点的新位姿
const auto& node_data = optimization_problem_->node_data();
// 更新轨迹内的节点位置
for (const int trajectory_id : node_data.trajectory_ids()) {
// Step: 根据优化后的结果对data_.trajectory_nodes的global_pose进行更新
for (const auto& node : node_data.trajectory(trajectory_id)) {
// node 是 IdDataReference 类型
// mutable_trajectory_node是TrajectoryNode类型
auto& mutable_trajectory_node = data_.trajectory_nodes.at(node.id);
// 进行对优化后的global位姿进行重力矫正，然后赋值给 data_.trajectory_nodes，
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.
// 推断尚未包含在“optimization_problem_”中的所有点云姿势
// 根据submap_data最后一个被优化的位姿, 计算global坐标系指向local坐标系的坐标变换
const auto local_to_new_global =
ComputeLocalToGlobalTransform(submap_data, trajectory_id);
// 优化前的 global坐标系指向local坐标系的坐标变换
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();
// 这一次优化的node的最后一个id
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));
// Step: 根据之前的位姿改变量, 对没有优化过的位姿进行校正
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;
}
} // end for trajectory_id
// 更新data_.landmark_nodes
for (const auto& landmark : optimization_problem_->landmark_data()) {
data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;
}
// 更新所有submap的位姿
data_.global_submap_poses_2d = submap_data;
}
四、PoseGraph2D::RunOptimization()-细节分析
（ 1 ）： \color{blue}（1）： （1）：首先调用 optimization_problem_->submap_data().empty() 判断一下
optimization_problem_ 存储的子图数据是否为空，如果为空则不进行优化直接放回。否则调用 optimization_problem_->Solve() 进行优化，其还会把 data_.landmark_nodes 作为实参传入，该函数的执行十分耗时，源码中是没有上锁操作的。且根据前面的分析，可以知道只有一个线程对 optimization_problem_、data_.constraints,、data_.frozen_trajectories 以及 data_.landmark_nodes 进行访问，也是没有必要上锁的，以避免阻塞前台处理。
（ 2 ）： \color{blue}（2）： （2）：先进行上锁的操作，可能有的朋友就好奇了，为甚接着就要上锁了，大家可以在 pose_graph_2d.cc 搜索一下 optimization_problem_->Add 关键字，然后可以发现，可以知道添加子图是在消耗 work_queue_ 队列这个线程中完成的，但是对子图的访问确有可能发生在运行执行 PoseGraph2D 函数的主线程中，如下函数：
int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const {
const auto& submap_data = parent_->optimization_problem_->submap_data();
return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
}
所以 PoseGraph2D::RunOptimization() 函数在对 optimization_problem_->submap_data() 相关数据进行优化改变时，需要进行上锁操作。
（ 3 ）： \color{blue}（3）： （3）：上锁之后，先获得所有轨迹的节点数据与子图数据。然后对所有轨迹进行遍历，内部再嵌套一个循环，对轨迹的节点数据进行遍历，其目的就是使用优化后的global全局位姿对节点位姿进行调整。
（ 4 ）： \color{blue}（4）： （4）：调整的过程通过多个步骤，首先时对 global 位姿进行重力矫正，在前面的博客中，对于节点位姿的重力矫正就不用说了，逻辑已经比较清楚，但是对于节点的全局位姿时没有进行重力矫正的，所以这里先对节点的全局位姿进行重力矫正，代码如下：
// 将优化后的二维节点位姿旋转到机器人的姿态上得到global_pose
mutable_trajectory_node.global_pose =
transform::Embed3D(node.data.global_pose_2d) *
transform::Rigid3d::Rotation(
mutable_trajectory_node.constant_data->gravity_alignment);
需要提及一点的时，此时获取到的 node.data.global_pose_2d 来自于 optimization_problem_，其已经过 optimization_problem_->Solve() 函数优化了，矫正之后赋值给 mutable_trajectory_node.global_pose，实际上是赋值给 data_.trajectory_nodes。也就是说，经过该操作 data_.trajectory_nodes 中节点的 global 位姿已经优化，且经过重力矫正。
（ 5 ）： \color{blue}（5）： （5）：根据优化后的子图位姿与节点位姿，计算优化后global系坐标到local坐标系的坐标变换 local_to_new_global。同时还会计算之前没有优化过的 global系坐标到local坐标系的坐标变换 old_global_to_new_global，通过 如下代码获得优化的改变量。
// 优化产生的改变量
const transform::Rigid3d old_global_to_new_global =
local_to_new_global * local_to_old_global.inverse();
（ 6 ）： \color{blue}（6）： （6）：前面提到，只有每次增加的节点的数目达到一定数量时，才会执行优化。且优化过程时比较耗时的，可想而知在优化的过程中，data_.trajectory_nodes 中可能增加了新的节点，那么分别根据 optimization_problem_ 与 data_ 轨迹节点的起始与结束位置，可以计算出新增加的节点。接着根据前面计算出来的优化改变量，对这些新增的节点进行优化即可。
（ 7 ）： \color{blue}（7）： （7）： 虽然 PoseGraph2D::AddLandmarkData() 函数中，先把数据存储于 data_.landmark_nodes 之中，但是 PoseGraph2D::RunOptimization() 函数之中在执行
optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),data_.landmark_nodes);
对节点位姿与子图位姿进行优化时，把 data_.landmark_nodes 传递给了 optimization_problem_。其内部会对其的 global 位姿进行调整优化，所以还需要对 data_.landmark_nodes 的 global_landmark_pose 进行调整。最后，还执行了代码 data_.global_submap_poses_2d = submap_data，其就是对data_.global_submap_poses_2d 进行更新了。
五、PoseGraph2D::RunOptimization()-整体注释
总体分析下来，可以发现与前面的猜测大致相同，PoseGraph2D::data_ 中主要存储优化之后的数据，供外界获取。PoseGraph2D::optimization_problem_ 主要时通过调用 optimization_problem_->Solve()，根据添加的数据以及传入的参数对 OptimizationProblem2D::node_data_、OptimizationProblem2D::submap_data_ 进行优化。
到这里，对于 疑问 1 : \color{red}疑问1: 疑问1: global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。已经有了比较清晰的答案。根据 PoseGraph2D::RunOptimization() 函数如下两句代码：
// 获取优化后的结果
// submap_data的类型是 MapById<SubmapId, optimization::SubmapSpec2D>
const auto& submap_data = optimization_problem_->submap_data()
data_.global_submap_poses_2d = submap_data;
可以知道，其是在 OptimizationProblem2D 中完成优化的，具体是如何优化的呢？就是后面要讲解的内容。