当前位置： 首页 > news >正文

cartographer 后端优化流程

news 2025/8/13 8:48:15

目录

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

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坐标系系下, 子图原点指向校准后的节点间的坐标变换
http://www.dtcms.com/a/327923.html

相关文章：

  • 渗透测试现已成为 CISO 战略的核心
  • @RequestMapping接收文件格式的形参（方法参数）
  • 数字孪生赋能全场景智慧化：从水利工厂到城市治理的综合解决方案
  • Akamai字符串解混淆
  • RSA各种密钥格式
  • C++ Rust与Go
  • 【taro react】 ---- 实现 RuiPaging 滚动到底部加载更多数据
  • 使用 Docker 一键部署火山引擎 Sandbox-Fusion，并开放 8182 端口
  • QT6 如何在Linux Wayland 桌面系统抓屏和分享屏幕
  • 力扣hot100 | 双指针 | 283. 移动零、11. 盛最多水的容器、42. 接雨水
  • 2787. 将一个数字表示成幂的和的方案数
  • 三维工厂设计软件 AutoCAD Plant 3D 安装图文教程
  • 3DTiles转OSGB格式逆向转换方法研究
  • 国产3D大型装配设计新突破②：装配约束智能推断 | 中望3D 2026
  • Go之封装Http请求和日志
  • 【新启航】飞机起落架减震筒的深孔测量方法探究 - 激光频率梳 3D 轮廓检测
  • 简单认识CSRF
  • 常见认证信息的传递方式
  • 深入理解数据库架构：从原理到实践的完整指南
  • 【QT】QT6下载安装
  • @（AJAX）
  • JS 模块化与打包工具
  • 基于Hadoop的农产品价格数据分析与可视化【Springboot】
  • 【已解决】win10为什么git无法弹出用户登录框呢？
  • 家政小程序系统开发：推动家政行业数字化转型，共创美好未来
  • unity shader ——屏幕故障
  • hashmap如何解决碰撞
  • Pytorch编译
  • 1.Ansible 自动化介绍
  • 网站测评-利用缓存机制实现XSS的分步测试方法