讲解关于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 与 OptimizationProblem2D 的交互过程。且知道最后执行优化的指令是 PoseGraph2D::RunOptimization() 中的如下代码：
optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),data_.landmark_nodes);
另外我们该解答了:
疑问 1 : \color{red}疑问1: 疑问1: global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。
也就是说，针对于前面的所有困惑都在源码中找到了答案。当然，此时又出现了新的疑问，那就是 optimization_problem_->Solve() 具体是如何实现优化的。根据前面的分析，可以知道该接收的实参还是比较简单的 分别是节点与子图的约束 data_.constraints、轨迹的状态 GetTrajectoryStates()、以及landmark数据 data_.landmark_nodes。
二、相对位姿
对 OptimizationProblem2D 进行正式分析之前，需要回顾一下前面的内容，那就是变换关系，例如 local 系到 robot 系，global 系到 robot 系，submap 系到 robot 系，亦或者 global 系到 local 系的坐标变换，或者是多个坐标系之间的相对位姿。
1、节点在global系下的位姿
// 将节点在local坐标系下的坐标转成global坐标系下的坐标
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
GetLocalToGlobalTransform 返回一个由 local 系到 global 系的位姿变换，注意但是 constant_data->local_pose 是在前端被优化过的。之前有提到过，GetLocalToGlobalTransform() ，当轨迹 data_.global_submap_poses_2d 中没有任何子图global位姿时，返回单位变换 transform::Rigid3d::Identity()，可以简单理解为第一个子图的 global 系与 lcoal 的原点是相同的。
2、submap 在 global 坐标系下的位姿
需要注意的是，子图增加之后，data_.global_submap_poses_2d 有了数据之后，则是利用最后一个子图的 global 位姿乘以 最后一个子图 local 系位姿，求得 global 到 local 系的位姿变换。
这样分析下来似乎出现了一个疑问， 那就是 global 系与 local 第一个子图的位姿是相同的， 那他们后面子图的位姿会存在差距吗？前面在分析 PoseGraph2D::InitializeGlobalSubmapPoses() 时提到，子图 global 位姿，是根据节点在相邻子图中的位姿，求解处两子图的位姿变换，再根据前一子图的 global 位姿(第1个子图的global系位姿与其local 位姿相同)，技术出新子图的全局位姿。这里可以看出，子图 global 位姿有个累乘的关系。
子图的 global 位姿与 local 位姿虽然起点相同，但是子图的 global 位姿在后端是有一直进行优化的，所以随着时间的推移，子图的增加，他们之间的位姿是存在差距的。
3、子图内约束(节点所属子图系到该节点的位姿变换)
PoseGraph2D::ComputeConstraintsForNode() 函数中，可以看到如下代码：
// 计算该Node投影到平面后的位姿 gravity_alignment是机器人的姿态
const transform::Rigid2d local_pose_2d =
transform::Project2D(constant_data->local_pose * // 三维转平面
transform::Rigid3d::Rotation(
constant_data->gravity_alignment.inverse()));
// 计算该Node在global坐标系下的二维位姿
// global_pose * constraints::ComputeSubmapPose().inverse() = globla指向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;
其首先求得子图 local 系到 global 系的坐标变换，在把节点在 local 系下的位姿变换到 global 系。这里的 optimization_problem_->submap_data().at(matching_id).global_pose 位姿可能被优化了，也可能没有被优化，但是 local_pose_2d 是经过前端扫描匹配优化过的，我们认为其是比较精确的
4、子图间约束(节点非所属子图系到该节点的位姿变换)
子图间约束的计算分两种，一种时全局的，也就是节点与所有完成的子图都计算约束关系(只在重定位模式下使用)，另外一种就是局部，节点只与一定距离内的子图建立约束(只在建图模式中使用)，其主要涉及到代码如下：
// submap原点在global坐标系下的坐标的逆 * 节点在global坐标系下的坐标 = submap原点指向节点的坐标变换
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;
// Step:1 得到节点在local frame下的位姿，如果是全局匹配 initial_relative_pose=transform::Rigid2d::Identity(),
const transform::Rigid2d initial_pose =ComputeSubmapPose(*submap) * initial_relative_pose;
// Step:4 获取节点到submap坐标系原点间的坐标变换
// pose_estimate 是 节点在 loacl frame 下的坐标
const transform::Rigid2d constraint_transform =ComputeSubmapPose(*submap).inverse() * pose_estimate;
这里的约束经先通过多分辨率地图的分支定界与暴力匹配的到粗解，在通过 ceres 进行优化，让所有的点云都尽量打在障碍物上。所以其可以说是精确解。
5、总结
（ 1 ）： \color{blue}（1）： （1）： 节点 通过 GetLocalToGlobalTransform * constant_data->local_pose 进行 global 下位姿的计算
（ 2 ）： \color{blue}（2）： （2）：子图 通过对前一个子图到后一个子图的坐标变换进行累计, 得到子图在 global 坐标系下的位姿
（ 3 ）： \color{blue}（3）： （3）：子图内约束 local 坐标系系下, 子图原点指向节点间的坐标变换
（ 4 ）： \color{blue}（4）： （4）： 子图间约束 根据 global 坐标计算初值, 然后通过分支定界算法粗匹配与 ceres 的精匹配, 获取校准后的位姿, 最后计算 local 坐标系系下, 子图原点指向校准后的节点间的坐标变换
三、后端优化核心理论
个人理解 : \color{red}个人理解: 个人理解: 这里，本人抛砖引为引玉，谈一下个人对后端优化的理解。首先后端优化是从一个整理来考虑的，其基于 global 系。利用到到子图与子图之间的变换关系，可是在前端(基于local系)过程中，子图位姿是单个相对于 local系 的位姿，可以说多个子图之间他们是没有直接联系到一起的。除了子图，节点也一样，前端的节点都是根据活跃子图计算出相对于 local系 的位姿，但是却没有和前面已经完成的子图联系到一起，也显得比较孤立。
但是在后端优化中，这些都被考虑了进去，其估算出来的节点位姿，不仅希望其在活跃的子图位姿比较准确，还希望其相对于其他子图的位姿也比较准确，并且子图与子图之间的位姿也比较准确。同时呢，还要这些准确的位姿传递给到前端，因为全局优化是以一定频率进行优化，而不是只优化一次。
优化的过程，就是对位姿进行调整，让约束(精确解)不变的情况下，对节点与子图的global位姿进行调整优化，优化过的位姿节点位姿与子图global位姿分别保存在 PoseGraph2D::data_::trajectory_nodes 与 PoseGraph2D::data_::global_submap_poses_2d 之中。另外，后端优化中还可以结合了 OdometryData、FixedFramePoseData、ImuData、LandmarkData 等数据对位姿进行优化，也可以理解为约束。
总的来说，就是保证约束不变的前提下，对其他的位姿进行调整，就好像又一个渔网，关系错综复杂，现在我们要把这个网展开。应该怎么做呢？可以找其中有特点的地方，比如四个角，或者之前打过标识的地方，先把这些位置固定好，固定之后再一点一点的调整其他的位置。直到把网铺平，展开为止。
最后，本人为了验证前面提到的， local 系与 global系初始位姿相同，后续虽然存在一定误差，但是不会太大，所以本人在 Result PoseGraph2D::ComputeConstraintsForNode() 函数中的代码篇段中增加了cout输出：
// 计算该Node在global坐标系下的二维位姿
// global_pose * constraints::ComputeSubmapPose().inverse() = globla指向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;
std::cout << optimization_problem_->submap_data().at(matching_id).global_pose *
constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse()
<< std::endl;
打印的是 global 系 local 系的变换，子图相同时，打赢时一样的，我就过滤，摘录如下：
{
t: [0, 0], r: [0] } submap (0, 0).
{
t: [0, 0], r: [0] } submap (0, 1).
......
{
t: [0.0942178, -0.166682], r: [-0.00452089] } submap (0, 7)
......
可以看到这些数值差距都是很小的，t 表示x,y 轴的距离差值，r 表示旋转弧度差。这里应该是以栅格，或者说像素为单位的。
四、OptimizationProblem2D::Solve() 输入
基于前面的理论，在来分析优化相关的代码就不会显得那么迷惘。来看 optimization_problem_2d.cc 文件中的OptimizationProblem2D::Solve() 函数，该函数是有点恐怖，竟然有200多行代码。本人也只能一点一点的讲解，就暂时不给整体注释了。先看到如下代码：
/**
* @brief 搭建优化问题并进行求解
*
* @param[in] constraints 所有的约束数据
* @param[in] trajectories_state 轨迹的状态
* @param[in] landmark_nodes landmark数据
*/
void OptimizationProblem2D::Solve(
const std::vector<Constraint>& constraints,
const std::map<int, PoseGraphInterface::TrajectoryState>&
trajectories_state,
const std::map<std::string, LandmarkNode>& landmark_nodes) {
if (node_data_.empty()) {
// Nothing to optimize.
return;
}
// 记录下所有FROZEN状态的轨迹id
std::set<int> frozen_trajectories;
for (const auto& it : trajectories_state) {
if (it.second == PoseGraphInterface::TrajectoryState::FROZEN) {
frozen_trajectories.insert(it.first);
}
}
输入参数就是来自于 PoseGraph2D::data_::constraints 的约束，PoseGraph2D::GetTrajectoryStates() 的轨迹状态、以及 PoseGraph2D::data_::landmark_nodes 的 landmark 数据。接着判断一下目前节点数据 node_data_ 是否为空，为空则不进行优化。同时把冻结轨迹 di 记录在 frozen_trajectories 变量中。
五、OptimizationProblem2D::Solve() 构建Problem
// 创建优化问题对象
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
其根据 src/cartographer/configuration_files/pose_graph.lua 文件中的 optimization_problem 配置参数构建 ceres::Problem 对象，用于后续的非线性优化。主要有如下参数
-- 优化残差方程的相关参数
optimization_problem = {
huber_scale = 1e1, -- 值越大,（潜在）异常值的影响就越大
acceleration_weight = 1.1e2, -- 3d里imu的线加速度的权重
rotation_weight = 1.6e4, -- 3d里imu的旋转的权重
-- 前端结果残差的权重
local_slam_pose_translation_weight = 1e5,
local_slam_pose_rotation_weight = 1e5,
-- 里程计残差的权重
odometry_translation_weight = 1e5,
odometry_rotation_weight = 1e5,
-- gps残差的权重
fixed_frame_pose_translation_weight = 1e1,
fixed_frame_pose_rotation_weight = 1e2,
fixed_frame_pose_use_tolerant_loss = false,
fixed_frame_pose_tolerant_loss_param_a = 1,
fixed_frame_pose_tolerant_loss_param_b = 1,
log_solver_summary = false,
use_online_imu_extrinsics_in_3d = true,
fix_z_in_3d = false,
ceres_solver_options = {
use_nonmonotonic_steps = false,
max_num_iterations = 50,
num_threads = 7,
},
},
六、OptimizationProblem2D::Solve() 设置优化参数
构建好 ceres::Problem 实例 problem 之后，接着要告诉 problem 那些参数是需要进行优化的。源码中创建了如下三个变量(前三行代码)
// Set the starting point.
// TODO(hrapp): Move ceres data into SubmapSpec.
// ceres需要double的指针, std::array能转成原始指针的形式
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;
因为 ceres 优化只能传递 double 类型的指针，所以构建这三个变量用于临时存储与转换。first_submap 表示其是否为第一个子图，如果是第一个子图，他的全局位姿是不需要优化的(前面已经证明过第一个子图的全局位姿与局部位姿是相等的)，代码如下：
// 将需要优化的子图位姿设置为优化参数
for (const auto& submap_id_data : submap_data_) {
// submap_id的轨迹 是否是 已经冻结的轨迹
const bool frozen =
frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
// 将子图的global_pose放入C_submaps中
C_submaps.Insert(submap_id_data.id,
FromPose(submap_id_data.data.global_pose));
// c++11: std::array::data() 返回指向数组对象中第一个元素的指针
// Step: 添加需要优化的数据 这里显式添加参数块,会进行额外的参数块正确性检查
problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3);
if (first_submap || frozen) {
first_submap = false;
// Fix the pose of the first submap or all submaps of a frozen
// trajectory.
// Step: 如果是第一幅子图, 或者是已经冻结的轨迹中的子图, 不优化这个子图位姿
problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());
}
}
其首先是对添加到 OptimizationProblem2D 中的子图 submap_data_ 进行遍历，判断其所属轨迹是否冻结，无论是否冻结都会转换成 std::array<double, 3> 格式，然后把其global位姿插入到 C_submaps 且调用 problem.AddParameterBlock() 添加到参数块中，不过，如果是冻结的，其会调用 problem.SetParameterBlockConstant() 告知 problem 其是冻结的，不需要进行优化。problem.AddParameterBlock() 函数的调用也十分简单，就是给定一个起始指针，以及元素个数即可。把子图global位姿添加到参数块之后，就是添加节点节点的global位姿到参数块了，代码如下：
// Step: 第一种残差 将节点与子图原点在global坐标系下的相对位姿 与 约束 的差值作为残差项
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
problem.AddResidualBlock(
// 根据SPA论文中的公式计算出的残差的CostFunction
CreateAutoDiffSpaCostFunction(constraint.pose),
// Loop closure constraints should have a loss function.
// 为闭环约束提供一个Huber的核函数,用于降低错误的闭环检测对最终的优化结果带来的负面影响
constraint.tag == Constraint::INTER_SUBMAP // 核函数
? new ceres::HuberLoss(options_.huber_scale()) // param: huber_scale
: nullptr,
C_submaps.at(constraint.submap_id).data(), // 2个优化变量
C_nodes.at(constraint.node_id).data());
}
该处会对所有节点进行优化，即都添加到参数块之中，其不调用 problem.SetParameterBlockConstant() 函数固定位姿，当然，如果节点所属轨迹是冻结的，添加到参数块中，也会被固定，不做优化。
七、结语
通过前面的一系列操作，基本工作已经完成了，下一步就是要进行残差块的构建了，不用多说，这肯定是最核心的部分，下篇博客开始进行详细的分析。