cartographer占用CPU过高？那只是作者留了道练习题

cartographer的博物馆demo之所以cpu过高，是因为他的前端和后端都使用了比较冗余的计算参数，其实有些是不必要的，
比如可以把激光的采样间隔设置大一些，demo的帧率为30Hz，设置为0.25则变成12Hz，够用。

  rangefinder_sampling_ratio = 0.25, --1.

前端不要使用CSM，有IMU的情况不用使用CSM激光匹配。

TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false

接着是后端参数，回环检测的匹配得分不要太高，不然在有的地方不能回环，设置为

MAP_BUILDER.pose_graph.constraint_builder.min_score = 0.55

回环检测的最远范围不要太大，会增加计算量

MAP_BUILDER.pose_graph.constraint_builder.max_constraint_distance = 6 --15

回环检测的采样频率设小一些，因为太大也没有必要

MAP_BUILDER.pose_graph.constraint_builder.sampling_ratio = 0.05 --0.3

其他没有什么就默认就好，但是还是要改下代码，因为作者为了好的结果添加了太多冗余的优化约束，Maybe为了不受到全世界slamer的骚扰吧。

来把他们去掉一些，并不是最优方案（最优方案是增量平滑gtsam）

修改1-

注释这段代码，如果你使用了/odom，把下面的也注释，因为作者为了防止优化结果异常，增加了每个位姿之间的里程计约束，这个在数据增加时会不断增长。感觉可以去掉。

      // Add a relative pose constraint based on consecutive local SLAM poses.
      
      // const transform::Rigid3d relative_local_slam_pose =
      //     transform::Embed3D(first_node_data.local_pose_2d.inverse() *
      //                        second_node_data.local_pose_2d);
      // problem.AddResidualBlock(
      //     CreateAutoDiffSpaCostFunction(
      //         Constraint::Pose{relative_local_slam_pose,
      //                          options_.local_slam_pose_translation_weight(),
      //                          options_.local_slam_pose_rotation_weight()}),
      //     nullptr /* loss function */, C_nodes.at(first_node_id).data(),
      //     C_nodes.at(second_node_id).data());

使用了/odom，把这个也注释掉。

      // std::unique_ptr relative_odometry =
      //     CalculateOdometryBetweenNodes(trajectory_id, first_node_data,
      //                                   second_node_data);
      // if (relative_odometry != nullptr) {
      //   problem.AddResidualBlock(
      //       CreateAutoDiffSpaCostFunction(Constraint::Pose{
      //           *relative_odometry, options_.odometry_translation_weight(),
      //           options_.odometry_rotation_weight()}),
      //       nullptr /* loss function */, C_nodes.at(first_node_id).data(),
      //       C_nodes.at(second_node_id).data());
      // }

修改2:

作者把每个位姿与所有子图进行匹配来查找回环，并把所有满足距离要求的位姿都添加到约束里面，oh my god，这增长起来不得了，改！思路：查找与该位姿最近的2个子图submap，其余的子图不要。（当有新的子图产生，作者把所有位姿与该新子图一一匹配，找到合适的就添加约束，oh my god,这也是增长起来不得了，改：思路：查找与该子图最近的2个位姿node）

增加一个数据类型用于存储满足要求的子图或者位姿

struct submap_sort
{   
      double distance;
      int trajectory_id;
      int index; //submap or node

      submap_sort(double distance_value, int trajectory_id_value, int index_value)
      {
              distance = distance_value;
              trajectory_id = trajectory_id_value;
              index = index_value;
      }

      bool operator<(const submap_sort& other) const
      {
              return distance > other.distance;
      }
};

查找与该位姿最近的2个子图，其余的子图不要。

    std::priority_queue<submap_sort> submap_sort_1;
    for (const auto& submap_id : finished_submap_ids) {
      //decreace the back-end computation.
      const transform::Rigid2d initial_relative_pose =
          optimization_problem_->submap_data()
              .at(submap_id)
              .global_pose.inverse() *
          optimization_problem_->node_data().at(node_id).global_pose_2d;

      if (initial_relative_pose.translation().norm() < 6.0) {
        submap_sort_1.emplace(initial_relative_pose.translation().norm(),
                              submap_id.trajectory_id, submap_id.submap_index);
      }
    }

    // only use the nearist 1 or 2 or 3 constraint
    for (int i = 0; i < 2; i++) {
      if (submap_sort_1.empty()) break;
      SubmapId id(submap_sort_1.top().trajectory_id,
                  submap_sort_1.top().index);
      ComputeConstraint(node_id, id);
      LOG(INFO) << "peak.ding submap_sort_1 : distance " << submap_sort_1.top().distance;
      submap_sort_1.pop();
    }

查找与该子图最近的2个位姿node

     std::priority_queue<submap_sort> submap_sort_2;
      for (const auto& node_id_data : optimization_problem_->node_data()) {
        const NodeId& node_id = node_id_data.id;

        const transform::Rigid2d initial_relative_pose =
            optimization_problem_->submap_data()
                .at(newly_finished_submap_id)
                .global_pose.inverse() *
            optimization_problem_->node_data().at(node_id).global_pose_2d;

        if (initial_relative_pose.translation().norm() < 6.0) {
          submap_sort_2.emplace(initial_relative_pose.translation().norm(),
                                node_id.trajectory_id, node_id.node_index);
        }
      }
      // only use the nearist 1 or 2 or 3 constraint
      for (int i = 0; i < 2; i++) {
        if (submap_sort_2.empty()) break;
        NodeId id(submap_sort_2.top().trajectory_id,
                  submap_sort_2.top().index);
        ComputeConstraint(id, newly_finished_submap_id);
        LOG(INFO) << "peak.ding submap_sort_2 : distance " << submap_sort_2.top().distance;
        submap_sort_2.pop();
      }

ＯＫ，虽不是最优解，测试结果：4倍速度运行bag包，cpu没有超过16%
我的配置是标压 i5,固态硬盘，8Ｇ内存。实际正常运行cpu应该在5%以内，前端还没有怎么大的改动。
腾讯视频
https://v.qq.com/x/page/t3233y5yx8f.html