优化后的recover有两个目的:
1. rviz显示
2. for 下一次优化
具体的recover操作在pose_graph_2d.cc 的 RunOptimization 函数:
//正式进行优化的函数.
void PoseGraph2D::RunOptimization()
{
if (optimization_problem_->submap_data().empty())
{
return;
}
// No other thread is accessing the optimization_problem_,
// data_.constraints, data_.frozen_trajectories and data_.landmark_nodes
// when executing the Solve. Solve is time consuming, so not taking the mutex
// before Solve to avoid blocking foreground processing.
// 正式进行求解.
optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
data_.landmark_nodes);
absl::MutexLock locker(&mutex_);
// 利用优化结果recover
// 1. recover node
const auto& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_->node_data();
for (const int trajectory_id : node_data.trajectory_ids())
{
for (const auto& node : node_data.trajectory(trajectory_id))
{
auto& mutable_trajectory_node = data_.trajectory_nodes.at(node.id);
mutable_trajectory_node.global_pose =
transform::Embed3D(node.data.global_pose_2d) *
transform::Rigid3d::Rotation(
mutable_trajectory_node.constant_data->gravity_alignment);
}
// Extrapolate all point cloud poses that were not included in the
// 'optimization_problem_' yet.
const auto local_to_new_global =
ComputeLocalToGlobalTransform(submap_data, trajectory_id);
const auto local_to_old_global = ComputeLocalToGlobalTransform(
data_.global_submap_poses_2d, trajectory_id);
const transform::Rigid3d old_global_to_new_global =
local_to_new_global * local_to_old_global.inverse();
const NodeId last_optimized_node_id =
std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
auto node_it =
std::next(data_.trajectory_nodes.find(last_optimized_node_id));
for (; node_it != data_.trajectory_nodes.EndOfTrajectory(trajectory_id);
++node_it) {
auto& mutable_trajectory_node = data_.trajectory_nodes.at(node_it->id);
mutable_trajectory_node.global_pose =
old_global_to_new_global * mutable_trajectory_node.global_pose;
}
}
// 2. recover landmark
for (const auto& landmark : optimization_problem_->landmark_data()) {
data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;
}
// 3. recover submap
data_.global_submap_poses_2d = submap_data;
}
注意:data_.global_submap_poses_2d = submap_data; 只是更新了 data中的子图全局位姿变量,for 显示;并未改变真正的 子图 变量,所以,在之后的前端中,依旧在未闭环的pose上进行match。不过,不必担心,因为前端得到的是local pose , 进入优化器时 需要转换为 global pose 。 这里需要计算 local 2 global 的变换关系,其实就是最新的闭环误差。
for 优化:
optimization_problem_ 内,在优化完成后,顶点的pose自动更新了
for 显示:
在node.cc文件中,
1)轨迹显示:
PublishTrajectoryNodeList ----> GetTrajectoryNodeList ------>
pose_graph()->GetTrajectoryNodePoses() // 获得实时轨迹信息: pose graph 的 data.nodepose.globalpose ;即经过优化的全局位姿
2) 子图显示:
PublishSubmapList -------> pose_graph()->GetAllSubmapPoses()
--------> GetSubmapDataUnderLock
// 位于pose_graph_2d.cc 获得子图的pose
PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapDataUnderLock(
const SubmapId& submap_id) const {
const auto it = data_.submap_data.find(submap_id);
if (it == data_.submap_data.end()) {
return {};
}
auto submap = it->data.submap;
// 如果所需子图经过优化了,则返回优化后的pose
if (data_.global_submap_poses_2d.Contains(submap_id)) {
// We already have an optimized pose.
return {submap,
transform::Embed3D(
data_.global_submap_poses_2d.at(submap_id).global_pose)};
}
// 否则就返回未优化的pose
// We have to extrapolate.
return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_2d,
submap_id.trajectory_id) *
submap->local_pose()};
}
下一步,咋rviz显示概率栅格图呢?概率值在哪里体现呢?