cartographer 后端优化流程

江山如画，佳人北望2025-08-13 8:51

目录

子图的保存与任务处理函数

2D后端优化

[子图间约束的计算(回环检测）](#子图间约束的计算(回环检测）)

基于多分辨率地图的分支定界粗匹配

后端计算位姿的方法总结

几种计算相对位姿的方式

4种计算相对位姿的方式总结

节点与约束的概念
节点 : tracking_frame 的位姿 , 子图原点的位姿
约束 : tracking_frame 与子图原点间的坐标变换
ComputeLocalToGlobalTransform
后端优化的 global 到 local 间的坐标变换 , 如果没提前设置的话就是平移 0 与旋转 0 .
下边的函数 , 如果是后端的第一个节点 , GetLocalToGlobalTransform 会返回 (0, 0, 0), 然后还要乘以这个节点再
local 坐标系下的 pose, 乘的结果才是 global 坐标系下第一个节点的位姿

复制代码 
NodeId PoseGraph2D::AddNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data,
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
// GetLocalToGlobalTransform
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);

transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses,
const int trajectory_id) const {
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
// 没找到这个轨迹id
if (begin_it == end_it) {
const auto it = data_.initial_trajectory_poses.find(trajectory_id);
// 如果设置了初始位姿
if (it != data_.initial_trajectory_poses.end()) {
return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id,it->second.time) *
it->second.relative_pose;
}
// note: 没设置初始位姿就将返回(0,0,0)的平移和旋转
else {
return transform::Rigid3d::Identity();
}
}
// 找到了就获取优化后的最后一个子图的id
const SubmapId last_optimized_submap_id = std::prev(end_it)->id;
// Accessing 'local_pose' in Submap is okay, since the member is const.
// 通过最后一个优化后的 global_pose * local_pose().inverse() 获取 global_pose->local_pose的
坐标变换
// tag: 画图说明一下
return transform::Embed3D(
global_submap_poses.at(last_optimized_submap_id).global_pose) *
data_.submap_data.at(last_optimized_submap_id)
.submap->local_pose()
.inverse();
}

子图的保存与任务处理函数

第一次看到的子图的指针进行保存
data_.submap_data: 保存子图的指针

2D****后端优化

子图内约束的计算
子图内约束 就是 属于这个 submap 的节点 ( 是组成 submap 的节点之一 ) 与这个 submap 坐标系原点间的坐标变换 .
这个坐标变换是在 local 坐标系下 , 由 submap 的坐标系原点指向 tracking_frame 的
InitializeGlobalSubmapPoses
data_.global_submap_poses_2d: 全都是优化后的子图在 global 坐标系下的 pose
optimization_problem_->submap_data(): 包含了优化后和还没有进行优化的 子图在 global 坐标系下的 pose
ComputeLocalToGlobalTransform() 这个函数的参数 , 始终都是 data_.global_submap_poses_2d, 计算的是优化后
的 global 指向 local 的坐标变换

复制代码 
transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses,
const int trajectory_id) const {
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
const SubmapId last_optimized_submap_id = std::prev(end_it)->id;
return transform::Embed3D(
global_submap_poses.at(last_optimized_submap_id).global_pose) *
data_.submap_data.at(last_optimized_submap_id)
.submap->local_pose()
.inverse();
}
// 1 如果只有1个子图
optimization_problem_->AddSubmap(
trajectory_id, transform::Project2D(
ComputeLocalToGlobalTransform( // 会返回(0, 0,0)
data_.global_submap_poses_2d, trajectory_id) *
insertion_submaps[0]->local_pose()));
// 2 有2个子图, 但是第二个子图没保存位姿的情况
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]));

计算子图内约束

复制代码 
const transform::Rigid2d local_pose_2d =
transform::Project2D(constant_data->local_pose * // 三维转平面
transform::Rigid3d::Rotation(
constant_data->gravity_alignment.inverse()));
const transform::Rigid2d global_pose_2d =
optimization_problem_->submap_data().at(matching_id).global_pose *
constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
local_pose_2d;
const transform::Rigid2d constraint_transform =
constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
local_pose_2d;

子图间约束的计算**(**回环检测）

子图间约束 就是 不属于这个 submap 的节点与这个 submap 坐标系原点间的坐标变换 .
与子图内约束是一样的 , 也是在 local 坐标系下 , 由这个 submap 的坐标系原点指向 tracking_frame 的

基于多分辨率地图的分支定界粗匹配

分支定界算法
分支 : 对当前层候选解进行分支 ( 扩充 ), 生成下一层分辨率地图上的 4 个候选解
排序 : 对下一层分辨率地图上的 4 个候选解进行打分并 降序排序
定界 : 将当前层的最高得分 , 当做下一次分支定界算法的分数阈值
剪枝 : 只要当前层的候选解的得分 , 有小于传入的阈值的 , 就 break, 因为是排好序的

后端计算位姿的方法总结

向优化问题中添加数据
添加节点数据

复制代码 
optimization_problem_->AddTrajectoryNode(
matching_id.trajectory_id,
optimization::NodeSpec2D{constant_data->time, local_pose_2d,
global_pose_2d,
constant_data->gravity_alignment});

添加子图坐标原点数据

复制代码 
optimization_problem_->AddSubmap(
trajectory_id,
first_submap_pose *
constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
constraints::ComputeSubmapPose(*insertion_submaps[1]));

添加其他传感器数据

复制代码 
optimization_problem_->AddImuData(trajectory_id, imu_data);
optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
optimization_problem_->AddFixedFramePoseData(trajectory_id, fixed_frame_pose_data);

几种计算相对位姿的方式

节点在 global 坐标系下的位姿

复制代码 
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
ComputeLocalToGlobalTransform() // 传入的始终是data_.global_submap_poses_2d
data_.global_submap_poses_2d // 只有在第一次优化完之后才有数据

节点在 global 坐标系下的第一帧的位姿 , 就是这个节点在 local 坐标系下的位姿 .
之后的节点在 global 坐标系下的位姿 是通过 global 到 local 的坐标变换乘以 local 坐标系下的位姿得到的 .
submap 在 global 坐标系下的位姿

复制代码 
optimization_problem_->AddSubmap(
trajectory_id, transform::Project2D(
• ComputeLocalToGlobalTransform(
• data_.global_submap_poses_2d, trajectory_id) *
• insertion_submaps[0]->local_pose()));
optimization_problem_->AddSubmap(
trajectory_id,
first_submap_pose *
• constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
• constraints::ComputeSubmapPose(*insertion_submaps[1]));

子图在 global 坐标系下的第一帧的位姿 , 就是这个子图在 local 坐标系下的位姿 .
之后的子图在 global 坐标系下的位姿 是通过 第一个子图在 global 坐标系下的 pose 乘以 第一个子图到第二个子图在
local 坐标系下的位姿变换 得到的
子图内约束
local 坐标系下 , 子图原点指向 tracking_frame 的坐标变换

复制代码 
const transform::Rigid2d local_pose_2d =
transform::Project2D(constant_data->local_pose * // 三维转平面
transform::Rigid3d::Rotation(
constant_data->gravity_alignment.inverse()));
// 计算 子图原点 指向 node坐标 间的坐标变换(子图内约束)
const transform::Rigid2d constraint_transform =
constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
local_pose_2d;

子图间约束
先通过子图在 global 坐标系下的坐标的逆 , 乘以节点在 global 坐标系下的坐标 , 获取子图原点在 glboal 坐标系下指向
节点的相对坐标变换 .
然后根据子图在 local 坐标系下的位姿乘以这个坐标变换 , 得到节点在 local 坐标系下的预测位姿 , 在通过分枝定界粗
匹配与 ceres 的精匹配 , 对这个节点位姿进行校准 , 校准后的位姿还是 local 坐标系下的 .
最后 , 通过子图在 local 坐标系下位姿的逆 , 乘以这个节点校准后的位姿 , 得到子图间约束 , 是 local 坐标系下 , 子图原点
指向节点的相对坐标变换

复制代码 
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;
const transform::Rigid2d initial_pose =
ComputeSubmapPose(*submap) * initial_relative_pose;
const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate;

4****种计算相对位姿的方式总结

  1. 节点 通过 GetLocalToGlobalTransform * constant_data - >local_pose 进行 global 下位姿的计算
  2. 子图 通过对前一个子图到后一个子图的坐标变换进行累计 , 得到子图在 global 坐标系下的位姿
  3. 子图内约束 local 坐标系系下 , 子图原点指向节点间的坐标变换
  4. 子图间约束 根据 global 坐标计算初值 , 然后通过分支定界算法粗匹配与 ceres 的精匹配 , 获取校准后的位姿 , 最
    后计算 local 坐标系系下 , 子图原点指向校准后的节点间的坐标变换
热门推荐
01UV安装并设置国内源02全球最强模型Grok4，国内已可免费使用！（附教程）032025最新国内服务器可用docker源仓库地址大全（2025年8月更新）04Qwen3-Coder 快速上手教程 | Qwen Code + Claude Code05[已解决]VSCode右键菜单消失恢复06KGG转MP3工具|非KGM文件|解密音频07GPT-5 使用限制与国内升级全攻略（免费 / Plus / Pro）【2025 最新】08Cursor 终端“卡死/无响应”问题的解法09OpenAI重返开源！GPT-OSS本地部署完全指南10TRAE Rules 实践：为项目配置 6A 工作流