讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

上一篇博客介绍的 landmark 数据的残差与优化，最后本人分享了如下：

$核心理解:$ 在上一篇博客中介绍的约束残差(节点相对与子图位姿的残差)，其主是优化每个节点以及每个子图的位姿。可想而知，其是离散的。但是轨迹是一条连续平滑的曲线，所以本质上我们希望通过插值获取到的位姿也是准确的。恰好， landmark帧的时间点，也激光雷达数据帧的时间点是不一样的，所以如果 landmark帧的时间点位于两个激光雷达数据帧的时间点，那么可以把landmark帧相对于机器人的位姿作为一个约束，对两节点插值之后的位姿进行优化，从而广播到两个节点，间接对两个节点进行优化。

现在回到 OptimizationProblem2D::Solve() 函数，到目前为止，已经把：

  // Add cost functions for landmarks.// Step: landmark数据 与 通过2个节点位姿插值出来的相对位姿 的差值作为残差项AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,&problem, options_.huber_scale());

讲解完成了，不过看起来，任务量还是很多的，似乎还剩下很多代码都没有讲解。起始不然，因为后面的代码比较简单，另外再前面已经接触过类似的代码，分析起来就没有那么困难。

二、残差准备

接着代码 OptimizationProblem2D::Solve() 函数调用的 AddLandmarkCostFunctions() 之后继续分析。首先其又运行了一个for循环，且其中也嵌套一个层for循环，先来看第一层循环，代码如下：

  // 遍历多个轨迹, 添加里程计与local结果的残差for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {// 获取每个节点的轨迹idconst int trajectory_id = node_it->id.trajectory_id;// 获取这条轨迹的最后一个位置的迭代器const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);// 如果轨迹是frozen的, 则无需处理直接跳过if (frozen_trajectories.count(trajectory_id) != 0) {node_it = trajectory_end;continue;}auto prev_node_it = node_it;

需要注意，这里的for循环时对轨迹遍历，并不是对节点的遍历，如果 node_data_ 中包含了多条轨迹的节点，node_data_.begin() 表示 trajectory_id==0 的第一个节点，而 node_data_.end() 表示 trajectory_id 等于最大轨迹的第一个节点。先获得轨迹起始节点 node_it 对应轨迹标识 trajectory_id。然后获得这条轨迹结束位姿的迭代器，记录在 trajectory_end，接着判断一下目前节点对应的轨迹是否为冻结状态，如果为冻结状态则 continue 跳出第一层循环。然后把当前 node_it 记录给 prev_node_it，也就是 prev_node_it 记录的目前轨迹的起始节点。接着代码分析，可以看到其嵌套了一个循环：

    // 遍历一个轨迹的所有节点, 添加里程计与local结果的残差for (++node_it; node_it != trajectory_end; ++node_it) {const NodeId first_node_id = prev_node_it->id;const NodeSpec2D& first_node_data = prev_node_it->data;prev_node_it = node_it;const NodeId second_node_id = node_it->id;const NodeSpec2D& second_node_data = node_it->data;// 如果节点的索引不连续, 跳过if (second_node_id.node_index != first_node_id.node_index + 1) {continue;}

该循环就是对一条轨迹的节点进行遍历，prev_node_it 主要用于记录上一节点，然后判断当前节点与上一节点索引是否连续，如果不连续，则会 continue 跳出循环，该节点不参与下面的残差计算。

另外，其还获取了当前节点与上一节点的节点数据，分别存储于 first_node_data 与 second_node_data。

三、里程计插值

首先根据两个连续的节点数据 first_node_data 与 second_node_data 的时间，然后对里程计数据进行插值处理，获得里程计在这两个时间点的 local 系下的位姿，里程计数据的线性插值，被调代码如下：

      // Add a relative pose constraint based on the odometry (if available).// 根据里程计数据进行插值得到的2个节点间的坐标变换std::unique_ptr<transform::Rigid3d> relative_odometry =CalculateOdometryBetweenNodes(trajectory_id, first_node_data,second_node_data);

函数 CalculateOdometryBetweenNodes() 实现于 optimization_problem_2d.cc 文件中，代码注释如下：

/*** @brief 根据里程计数据算出两个节点间的相对坐标变换* * @param[in] trajectory_id 轨迹的id* @param[in] first_node_data 前一个节点数据* @param[in] second_node_data 后一个节点数据* @return std::unique_ptr<transform::Rigid3d> 两个节点的坐标变换*/
std::unique_ptr<transform::Rigid3d>
OptimizationProblem2D::CalculateOdometryBetweenNodes(const int trajectory_id, const NodeSpec2D& first_node_data,const NodeSpec2D& second_node_data) const {if (odometry_data_.HasTrajectory(trajectory_id)) {// 插值得到time时刻的里程计数据const std::unique_ptr<transform::Rigid3d> first_node_odometry =InterpolateOdometry(trajectory_id, first_node_data.time);const std::unique_ptr<transform::Rigid3d> second_node_odometry =InterpolateOdometry(trajectory_id, second_node_data.time);if (first_node_odometry != nullptr && second_node_odometry != nullptr) {// 根据里程计数据算出的相对坐标变换// 需要注意的是, 实际上在optimization_problem中, node的位姿都是2d平面上的// 而odometry的pose是带姿态的, 因此要将轮速计插值出来的位姿转到平面上transform::Rigid3d relative_odometry =transform::Rigid3d::Rotation(first_node_data.gravity_alignment) *first_node_odometry->inverse() * (*second_node_odometry) *transform::Rigid3d::Rotation(second_node_data.gravity_alignment.inverse());return absl::make_unique<transform::Rigid3d>(relative_odometry);}}return nullptr;
}

其代码也比较简单，就是先判断一下 odometry_data_ 中有没有 trajectory_id 对应的里程计数据，如果有就根据 first_node_data 与 second_node_data 获得需要插值求得位姿的时刻，然后进行插值。这里的插值函数 InterpolateOdometry() 其权重依旧由 time 与前后两个里程计节点的时间距离来决定的。求得 first_node_data.time 与 second_node_data.time 对应插值里程计位姿之后的 local 位姿之后，再进一步获得两节点基于里程计的相对位姿变换。

四、里程计残差

      // Step: 第三种残差 节点与节点间在global坐标系下的相对坐标变换 与 通过里程计数据插值出的相对坐标变换 的差值作为残差项// 如果存在里程计则可增加一个残差if (relative_odometry != nullptr) {problem.AddResidualBlock(CreateAutoDiffSpaCostFunction(Constraint::Pose{*relative_odometry, options_.odometry_translation_weight(),options_.odometry_rotation_weight()}),nullptr /* loss function */, C_nodes.at(first_node_id).data(),C_nodes.at(second_node_id).data());}

如果存在里程计则可增加一个残差，该残差以 relative_odometry 作为帧值(根据里程计计算出来两节点的位姿变换)，然后让根据两节点global系下的节点位姿计算出两节点的相对位姿，优化过程就是让后者向前者靠近。

四、local残差

这是一种比较好理解的残差，再前端求得节点位姿时，使用了扫描匹配，以及Ceres优化，尽量让一帧点云全部打在障碍物上，很明显其时比较优的解，这里我们认为其时正确的，对于相邻的两个节点，都是通过这种方式估算出来的 local 位姿，因为间隔时间短，他们之间的累计误差时可以忽略的，也就是说， local 系下两个节点的相对变换基本同理基本时正确的，可以把其作为一个约束来对待，让 global 系下两个节点之间的位姿变换通过Ceres优化，逼近于local系下对应两个节点的位姿变换。代码如下所示：

      // Add a relative pose constraint based on consecutive local SLAM poses.// 计算相邻2个节点在local坐标系下的坐标变换const transform::Rigid3d relative_local_slam_pose =transform::Embed3D(first_node_data.local_pose_2d.inverse() *second_node_data.local_pose_2d);// Step: 第四种残差 节点与节点间在global坐标系下的相对坐标变换 与 相邻2个节点在local坐标系下的相对坐标变换 的差值作为残差项problem.AddResidualBlock(CreateAutoDiffSpaCostFunction(Constraint::Pose{relative_local_slam_pose,options_.local_slam_pose_translation_weight(),options_.local_slam_pose_rotation_weight()}),nullptr /* loss function */, C_nodes.at(first_node_id).data(),C_nodes.at(second_node_id).data());}}

五、GPS残差-准备工作与约束计算

对于GPS残差部分，又是两个for循环进行嵌套的代码，且与前面十分相似如下，第一层for循环依旧是对轨迹的遍历。

 // 遍历多个轨迹, 添加gps的残差std::map<int, std::array<double, 3>> C_fixed_frames;for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {const int trajectory_id = node_it->id.trajectory_id;const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);if (!fixed_frame_pose_data_.HasTrajectory(trajectory_id)) {node_it = trajectory_end;continue;}const TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);bool fixed_frame_pose_initialized = false;

其先判断一下该轨迹是否存在GPU数据，没有没有，则 continue 遍历下一条轨迹。如果存在则获得该轨迹信息，轨迹信息主要包含了如下内容：

  struct TrajectoryData {double gravity_constant = 9.8;std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};absl::optional<transform::Rigid3d> fixed_frame_origin_in_map;};

先把 fixed_frame_pose_initialized 设置为 false，接着就是对轨迹的每个节点进行遍历了：

    // 遍历一个轨迹的所有节点, 添加gps的残差for (; node_it != trajectory_end; ++node_it) {const NodeId node_id = node_it->id;const NodeSpec2D& node_data = node_it->data;// 根据节点的时间对gps数据进行插值, 获取这个时刻的gps数据的位姿const std::unique_ptr<transform::Rigid3d> fixed_frame_pose =Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time);// 要找到第一个有效的数据才能进行残差的计算if (fixed_frame_pose == nullptr) {continue;}// gps数据到gps第一个数据间的坐标变换const Constraint::Pose constraint_pose{*fixed_frame_pose, options_.fixed_frame_pose_translation_weight(),options_.fixed_frame_pose_rotation_weight()};      

先获得遍历轨迹节的time，利用 FixedFramePoseData 数据进行插值，获得 GPS该时刻的位姿 fixed_frame_pose，需要注意 gps数据是到gps第一个数据间的坐标变换，那么可以为这个变换创建一个约束constraint_pose，其中 constraint_pose.zbar_ij 就是 *fixed_frame_pose。

五、GPS残差-global系到gps系变换

在建立好 约束 constraint_pose 之后，如果 fixed_frame_pose_initialized 不为true，也就说目前遍历的是该轨迹的第一个节点，那么则对GPS位姿进行一个初始化处理：

      // 计算gps坐标系原点在global坐标系下的坐标if (!fixed_frame_pose_initialized) {transform::Rigid2d fixed_frame_pose_in_map;// 如果设置了gps数据的原点if (trajectory_data.fixed_frame_origin_in_map.has_value()) {fixed_frame_pose_in_map = transform::Project2D(trajectory_data.fixed_frame_origin_in_map.value());} // 第一次优化之前执行的是这里else {// 计算gps第一个数据在global坐标系下的坐标, 相当于gps数据的坐标系原点在global坐标系下的坐标fixed_frame_pose_in_map =node_data.global_pose_2d *transform::Project2D(constraint_pose.zbar_ij).inverse();}// 保存gps坐标系原点在global坐标系下的坐标C_fixed_frames.emplace(trajectory_id,FromPose(fixed_frame_pose_in_map));fixed_frame_pose_initialized = true;}

其先判断一下该轨迹是否设置了GPS原点，也就是 trajectory_data.fixed_frame_origin_in_map 参数是否设置，如果设置了，则把其映射到2D平面赋值给 fixed_frame_pose_in_map。

如果没有设置，node_data.global_pose_2d 表示global下机器人位姿，ransform::Project2D(constraint_pose.zbar_ij) 表示机器人在GPS下的位姿，

          fixed_frame_pose_in_map =node_data.global_pose_2d *transform::Project2D(constraint_pose.zbar_ij).inverse();

那么 fixed_frame_pose_in_map 就表示GPS系相对于 Global系的位姿了。也就是说
fixed_frame_pose_in_map 表示GPU第一个数据在global系下的位姿，计算出来之后，fixed_frame_pose_initialized 赋值为 true，就不会再为该轨迹计算 fixed_frame_pose_in_map 了。求得 fixed_frame_pose_in_map 会被添加到 C_fixed_frames 之中。

六、GPS残差-残差构建

最后就是最核心的残差构建部分了，主体代码如下所示：

      // Step: 第五种残差 节点与gps坐标系原点在global坐标系下的相对坐标变换 与 通过gps数据进行插值得到的相对坐标变换 的差值作为残差项problem.AddResidualBlock(CreateAutoDiffSpaCostFunction(constraint_pose),options_.fixed_frame_pose_use_tolerant_loss()? new ceres::TolerantLoss(options_.fixed_frame_pose_tolerant_loss_param_a(),options_.fixed_frame_pose_tolerant_loss_param_b()): nullptr,C_fixed_frames.at(trajectory_id).data(), // 会自动调用AddParameterBlockC_nodes.at(node_id).data());}

constraint_pose 是利用GPS数据插值出来当前遍历节点 node_it 对应的位姿。ceres::TolerantLoss() 同样用于残差结果的抑制异常点。根据 C_fixed_frames.at(trajectory_id).data() 表示GPU系在global系下的位姿，
C_nodes.at(node_id).data()) 表示当前节点在global系下的位姿，那么可以求得当前节点相对于GPS系的位姿。再与约束 constraint_pose 做残差。

七、开始优化，且更新数据

  // Solve. 进行求解ceres::Solver::Summary summary;ceres::Solve(common::CreateCeresSolverOptions(options_.ceres_solver_options()),&problem, &summary);// 如果开启了优化的log输出, 就输出ceres的报告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_fixed_frame : C_fixed_frames) {trajectory_data_.at(C_fixed_frame.first).fixed_frame_origin_in_map =transform::Embed3D(ToPose(C_fixed_frame.second));}for (const auto& C_landmark : C_landmarks) {landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();}

八、结语

到这里位姿，可以说后端优化或者位姿图的讲解基本就完成了，总的来说，后端优化中共有五种残差，分别如下所示：

1、基于节点与子图(子图内，子图间)约束的残差
2、基于Landmark的残差
3、基于odometry里程计的残差
4、基于节点local系下的残差
5、基于GPS数据的残差

下一篇博客，就带代价对后端优化进行一个整理的复盘，让大家的理解更加