Local SLAM的核心——LocalTrajectoryBuilder2D

在分析map_builder的接口实现的时候， 我们就已经了解到实际完成局部建图和轨迹跟踪任务的是一个LocalTrajectoryBuilder2D类型的对象。在map_builder的AddTrajectoryBuilder接口中根据用户配置创建的该对象。 它提供了进行局部SLAM的环境，实现了位姿估计、扫描匹配等功能，但并不具备闭环检测的功能。

走到这里，我们终于看到了算法的一点影子了。本文中，我们先从总体上了解一下LocalTrajectoryBuilder2D(.h, .cc)类。

1. 成员变量

下表列出了LocalTrajectoryBuilder2D类中定义的所有成员变量。从中，我们可以看到子图、扫描匹配器、位姿估计器等对象，这些对象正是进行Local SLAM的要素。

让我们再回到Cartographer的总体框架上来。下图是从总体框图中抠下来的关于Local SLAM的框图。虽然原图中输入的传感器数据还有里程计估计的位姿(Odometry Pose)和固定坐标系位姿(Fixed Frame Pose, 应该是指类似GPS这样具有全局定位能力的传感器数据)， 但是在分析node对象的时候，我们发现demo所用的传感器只有激光雷达和IMU。所以， 在这幅图中只保留了这两个传感器。

图中虚框里的模块就是Local SLAM的框图，如果没有猜错，描述的就应该是这里的LocalTrajectoryBuilder2D。它有扫描匹配器(Scan Matiching)，以激光雷达的扫描数据和位姿估计为输入， 使用Ceres库完成扫描匹配，输出位姿的观测值。一方面反馈给位姿估计器用于修正估计值，另一方面提供给运动滤波器(Motion Filter)用于判定机器人是否产生了运动。 如果产生了运动，则将此时的点云数据插入到当前正在维护的子图中。同时输出插入的结果，包括时间、位姿、子图、扫描数据等信息。

2. 内部定义的结构体

        struct InsertionResult {
            std::shared_ptr<const TrajectoryNode::Data> constant_data;
            std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
        };
        struct MatchingResult {
            common::Time time;
            transform::Rigid3d local_pose;
            sensor::RangeData range_data_in_local;
            // 'nullptr' if dropped by the motion filter.
            std::unique_ptr<const InsertionResult> insertion_result;
        };

为方便管理子图的插入结果和扫描匹配结果LocalTrajectoryBuilder2D还定义了InsertionResult和MatchingResult两个结构体。 右面的代码片段是这两个结构体的定义。

结构体InsertionResult有两个字段，分别记录了插入的节点数据和被插入的子图。 字段constant_data的数据类型TrajectoryNode::Data， 实际上是结构体TrajectoryNode内部定义的结构体，包含了处理之后的点云数据。 关于Submap2D的定义以及相关的结构体我们将专门用一篇文章来分析。

结构体MatchingResult记录了扫描匹配发生的时间(time)、在局部地图坐标系下的位姿(local_pose)、局部的扫描数据(range_data_in_local)、以及子图插入结果(insertion_result)。 根据注释的解释，如果扫描匹配的结果被运动滤波器过滤了，那么字段insertion_result中记录的是一个空指针"nullptr"。

根据这两个结构体的定义，我在这里做了一些猜想。在Cartographer中机器人的运动轨迹是由一个个前后串联的节点(TrajectionNode)构成的。每当有新的扫描数据输入， LocalTrajectoryBuilder2D都会先通过扫描匹配器生成一个MatchingResult的对象，但是它的insertion_result字段是留空的。只有当通过运动滤波器成功插入到子图中之后，才会将插入的结果记录下来， 于此同时子图就得到了更新。

3. 成员函数

下面是类LocalTrajectoryBuilder2D中关于成员函数的代码片段。这两个函数是它的构造函数和析够函数的声明，实际上这两个函数体都是空的，没做任何事情。 其中构造函数在它的构造列表中通过输入参数options完成了对各个成员变量的构造。

        class LocalTrajectoryBuilder2D {
        public:
            explicit LocalTrajectoryBuilder2D(const proto::LocalTrajectoryBuilderOptions2D& options, const std::vector<std::string>& expected_range_sensor_ids)
                : options_(options),
                  active_submaps_(options.submaps_options()),
                  motion_filter_(options_.motion_filter_options()),
                  real_time_correlative_scan_matcher_(options_.real_time_correlative_scan_matcher_options()),
                  ceres_scan_matcher_(options_.ceres_scan_matcher_options()),
                  range_data_collator_(expected_range_sensor_ids) {}
            ~LocalTrajectoryBuilder2D() {}

此外为了防止一些意外的情况发生，该类屏蔽了拷贝构造函数和拷贝赋值运算符。

            LocalTrajectoryBuilder2D(const LocalTrajectoryBuilder2D&) = delete;
            LocalTrajectoryBuilder2D& operator=(const LocalTrajectoryBuilder2D&) = delete;

LocalTrajectoryBuilder2D接收激光传感器的扫描数据、IMU数据和里程计数据，它们分别对应着AddRangeData、AddImuData和AddOdometryData三个函数。 其中AddRangeData的返回值就是我们刚才讨论的MatchingResult类型的对象，所以在这个函数中一定直接或者间接的调用其他函数进行了扫描匹配。关于该函数， 我们将在详细分析Local SLAM的业务主线的时候仔细分析它。另外两个函数由于比较短， 我们将在本文后面予以介绍。

            std::unique_ptr<MatchingResult> AddRangeData(const std::string& sensor_id, const sensor::TimedPointCloudData& range_data);
            void AddImuData(const sensor::ImuData& imu_data);
            void AddOdometryData(const sensor::OdometryData& odometry_data);

在没有仔细分析函数的实现，不太清楚是干什么用的。大概是用于进行扫描匹配时指定匹配程度评价方法的吧。

            static void RegisterMetrics(metrics::FamilyFactory* family_factory);

接下来是一些私有的成员函数。这几个函数都是直接或者间接的在被函数AddRangeData调用，所以将在Local SLAM的业务主线一文中解释。 大体上，函数AddAccumulatedRangeData会调用ScanMatch进行扫描匹配，并通过函数InsertIntoSubmap插入数据更新子图。 函数TransformToGravityAlignedFrameAndFilter 用于根据重力加速度的方向来估计激光扫描数据在水平面上的投影。函数InitializeExtrapolator用于初始化位姿估计器。

            std::unique_ptr<MatchingResult> AddAccumulatedRangeData(common::Time time, const sensor::RangeData& gravity_aligned_range_data, const transform::Rigid3d& gravity_alignment);
            sensor::RangeData TransformToGravityAlignedFrameAndFilter(const transform::Rigid3f& transform_to_gravity_aligned_frame, const sensor::RangeData& range_data) const;
            std::unique_ptr<InsertionResult> InsertIntoSubmap(common::Time time, const sensor::RangeData& range_data_in_local, const sensor::RangeData& gravity_aligned_range_data, const transform::Rigid3d& pose_estimate, const Eigen::Quaterniond& gravity_alignment);
            std::unique_ptr<transform::Rigid2d> ScanMatch(common::Time time, const transform::Rigid2d& pose_prediction, const sensor::RangeData& gravity_aligned_range_data);
            void InitializeExtrapolator(common::Time time);
        }

4. AddImuData和AddOdometryData

下面的函数是AddImuData的实现，它首先检查配置项是否要求使用IMU数据。然后通过函数InitializeExtrapolator完成位姿估计器的初始化工作，最后将IMU的数据喂给extrapolator_对象。

        void LocalTrajectoryBuilder2D::AddImuData(const sensor::ImuData& imu_data) {
            CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
            InitializeExtrapolator(imu_data.time);
            extrapolator_->AddImuData(imu_data);
        }

下面的函数是AddOdometryData的实现。它需要先通过检查对象extrapolator_是否为空指针来判定位姿估计器是否已经完成初始化工作了，若通过则直接将里程计的数据喂给extrapolator_对象。

        void LocalTrajectoryBuilder2D::AddOdometryData(const sensor::OdometryData& odometry_data) {
            if (extrapolator_ == nullptr) {
                // Until we've initialized the extrapolator we cannot add odometry data.
                LOG(INFO) << "Extrapolator not yet initialized.";
                return;
            }
            extrapolator_->AddOdometryData(odometry_data);
        }

5. InitializeExtrapolator

从构造函数的构造列表中几乎完成了所有对象的构建，但是留下了位姿估计器对象extrapolator_。这个对象是在函数InitializeExtrapolator中完成构造的。 下面的代码片段是该函数的实现，在函数的一开始检查对象extrapolator_是否是一个空指针，如果不是意味着已经创建了一个位姿估计器对象，直接返回。 否则创建一个PoseExtrapolator类型的位姿估计器，并在最后添加一个初始位姿。

        void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) {
            if (extrapolator_ != nullptr)
                return;
            constexpr double kExtrapolationEstimationTimeSec = 0.001;
            extrapolator_ = common::make_unique<PoseExtrapolator>(::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), options_.imu_gravity_time_constant());
            extrapolator_->AddPose(time, transform::Rigid3d::Identity());
        }

6. 完

本文中，通过对类LocalTrajectoryBuilder2D的分析，大体上能够找到进行Local SLAM的一些核心要素以及实现这些要素的对象。我们看到了系统框图中Local SLAM下的扫描匹配、运动滤波、更新子图等相关的功能。 基本上函数AddRangeData负责完成了这三个功能，我们将在后序的文章中详细的分析它。