后端总结
Kong Liangqian Lv6
 2021-12-17 18:38:29  

优化后的操作

保存优化的变量内容

位于文件mapping/internal/optimization/optimization._problem._2d.cc，solve函数中，

这些变量全部保存在优化类OptimizationProblem2D中

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
// 将优化后的所有数据进行更新 Store the result.
for (const auto& C_submap_id_data : C_submaps) {
  submap_data_.at(C_submap_id_data.id).global_pose =
      ToPose(C_submap_id_data.data);
}
for (const auto& C_node_id_data : C_nodes) {
  node_data_.at(C_node_id_data.id).global_pose_2d =
      ToPose(C_node_id_data.data);
}
for (const auto& C_fixed_frame : C_fixed_frames) {
  trajectory_data_.at(C_fixed_frame.first).fixed_frame_origin_in_map =
      transform::Embed3D(ToPose(C_fixed_frame.second));
}
for (const auto& C_landmark : C_landmarks) {
  landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
}

还有一份其实存在PoseGraph2D的RunOptimization函数中,

分别获取了优化后的节点和子图信息

1
2
3
4
5
6
7
// 获取优化后的结果
// submap_data的类型是 MapById<SubmapId, optimization::SubmapSpec2D> 
const auto& submap_data = optimization_problem_->submap_data();

// node_data的类型是 MapById<NodeId, NodeSpec2D>
// node_data是优化后的所有节点的新位姿
const auto& node_data = optimization_problem_->node_data();

更新节点的信息，并且对没有处理的节点进行简单的位姿预测更新

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
// 更新轨迹内的节点位置
for (const int trajectory_id : node_data.trajectory_ids()) {

  // Step: 根据优化后的结果对data_.trajectory_nodes的global_pose进行更新
  for (const auto& node : node_data.trajectory(trajectory_id)) {
    // node 是 IdDataReference 类型
    // mutable_trajectory_node是TrajectoryNode类型
    auto& mutable_trajectory_node = data_.trajectory_nodes.at(node.id);
    // 将优化后的二维节点位姿旋转到机器人的姿态上得到global_pose
    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.
  // 推断尚未包含在“optimization_problem_”中的所有点云姿势

  // 根据submap_data最后一个被优化的位姿, 计算global坐标系指向local坐标系的坐标变换
  const auto local_to_new_global =
      ComputeLocalToGlobalTransform(submap_data, trajectory_id);
  // 优化前的 global坐标系指向local坐标系的坐标变换
  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();
  // 这一次优化的node的最后一个id
  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));

  // Step: 根据之前的位姿改变量, 对没有优化过的位姿进行校正
  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;
  }

} // end for trajectory_id

更新landmark和submap的位姿

1
2
3
4
5
6
7
8
  // 更新data_.landmark_nodes
  for (const auto& landmark : optimization_problem_->landmark_data()) {
    data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;
  }

  // 更新所有submap的位姿
  data_.global_submap_poses_2d = submap_data;
}

优化前后的不变量和改变量

不变量

  • 第一帧子图在global坐标系下的位姿不会被优化所改变
1
problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());

由于优化前后第一个子图原点在global坐标系下的位姿是不会发生变化的, 所以global坐标系是不
会改变的

  • 冻结轨迹的 节点pose, 子图pose, landmark的pose 是不变的

变化量

  • 除第一个子图外的其他子图的global坐标系下的pose
  • 所有节点在global坐标系下的pose
  • 所有landmark数据在global坐标系下的pose
  • gps第一帧数据在global坐标系下的pose

优化有什么作用

  • 第一个贡献 优化了位姿: 子图, 节点, landmark, gps第一帧坐标
  • 第二个贡献 预测了没有优化的节点位姿
  • 第三个贡献 ComputeLocalToGlobalTransform

可以保证在优化之后，每一次计算从局部到全局的变化都是最新的

1
2
3
4
5
6
7
8
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)
{
// 将节点在local坐标系下的坐标转成global坐标系下的坐标
	const transform::Rigid3d optimized_pose(
		GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);

ComputeLocalToGlobalTransform() 函数是计算最后一个子图原点的global坐标指向local坐标的坐标变换, 为什么要是最后一个子图的坐标呢?

local坐标系下的位姿都是相对与前一个节点的相对位姿, 时间越短, 相对位姿越准.

优化后的最后一个子图位姿, 与当前节点的位姿在时间上距离最近.

所以, 最后的一个优化后的子图位姿的相对坐标变换, 在时间上最接近当前节点与当前子图, 时间越短, 乘以相对坐标变换后得到的节点或者子图在global坐标系下的位姿越接近真实值

 Comments