cartographer 后端优化流程
目录
子图的保存与任务处理函数
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坐标系系下, 子图原点指向校准后的节点间的坐标变换