map_builder的接口实现

在上一章中，我们简单的浏览了map_builder对象，对它有了一个总体的认识。 也了解到它继承自接口类MapBuilderInterface， 下表总结了各个接口函数及其相关的功能。本文中，我们来讨论map_builder对象的接口函数的实现。

在这些众多的接口当中，我们最主要关心的是AddTrajectoryBuilder因为它创建了一个新的轨迹跟踪器，用于进行局部的SLAM。其次关心FinishTrajectory，它用于结束一段轨迹。 至于那些与proto流相关的接口，有的实现还很长，比如说LoadState，但是它们与实际的建图过程没有关系，所以本文不分析它们。还有一类Get类的接口，用于获取map_builder的一些属性或者对象， 代码很短，我们将一带而过。

1. AddTrajectoryBuilder

下面是函数AddTrajectoryBuilder的代码片段。它有三个参数，其中expected_sensor_ids中记录了用于建图的所有传感器名称和类型，trajectory_options是新建的轨迹跟踪器的配置， 最后的local_slam_result_callback则是一个回调函数对象，用于响应局部地图构建完成的事件。

        int MapBuilder::AddTrajectoryBuilder(const std::set<SensorId>& expected_sensor_ids,
                                             const proto::TrajectoryBuilderOptions& trajectory_options,
                                             LocalSlamResultCallback local_slam_result_callback) {

在函数的一开始，先通过容器trajectory_builders_获取轨迹跟踪器的数量。如果构建成功，该值将作为新的跟踪器的索引。

            const int trajectory_id = trajectory_builders_.size();

然后根据配置是否进行三维建图构建跟踪器。由于我们的demo只是二维建图，所以这里省略了条件判定为真的分支。在二维建图的分支中，一开始先创建了一个LocalTrajectoryBuilder2D类型的对象。这个对象并不是我们一直说的轨迹跟踪器，但它应该是轨迹跟踪器的核心。因为它几乎完成了一个局部SLAM的所有功能，包括位姿估计、扫描匹配等，就是没有闭环检测。

接口还会检查一下是否有关于2D的轨迹跟踪器的配置项，如果有的话，就根据其配置项以及传感器配置具例化对象local_trajectory_builder。

            if (options_.use_trajectory_builder_3d()) {
                // 省略
            } else {
                std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
                if (trajectory_options.has_trajectory_builder_2d_options())
                    local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder2D>(trajectory_options.trajectory_builder_2d_options(),SelectRangeSensorIds(expected_sensor_ids));

接着通过dynamic_cast将pose_graph_对象强制转换为PoseGraph2D，并检查数据类型是否正确。

                DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));

接下来的语句所构建的CollatedTrajectoryBuilder类型的对象才是所谓的轨迹跟踪器， 它继承自接口类TrajectoryBuilderInterface， 这个接口适用于2D和3D建图的对象，如此上层的代码就不必关心具体的建图内核，使用同一个容器trajectory_builders_就可以保存两种local SLAM的对象。构建好的对象就直接被塞进了容器中。

此外CollatedTrajectoryBuilder类型还使用sensor::CollatorInterface接口来收集传感器数据，所以我们可以在它的构造参数列表中看到对象sensor_collator_。在构建轨迹跟踪器的时候， 还通过函数CreateGlobalTrajectoryBuilder2D构建了一个GlobalTrajectoryBuilder类型的对象， 从它的参数列表中来看，除了含有刚刚构建的local_trajectory_builder对象之外，还引入了位姿图对象pose_graph_。个人猜测这是一个具有闭环功能的SLAM对象，所以称之为Global的。

                trajectory_builders_.push_back(common::make_unique<CollatedTrajectoryBuilder>(
                                               sensor_collator_.get(), trajectory_id, expected_sensor_ids,
                                               CreateGlobalTrajectoryBuilder2D(std::move(local_trajectory_builder), trajectory_id,
                                                                               static_cast<PoseGraph2D*>(pose_graph_.get()),
                                                                               local_slam_result_callback)));

针对二维建图还有一个配置项overlapping_submaps_trimmer_2d， 决定是否为pose_graph_对象添加一个OverlappingSubmapsTrimmer2D类型的修饰器， 用于根据子图之间重叠的部分修饰地图。

                if (trajectory_options.has_overlapping_submaps_trimmer_2d()) {
                    const auto& trimmer_options = trajectory_options.overlapping_submaps_trimmer_2d();
                    pose_graph_->AddTrimmer(common::make_unique<OverlappingSubmapsTrimmer2D>(trimmer_options.fresh_submaps_count(),
                                            trimmer_options.min_covered_area() / common::Pow2(trajectory_options.trajectory_builder_2d_options().submaps_options().grid_options_2d().resolution()),
                                            trimmer_options.min_added_submaps_count()));
                }
            }

如果配置项要求只进行定位，那么可以在pose_graph_中添加一个PureLocalizationTrimmer类型修饰器，来完成这一功能。

            if (trajectory_options.pure_localization()) {
                constexpr int kSubmapsToKeep = 3;
                pose_graph_->AddTrimmer(common::make_unique<PureLocalizationTrimmer>(trajectory_id, kSubmapsToKeep));
            }

如果开始建图之前已经有了初始位置，那么我们可以将初始位置提供给pose_graph_对象。

            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()));
            }

接下来，将轨迹跟踪器的配置信息和传感器配置信息保存到容器all_trajectory_builder_options_中。

            proto::TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids_proto;
            for (const auto& sensor_id : expected_sensor_ids)
                *options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id);
            *options_with_sensor_ids_proto.mutable_trajectory_builder_options() = trajectory_options;
            all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto);

最后检查一下轨迹跟踪器对象及其配置的数量，确保两者相等之后返回新建的跟踪器对象的索引。

            CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size());
            return trajectory_id;
        }

2. FinishTrajectory和那些一带而过的接口

下面是FinishTrajectory的代码片段，内容简单的有些枯燥，就是通知sensor_collator_和pose_graph_两个对象终止trajectory_id所对应的轨迹处理。

        void MapBuilder::FinishTrajectory(const int trajectory_id) {
            sensor_collator_->FinishTrajectory(trajectory_id);
            pose_graph_->FinishTrajectory(trajectory_id);
        }

剩下的一些接口，我们也不再细说了，都只有一句话。

        mapping::PoseGraphInterface *pose_graph() override {
            return pose_graph_.get();
        }

        int num_trajectory_builders() const override {
            return trajectory_builders_.size();
        }

        mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder(int trajectory_id) const override {
            return trajectory_builders_.at(trajectory_id).get();
        }

        const std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>& GetAllTrajectoryBuilderOptions() const override {
            return all_trajectory_builder_options_;
        }

3. 完

本文中我们简单看了一下map_builder对象的接口实现，重点分析了AddTrajectoryBuilder函数，看到实际完成局部建图任务的是一个LocalTrajectoryBuilder2D类型的对象， 而与轨迹规划器相关的类型还有CollatedTrajectoryBuilder、GlobalTrajectoryBuilder、TrajectoryBuilderInterface它们应该说是一种封装。