cartographer导航的时候更新地图

Posted 2021-10-22 COCO_PEAK_NOODLE

我的方案，
1- 使用谷歌作者的overlapping_submaps_trimmer_2d来缩减地图，但是这个计算量太大了，我使用了局部计算来代替全局计算，这样间隔一段时间就缩减1-2个地图，计算量有所降低，其实还有更低的（还未实现，思路是不重复计算所有地图，维护一个数据，每次只更新新增的部分）

      std::vector<SubmapId> near_submap;
      if(newly_finished_submap_flag)
      {
        cartographer::transform::Rigid3d submap_global_pose = 
        GetSubmapDataUnderLock(newly_finished_submap_id).pose;
        near_submap = FindPoseOfSubmapInTrajectory(submap_global_pose, 3);
        
        newly_finished_submap_flag = false;
        
      }

std::vector<SubmapId> PoseGraph2D::FindPoseOfSubmapInTrajectory(
    const transform::Rigid3d& initial_pose, int near_count) const {
  // find the nearist submap of initial_pose
  // double min_distance = INT_MAX;
  // SubmapId nearist_submapId(current_map_trajectory_id_, 0);

  std::priority_queue<submap_sort> submap_sort_by_distance;

  const transform::Rigid2d initial_pose_2d = transform::Project2D(initial_pose);
  // for (const auto& submap_id_data :
  //      data_.submap_data.trajectory(current_map_trajectory_id_))
  for (const auto& submap_id_data : data_.submap_data) {
    // if(submap_id_data.id.trajectory_id != current_map_trajectory_id_)
    // continue;

    if(submap_id_data.data.state != SubmapState::kFinished) continue;

    auto submap_data = data_.global_submap_poses_2d.at(submap_id_data.id);
    double distance =
        (initial_pose_2d.inverse() * submap_data.global_pose).translation().norm();

    // submap_sort_by_distance.emplace(distance, current_map_trajectory_id_,
    //                                 submap_id_data.id.submap_index);
    submap_sort_by_distance.emplace(distance, submap_id_data.id.trajectory_id,
                                    submap_id_data.id.submap_index);
  }
  
    std::vector<SubmapId> submap_ids;

  int count_num = submap_sort_by_distance.size();
  for (int i = 0; i < count_num; i++) {
    if (i == near_count) break;
    SubmapId id(submap_sort_by_distance.top().trajectory_id,
                submap_sort_by_distance.top().index);
    submap_ids.push_back(id);
    // LOG(INFO) << "Localize trajectory " << id.trajectory_id << " submap index
    // "
    //          << id.submap_index;
    submap_sort_by_distance.top().distance;
    submap_sort_by_distance.pop();
  }

  return submap_ids;
}

2- 当前活跃的子图只有最前面的3个，其余的在导航过程中逐渐固定。

  optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
                                   data_.landmark_nodes);

  // if(empty_landmark == true) data_.landmark_nodes.clear();

  //  author add it , but peak remove it
  absl::MutexLock locker(&mutex_);

  std::vector<SubmapId> navigation_submap_ids;
  for (const auto& it : optimization_problem_->submap_data().trajectory(1001)) {
    navigation_submap_ids.push_back(it.id);
  }

  // 
  if (current_trajectory_type_ == TrajectoryType::NAVIGATION &&
      global_optimization_in_navigation_ == true &&
      navigation_submap_ids.size() > 3) {
    auto& mutable_submap_data = optimization_problem_->mutable_submap_data();
    auto& mutable_node_data = optimization_problem_->mutable_node_data();

    for (int i = 0; i + 3 < navigation_submap_ids.size(); i++) {
      const auto submap_id = data_.submap_data.find(navigation_submap_ids[i]);
      if (submap_id == data_.submap_data.end()) {
        continue;
      } else {
        mutable_submap_data.at(navigation_submap_ids[i]).state =
            optimization::State::kFrozen;
            LOG(INFO) << "Peak.ding frozen " << navigation_submap_ids[i];
      }

      std::set<NodeId> nodes_id_frozen =
          data_.submap_data.at(navigation_submap_ids[i]).node_ids;

      for (auto node_id : nodes_id_frozen) {
        mutable_node_data.at(node_id).state = optimization::State::kFrozen;
      }
    }

    /*
    for (auto it : mutable_node_data) {
      if (it.id.trajectory_id != 0) {
        mutable_node_data.at(it.id).state = optimization::State::kFrozen;
      }
    }
    */

    global_optimization_in_navigation_ = false;
  }

ok，来张来回运行6趟的图

后续实验结果再来分析