连接前端与后端的桥梁——GlobalTrajectoryBuilder

通过对map_builder及其接口实现的分析，了解到实际完成局部建图任务的是一个LocalTrajectoryBuilder2D类型的对象。后文中详细分析了这个类型， 它并不具备任何闭环检测的能力，只是在不断的进行扫描匹配和子图更新。前后端之间一定存在某种通信使得后端能够看到前端所构建的子图和定位信息，进而达到闭环检测和全局优化的目的。 这一工作被封装在了一个GlobalTrajectoryBuilder类型的对象中。

1. GlobalTrajectoryBuilder的构建

对象map_builder构建轨迹跟踪器trajectory_builders_的时候，通过函数CreateGlobalTrajectoryBuilder2D构建了一个GlobalTrajectoryBuilder类型的对象。下面是该函数的代码，可以看出GlobalTrajectoryBuilder实际上是一个模板类。 其模板列表中的LocalTrajectoryBuilder2D和PoseGraph2D分别是前端和后端的两个核心类型。 LocalTrajectoryBuilder2D负责接收来自激光雷达的数据，进行扫描匹配，估计机器人位姿，并将传感器数据插入子图中，更新子图。PoseGraph2D在后台进行闭环检测全局优化。

        std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
            std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
            const int trajectory_id,
            mapping::PoseGraph2D* const pose_graph,
            const TrajectoryBuilderInterface::LocalSlamResultCallback& local_slam_result_callback)
        {
            return common::make_unique<GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
                std::move(local_trajectory_builder), trajectory_id, pose_graph, local_slam_result_callback);
        }

GlobalTrajectoryBuilder是定义在global_trajectory_builder.cc中的一个模板类，它继承自接口类TrajectoryBuilderInterface。 下表是该类定义的四个成员变量。

按照惯例，我们还是先来看一下它的构造函数，如下面的代码片段所示。它有四个输入参数，分别记录了轨迹跟踪器、轨迹索引、位姿图、前端回调函数。这些参数在成员构造列表中， 被拿来一一构建上表中罗列的对象了。函数体是空的没有任何操作。

        GlobalTrajectoryBuilder(std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder,
                                const int trajectory_id,
                                PoseGraph* const pose_graph,
                                const LocalSlamResultCallback& local_slam_result_callback)
            : trajectory_id_(trajectory_id),
              pose_graph_(pose_graph),
              local_trajectory_builder_(std::move(local_trajectory_builder)),
              local_slam_result_callback_(local_slam_result_callback) {}

除此之外，该模板类的析够函数也是空的什么也没做。它还屏蔽了拷贝构造和拷贝赋值两个构造对象的途径。

        ~GlobalTrajectoryBuilder() override {}
            GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
            GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;

2. 接口类TrajectoryBuilderInterface

接口类TrajectoryBuilderInterface是为了给2D和3D建图提供一个统一的访问接口。通过该接口我们可以获得一个完整的SLAM技术栈，包括局部建图方法、局部位姿估计、闭环检测方法、以及面向稀疏位姿图的全局优化方法。

        struct InsertionResult {
            NodeId node_id;            
            std::shared_ptr<const TrajectoryNode::Data> constant_data;
            std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
        };

在该接口类内部，还定义了结构体InsertionResult来描述前端的一次子图更新操作，将传感器的扫描数据插入子图中。该结构体的定义如右面的代码所示。

实际上前端的核心中也定义了一个InsertionResult。 两者的不同就在于这里的InsertionResult多了一个node_id的字段。这个字段的数据类型是定义在id.h中的一个结构体NodeId。它有两个字段分别记录了轨迹的索引(trajectory_id)以及一个从零开始计数的节点编号(node_index)。根据NodeId的定义，我们可以把一条轨迹理解为由若干个节点串联起来的一个数据结构。 由于前端的核心只关系更新子图时的位姿和点云信息，不考虑在它与整个运动轨迹之间的关系，所以没有字段node_id。

字段constant_data记录了子图更新时在局部地图中的位姿，以及有传感器原始数据转换之后的点云信息。此外还记录了更新子图的时刻和重力方向。字段insertion_submaps则记录了被更新的子图对象。

接口类TrajectoryBuilderInterface通过关键字using和std::function定义了回调函数LocalSlamResultCallback的类型。回调函数有5个参数，分别记录了轨迹索引、更新子图的时间、局部的位姿估计、 激光传感器的扫描数据在局部坐标系下的点云信息、子图更新结果。在我们的demo中最终会调用到cartographer_ros中定义的函数OnLocalSlamResult。

        using LocalSlamResultCallback = std::function<void(int /* trajectory ID */, common::Time,
                                                           transform::Rigid3d /* local pose estimate */,
                                                           sensor::RangeData /* in local frame */,
                                                           std::unique_ptr<const InsertionResult>)>;

针对不同的传感器数据，接口类TrajectoryBuilderInterface定义了5个AddSensorData接口。

        virtual void AddSensorData(const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) = 0;
        virtual void AddSensorData(const std::string& sensor_id, const sensor::ImuData& imu_data) = 0;
        virtual void AddSensorData(const std::string& sensor_id, const sensor::OdometryData& odometry_data) = 0;
        virtual void AddSensorData(const std::string& sensor_id, const sensor::FixedFramePoseData& fixed_frame_pose) = 0;
        virtual void AddSensorData(const std::string& sensor_id, const sensor::LandmarkData& landmark_data) = 0;

除此之外，接口类TrajectoryBuilderInterface还提供了一个接口AddLocalSlamResultData来直接将一个Local SLAM的结果添加到后端位姿图上。 但是那些使用LocalTrajectoryBuilder2D/3D作为前端的对象不能使用该接口。

        virtual void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) = 0;

3. GlobalTrajectoryBuilder的接口实现

在众多的接口当中，最主要的是如下代码片段中处理点云数据的接口，可以说该接口控制了整个Cartographer系统的运行过程。它有两个参数，其中sensor_id记录了产生数据的传感器， timed_point_cloud_data则是传感器产生的数据。在demo中， cartographer_ros使用SensorBridge将ROS系统中的激光扫描数据转换成这里的TimedPointCloudData类型。

        void AddSensorData(const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) override {

在函数的一开始，先检查一下前端核心对象是否存在。 如果存在就通过它的成员函数AddRangeData完成Local SLAM的业务主线。如果一切正常， 就会返回扫描匹配的结果，在该结果中同时记录了子图的更新信息。返回结果通过智能指针matching_result记录。

            CHECK(local_trajectory_builder_) << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
            std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult> matching_result = local_trajectory_builder_->AddRangeData(sensor_id, timed_point_cloud_data);
            if (matching_result == nullptr)
                return;

前端工作完成之后，GlobalTrajectoryBuilder就要将前端的输出结果喂给后端进行闭环检测和全局优化。首先控制计数器kLocalSlamMatchingResults自增，记录下前端的输出次数。 然后通过查询字段insertion_result判定前端成功的是否将传感器的数据插入到子图中。若是，则通过后端的位姿图接口AddNode创建一个轨迹节点，并把前端的输出结果喂给后端。 如果一切正常，我们就会得到新建的轨迹节点索引，它被记录在临时对象node_id中。最后结合后端输出的节点索引，以及前端输出的扫描匹配结果实例化对象insertion_result。

            kLocalSlamMatchingResults->Increment();
            std::unique_ptr<InsertionResult> insertion_result;
            if (matching_result->insertion_result != nullptr) {
                kLocalSlamInsertionResults->Increment();
                auto node_id = pose_graph_->AddNode(matching_result->insertion_result->constant_data, trajectory_id_, matching_result->insertion_result->insertion_submaps);
                CHECK_EQ(node_id.trajectory_id, trajectory_id_);
                insertion_result = common::make_unique<InsertionResult>(InsertionResult{
                    node_id, matching_result->insertion_result->constant_data,
                    std::vector<std::shared_ptr<const Submap>>(matching_result->insertion_result->insertion_submaps.begin(),
                                                               matching_result->insertion_result->insertion_submaps.end())});
            }

最后，如果我们提供了回调函数，就调用回调函数，并将前端的输出和刚刚构建的insertion_result对象传参。

            if (local_slam_result_callback_) {
                local_slam_result_callback_(trajectory_id_, matching_result->time, matching_result->local_pose,
                                            std::move(matching_result->range_data_in_local), std::move(insertion_result));
            }
        }

由于IMU和里程计的数据都可以拿来通过积分运算进行局部的定位，所以这两个传感器的数据处理方式基本一样。如下面的代码片段所示， 都是先判定存在前端对象，将数据喂给前端对象进行局部定位，然后通过后端的位姿图将传感器的信息添加到全局地图中。

        void AddSensorData(const std::string& sensor_id, const sensor::ImuData& imu_data) override {
            if (local_trajectory_builder_)
                local_trajectory_builder_->AddImuData(imu_data);
            pose_graph_->AddImuData(trajectory_id_, imu_data);
        }
        void AddSensorData(const std::string& sensor_id, const sensor::OdometryData& odometry_data) override {
            CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
            if (local_trajectory_builder_)
                local_trajectory_builder_->AddOdometryData(odometry_data);
            pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
        }

在Cartographer中将类似于GPS这种具有全局定位能力的传感器输出的位姿称为固定坐标系位姿(fixed frame pose)。由于它们的测量结果是全局的信息，所以没有喂给前端用于局部定位，接口实现如下代码所示。

        void AddSensorData(const std::string& sensor_id, const sensor::FixedFramePoseData& fixed_frame_pose) override {
            if (fixed_frame_pose.pose.has_value())
                CHECK(fixed_frame_pose.pose.value().IsValid()) << fixed_frame_pose.pose.value();
            pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
        }

此外路标数据也可以认为是全局的定位信息，也直接喂给了后端，如下代码所示。

        void AddSensorData(const std::string& sensor_id, const sensor::LandmarkData& landmark_data) override {
            pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
        }

最后是关于直接给后端添加Local SLAM的结果数据的接口。因为我们的前端对象的数据类型是LocalTrajectoryBuilder2D，所以如果前端对象存在就不能调用该接口。

        void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) override {
            CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with local_trajectory_builder_ present.";
            local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_);
        }

4. 完

本文，我们在GlobalTrajectoryBuilder的成员变量中，看到了前端和后端的核心LocalTrajectoryBuilder2D和PoseGraph2D。 在GlobalTrajectoryBuilder处理点云数据的接口中，我们看到该类把前端的输出结果喂给了位姿图。