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

成都私人视频网站制作平台沈阳看男科的权威医院

news 2025/9/30 11:17:50
成都私人视频网站制作平台,沈阳看男科的权威医院,网页生成应用工具,湖北建设厅网站查询目录 子图的保存与任务处理函数 2D后端优化 子图间约束的计算(回环检测） 基于多分辨率地图的分支定界粗匹配 后端计算位姿的方法总结 几种计算相对位姿的方式 4种计算相对位姿的方式总结 节点与约束的概念节点: tracking_frame的位姿, 子图原点的位姿 约束: trackin…

目录

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

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/423893.html

相关文章：

  • 从零开始掌握 uv：新一代超快 Python 项目与包管理器（含 Windows 支持）
  • 天津网站建设 泰姆仕四川网络营销推广
  • 政务网站建设经验做法东莞企业网站建设预算大概多少
  • 怎么做网站icp备案有没有学做家具的网站
  • 新天力科技冲刺北交所：供应链韧性成资本市场“加分项”
  • Linux之线程池
  • 摄影师网站html5wordpress分类加密
  • 免费机械网站模板绵阳市建设工程监督网站
  • React Native 中的 useCallback
  • 哪些网站做的比较好看480元做网站
  • DSC蓝宝石法：精准测量材料比热容的核心技术
  • 在 Kubernetes 集群中运行并发布应用程序
  • Node.js面试题及详细答案120题（81-92） -- 框架与生态篇
  • 永久网站域名注册网页版传奇大全
  • 软考~系统规划与管理师考试—知识篇—第二版—1.2 信息技术及其发展
  • 常德网站开发服务抚顺网络推广
  • 建设网站的个人心得青冈网站建设
  • LeetCode 分类刷题：33. 搜索旋转排序数组
  • Pi Network创始人Dr. Chengdiao Fan将在TOKEN2049发表演讲，探讨加密货币现实应用
  • 网站建设工具哪家好邵阳网站建设制作
  • 【WSL2】win11访问ubuntu
  • 网站建设专家排名信誉好的龙岗网站设计
  • SpringWebFlux：响应式Web框架
  • 网站建设中的图片及视频要求青岛的互联网企业
  • CS231n 2025——作业参考与学习笔记导航页
  • 【Android之路】 Kotlin 的 data class、enum class、sealed interface
  • 公司网站注册要多少钱网页设计作业 介绍家乡
  • [特殊字符]函数指针：C语言的动态灵魂，嵌入式的超能力（202589)
  • 海口网站建设高端asp.net做电商网站
  • C++ 面向对象进阶：继承深化与多态详解