Cartographer之基于运动阈值筛选约束的方法

任务动机：合理地筛选运动约束，提高cartographer后端优化的实时性。

任务描述：针对传统cartographer后端对nodes和submaps之间的约束关系筛选不均衡的情况，提出一种基于运动阈值筛选约束的方法。该方法可以筛除重复和无效的约束，使得有效的约束插入到优化队列进行位姿优化，进而提高Cartographer建图的实时性。

1. Cartographer回环检测和后端优化

在前端匹配完成后，调用InsertToSubmap()函数，将scan插入匹配完成的submap。然后调用pose graph进行回环检测和后端优化。

回环检测的逻辑是每一个新生成的node都会和已完成的submap尝试构建约束。同时，每一个新生成的submap也会和所有的node尝试构建约束。每一个约束会关联对应的node位姿和submap位姿，然后利用最小二乘进行优化求解准确的node位姿和submap位姿。

2. 基于运动阈值筛选约束

建图过程中，后端优化时间的长短主要取决于位姿图优化队列中的运动约束的个数，nodes和submaps之间的运动约束包含两种：

(1)新生成的node和所有已完成的submaps进行构建约束；

(2)新生成的submap和所有历史的nodes来构建约束。

随着建图范围的逐渐增大，node的数量也快速增加。一个submap对应100个node，20个submap就对应2000个node。对于第二种情况，当新的submap生成的时候，新的submap会与所有历史的node尝试建立约束。此时会有大量的约束添加到优化队列中，造成优化队列积压。

Cartographer的后端是通过采样频率的方法对nodes和submaps之间的约束关系进行筛选。采样频率方法的具体实现如下：

```
bool FixedRatioSampler::Pulse() {

    ++num_pulses_;

    if (static_cast < double > (num_samples) / num_pulses < ratio_) {

        ++num_samples_;

        return true;

    }

    return false;

}

```

如果采样频率值较小，此时构建的约束过少，nodes和submaps之间缺少有效约束的相互关联，优化过程产生的偏移不能及时矫正。如果采样频率值较大，nodes和submaps之间构建的约束太多，其中会有过多重复，无法实时计算优化，导致位姿图优化队列产生积压，影响实时建图效果。针对传统Cartographer后端对nodes和submaps之间的约束关系筛选不均衡的情况，本文提出一种基于运动阈值筛选约束的方法。

通过运动阈值筛选nodes和submaps之间的约束方法，将nodes中在空间上等距分布的稀疏骨架nodes，来和最新submap进行匹配，这样构建出来的运动约束，不但在数目上会有所减少，而且还保留了原始nodes的空间结构信息，在保证位姿图优化结果精度的同时，会减少计算量。

2.1 实现步骤

步骤１：定义运动阈值。运动阈值包括平移阈值和角度阈值。平移阈值定义为两个node之间的平移相对变化量为0.5米。角度阈值定义为两个node之间的角度相对变化量为0.05弧度；

步骤２：判断当前node和新完成的submap构建约束是否插入优化队列。如果当前node和上一个构建约束的node之间的相对运动变化量大于平移阈值或者大于角度阈值，则插入优化队列，否则，抛弃此次约束；

步骤３：对所有历史的nodes执行步骤２。

2.2 代码实现

修改pose_graph_2d.cc代码如下：

```

if (newly_finished_submap) {

  NodeId first_node_id = optimization_problem_->node_data().BeginOfTrajectory(0)->id;

  transform::Rigid2d last_reference_node_pose =

optimization_problem_->node_data().at(first_node_id).global_pose_2d;

  const SubmapId newly_finished_submap_id = submap_ids.front();

  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) {

      // Select nodes by distance || by rotate angle

      const transform::Rigid2d cur_node_pose =

         optimization_problem_->node_data().at(node_id).global_pose_2d;

      const transform::Rigid2d temp_pose =

          last_reference_node_pose.inverse() * cur_node_pose;

      double angle_correct = std::atan2(sin(temp_pose.rotation().angle()),

cos(temp_pose.rotation().angle()));

    if (temp_pose.translation().norm() > 0.5 || std::abs(angle_correct) > 0.05) {

        ComputeConstraint(false, node_id, newly_finished_submap_id);

        last_reference_node_pose = cur_node_pose;

    }

}

  }

}

```

3. 实验验证

3.1 只通过平移阈值筛选时，插入到优化队列的node_id：

1. 当diff_pose.translation().norm()>0.7时，选择的node_id:0, 51, 68, 87, 97, 116, 118, 127, 136, 138, 157, 161, 173, 181...
2. 当diff_pose.translation().norm()>0.6时，选择的node_id:0, 25, 32, 45, 58, 69, 97, 114, 117, 125, 130, 137, 142, 163...
3. 当diff_pose.translation().norm()>0.5时，选择的node_id:0, 9, 17, 22, 32, 50, 66, 92, 99, 121, 130, 138, 142, 178, 186....

3.2 只通过角度阈值筛选时，插入到优化队列的node_id：

1. 当std::abs(diff_angle)>0.07时，选择的node_id:0, 51, 68, 87, 97, 118, 137, 157, 161, 172, 181, 186, 195....
2. 当std::abs(diff_angle)>0.05时，选择的node_id:0, 51, 55, 87, 95, 99, 104, 116, 117, 132, 138, 157, 160, 163, 171, 173, 179…

3.3 通过平移阈值或者角度阈值筛选时，插入到优化队列的node_id：

1. 当temp_pose.translation().norm()>0.5 || std::abs(angle_correct)>0.06时，选择的node_id:0, 9, 15, 19, 27, 45, 51, 57, 87, 91, 96, 116, 117, 126, 132, 139...
2. 当temp_pose.translation().norm()>0.5 || std::abs(angle_correct)>0.05时，选择的node_id:0, 9, 17, 22, 30, 48, 55, 59, 91, 95, 99, 103, 108, 120, 121, 121, 130, 136...
3. 当temp_pose.translation().norm()>0.5 || std::abs(angle_correct)>0.05时，bag的建图效果如下：

4. 结论

相比与于传统Cartographer通过采样频率筛选的约束方法，基于运动阈值筛选约束的方法不但在插入优化队列的约束数目上会有所减少，而且还保留了原始nodes的空间结构信息，在保证位姿图优化结果精度的同时，会减少计算量。