沉默杀手HomeArchivesTagsCategories
后端 3 ComputeConstraintsForNode
|激光SLAMCartographer源码解读|
Word count: 2k|Reading time: 8 min
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
struct Constraint
{
    struct Pose
    {
      transform::Rigid3d   zbar_ij;
      double translation_weight;
      double rotation_weight;
    };
    SubmapId submap_id;  // 'i' in the paper.
    NodeId node_id;      // 'j' in the paper.
    // Pose of the node 'j' relative to submap 'i'.
    Pose pose;
    // Differentiates between intra-submap (where node 'j' was inserted into
    // submap 'i') and inter-submap constraints (where node 'j' was not 
    // inserted  into submap 'i').
    enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
};

现在看PoseGraph2D::AddNode最后的计算约束

1
2
3
4
5
6
7
8
WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
    const NodeId& node_id,
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
    const bool newly_finished_submap)
{
  std::vector<SubmapId> submap_ids;
  std::vector<SubmapId> finished_submap_ids;
  std::set<NodeId> newly_finished_submap_node_ids;

下面的内容都放到一个大括号内了

1
2
3
4
5
6
7
{
  absl::MutexLock locker(&mutex_);
  const auto& constant_data =
      data_.trajectory_nodes.at(node_id).constant_data;
  //如果只有1个子图，则返回index为0，否则返回最新的两个连续的submap id
  submap_ids = InitializeGlobalSubmapPoses(
      node_id.trajectory_id, constant_data->time, insertion_submaps);

这里有重要函数InitializeGlobalSubmapPoses

InitializeGlobalSubmapPoses

这个主要是为了计算论文里的 ，最终返回的是子图Id

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
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
    const int trajectory_id, const common::Time time,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps)
{
  CHECK(!insertion_submaps.empty());
  // std::unique_ptr<optimization::OptimizationProblem2D>  optimization_problem_;
  // MapById<SubmapId, SubmapSpec2D>   submap_data
  const auto& submap_data = optimization_problem_->submap_data();
  // insertion_submaps 只有一个子图的情况
  // 说明这是该trajectory的第一张 submap
  if (insertion_submaps.size() == 1)
  {
    // If we don't already have an entry for the first submap, add one.
    if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0)
    {
      if (data_.initial_trajectory_poses.count(trajectory_id) > 0)
      {
        data_.trajectory_connectivity_state.Connect(
            trajectory_id,
            data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id,
            time);
      }
      // 计算该submap的global_pose(查找轨迹的初始位姿来计算), 后加入优化问题
      // ComputeLocalToGlobalTransform 是 local to global map frame transform
      optimization_problem_->AddSubmap(
          trajectory_id, transform::Project2D(
          ComputeLocalToGlobalTransform(
          data_.global_submap_poses_2d, trajectory_id) *
            insertion_submaps[0]->local_pose()
               )
          );
    }
    CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
    // 该trajectory的第一张 submap， 索引 0
    const SubmapId submap_id{trajectory_id, 0};
    CHECK(data_.submap_data.at(submap_id).submap == insertion_submaps.front());
    return {submap_id};
  }
  // 前端维护两个子图了
  CHECK_EQ(2, insertion_submaps.size());
  const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
  CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
  const SubmapId  last_submap_id  =  std::prev(end_it)->id;
   // submap 是 InternalSubmapData类型的成员
  if (data_.submap_data.at(last_submap_id).submap ==
      insertion_submaps.front()  )
  {
    // 此时，'last_submap_id' is the ID of 'insertions_submaps.front()'
    // 'insertions_submaps.back()' is new，也就是没插入

    // 如果有2个子图，把第1个子图的 global_pose 加入优化问题
    // 计算第一张子图的 global_pose，last_submap_id已经在后端里
    const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
    // ComputeSubmapPose函数  其实就是 submap.local_pose()
    optimization_problem_->AddSubmap(
        trajectory_id,
        first_submap_pose *
            constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
            constraints::ComputeSubmapPose(*insertion_submaps[1])  );
    return { last_submap_id,  
            SubmapId{trajectory_id, last_submap_id.submap_index + 1}  };
  }
  // 不是上面两种情况，也就是已经加入了 back()
  CHECK(data_.submap_data.at(last_submap_id).submap ==
        insertion_submaps.back() );
  const SubmapId front_submap_id{trajectory_id,
                                 last_submap_id.submap_index - 1};
  CHECK(data_.submap_data.at(front_submap_id).submap ==
        insertion_submaps.front());
  // 如果不是上面两种情况，那就返回目前最后的两个子图，front_submap_id是更旧的子图
  return {front_submap_id, last_submap_id};
}

这里之所以出现三种情况，是之前的AppendNode中的data_.submap_data.Append插入子图。

第1种情况用到的ComputeLocalToGlobalTransform在前面讲过。

第2种情况的公式实际是 ， 也就是 ，把现有的local to global map的变换用作新子图的位姿计算。

第3种很简单，相当于没处理全局位姿。

计算 INTRA_SUBMAP

返回继续看 ComputeConstraintsForNode

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
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
const SubmapId matching_id = submap_ids.front();
// node(scan) 在 local map坐标系的位姿
const transform::Rigid2d   local_pose_2d =
    transform::Project2D(constant_data->local_pose *
                         transform::Rigid3d::Rotation(
                             constant_data->gravity_alignment.inverse() ) );
// 全局位姿，通过优化后的 submap全局位姿和 local位姿求出矩阵
// ComputeSubmapPose返回 参数子图的 local_pose
const transform::Rigid2d global_pose_2d =
    optimization_problem_->submap_data().at(matching_id).global_pose *
    constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
    local_pose_2d;
 // 优化器中增加轨迹节点信息
optimization_problem_->AddTrajectoryNode(
    matching_id.trajectory_id,
    optimization::NodeSpec2D{constant_data->time, local_pose_2d,
                             global_pose_2d,
                             constant_data->gravity_alignment}  );

for (size_t i = 0; i < insertion_submaps.size(); ++i)
{
  const SubmapId  submap_id = submap_ids[i];
  // Even if this was the last node added to 'submap_id', the submap will
  // only be marked as finished in 'data_.submap_data' further below.
  CHECK( data_.submap_data.at(submap_id).state ==
        SubmapState::kNoConstraintSearch );
  data_.submap_data.at(submap_id).node_ids.emplace(node_id);
  // node_id的 scan pose 在子图中的 相对位置，就是后面的 intra constrait
  const transform::Rigid2d   constraint_transform =
    constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
      local_pose_2d;
  /*  遍历matching结果的所有submap，其新增加的轨迹节点必定组成了其submap，
  故其约束为 INTRA_SUBMAP 类型约束，可直接将相对位置加入约束队列中  */
  data_.constraints.push_back(
      Constraint{ submap_id, node_id,
                { transform::Embed3D(constraint_transform),
                  options_.matcher_translation_weight(),
                  options_.matcher_rotation_weight()},
                 Constraint::INTRA_SUBMAP}   );
}

这里是计算 intra 约束的地方。

匹配的过程就是建立约束的过程，也就是回环检测的过程，因为Cartographer 采用的是图优化，有节点和约束的概念，，节点可以理解为激光点的位姿，约束就是激光数据与子图之间相对位姿。

一共建立3种约束：

  • 匹配结果的所有子图，必定包含了新增加的轨迹节点，它们的约束是 INTRA_SUBMAP
  • 新增的节点和已经完成约束计算的子图都要计算约束
  • 对于新的子图不再添加新的轨迹节点时，建立其他节点和该子图之间的约束

后两个就是下面的两次ComputeConstraint函数，是INTER约束

子图在后台线程有两种状态 kNoConstraintSearch 和 kFinished。对于前者的子图，只有内部node去匹配；对后者的子图，所有node(包括新node)都匹配这个子图

两次 ComputeConstraint

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
for (const auto& submap_id_data : data_.submap_data)
{
  // 完成过约束计算的子图
  if (submap_id_data.data.state == SubmapState::kFinished)
  {
    // 不包含当前的节点  node_id
    CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
    finished_submap_ids.emplace_back(submap_id_data.id);
  }
}
// 前端匹配输出的submap是刚完成的一个，即维护的两个submap中front不再更新
if (newly_finished_submap)
{
  const SubmapId  newly_finished_submap_id = submap_ids.front();
  InternalSubmapData&   finished_submap_data =
      data_.submap_data.at(newly_finished_submap_id);
  CHECK(finished_submap_data.state == SubmapState::kNoConstraintSearch);
  // 将新的submap设置为finished，表明已经增加约束条件了
  finished_submap_data.state = SubmapState::kFinished;
  newly_finished_submap_node_ids = finished_submap_data.node_ids;
}
}

要遍历的submap_data 是 MapById<SubmapId, InternalSubmapData>，该容器记录了所有的子图数据及其内部节点，InternalSubmapData除了描述了子图的数据之外还记录了所有内部的节点，成员是

1
2
3
4
5
6
7
8
// enum class SubmapState { kActive, kFinished };    
struct InternalSubmapData
{
    std::shared_ptr<const Submap2D> submap;
    // 所有直接插入submap的节点
    std::set<NodeId> node_ids;
    SubmapState state = SubmapState::kActive;
};

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
  // 新加入的节点与之前所有完成约束计算的子图，均计算一次约束
for (const auto& submap_id : finished_submap_ids)
    ComputeConstraint(node_id, submap_id);
  /* 每次新的子图完成，不再更新即不再添加新的轨迹节点时，
  则需要计算此子图与所有优化位姿图node之间的约束 */
if (newly_finished_submap)
{
    const SubmapId newly_finished_submap_id = submap_ids.front();
    // We have a new completed submap, so we look into 
    // adding constraints for  old nodes
    for (const auto& node_id_data : optimization_problem_->node_data() )
    {
      const NodeId&  node_id = node_id_data.id;
      if (newly_finished_submap_node_ids.count(node_id) == 0)
      {
        ComputeConstraint(node_id, newly_finished_submap_id);
      }
    }
}

子图的状态主要是给后台的线程提供的。一开始子图的状态都是kActive的，当它完成之后(kFinished的状态)就会与所有旧的节点 计算约束

此外新增的节点也会与所有完成的子图 计算约束。这一次可以理解为是进行闭环检测，根据节点和循环中的各个子图之间的情况不同，可能构建的是全局约束，也可能是局部约束。 回环检测是一种全局约束。

PoseGraph2D::ComputeConstraint只有这里的两次调用。

最后部分

1
2
3
4
5
6
7
8
9
10
11
12
13
  // 每次新添加一个节点时，均需执行，通知约束构建器。
  constraint_builder_.NotifyEndOfNode();
  absl::MutexLock locker(&mutex_);
  // 记录距离上次闭环处理增加的节点个数
  ++num_nodes_since_last_loop_closure_;
  // 当新增加节点个数大于阈值时，则调用优化处理结果
  if (options_.optimize_every_n_nodes() > 0 &&
      num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes())
  {
    return WorkItem::Result::kRunOptimization;
  }
  return WorkItem::Result::kDoNotRunOptimization;
}

添加节点和约束是实时的，但是优化不是，更不需要每个节点都进行。 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
void ConstraintBuilder2D::NotifyEndOfNode()
{
  absl::MutexLock locker(&mutex_);
  CHECK(finish_node_task_ != nullptr);
  // 完成一个后端节点
  finish_node_task_->SetWorkItem([this] {
    absl::MutexLock locker(&mutex_);
    ++num_finished_nodes_;
  });
  auto finish_node_task_handle =
      thread_pool_->Schedule(std::move(finish_node_task_));
  // 重新开始添加节点任务
  finish_node_task_ = absl::make_unique<common::Task>();
  when_done_task_->AddDependency(finish_node_task_handle);
  ++num_started_nodes_;
}

这里就是处理finish_node_task_，每次完成一个node约束计算时均会进行调用。目的是完成finish_node_task_，同时开启一个新的task用于下一次约束任务。