沉默杀手HomeArchivesTagsCategories
纯定位机制的流程
|激光SLAMCartographer源码解读|
Word count: 1.1k|Reading time: 4 min

纯定位其实就是SLAM，只是不保存所有子图。因为node_main.cc里会先要运行map_builder，然后再是node.LoadState，最后StartTrajectoryWithDefaultTopics进行建图

Node::LoadState(const std::string& state_filename, const bool load_frozen_state) —— map_builder_bridge_.LoadState —— map_builder.LoadState

MapBuilder::AddTrajectoryBuilder中有一句MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options, pose_graph_.get() );，实际就是

1
2
3
4
5
6
if (trajectory_options.has_pure_localization_trimmer()) 
{
  pose_graph->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
      trajectory_id,
      trajectory_options.pure_localization_trimmer().max_submaps_to_keep() )  );
}

PureLocalizationTrimmer这个类的解释就是 Keeps the last num_submaps_to_keep of the trajectory with trajectory_id to implement localization without mapping.

初值的处理

1
2
3
4
5
6
7
8
9
if (trajectory_options.has_initial_trajectory_pose())
{
    const auto& initial_trajectory_pose =
        trajectory_options.initial_trajectory_pose();
    pose_graph_->SetInitialTrajectoryPose(
        trajectory_id, initial_trajectory_pose.to_trajectory_id(),
        transform::ToRigid3(initial_trajectory_pose.relative_pose()),
        common::FromUniversal(initial_trajectory_pose.timestamp()) );
}

这里涉及到PoseGraph2D::SetInitialTrajectoryPose，其实就一句 data_.initial_trajectory_poses[from_trajectory_id] = InitialTrajectoryPose{to_trajectory_id, pose, time};

MapBuilder::LoadState

先是轨迹ID的增加

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
std::map<int, int> trajectory_remapping;
for (int i = 0; i < pose_graph_proto.trajectory_size(); ++i)
{
  auto& trajectory_proto = *pose_graph_proto.mutable_trajectory(i);
  const auto& options_with_sensor_ids_proto =
      all_builder_options_proto.options_with_sensor_ids(i);
  // 新的轨迹id
  const int new_trajectory_id =
      AddTrajectoryForDeserialization(options_with_sensor_ids_proto);

  CHECK(trajectory_remapping
            .emplace(trajectory_proto.trajectory_id(), new_trajectory_id)
            .second)
      << "Duplicate trajectory ID: " << trajectory_proto.trajectory_id();
  trajectory_proto.set_trajectory_id(new_trajectory_id);

  if (load_frozen_state) {
    pose_graph_->FreezeTrajectory(new_trajectory_id);
  }
}

然后从pbstream文件添加轨迹，更新约束中节点和子图的轨迹id，从获取的pose graph生成 submap_poses和 node_poses，将landmark_poses添加到pose graph，向pose graph添加各类信息，添加子图的附属节点。

以添加子图为例

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
io::ProtoStreamDeserializer deserializer(reader);

// Create a copy of the pose_graph_proto, such that we can re-write the
// trajectory ids.
proto::PoseGraph pose_graph_proto = deserializer.pose_graph();
const auto& all_builder_options_proto =
//  子图位姿
MapById<SubmapId, transform::Rigid3d> submap_poses;
for (const proto::Trajectory& trajectory_proto :
     pose_graph_proto.trajectory())
{
  for (const proto::Trajectory::Submap& submap_proto :
       trajectory_proto.submap())
  {
    submap_poses.Insert(SubmapId{trajectory_proto.trajectory_id(),
                                 submap_proto.submap_index()},
                        transform::ToRigid3(submap_proto.pose()));
  }
}
MapById<SubmapId, mapping::proto::Submap> submap_id_to_submap;
// while 读子图数据
proto.mutable_submap()->mutable_submap_id()->set_trajectory_id(
    trajectory_remapping.at(
        proto.submap().submap_id().trajectory_id()));
submap_id_to_submap.Insert(
    SubmapId{proto.submap().submap_id().trajectory_id(),
             proto.submap().submap_id().submap_index()},
    proto.submap());
// 进入后端
for (const auto& submap_id_submap : submap_id_to_submap) {
  pose_graph_->AddSubmapFromProto(submap_poses.at(submap_id_submap.id),
                                  submap_id_submap.data);
}

后端的处理

1
2
3
4
5
6
7
8
9
10
void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer)
{
    PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
    AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_)
    {
      absl::MutexLock locker(&mutex_);
      trimmers_.emplace_back(trimmer_ptr);
      return WorkItem::Result::kDoNotRunOptimization;
    });
}

这里作为WorkItem加入到了队列里，到了HandleWorkQueue才处理

1
2
3
4
5
6
7
8
9
10
11
12
13
14
// 纯定位时，子图的裁剪，如果没有裁剪器就不裁剪
TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_)
{
	// 这里实际就是 PureLocalizationTrimmer::Trim
	trimmer->Trim(&trimming_handle);
}
// 裁剪器完成状态，删除裁剪器
trimmers_.erase(
    std::remove_if(trimmers_.begin(), trimmers_.end(),
                   [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
                     return trimmer->IsFinished();
                   }),
    trimmers_.end());

这里的裁剪子图就是PureLocalizationTrimmer::Trim
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph)
{ 
  // trajectory 是FINISHED状态，或者没有 trajectory
  if (pose_graph->IsFinished(trajectory_id_)) {
      num_submaps_to_keep_ = 0;
  }
  // 实际调用 TrimmingHandle::GetSubmapIds
  auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_);
  // 从序列号0开始裁剪子图，并删除相关的约束和节点
  // 剩下序列号大的子图
  for (std::size_t i = 0; i + num_submaps_to_keep_ < submap_ids.size(); ++i)
  {
      // 这是个大函数
      pose_graph->TrimSubmap(submap_ids.at(i)  );
  }
  if (num_submaps_to_keep_ == 0)
  {
      finished_ = true;
      pose_graph->SetTrajectoryState(
          trajectory_id_, 
          PoseGraphInterface::TrajectoryState::DELETED );
  }
}

GetSubmapIds有必要学习
1
2
3
4
5
6
7
8
9
10
11
12
std::vector<SubmapId> PoseGraph2D::TrimmingHandle::GetSubmapIds(
    int trajectory_id) const
{
  std::vector<SubmapId> submap_ids;
  // PoseGraph2D* const parent_;
  const auto& submap_data = parent_->optimization_problem_->submap_data();
  for (const auto& it : submap_data.trajectory(trajectory_id)  )
  {
      submap_ids.push_back(it.id);
  }
  return submap_ids;
}

核心是pose_graph->TrimSubmap，函数太长了，功能如下：

  • 获取除submap_id外的所有子图的所有节点的id(nodes_to_retain)，这些节点是需要保留的
  • 找到在submap.id的子图内部，同时不在别的子图内的节点(nodes_to.remove)，这些节点需要删除
  • 删除submap_id相关的约束
  • 删除与nodes_to_remove中节点相关联的约束，并对对应的submap_id进行标记
  • 对标记的submap_id进行检查，看是否还存在其他子图间约束
  • 删除没有子图间约束的标记的子图的扫描匹配器
  • 删除submap_id这个子图的指针
  • 删除submap_id这个子图的匹配器，与多分辨率地图
  • 删除optimization_problem_中的submap_id这个子图
  • 删除nodes_to_remove中的节点

纯定位时的匹配流程

PoseGraph2D::ComputeConstraint(node_id, submap_id) —— ConstraintBuilder2D::MaybeAddGlobalConstraint(submap_id, submap, node_id, constant_data) —— ConstraintBuilder2D::ComputeConstraint

第2步中

1
2
3
4
// 分别根据 node_id 和 submap_id 获得 节点数据和子图
const TrajectoryNode::Data* constant_data = data_.trajectory_nodes.at(node_id).constant_data.get();
const Submap2D* submap = static_cast<const Submap2D*>(data_.submap_data.at(submap_id).submap.get() );

第3步中，submap唯一的作用是获得初值：

1
2
const transform::Rigid2d initial_pose =
      ComputeSubmapPose(*submap) * initial_relative_pose;

这里就想到那句话：前端建图，后端并不改变子图本身，只改变位姿。