Cartographer源码分析（转载）

目录

【common】

common/port.h：

common/time.h

common/fixed_ratio_sampler.h

common/rate_timer.h

common/histogram.h

common/math.h

common/make_unique.h

common/mutex.h

common/thread_pool.h

common/blocking_queue.h

【transform】

transform/rigid_transform.h：

transform/transform.h：

transform/transform_interpolation_buffer.h：

【sensor】

sensor/point_cloud.h：

sensor/compressed_point_cloud.h

sensor/range_data.h

.sensor/Data.h

sensor/ordered_multi_queue.h

sensor/collator.h

sensor/voxel_filter.h

sensor/configuration.h

【io】

file_writer.h

io/points_batch.h：

流水线-points_processor

io/points_processor.h

min_max_range_filtering_points_processor

counting_points_processor.h

xray_points_processor.h

intensity_to_color_points_processor.h

ply_writing_points_processor.h

coloring_points_processor.h

fixed_ratio_sampling_points_processor.h

outlier_removing_points_processor.h

pcd_writing_points_processor.h

null_points_processor.h

【kalman_filter】

kalman_filter/gaussian_distribution.h

unscented_kalman_filter.h

pose_tracker.h

【mapping】

mapping/probability_values.h

id.h

imu_tracker.h

odometry_state_tracker.h

detect_floors.h

submaps.h

trajectory_node.h

trajectory_connectivity.h

trajectory_builder.h

collated_trajectory_builder.h

sparse_pose_graph.h

map_builder.h

---

【common】

common/port.h：

    int RoundToInt(const float x); //四舍五入取整
    FastGzipString(const string& uncompressed, string* compressed); //压缩字符串string
    FastGunzipString(const string& compressed, string* decompressed); //解压缩字符串

common/time.h

主要功能是提供时间转换函数，UniversalTimeScaleClock类实现c++11的clock接口，以0.1us为时间精度。
定义2个别名：
using Duration = UniversalTimeScaleClock::duration;//微秒,0.1us
using Time = UniversalTimeScaleClock::time_point; //时间点
并提供多个函数方便时间转换.

    FromSeconds(double seconds); //秒数seconds转为c++的duration实例对象：
    FromMilliseconds(int64 milliseconds)；
    ToSeconds(Duration duration); //将duration实例对象转为秒数s
    ToSeconds(Duration duration);
    ToUniversal(Time time);将c++的time_point对象转为TUC时间,单位是0.1us

port.h重载了<<操作符,方便将time_point以string输出
std::ostream& operator<<(std::ostream& os, Time time);

common/fixed_ratio_sampler.h

该文件定义了FixedRatioSampler类。FixedRatioSampler是频率固定的采样器类，目的是从数据流中均匀的按照固定频率采样数据，提供2个成员函数：

    Pulse()产生一个事件pulses,并且:如果计入采样samples，返回true
    DebugString():以string形式输出采样率

common/rate_timer.h

定义了 RateTimer-脉冲频率计数类,作用是计算在一段时间内的脉冲率

    RateTimer只有一个构造函数RateTimer(const common::Duration ),提供时间段Duration
    ComputeRate()返回事件脉冲率,单位hz
    ComputeWallTimeRateRatio()返回真实时间与墙上挂钟时间的比率
    内部类Event封装了某个事件发生的时间点.
    调用一次Pulse()即产生了一次事件
    DebugString() 以字符串形式输出
    ComputeDeltasInSeconds() 计算以秒计的delta序列
    DeltasDebugString()计算delta序列的均值和方差

common/histogram.h

Histogram:直方图类
提供2个操作:
1，Add()//添加value,可乱序
2，ToString(int buckets )以字符串的形式输出buckets个直方图信息,bin大小是篮子个数，表示分为几块统计

common/math.h

common/math.h文件主要实现数学计算，包括：区间截断.求n次方.求平方.幅度角度转换.归一化.求反正切值

    Clamp(const T value, const T min, const T max)；//将val截取到区间min至max中.
    Power(T base, int exponent)；//计算base的exp次方
    double DegToRad(double deg)；//.角度到弧度的转换. 60° -> pi/3
    double RadToDeg(double rad)；//弧度到角度的转换, pi/3 -> 60°
    NormalizeAngleDifference(); //将角度差转换为[-pi;pi]
    atan2(const Eigen::Matrix

common/make_unique.h

make_unique.h在不支持c++14的环境下实现 std::make_unique的功能.
实现细节:完美转发和移动语义

common/mutex.h

common/mutex.h主要是对c++11 的mutex的封装。Mutex类有一个内部类Locker。

    Locker 是一个RAII的类型.本质是使用std::unique_lock和std::condition_variable实现的
        在构造Locker时,对mutex上锁,在析构Locker时对mutex解锁.并提供2个成员函数：
        Await(Predicate predicate)（）
        AwaitWithTimeout(Predicate predicate, common::Duration timeout)

    功能是利用c++11的条件变量和unique_lock实现在谓词predicate为真的情况下对mutex解锁。

common/thread_pool.h

ThreadPool 是对c++11 thread的封装，线程数量固定的线程池类

    构造函数ThreadPool(int num_threads) 初始化一个线程数量固定的线程池。
    Schedule(std::function

common/blocking_queue.h

BlockingQueue类是线程安全的阻塞队列,(生产者消费者模式)

    构造函数BlockingQueue()初始化队列大小,kInfiniteQueueSize=0默认不限制容量。queue_size限制容量：通过条件变量做到.
    Push()添加元素,容量不够时,阻塞等待
    Pop()删除元素,没有元素时,阻塞等待
    Peek()返回下一个应该弹出的元素
    PushWithTimeout(),添加元素,若超时则返回false
    PopWithTimeout(),删除元素，若超时则返回false

【transform】

transform/rigid_transform.h：

rigid_transform.h主要定义了Rigid2 和Rigid3，并封装了2D变换和3D变换的相关函数。

Rigid2 封装了2D平面的旋转和平移操作，方便使用2D变换。

    含有2个数据成员
        translation_代表平移向量[dx,dy]
        Rotation2D rotation_;即Eigen::Rotation2D,代表旋转的方向角 [θ]旋转变换
        .

    提供2个构造函数
        Rigid2();//平移向量为单位向量[1,0]^t,旋转角度为0,debugstring()输出:[1,0,0]
        Rigid2(const Vector& translation, const Rotation2D& rotation);//双参构造函数，给定平移向量[dx,dy]和旋转角度0
        .

    提供4个静态成员函数
        static Rigid2 Rotation(const double rotation) { //给定旋转角度θ,返回Rigid2,debugstring()是[0,0,θ ]
        static Rigid2 Rotation(const Rotation2D& rotation) {//角度为θ,返回Rigid2,debugstring()是[0,0,θ ]
        static Rigid2 Translation(const Vector& vector);//旋转角度是单位矩阵,即θ为0,debugstring()是[dx,dy,0]
        static Rigid2 Identity()；//静态成员函数，返回Rigid2,[0,0,0]
        .

    提供6个成员函数
        Rigid2 cast();//按照指定的参数类型将数据成员进行类型转换
        const Vector& translation();// //返回平移向量[dx,dy]
        Rotation2D rotation();// 返回Eigen旋转矩阵 Rotation2D
        double normalized_angle() ;//归一化角度 ,弧度[-pi;pi]
        inverse() ;逆变换,[-dx’,-dy‘,-θ]
        string DebugString() const；//返回string形式的变换内容
        .
    提供2个友元函数
        Rigid2 operator*(const Rigid2& lhs,const Rigid2& rhs) ;//2个Rigid2相乘,得到第三个Rigid2,等效于连续变换2次。
        Rigid2::Vector operator*(const Rigid2& rigid,const typename Rigid2::Vector& point);//公式1的实现。

Rigid3是三维变换。使用Eigen的四元数进行3D变换

    含有2个数据成员
        Vector translation_;//x,y,z方向上的平移向量[dx,dy,dz]
        Quaternion rotation_;//四元数。旋转表达。
        .
    含有3个构造函数
        Rigid3();//构造函数，DebugString()默认是[1,0,0]和[1,0,0,0]。
        Rigid3(const Vector& translation, const Quaternion& rotation)；//构造函数，提供平移向量[dx,dy,dz]和旋转四元数参数
        Rigid3(const Vector& translation, const AngleAxis& rotation)；//构造函数，提供平移向量[dx,dy,dz]和绕坐标轴的旋转量
        .
    含有4个静态成员函数
        static Rigid3 Rotation(const AngleAxis& angle_axis);//静态成员函数.[dx,dy,dz]为0,只绕坐标轴旋转。
        static Rigid3 Rotation(const Quaternion& rotation); //静态成员函数, 只旋转，不平移。
        static Rigid3 Translation(const Vector& vector) ;//不旋转,只平移[dx,dy,dz]
        static Rigid3 Identity();//单位旋转，DebugString()是[0,0,0]和[1,0,0,0]。
        .
    含有4个成员函数
        Rigid3 cast() //类型转换
        const Vector& translation() //获取数据成员 translation_
        const Quaternion& rotation() //获取四元数参数
        Rigid3 inverse() //求逆,即逆方向旋转和平移。
        string DebugString() const；//返回string形式的变换内容
        .
    提供2个友元函数
        Rigid3 operator*(const Rigid3& lhs,const Rigid3& rhs) //乘法操作Rigid3*Rigid3,得到Rigid3
        Rigid3::Vector operator*(const Rigid3& rigid,const typename Rigid3::Vector& point) //rigid3*Vector,得到Vector
        .
    全局函数：Eigen::Quaterniond RollPitchYaw(double roll, double pitch, double yaw);//根据x，y，z旋转角返回由roll,pathch和yaw构成的4元数

transform/transform.h：

transform.h封装了多个关于3D变换的函数，包括

    获取旋转角度值
        FloatType GetAngle(const Rigid3& transform)；返回3维变换的四元数的角度θ，四元数q=[cos(θ/2),sin(θ/2)x,sin(θ/2)y,sin(θ/2)z]
        T GetYaw(const Eigen::Quaternion& rotation) ；返四元数yaw方向的弧度值,也就是z轴方向的弧度。
        T GetYaw(const Rigid3& transform) ；返回3D变换yaw方向(z轴)的弧度值
        .
    根据四元数获取旋转矩阵
        Eigen::Matrix

transform/transform_interpolation_buffer.h：

TransformInterpolationBuffer类定义了离散时间段内的transform变换信息。作用与ROS的tf2函数族类似

    1个数据成员：
        std::deque deque_; //队列，元素是带时间戳的变换,存储了一段时间内的变换矩阵信息

    6个成员函数:
        void Push(common::Time time, const transform::Rigid3d& transform);添加变换到队列尾部,当缓冲区已满时,删除队首元素
        bool Has(common::Time time) const;//返回能否在给定时间内计算的插值变换。time应在early-old之间，可以插值。
        transform::Rigid3d Lookup(common::Time time) const;//返回time处的变换,可插值
        common::Time earliest_time() const;返回队列缓冲区内变换的最早时间，也就是队首元素。
        common::Time latest_time() const; 最晚时间，也就是队尾元素
        bool empty() const; 队列是否为空

【sensor】

sensor/point_cloud.h：

    点云数据是指在一个三维坐标系统中的一组向量的集合。
    cartographer的PointCloud是由Vector3f组成的vector即std::vector
    PointCloudWithIntensities则是由点云和光线强度组成的struct类。
    point_cloud.h主要定义了跟点云相关的处理操作。包括4个函数
        PointCloud TransformPointCloud(const PointCloud& point_cloud, const transform::Rigid3f& transform);根据三维网格参数转换点云
        PointCloud Crop(const PointCloud& point_cloud, float min_z, float max_z);去掉z轴区域外的点云,返回一个新的点云
        proto::PointCloud ToProto(const PointCloud& point_cloud);序列化
        PointCloud ToPointCloud(const proto::PointCloud& proto);反序列化

sensor/compressed_point_cloud.h

    CompressedPointCloud点云压缩类,压缩ponits以减少存储空间，压缩后有精度损失。方法：按照block分组。提供5个函数：
        CompressedPointCloud(const PointCloud& point_cloud) 使用点云数据初始化，并将点云压缩到 std::vector point_data_中，num_points_为点云数量
        PointCloud Decompress() const;返回解压缩的点云
        bool empty() const; 点云是否为空
        size_t size() const; 点云数量
        ConstIterator begin() const;访问点云block的迭代器
        ConstIterator end() const;点云block的尾后迭代器

sensor/range_data.h

    RangeData定义一系列激光雷达传感器测量数据的存储结构，包括
        Eigen::Vector3f origin; {x0,y0,z0},sensor坐标。
        PointCloud returns; 反射位置{x,y,z}，表征有物体反射。
        PointCloud misses; 无反射,自由空间

    CompressedRangeData定义了一些用于压缩点云的存储结构，包括：
        Eigen::Vector3f origin;
        CompressedPointCloud returns;
        CompressedPointCloud misses;

    定义了6个全局函数：
        proto::RangeData ToProto(const RangeData& range_data); 序列化
        RangeData FromProto(const proto::RangeData& proto);反序列化
        RangeData TransformRangeData(const RangeData& range_data,const transform::Rigid3f& transform);对数据进行3d变换，转换为机器坐标
        RangeData CropRangeData(const RangeData& range_data, float min_z, float max_z);根据min_z和max_z把不在z轴范围内的点云丢弃，剪裁到给定范围
        CompressedRangeData Compress(const RangeData& range_data);压缩,有精度丢失。
        RangeData Decompress(const CompressedRangeData& compressed_range_Data);解压缩,有精度丢失。

.sensor/Data.h

Data是针对某一类的传感器的数据的封装。

    类内数据结构：
        Type :
            传感器类型可以是{ kImu, kRangefinder, kOdometer }中的一类。imu惯性测量单元，雷达，里程计
        Imu :
            Eigen::Vector3d linear_acceleration; //线性加速度,m/s2
            Eigen::Vector3d angular_velocity; //角速度, rad/s
        Rangefinder:
            Eigen::Vector3f origin; //sensor的位姿
            PointCloud ranges;//测距点云
            .

    数据成员:
        Type type;传感器类型,包括imu，雷达，里程计
        common::Time time;测量时间,time
        Imu imu;Imu测量值
        Rangefinder rangefinder;rangefinder,测距仪测量值
        transform::Rigid3d odometer_pose;里程计测量值
    3个构造函数:
        Data(const common::Time time, const Imu& imu)//传感器类型是imu
        Data(const common::Time time, const Rangefinder& rangefinder)//传感器类型是雷达
        Data(const common::Time time, const transform::Rigid3d& odometer_pose)//传感器类型是里程计

sensor/ordered_multi_queue.h

ordered_multi_queue.h定义了一系列处理多个传感器的数据的类，用于接收/标识来自传感器的数据。

定义了一个QueueKey用于标识传感器数据，并将其作为OrderedMultiQueue的关键字key。

    QueueKey
        int trajectory_id;// 轨线id;
        string sensor_id; //传感器id

    OrderedMultiQueue

        用于管理多个有序的传感器数据，是有序的多队列类,每个队列有一个key,并且有一个自定义排序函数.
        queues_的形式为：
        key1:Queue
        key2：Queue
        key3：Queue
            Queue的形式为
            struct Queue {
            common::BlockingQueue

sensor/collator.h

Collator,采集者，抽象了设备采集器。将多传感器采集的数据归并到轨迹上。只有一个默认构造函数，有2个数据成员

    Collator
        数据成员:
            std::unordered_map

sensor/voxel_filter.h

voxel_filter.h定义了与3维网格grid体素的滤波相关的数据结构和相关函数。作用：将点云转换为稀疏体素grid表达。以节省空间

体素滤波器,对每一个体素voxel,采用第一个point代替该体素内的所有points

    VoxelFilter类：
        数据成员:
            mapping_3d::HybridGridBase voxels_;//以体素表示的网格，Grid/Pixel 表征一系列的Cell，每个Cell有一个size，piont以size为间距分布，实现稀疏表示。Cell的数量和即为稀疏表达的point的数量和。
            PointCloud point_cloud_; //网格内的点云
        构造函数 VoxelFilter(float size);初始化网格voxels_的大小
        *成员函数:
            void InsertPointCloud(const PointCloud& point_cloud);将点云插入体素网格中
            const PointCloud& point_cloud() const;返回表达occupied 体素的点云

    全局函数CreateAdaptiveVoxelFilterOptions(),根据pb.h配置文件设置Options参数
        message AdaptiveVoxelFilterOptions {
        optional float max_length = 1;
        optional float min_num_points = 2;
        optional float max_range = 3;
        }

        2d:adaptive_voxel_filter = {
        max_length = 0.9, //voxel_的大小edge的最大值
        min_num_points = 100, //voxel_最多“占据”的points数量
        max_range = 50.,
        },
        *
    AdaptiveVoxelFilter类：
    *
        构造函数 AdaptiveVoxelFilter(const proto::AdaptiveVoxelFilterOptions& options);//根据配置文件设置自适应体素滤波的options
        PointCloud Filter(const PointCloud& point_cloud) const;对点云进行体素滤波,返回过滤后的点云

sensor/configuration.h

configuration.h 主要配置了和传感器设备相关的参数。
提供4个全局函数:

    Sensor CreateSensorConfiguration(common::LuaParameterDictionary* parameter_dictionary);从sensor配置文件解析sensor的数据参数。主要是sensor到机器坐标的转换
    Configuration CreateConfiguration(common::LuaParameterDictionary* parameter_dictionary)//求得多个sensor的配置的集合。
    bool IsEnabled(const string& frame_id, const sensor::proto::Configuration& sensor_configuration);//系统是否支持某一传感器。
    transform::Rigid3d GetTransformToTracking(const string& frame_id,const sensor::proto::Configuration& sensor_configuration); 将sensor采集的data经过3d坐标变换为机器坐标。

【io】

file_writer.h

file_writer.h定义了多个用于文件写入的类

    FileWriter
        FileWriter是负责文件写入的虚基类没有数据成员.只提供一系列抽象接口.
        包括3个抽象接口：
            WriteHeader(),写入文件head
            Write(),写入数据
            Close(),关闭文件

    StreamFileWriter
        StreamFileWriter 文件流写入类,继承自FileWriter
        数据成员只有一个std::ofstream out_负责将文件写入磁盘.
        包括3个成员函数：
            WriteHeader(),写入文件head
            Write(),写入数据
            Close(),关闭文件

    全局函数:
        using FileWriterFactory =
        std::function< std::unique_ptr< FileWriter >(const string& filename)>;
        工厂模式,
        创建一个FileWriter对象,由智能指针管理生命期,
        返回值是std::unique_ptr,
        函数参数是string.

io/points_batch.h：

    PointsBatch类是对多个点云point的抽象.这些point在由同一时刻,同一机器人坐标地点的传感器采集而得。
    数据成员主要描述了point的特性.
    PointsBatch数据成员:
        common::Time time; point采集时间.
        Eigen::Vector3f origin; sensor的世界坐标, 传感器位姿
        string frame_id;关键帧的id
        int trajectory_index;轨迹线id
        std::vector points;point的几何参数,vector<{x,y,z}>
        std::vector intensities;光强
        std::vector colors;point的rgb值

    全局函数
        void RemovePoints(std::vector to_remove, PointsBatch* batch);按照to_temove中的索引,在batch中移除某些point.

流水线-points_processor

整个IO文件夹实现了对点云数据的读取和存储，并且为了模块化，cartographer使用流水线作业的方式对点云进行处理(pipeline)。不同的.h文件抽象了流水线不同的处理器processor。并且类似于链表，每个在流水线上的processor都含有一个Next指针，执行下一阶段的processor。以此来执行作业。在assets_writer_backpack_2d.lua文件中有各个pipeline的处理流程.

io/points_processor.h

points_processor.h文件夹定义了一个抽象基类PointsProcessor，抽象了所有在流水线上的processor的公有接口。提供一种批量处理points的方法。

    类内数据结构:
        enum class FlushResult {
        kRestartStream,//重启流水线
        kFinished,//已完成作业
        };
        用于表达本处理器的当前状态，枚举值

    2个抽象接口:
        virtual void Process(std::unique_ptr points_batch) =0;纯虚函数，Process()负责对PointsBatch进行处理
        Flush()刷新点云数据.

min_max_range_filtering_points_processor

MinMaxRangeFiteringPointsProcessor是PointsProcessor的第一个子类(1).

    .lua配置,可修改
        pipeline = {
        {
        action = “min_max_range_filter”,
        min_range = 1.,
        max_range = 60.,
        },

    功能:继承自PointsProcessor点云虚基类.距离过滤器,L2距离不在Min-Max范围内的都过滤掉.
    数据成员:
        min_range_ 最小范围。
        max_range_ 最大范围。
        PointsProcessor* const next_:完成本轮Processor,接下来进行下一次Processor.
        kConfigurationFileActionName=”min_max_range_filter” ：每个处理器processor用于标识自己的名称
        .
    构造函数
        MinMaxRangeFiteringPointsProcessor(double min_range, double max_range,
        PointsProcessor* next); 指定过滤范围和流水线next指针。
        成员函数:
        Process(std::unique_ptr batch) ;//点云处理,删除[min,max]以外的point，并把数据传递到下一轮Processor处理。
        FlushResult Flush() override;调用next_->Flush():父类指针调用Flush(),但在运行时才会决定是否调用子类的Flush()，即达到动态绑定的效果。

counting_points_processor.h

CountingPointsProcessor是PointsProcessor的第二个子类(2).

    .lua配置,可修改
        pipeline = {
        action = “dump_num_points”,
        }
    功能:继承自PointsProcessor点云虚基类.记录有多少 point被输出处理
    数据成员:
        num_points_记录points数量
        PointsProcessor* next_:完成本轮Processor,接下来进行下一次Processor.
        kConfigurationFileActionName =”dump_num_points”：每个处理器processor用于标识自己的名称
        .
    构造函数
        CountingPointsProcessor(PointsProcessor* next); 指定流水线next指针。
        成员函数:
        Process(std::unique_ptr batch)；//不处理points,而是将num_points_加上batch.size()，即统计点云数据。然后直接流水线到下一PointsProcessor
        FlushResult Flush() override;调用next_->Flush()，依据下一阶段的状态重置本阶段的状态。

xray_points_processor.h

XRayPointsProcessor是PointsProcessor的第三个子类(3).

    .lua配置,可修改

        VOXEL_SIZE = 5e-2

        YZ_TRANSFORM = {
        translation = { 0., 0., 0. },
        rotation = { 0. , 0., math.pi, },
        }
        *{
        action = “write_xray_image”,
        voxel_size = VOXEL_SIZE,
        filename = “xray_yz_all”,
        transform = YZ_TRANSFORM,
        }

    类内数据结构:
        struct ColumnData {
        double sum_r = 0.;
        double sum_g = 0.;
        double sum_b = 0.;
        uint32_t count = 0;
        };
        struct Aggregation {
        mapping_3d::HybridGridBase voxels;
        std::map

intensity_to_color_points_processor.h

ColoringPointsProcessor是PointsProcessor的第四个子类(4).处理强度到color point 的强度变换类

    .lua配置,可修改
        pipeline = {
        action = “intensity_to_color”,
        min_intensity = 0.,
        max_intensity = 4095.,
        },

    功能:继承自PointsProcessor点云虚基类. 功能:将光强度转换为color

    数据成员:
        const float min_intensity_;
        const float max_intensity_;
        const string frame_id_;只有相同的id才将光强度转换为color。horizontal_laser_link或者vertical_laser_link
        PointsProcessor* next_:完成本轮Processor,接下来进行下一次Processor.
        kConfigurationFileActionName =”intensity_to_color”：每个处理器processor用于标识自己的名称
        .

    构造函数
        IntensityToColorPointsProcessor(float min_intensity, float max_intensity,
        const string& frame_id,
        PointsProcessor* next);;初始化min和max，frame_id和流水线next指针。

    成员函数:
        Process(std::unique_ptr batch)；//成员函数执行转换:(‘intensity’ - min ) / (max - min) * 255 然后直接流水线到下一PointsProcessor
        FlushResult Flush() override;调用next_->Flush()，依据下一阶段的状态重置本阶段的状态。

ply_writing_points_processor.h

PlyWritingPointsProcessor是PointsProcessor的第五个子类(5).
PlyWritingPointsProcessor负责将点云point以PLY的格式写入磁盘.

    .lua配置,可修改
        pipeline = {
        action = “write_ply”,
        filename = “points.ply”,
        }

    功能:继承自PointsProcessor点云虚基类.负责将点云point以PLY的格式写入磁盘.

    数据成员:
        num_points_记录points数量
        bool has_colors_;//是否是RGB
        PointsProcessor* next_:完成本轮Processor,接下来进行下一次Processor.
        kConfigurationFileActionName =”dump_num_points”：每个处理器processor用于标识自己的名称
        .

    构造函数
        PlyWritingPointsProcessor(std::unique_ptr file,
        PointsProcessor* next); 指定文件写入的工厂和流水线next指针。

    成员函数:
        Process(std::unique_ptr batch)；//按照ply格式写点云信息 。然后直接流水线到下一PointsProcessor
        FlushResult Flush() override;调用next_->Flush()，依据下一阶段的状态重置本阶段的状态。

coloring_points_processor.h

ColoringPointsProcessor是PointsProcessor的第六子类(6).

    .lua配置,可修改
        pipeline = {
        action = “color_points”,
        frame_id = “horizontal_laser_link”,
        color = { 255., 0., 0. },
        }

    功能:继承自PointsProcessor点云虚基类. 用固定的Color填充PointsBatch的Color分量。着色

    数据成员:
        color_：rgb值，color一般为[255,0,0],[0,255,0]
        frame_id_:只有相同的id才填充Color。horizontal_laser_link或者vertical_laser_link
        PointsProcessor* next_:完成本轮Processor,接下来进行下一次Processor.
        kConfigurationFileActionName =”color_points”：每个处理器processor用于标识自己的名称
        .

    构造函数
        ColoringPointsProcessor(const Color& color, const string& frame_id,
        PointsProcessor* next);初始化 color_，frame_id和流水线next指针。

    成员函数:
        Process(std::unique_ptr batch)；//着色，只对相同的frame_id_处理。然后直接流水线到下一PointsProcessor
        FlushResult Flush() override;调用next_->Flush()，依据下一阶段的状态重置本阶段的状态。

fixed_ratio_sampling_points_processor.h

FixedRatioSamplingPointsProcessor是PointsProcessor的第七个子类(7).
FixedRatioSamplingPointsProcessor是采样过滤器

    .lua配置,可修改
        sampling_ratio = 0.3,

    功能:继承自PointsProcessor点云虚基类.该class只让具有固定采样频率的点通过,其余全部 remove.sampling_ratio=1,即无任何操作,全通滤波.

    数据成员:
        const double sampling_ratio_;采样率
        std::unique_ptr sampler_;具有固定采样率的采样函数
        PointsProcessor* next_:完成本轮Processor,接下来进行下一次Processor.
        kConfigurationFileActionName =”fixed_ratio_sampler”：每个处理器processor用于标识自己的名称
        .

    构造函数

        FixedRatioSamplingPointsProcessor(double sampling_ratio,
        PointsProcessor* next);初始化采样率和流水线next指针。

    成员函数:
        Process(std::unique_ptr batch)；//根据采样率采集点云，不在采样点上的全部删除。
        FlushResult Flush() override;调用next_->Flush()，依据下一阶段的状态重置本阶段的状态。

outlier_removing_points_processor.h

OutlierRemovingPointsProcessor是PointsProcessor的第八个子类(8).

    .lua配置,可修改
        VOXEL_SIZE = 5e-2
    功能:继承自PointsProcessor点云虚基类.体素过滤器,Remove有移动痕迹的体素。只保留没有移动的体素
    类内数据结构:
        struct VoxelData {
        int hits = 0;
        int rays = 0;
        };
        enum class State {
        kPhase1,
        kPhase2,
        kPhase3,
        };

    数据成员:
        const double voxel_size_;//体素大小
        State state_;
        mapping_3d::HybridGridBase voxels_;包含多个体素的网格Grid。
        PointsProcessor* next_:完成本轮Processor,接下来进行下一次Processor.
        kConfigurationFileActionName=”voxel_filter_and_remove_moving_objects”：每个处理器processor用于标识自己的名称
        .

    构造函数
        OutlierRemovingPointsProcessor(double voxel_size, PointsProcessor* next);初始化体素大小和流水线next指针。

    成员函数:
        Process(std::unique_ptr batch)；//删除移动的体素。
        FlushResult Flush() override;调用next_->Flush()，依据下一阶段的状态重置本阶段的状态。

pcd_writing_points_processor.h

PcdWritingPointsProcessor是PointsProcessor的第九个子类(9).

    .lua配置,可修改
        VOXEL_SIZE = 5e-2
    功能:继承自PointsProcessor点云虚基类.将点云按照pcd格式存储在pcd文件中.
    类内数据结构:
        struct VoxelData {
        int hits = 0;
        int rays = 0;
        };
        enum class State {
        kPhase1,
        kPhase2,
        kPhase3,
        };

    数据成员:
        int64 num_points_; //点云数量
        bool has_colors_; //是否是彩色
        PointsProcessor* next_:完成本轮Processor,接下来进行下一次Processor.
        kConfigurationFileActionName=”write_pcd”：每个处理器processor用于标识自己的名称
        .

    构造函数
        PcdWritingPointsProcessor(std::unique_ptr file_writer,
        PointsProcessor* next);初始化一个文件类名和流水线next指针。

    成员函数:
        Process(std::unique_ptr batch)；//负责将点云按照PCD格式写入文件
        FlushResult Flush() override;调用next_->Flush()，依据下一阶段的状态重置本阶段的状态。

null_points_processor.h

NullPointsProcessor是PointsProcessor的第十个子类(10)
空操作类。通常用于pipline的尾端，丢弃所有的点云，表示整个处理流程已经完毕。

    成员函数:
        Process(std::unique_ptr batch)；不作任何事情
        FlushResult Flush() override;返回”kFinished”状态，此操作会传递给上一个流水线。

【kalman_filter】

kalman_filter/gaussian_distribution.h

GaussianDistribution定义了多个变量的高斯分布。构造函数是N*1的均值矩阵和N*N的协方差矩阵

    数据成员:
        Eigen::Matrix mean_; //N*1，均值
        Eigen::Matrix covariance_; //N*N。协方差

    构造函数

    GaussianDistribution(const Eigen::Matrix& mean,
    const Eigen::Matrix& covariance)
    初始化均值和协方差

    全局函数

        重载"+"加号操作符
            GaussianDistribution operator+(const GaussianDistribution& lhs,
            const GaussianDistribution& rhs)
            高斯+高斯=对应均值+均值,对应协方差+协方差
            返回值：新高斯对象
            .

        重载乘法运算符
            GaussianDistribution operator*(const Eigen::Matrix& lhs,
            const GaussianDistribution& rhs)
            1,矩阵N*M
            2,高斯分布M*M
            返回值:高斯分布：N*N

unscented_kalman_filter.h

UnscentedKalmanFilter类是根据《Probabilistic Robotics》实现的无损卡尔曼滤波的算法，并且根据论文
A Quaternion-based Unscented Kalman Filter for Orientation，Kraft ,E.中的算法，扩展到了处理非线性噪声和传感器。卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。

    全局函数:
        constexpr FloatType sqr(FloatType a) 求平方
        Eigen::Matrix OuterProduct( const Eigen::Matrix& v) ；求 N*1× 1*N -> 外积,N*N
        void CheckSymmetric(const Eigen::Matrix& A) ；检查A是否是对称矩阵,A减去A的转置~=0
        Eigen::Matrix MatrixSqrt(const Eigen::Matrix& A) 返回对称半正定矩阵的平方根B,M=B*B

.
UnscentedKalmanFilter类

    使用2个别名
        using StateType = Eigen::Matrix; //状态矩阵N*1
        using StateCovarianceType = Eigen::Matrix; //协方差矩阵N*N

    数据成员
        GaussianDistribution belief_; N*1矩阵，对N个变量的估计
        const std::function add_delta_;加法操作
        const std::function compute_delta_;计算偏差操作

    构造函数

        explicit UnscentedKalmanFilter(
        const GaussianDistribution& initial_belief, //参数1

        std::function //参数2
        add_delta = [](const StateType& state,
        const StateType& delta) { return state + delta; },

        std::function //参数3
        compute_delta =
        [](const StateType& origin, const StateType& target)

        参数1,N*1矩阵,
        参数2,stl函数对象 add_delta(默认),
        参数3,stl函数对象 compute_delta(默认),

    私有的成员函数:
        StateType ComputeWeightedError(const StateType& mean_estimate,
        const std::vector& states)；//计算带权重的偏差
        StateType ComputeMean(const std::vector& states) ;计算均值

    公有成员函数:
        void Predict(std::function g, const GaussianDistribution& epsilon) ；预测，在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。
        void Observe( std::function(const StateType&)> h, const GaussianDistribution& delta) ；测量/观察，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。

    对UnscentedKalmanFilter类的理解最好通过写一些test函数用于理解。
        比如在test.cc中按照给定的g和h，运行500次循环后为何state[0]和state[1]均为5？

        运行unscented_kalman_filter_le_test.cc:

             before:0 42 #滤波前
             Predict:0 0 # 执行预测
             Observe:4.995 4.995 # 执行校正
             
             before:4.995 4.995 #第二次滤波前
             Predict4.995 4.995 # 执行预测
             Observe: 4.9975 4.9975 # 执行校正
            可见经过2次迭代就已经较为准确了。

pose_tracker.h

pose_tracker.h定义了根据UKF对位姿的滤波后的估计PoseTracker类

    全局定义
        typedef Eigen::Matrix3d Pose2DCovariance; //3*3矩阵
        typedef Eigen::Matrix PoseCovariance;// 6*6 矩阵
        struct PoseAndCovariance {
        transform::Rigid3d pose;
        PoseCovariance covariance; //6*6
        };
        PoseAndCovariance operator*(const transform::Rigid3d& transform,
        const PoseAndCovariance& pose_and_covariance);
        PoseCovariance BuildPoseCovariance(double translational_variance,
        double rotational_variance);

    PoseTracker类:

        类内数据结构:
            enum {
            kMapPositionX = 0,//位置信息{X,Y,Z}
            kMapPositionY,
            kMapPositionZ,
            kMapOrientationX,//方向信息,3
            kMapOrientationY,
            kMapOrientationZ,
            kMapVelocityX, //速度信息,6
            kMapVelocityY,
            kMapVelocityZ,
            kDimension //9, We terminate loops with this. 只追踪9个维度
            };

        类内别名:
        using KalmanFilter = UnscentedKalmanFilter;//9维的卡尔曼滤波
        using State = KalmanFilter::StateType; //N*1矩阵
        using StateCovariance = Eigen::Matrix;//9*9
        using Distribution = GaussianDistribution;

    .lua配置信息:
        trajectory_builder_3d.lua:
        pose_tracker = {
        orientation_model_variance = 5e-3,
        position_model_variance = 0.00654766,
        velocity_model_variance = 0.53926,
        -- This disables gravity alignment in local SLAM.
        imu_gravity_time_constant = 1e9,
        imu_gravity_variance = 0,
        num_odometry_states = 1,
        },

    数据成员:
        const proto::PoseTrackerOptions options_; //用于位姿估计的传感器特性
        common::Time time_; //测量时间
        KalmanFilter kalman_filter_; //卡尔曼滤波
        mapping::ImuTracker imu_tracker_; //imu跟踪
        mapping::OdometryStateTracker odometry_state_tracker_;//里程计跟踪

    私有的成员函数:
        static Distribution KalmanFilterInit();返回初始状态的状态变量的高斯分布
        const Distribution BuildModelNoise(double delta_t) const;建立零均值噪声模型
        void Predict(common::Time time);根据当前状态预测time时刻的状态
        transform::Rigid3d RigidFromState(const PoseTracker::State& state); 结合imu_tracker_和state,计算位姿pose的旋转变换。

.

    构造函数
        PoseTracker(const proto::PoseTrackerOptions& options, common::Time time);在给定的time时刻初始化卡尔曼滤波参数
    公有的成员函数:
        GetPoseEstimateMeanAndCovariance();通过指针获取pose的旋转参数和covariance方差
        AddImuLinearAccelerationObservation();根据imu观测值更新
        AddPoseObservation(); 根据map-frame的位姿估计更新
        AddOdometerPoseObservation();根据里程计的map-like frame位姿估计更新
        common::Time time() ;最新有效时间
        GetBelief(common::Time time);未来某一时刻的状态估计值
        Eigen::Quaterniond gravity_orientation()；imu的重力方向

【mapping】

mapping/probability_values.h

probability_values.h定义了一系列与概率相关的函数–多个用于计算概率的mapping命名空间下的全局函数。cartographer所有的概率不是以0-1.0表示，而是通过浮点数到整数的映射： [1, 32767]，避免浮点数运算。

    全局函数

    *  inline float Odds(float probability) {          //论文公式(2),求胜负比。y=x/(1-x)
         return probability / (1.f - probability);    }

    *  inline float ProbabilityFromOdds(const float odds) { //求概率，即x=y/(1+y)
        return odds / (odds + 1.f);    }

        * constexpr float kMinProbability = 0.1f;//p最小是0.1

        * constexpr float kMaxProbability = 1.f - kMinProbability;//最大是0.9

        * inline float ClampProbability(const float probability);限制概率p在[0.1,0.9]之间

        * constexpr uint16 kUnknownProbabilityValue = 0;//标记未初始化的概率

        * constexpr uint16 kUpdateMarker = 1u << 15;// 32768

        * inline uint16 ProbabilityToValue(const float probability) 将概率p映射为整数Value[1,32767]

        * extern const std::vector< float>* const kValueToProbability;   声明，定义在.cc文件。vector是value到p的映射

        * inline float ValueToProbability(const uint16 value);映射 [1,32767]->[0.1,0.9]

        std::vector< uint16> ComputeLookupTableToApplyOdds(float odds);//2份value:前一半对应没有hit，后一半对应hit。

        之前没有hit过，则没有update，按论文公式(2)计算：
        求p,
        求[1,32767],
        求[1,32767]+32768
        push_back()

        之前有hit过，则有update，按论文公式(3)计算：
        求(*kValueToProbability)[cell]->[0.1,0.9]即原始p
        求p'=odds*Odds(p)
        求p'映射到[1,32767]
        push_back():[1,32767]+32768.

id.h

该文件定义了一系列用于标记轨迹的数据结构，包括：

    struct NodeId

            {
          int trajectory_id;
          int node_index; }

    每一个轨迹trajectory上有多个节点node。节点标号:轨迹id+{0,1,2,…}.

    struct SubmapId {

      int trajectory_id;
      int submap_index;  }

    重建全局地图global map时，是由多个submap组成。submap标号: 轨迹id+ {0,1,2,3…}

    NestedVectorsById
        作用：嵌套存储多条轨迹线上的数据
        数据成员:vector< vector > data_;
        对外提供4个操作：
            IdType Append(int trajectory_id, const ValueType& value) 根据轨迹id和data添加数据
            const ValueType& at(const IdType& id)查询某轨迹对应的vector的data
            num_indices(),某轨迹对应的数据的大小
            int num_trajectories() 轨迹数量

imu_tracker.h

ImuTracker类利用来自imu测量仪器的数据对机器的位姿pose的方向角进行跟踪和预测。x,y轴不会drift漂移。z轴可能会产生漂移。(因为绕z轴旋转角有累计误差)

    作用：使用来自imu的角速度+加速度用于跟踪pose的orientation

    数据成员：
        const double imu_gravity_time_constant_; // g，10.0
        common::Time time_; //最新一次测量时间
        common::Time last_linear_acceleration_time_;//加速度测量时间
        Eigen::Quaterniond orientation_; //pose的方向角
        Eigen::Vector3d gravity_vector_; //加速度测量的方向
        Eigen::Vector3d imu_angular_velocity_; //角速度

    成员函数:
        void Advance(common::Time time); 系统时间增加t，更新方向角
        void AddImuLinearAccelerationObservation() //更新imu测量得到的加速度。
        void AddImuAngularVelocityObservation() //更新imu测量得到的角速度
        Eigen::Quaterniond orientation();返回目前估计pose的方向角。

odometry_state_tracker.h

odometry_state_tracker.h定义了与里程计相关的跟踪接口

    .lua设置：
    imu_gravity_time_constant = 10.,
    num_odometry_states = 1000,

    OdometryState类：
        含3个数据成员
            common::Time time ，时间
            transform::Rigid3d odometer_pose，里程计的位置
            transform::Rigid3d state_pose，状态位置

    OdometryStateTracker类：
        作用：
        数据成员：
            OdometryStates odometry_states_;记录多个里程计状态，是一个双端队列deque
            size_t window_size_;滑动窗大小,即队列大小
        构造函数 explicit OdometryStateTracker(int window_size);构造函数初始化滑动窗大小.
        成员函数:
            void AddOdometryState(const OdometryState& odometry_state);添加新的里程计状态,超出滑动窗大小时,旧的删除
            bool empty() const;队列是否为空
            const OdometryState& newest() const;里程计最新的状态

detect_floors.h

detect_floors.h定义了关于3D扫描楼层的数据结构。

    struct Timespan {
    common::Time start;
    common::Time end;
    };
    Timespan表征扫描的时间范围。

    struct Floor {
    std::vector< Timespan> timespans;
    double z; //z轴的中值
    };
    一个楼层对应多个扫描timespan：有可能重复的扫描多次
    但只有一个高度z。

    std::vector DetectFloors(const proto::Trajectory& trajectory);
    使用启发式搜索寻找building的不同楼层的z值。

submaps.h

    全局函数
        inline float Logit(float probability); //求odds(p)的log对数
        inline uint8 ProbabilityToLogOddsInteger(const float probability) 将[0.1,0.9]映射为0-255之间的数

    Submap
        数据成员
            const transform::Rigid3d local_pose;子图的位姿
            int num_range_data = 0;插入的数据量
            const mapping_2d::ProbabilityGrid* finished_probability_grid = nullptr;完成建图的概率网格。 当子图不再变化时，指向该子图的概率分布网格。

    Submaps
        Submaps是一连串的子图,初始化以后任何阶段均有2个子图被当前scan point影响：old submap 用于now match,new submap用于next match,一直交替下去。一旦new submap有足够多的scan point，那么old submap不再更新。此时new submap变为old，用于 scan-to-map匹配。
        Submaps是虚基类,没有数据成员,只提供成员函数用于实现接口。
        成员函数抽象接口：
            int matching_index() const;最新的初始化的子图索引，用于scan-to-map-match，size() - 2;
            std::vector< int> insertion_indices() const;待插入的子图的索引， {size() - 2, size() - 1};
            virtual const Submap* Get(int index) const;纯虚函数，按索引返回子图指针。
            virtual int size() const ；子图数量
            static void AddProbabilityGridToResponse();将子图对应的概率网格序列化到proto文件中
            virtual void SubmapToProto(); 将对应的子图序列化到proto文件中

trajectory_node.h

    轨迹节点TrajectoryNode类，含有一个内部类ConstantData。
    TrajectoryNode作用：在连续的轨迹上采样一些离散的点用于key frame，标识pose frame。

    类内数据结构

        ConstantData common::Time time;

         sensor::RangeData range_data_2d;//测量得到的2D range数据
        sensor::CompressedRangeData range_data_3d;//测量得到的3D range数据
        int trajectory_id;//节点所属id
        transform::Rigid3d tracking_to_pose; //tracking frame 到 pose frame的矩阵变换。
        };

    TrajectoryNode类
        数据成员：
            const ConstantData* constant_data;
            transform::Rigid3d pose;
        成员函数common::Time time();返回测量时间

trajectory_connectivity.h

TrajectoryConnectivity用于解决不同轨迹线的连通性问题.多条轨迹构成一颗森林，而相互联通的轨迹应该合并。

    数据成员：
        common::Mutex lock_;
        std::map < int, int > forest_ ；不同轨迹线组成的森林
        std::map < std::pair< int, int>, int> connection_map_ ；直接链接的轨迹线
    成员函数：
        void Add(int trajectory_id) EXCLUDES(lock_);添加一条轨迹线，默认不连接到任何轨迹线
        void Connect(int trajectory_id_a, int trajectory_id_b) ；将轨迹a和轨迹b联通
        TransitivelyConnected(int trajectory_id_a, int trajectory_id_b)；判断是否处于同一个连通域
        int ConnectionCount(int trajectory_id_a, int trajectory_id_b)；返回直接联通的数量
        std::vector < std::vector < int > > ConnectedComponents()；:由联通分量id组成的已联通分类组
    全局函数
        proto::TrajectoryConnectivity ToProto(
        std::vector< std::vector< int>> connected_components);
        编码已联通成分到proto文件
        ConnectedComponent FindConnectedComponent()//返回连接到联通id的所有联通分量。

trajectory_builder.h

TrajectoryBuilder:虚基类,提供多个抽象接口。没有数据成员

    作用：根据轨迹Builder收集data。

    类内数据结构：

        PoseEstimate{

        common::Time time = common::Time::min();测量时间
        transform::Rigid3d pose = transform::Rigid3d::Identity();世界坐标转换
        sensor::PointCloud point_cloud; 子图所在的点云
        }
    虚函数：
        virtual const Submaps* submaps() =0;一系列子图
        virtual const PoseEstimate& pose_estimate() const = 0;子图位姿及其采集的点云
        virtual void AddSensorData(const string& sensor_id,
        std::unique_ptr< sensor::Data> data) = 0;
        根据sensor_id添加data，虚函数。
        .
    非虚函数：下面3个函数都是非虚函数。分别是添加雷达/imu/里程计的data。
        AddRangefinderData();
        AddImuData();
        AddOdometerData();
            参数：
            1),sensor_id,标识传感器。
            2),time 测量时间
            3),PointCloud/Vector3d /Rigid/Rigid3d 测量得到的数据

collated_trajectory_builder.h

CollatedTrajectoryBuilder类继承自TrajectoryBuilder

    作用：使用Collator处理从传感器收集而来的数据.并传递给GlobalTrajectoryBuilderInterface。

    数据成员
        sensor::Collator* const sensor_collator_; 传感器收集类实例
        const int trajectory_id_; 轨迹id
        std::unique_ptr< GlobalTrajectoryBuilderInterface > wrapped_trajectory_builder_;全局的建图接口
        std::chrono::steady_clock::time_point last_logging_time_; 上一次传递数据时间
        std::map< string, common::RateTimer<>> rate_timers_;频率

    成员函数,重写了父类的接口
        const Submaps* submaps() const override;
        const PoseEstimate& pose_estimate() const override;
        void AddSensorData(const string& sensor_id,std::unique_ptr data) override;

sparse_pose_graph.h

SparsePoseGraph:稀疏位姿图模型,虚基类,提供多个抽象接口,不可拷贝/赋值

    作用：
    稀疏图用于闭环检测

    类内数据结构：

        struct Pose {

         transform::Rigid3d zbar_ij;
          double translation_weight;
          double rotation_weight;
        };

        struct Constraint { //约束
        struct Pose {

            mapping::SubmapId submap_id;  // 'i' in the paper.
            mapping::NodeId node_id;      // 'j' in the paper.
            Pose pose;
            enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
        };

    虚函数，抽象接口:
        virtual void RunFinalOptimization() = 0;计算优化后的位姿估计
        virtual std::vector< std::vector> GetConnectedTrajectories() = 0;获取已连接的轨迹集合
        virtual std::vector< transform::Rigid3d> GetSubmapTransforms( int trajectory_id) = 0; 获取优化后的位姿估计的3D变换
        virtual transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) = 0;获取局部图的3D变换
        virtual std::vector< std::vector< TrajectoryNode>> GetTrajectoryNodes() = 0;优化后的轨迹线
        virtual std::vector< Constraint> constraints() = 0; 获取约束集

map_builder.h

MapBuilder类,建图,不可拷贝/赋值

    作用：MapBuilder类和TrajectoryBuilder类即真正的开始重建局部子图submaps,并且采集稀疏位姿图用于闭环检测。

    数据成员:
        const proto::MapBuilderOptions options_; // 建图选项
        common::ThreadPool thread_pool_; //线程数量，不可变。
        std::unique_ptr< mapping_2d::SparsePoseGraph> sparse_pose_graph_2d_; //稀疏2D图
        std::unique_ptr< mapping_3d::SparsePoseGraph> sparse_pose_graph_3d_; //稀疏3D图
        mapping::SparsePoseGraph* sparse_pose_graph_; //稀疏位姿

    sensor::Collator sensor_collator_; //收集传感器采集的数据
    std::vector< std::unique_ptr< mapping::TrajectoryBuilder> > trajectory_builders_;//轨迹线集合

    成员函数:
        int AddTrajectoryBuilder()根据传感器id和options新建一个轨迹线，返回轨迹线的索引
        TrajectoryBuilder* GetTrajectoryBuilder(int trajectory_id);根据轨迹id返回指向该轨迹的TrajectoryBuilder对象指针。
        void FinishTrajectory(int trajectory_id);标记该轨迹已完成data采集，后续不再接收data
        int GetBlockingTrajectoryId() const;阻塞的轨迹，常见于该条轨迹上的传感器迟迟不提交data。
        proto::TrajectoryConnectivity GetTrajectoryConnectivity();获得一系列轨迹的连通域
        string SubmapToProto();把轨迹id和子图索引对应的submap，序列化到文件
        int num_trajectory_builders() const;在建图的轨迹数量

 

---------------------
作者：slamcode
来源：CSDN
原文：https://blog.csdn.net/learnmoreonce/article/details/76063997
原文：https://blog.csdn.net/learnmoreonce/article/details/76216542
原文：https://blog.csdn.net/learnmoreonce/article/details/76218220
原文：https://blog.csdn.net/learnmoreonce/article/details/76359021
原文：https://blog.csdn.net/learnmoreonce/article/details/76359116
原文：https://blog.csdn.net/learnmoreonce/article/details/76408392