后端优化问题求解器——optimization_problem_
前面两篇文章中，我们已经讨论了约束构建器和分支定界闭环检测的原理和实现，实际上它只是用来检测子图与路径节点之间可能的闭环关系，并为之建立约束。 对于整个地图的优化而言，它只是成功的更新了位姿图的拓扑结构。我们还需要根据后端优化问题，考察所有约束，调整子图和路径节点的位姿，来最小化全局估计与局部估计之间的偏差。
在Global SLAM的核心——PoseGraph2D中，我们已经研究了后端优化问题的数学形式。 PoseGraph2D通过对象optimization_problem_， 使用一种SPA(Sparse Pose Adjustment)的技术进行优化。 本文中我们将深入分析该对象，了解后端优化是如何实现的。
1. 再看后端优化
现在让我们再次祭出Global SLAM的框图，如下所示，后端优化器是通过一种称为SPA(Sparse Pose Adjustment)的技术，根据路径节点与子图之间的约束关系，优化路径节点与子图的世界坐标。 约束的构建由约束构建器通过分支定界的方法进行扫描匹配得到。而所谓的SPA技术，本质上还是通过LM(Levenberg-Marquardt)方法进行非线性寻优。由于并不是所有的子图和路径节点之间都有约束， 以至于LM的增量方程中的\(H\)矩阵是一个稀疏的矩阵，所以可以使用一些优化手段来降低对内存的需求，提高解算效率。所以才说是Sparse的位姿修正。
 |
优化过程是在PoseGraph2D处理工作队列的函数HandleWorkQueue中调用函数RunOptimization触发的。 下面是该函数的代码片段。该函数没有任何输入参数，在函数之初先检查一下是否给后端优化器喂过数据。
void PoseGraph2D::RunOptimization() {
if (optimization_problem_->submap_data().empty())
return;
接下来就通过后端优化器的接口Solve进行SPA优化。根据注释的说法，程序运行到这里的时候，实际上没有其他线程访问对象optimization_problem_, constraints_, frozen_trajectories_和landmark_nodes_这四个对象。又因为这个Solve接口实在太耗时了，所以没有在该函数之前加锁，以防止阻塞其他任务。
// No other thread is accessing the optimization_problem_, constraints_,
// frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve is
// time consuming, so not taking the mutex before Solve to avoid blocking foreground processing.
optimization_problem_->Solve(constraints_, frozen_trajectories_, landmark_nodes_);
common::MutexLocker locker(&mutex_);
再然后，就需要完成上面框图中Global SLAM的第三个任务，对在后端进行SPA优化过程中新增的节点的位姿进行调整，以适应优化后的世界地图和运动轨迹。 获取后端优化器中子图和路径节点的数据，用临时对象submap_data和node_data记录之，并遍历所有的轨迹。
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循环中遍历所有的节点，用优化后的位姿来更新轨迹点的世界坐标。
for (const auto& node : node_data.trajectory(trajectory_id)) {
auto& mutable_trajectory_node = 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);
}
然后计算SPA优化前后的世界坐标变换关系，并将之左乘在后来新增的路径节点的全局位姿上，得到修正后的轨迹。
const auto local_to_new_global = ComputeLocalToGlobalTransform(submap_data, trajectory_id);
const auto local_to_old_global = ComputeLocalToGlobalTransform(global_submap_poses_, 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(trajectory_nodes_.find(last_optimized_node_id));
for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id); ++node_it) {
auto& mutable_trajectory_node = trajectory_nodes_.at(node_it->id);
mutable_trajectory_node.global_pose = old_global_to_new_global * mutable_trajectory_node.global_pose;
}
}
最后，更新路标位姿，并用成员变量global_submap_poses_记录下当前的子图位姿。
for (const auto& landmark : optimization_problem_->landmark_data())
landmark_nodes_[landmark.first].global_landmark_pose = landmark.second;
global_submap_poses_ = submap_data;
}
2. 后端优化的数据准备
在分析了位姿图的更新过程之后，我们大体了解到， PoseGraph2D通过接口AddNode接收来自Local SLAM的子图和路径节点数据将之保存在容器对象submap_data_和trajectory_nodes_中。并通过工作对列调用函数ComputeConstraintsForNode计算路径节点与子图之间的约束关系， 在该函数中除了通过约束构建器计算约束检查可能存在的闭环之外，它将子图和路径节点的初始位姿提供给后端优化器了。提供路径节点只是通过调用优化器的接口AddTrejectoryNode实现的， 子图的提供则是通过PoseGraph2D的成员函数InitializeGlobalSubmapPoses完成的。现在我们回过头来看一下该函数的实现。
下面是该函数的代码片段，它有三个输入参数。其中trajectory_id是运行轨迹的索引，time则是调用AddNode时对应路径节点的时间戳， insertion_submaps则是从Local SLAM一路传递过来的新旧子图。 该函数的输出是一个vector容器，它将记录insertion_submaps中各个子图分配的SubmapId。在函数的一开始先检查insertion_submaps非空，并获取后端优化器的子图位姿信息记录到临时对象submap_data中。
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());
const auto& submap_data = optimization_problem_->submap_data();
根据Local SLAM中子图的维护方式，如果输入参数insertion_submaps中只有一个子图， 意味着重新开始了一条新的轨迹。此时检查后端优化器中是否已经存在一条索引为trajectory_id的轨迹，若没有则创建一个。首先在第7到10行中，检查trajectory_id是否与之前的轨迹有关联， 若有则根据initial_trajectory_poses_的描述构建连接关系。然后通过后端优化器的接口AddSubmap创建一条新的轨迹，并将子图的全局位姿信息喂给优化器。
if (insertion_submaps.size() == 1) {
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
if (initial_trajectory_poses_.count(trajectory_id) > 0) {
trajectory_connectivity_state_.Connect(trajectory_id,
initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time);
}
optimization_problem_->AddSubmap(trajectory_id,
transform::Project2D(ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id)
* insertion_submaps[0]->local_pose()));
}
接着，检查一下数据关系，为新建的子图赋予唯一的SubmapId，并返回之。
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
const SubmapId submap_id{trajectory_id, 0};
CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front());
return {submap_id};
}
如果函数的控制流走到了下面的代码片段，说明trajectory_id下已经至少有了一个子图，此时输入的insertion_submaps中应当有两个子图，也就是Local SLAM中维护的新旧子图。 用局部变量last_submap_id记录下后端优化器中最新子图的索引。
CHECK_EQ(2, insertion_submaps.size());
const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
const SubmapId last_submap_id = std::prev(end_it)->id;
然后根据last_submap_id检查一下后端优化器中最新的子图是否与insertion_submaps中的旧图是同一个对象。若是，则说明新图是Local SLAM新建的子图后端尚未记录。 此时需要将新图的全局位姿提供给后端优化器，并分配一个SubmapId。然后将新旧子图的SubmapId放到容器中一并返回。
if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) {
const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
optimization_problem_->AddSubmap(trajectory_id, first_submap_pose *
constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
constraints::ComputeSubmapPose(*insertion_submaps[1]));
return {last_submap_id, SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
}
最后只剩下一种情况，Local SLAM并没有再新建子图了，此时后端中记录了所有的子图，只需要将新旧子图对应的SubmapId返回即可。
CHECK(submap_data_.at(last_submap_id).submap == insertion_submaps.back());
const SubmapId front_submap_id{trajectory_id, last_submap_id.submap_index - 1};
CHECK(submap_data_.at(front_submap_id).submap == insertion_submaps.front());
return {front_submap_id, last_submap_id};
}
实际上，后端优化器除了要处理子图和路径节点的世界坐标关系之外，它还考虑了里程计、IMU等传感器的数据。 如下面的代码所示，PoseGraph2D中直接通过后端优化器的接口将这些数据喂给对象optimization_problem_。
void PoseGraph2D::AddImuData(const int trajectory_id, const sensor::ImuData& imu_data) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { optimization_problem_->AddImuData(trajectory_id, imu_data); });
}
void PoseGraph2D::AddOdometryData(const int trajectory_id, const sensor::OdometryData& odometry_data) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { optimization_problem_->AddOdometryData(trajectory_id, odometry_data); });
}
3. 后端优化器的成员变量和构造函数
后端优化器对象optimization_problem_的数据类型是OptimizationProblem2D， 派生自模板类OptimizationProblemInterface， 如下面的代码片段所示。该模板类有三个模板参数，其中NodeSpec2D和SubmapSpec2D分别用于描述路径节点和子图的位姿，transform::Rigid2d则说明了描述相对位姿的数据类型。
class OptimizationProblem2D : public OptimizationProblemInterface<NodeSpec2D, SubmapSpec2D, transform::Rigid2d>
NodeSpec2D和SubmapSpec2D是定义在头文件optimization_problem_2d.h中的两个结构体，如下面的代码所示。每个路径节点除了记录了全局的位姿信息之外，还有局部的位姿信息、重力方向，以及产生节点的时间。而子图则只记录了其在世界坐标系中的位姿。
|
|
下表中列举了该类的所有成员变量，这些成员变量基本上都是容器，它们记录了子图和路径节点的位姿信息，记录路标点以及IMU里程计的传感器信息。
| 对象名称 | 对象类型 | 说明 |
|---|---|---|
| options_ | OptimizationProblemOptions | 后端优化问题求解器的各种配置 |
| node_data_ | MapById<NodeId, NodeSpec2D> | 记录路径节点的位姿信息的容器 |
| submap_data_ | MapById<SubmapId, SubmapSpec2D> | 记录子图位姿信息的容器 |
| landmark_data_ | std::map<std::string, transform::Rigid3d> | 记录路标点信息的容器 |
| imu_data_ | sensor::MapByTime<sensor::ImuData> | 根据时间关系记录的IMU数据 |
| odometry_data_ | sensor::MapByTime<sensor::OdometryData> | 根据时间关系记录的里程计数据 |
下面是构造函数与析构函数，没有什么特别的操作，只是保存了各种配置而已。此外，还屏蔽了拷贝构造和拷贝赋值。
OptimizationProblem2D::OptimizationProblem2D(const proto::OptimizationProblemOptions& options) : options_(options) {}
OptimizationProblem2D::~OptimizationProblem2D() {}
OptimizationProblem2D(const OptimizationProblem2D&) = delete;
OptimizationProblem2D& operator=(const OptimizationProblem2D&) = delete;
类OptimizationProblem2D还定义了很多接口函数，用于将路径节点、子图以及各种传感器的数据添加到对应的容器中，其函数实现也很简单直白不再赘述。实际起关键作用的就是那个Solve函数， 我们将在后文中详细分析它。
4. 后端优化的求解过程
一路顺藤摸瓜下来，我们发现后端优化的核心就是类OptimizationProblem2D的成员函数Solve。在这个函数中，Cartographer通过Ceres库进行优化，调整子图和路径节点的世界位姿。 下面是该函数的代码片段，它有三个参数分别输入了位姿图的约束、运动轨迹、路标点。在函数的一开始如果发现路径节点为空，就没必要进行优化了。
void OptimizationProblem2D::Solve(const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories,
const std::map<std::string, LandmarkNode>& landmark_nodes) {
if (node_data_.empty())
return;
根据Ceres库的使用套路，先创建优化问题对象。同时创建了一些临时的变量，用于描述优化问题。
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
//
MapById<SubmapId, std::array<double, 3>> C_submaps;
MapById<NodeId, std::array<double, 3>> C_nodes;
std::map<std::string, CeresPose> C_landmarks;
bool first_submap = true;
bool freeze_landmarks = !frozen_trajectories.empty();
在一个for循环中，我们遍历所有的子图，将子图的全局位姿放置到刚刚构建的临时容器C_submaps中，并通过AddParameterBlock显式的将子图全局位姿作为优化参数告知对象problem。 如果是第一幅子图，或者已经冻结了，就通过SetParameterBlockConstant将对应的参数设定为常量，那么Ceres在迭代求解的过程中将不会改变这些参数。
for (const auto& submap_id_data : submap_data_) {
C_submaps.Insert(submap_id_data.id, FromPose(submap_id_data.data.global_pose));
problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3);
const bool frozen = frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
if (first_submap || frozen) {
first_submap = false;
problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());
}
}
同样的遍历所有的路径节点，将他们的全局位姿作为优化参数告知对象problem。如果该节点所在的路径被冻结了，就将它所对应的优化参数设为常量。
for (const auto& node_id_data : node_data_) {
const bool frozen = frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.global_pose_2d));
problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3);
if (frozen)
problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).data());
}
紧接着就是遍历所有的约束，描述优化问题的残差块。函数CreateAutoDiffSpaCostFunction用于提供对应约束的SPA代价计算。如果是通过闭环检测构建的约束，则为之提供一个Huber的核函数， 用于降低错误的闭环检测对最终的优化结果带来的负面影响。
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
problem.AddResidualBlock(CreateAutoDiffSpaCostFunction(constraint.pose),
// Only loop closure constraints should have a loss function.
constraint.tag == Constraint::INTER_SUBMAP ? new ceres::HuberLoss(options_.huber_scale()) : nullptr,
C_submaps.at(constraint.submap_id).data(), C_nodes.at(constraint.node_id).data());
}
然后调用成员函数AddLandmarkCostFunctions，根据路标点添加残差项。并且遍历所有的路径节点，根据Local SLAM以及里程计等局部定位的信息建立相邻的路径节点之间的位姿变换关系。 并将之描述为残差项提供给problem对象。
AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_, &C_nodes, &C_landmarks, &problem);
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
// 因为太长了，省略一段比较重要的代码
// 这段代码主要是给相邻路径节点之间添加约束的。
}
在描述完优化参数和残差计算方式之后，我们就可以通过ceres求解优化问题。局部对象summary将记录整个求解过程。
ceres::Solver::Summary summary;
ceres::Solve(common::CreateCeresSolverOptions(options_.ceres_solver_options()), &problem, &summary);
if (options_.log_solver_summary())
LOG(INFO) << summary.FullReport();
最后记录下优化结果。
// Store the result.
for (const auto& C_submap_id_data : C_submaps)
submap_data_.at(C_submap_id_data.id).global_pose = ToPose(C_submap_id_data.data);
for (const auto& C_node_id_data : C_nodes)
node_data_.at(C_node_id_data.id).global_pose_2d = ToPose(C_node_id_data.data);
for (const auto& C_landmark : C_landmarks)
landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
}
4. 完
本文，我们又重新分析了一下Global SLAM的系统框图。其中关于约束的构建我们已经在约束构建器和闭环检测中进行了介绍， SPA优化以及优化后路径节点的位姿调整是在PoseGraph2D的成员函数RunOptimization中进行的。本文，我们还简要分析了后端优化求解器的数据结构以及基于Ceres库的SPA求解方法。