58-总结-【cartographer源码分析】系列的第六部分【 mapping 】

【cartographer源码分析】系列的第六部分【 mapping】。

现总结 mapping 文件夹涉及到的主要功能：

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

---

【2】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() 轨迹数量

【3】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的方向角。

【4】 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;里程计最新的状态

【5】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值。

---

【6】 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文件中

---

【7】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;//节点所属idtransform::Rigid3d tracking_to_pose; //tracking frame 到 pose frame的矩阵变换。

    };

• TrajectoryNode类

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

【8】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的所有联通分量。

---

【9】 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 测量得到的数据

---

【10】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;

---

[11] 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; 获取约束集

---

[12] 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;在建图的轨迹数量

---

关于 mapping文件夹的源码分析已经完毕，更详细细节可在 这里 查看注释版源码。

本文发于：

• slam源码分析微信公众号:slamcode
• CSDN