沉默杀手HomeArchivesTagsCategories
后端的线程池 3 HandleWorkQueue和优化
|激光SLAMCartographer源码解读|
Word count: 884|Reading time: 3 min

优化的具体实现在优化器optimization_problem_。将优化的结果采用回调的方式，返回到result中。
后端将数据加入到 optimization_problem_ 的对应传感器队列中，并按时间排列。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
void PoseGraph2D::HandleWorkQueue(
    const constraints::ConstraintBuilder2D::Result&  result)
{
  {   // 将新的约束添加到全局约束队列中
    absl::MutexLock locker(&mutex_);
    data_.constraints.insert(data_.constraints.end(), result.begin(),
                             result.end() );
  }
  // ceres 在这里
  RunOptimization();
   			// 以下是 省略 的内容
  //如果设置了全局优化回调函数，则进行调用
  //根据约束结果，更新轨迹间的链接关系
  // 优化完成后，累计节点清零
  // 计算相同轨迹的 inter_constraints_same_trajectory  
  // 计算不同轨迹的 inter_constraints_different_trajectory

  // 优化结束后，重新开启任务队列，即继续执行
  // work_queue_里的 work_item
  DrainWorkQueue();
}

RunOptimization

优化的实际就是成员变量node_data_ 和 submap_data_，也就是

1
2
3
4
5
6
7
8
9
10
11
12
struct NodeSpec2D
{
  common::Time time;
  transform::Rigid2d  local_pose_2d;
  transform::Rigid2d  global_pose_2d;
  Eigen::Quaterniond  gravity_alignment;
};

struct SubmapSpec2D
{
  transform::Rigid2d  global_pose;
};

1
2
3
4
5
6
7
8
9
10
11
12
void PoseGraph2D::RunOptimization()
{
  if (optimization_problem_->submap_data().empty() )
    return;
  // std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_;
  
  // No other thread is accessing the optimization_problem_,
  // data_.constraints, data_.frozen_trajectories and data_.landmark_nodes
  // when executing Solve. 
  // 调用优化，Ceres在这里面，由于耗时间，故没加锁，防止阻塞其他线程
  optimization_problem_->Solve(data_.constraints, 
    GetTrajectoryStates(),  data_.landmark_nodes);

将所有内部约束和外部约束合并在一起执行Solve
遍历所有submap，建立参数块；遍历所有node，建立参数块。
根据约束，添加残差函数；
处理里程计问题，添加可能的残差；
求解返回结果;

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
45
46
47
48
49
50
51
52
  absl::MutexLock locker(&mutex_);
   // 优化后所有的submap和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. 注意是已经加入tarjectory但是还没有进行优化的新Node
    // 由于使用采样器建立约束，有的node没有建立约束，其pose就不会被优化
    // 因此，要通过已经优化的位姿转换关系来修正所有的node

    // 子图的local到global的新的转移矩阵
    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));
     // 后续未优化的节点的全局pose进行转移
    for (; node_it != data_.trajectory_nodes.EndOfTrajectory(trajectory_id);
         ++node_it)
    {
      // TrajectoryNode类型， 指针
      auto& mutable_trajectory_node = data_.trajectory_nodes.at(node_it->id);
      修正 global_pose
      mutable_trajectory_node.global_pose =
          old_global_to_new_global * mutable_trajectory_node.global_pose;
    }
  }
  // 更新所有路标位姿
  for (const auto& landmark : optimization_problem_->landmark_data()) {
    data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;
  }
  data_.global_submap_poses_2d = submap_data;
}

优化并非是实时，是在后台进行运行的，并且需要一定的时间。因此当完成优化时，前端输出结果已经对整个位姿图个数进行了增加。后面新加入的节点并未优化，所以返回优化的结果没有最新加入轨迹节点对应的结果。因此采用优化后结果中最后一个轨迹节点的位姿的转移矩阵，作为未参与优化轨迹节点的转移矩阵进行更新。

其中 为参与优化的节点，优化前位姿为 ，优化后的位姿为 。而 则为未参与优化的节点。