• 目标：

  • 目前手上有一套小车和激光雷达，要实现定位和移动控制。

  • 由于用的arduino平台，硬件资源不够，因此传输数据到pc，pc进行算法分析后进行控制

  • 因此需要用MapBuilder模块实现2d-slam建模

MapBuilder

1. 管理轨迹：

   1. 支持多个轨迹的独立创建和管理，每个轨迹可以与不同的传感器数据绑定。

2. 数据处理：

   1. 整合来自激光雷达、IMU、里程计等多种传感器的数据，用于 SLAM（同步定位与建图）任务。

3. 调用 Pose Graph 优化：

   1. 将所有轨迹数据和子图（Submap）传递给 Pose Graph，优化整体的位姿和约束。

4. 地图输出：

   1. 支持生成点云地图或栅格化地图，保存为 pbstream 或其他格式。

//创建轨迹
mapping::TrajectoryBuilderInterface* AddTrajectoryBuilder(
    const std::vector<std::string>& expected_sensor_ids,
    const mapping::proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback);

//完成轨迹
void FinishTrajectory(int trajectory_id);

//从 pbstream 加载轨迹
void LoadStateFromFile(const std::string& state_filename, bool load_frozen_state);

//保存地图
void SerializeStateToFile(bool include_unfinished_submaps, const std::string& filename);

//获取 Pose Graph
mapping::PoseGraphInterface* pose_graph();

TrajectoryBuilderInterface

• 作为传感器数据的入口。

• 实现传感器数据的预处理，如滤波或校准。

• 调用本地 SLAM（Local SLAM）算法，生成轨迹节点。

• 将轨迹节点和子图传递到全局优化模块（Pose Graph）

// 根据不同的重载，传输不同类型的传感器数据
激光雷达输入：

void AddSensorData(const std::string& sensor_id,
                   const sensor::TimedPointCloudData& data) override;

    参数：
        sensor_id：传感器 ID，与配置文件中定义的名称对应。
        data：时间戳点云数据。

IMU 输入：

void AddSensorData(const std::string& sensor_id,
                   const sensor::ImuData& data) override;

    参数：
        sensor_id：传感器 ID。
        data：IMU 数据，包括加速度和角速度。

里程计输入：

void AddSensorData(const std::string& sensor_id,
                   const sensor::OdometryData& data) override;

    参数：
        sensor_id：传感器 ID。
        data：里程计的位姿数据。

固定坐标系位姿：

void AddSensorData(const std::string& sensor_id,
                   const sensor::FixedFramePoseData& data) override;

    参数：
        sensor_id：传感器 ID。
        data：在固定参考系中的位姿数据（例如 GPS 提供的全球位姿）。

地标输入：

void AddSensorData(const std::string& sensor_id,
                   const sensor::LandmarkData& data) override;

    参数：
        sensor_id：传感器 ID。
        data：地标观测数据。

// 停止轨迹的构建，并触发一些清理操作。
void FinishTrajectory() override;

// 丢弃轨迹构建中的所有未处理数据。
void Flush() override;

PoseGraph

1. 维护轨迹和子图的结构：包括节点、子图和它们的全局位姿。

2. 处理约束：

   1. 增量添加新的约束（如回环检测的结果）。

   2. 执行全局优化以调整所有轨迹。

3. 与 Local SLAM 集成：接受本地 SLAM 生成的子图和轨迹节点。

4. 支持多轨迹处理：处理多个独立的轨迹并合并到同一个全局坐标系中。

// 添加 IMU 数据
  virtual void AddImuData(int trajectory_id,
                          const sensor::ImuData& imu_data) = 0;

  // 添加里程计数据
  virtual void AddOdometryData(int trajectory_id,
                               const sensor::OdometryData& odometry_data) = 0;

  // 添加固定参考帧的位姿数据
  virtual void AddFixedFramePoseData(
      int trajectory_id,
      const sensor::FixedFramePoseData& fixed_frame_pose_data) = 0;

  // 添加地标数据
  virtual void AddLandmarkData(int trajectory_id,
                               const sensor::LandmarkData& landmark_data) = 0;

  // 完成指定轨迹的构建
  virtual void FinishTrajectory(int trajectory_id) = 0;

  // 冻结指定轨迹，不再优化该轨迹的位姿
  virtual void FreezeTrajectory(int trajectory_id) = 0;

  // 从序列化的子图中添加子图信息
  virtual void AddSubmapFromProto(const transform::Rigid3d& global_pose,
                                  const proto::Submap& submap) = 0;

  // 从序列化的节点中添加轨迹节点
  virtual void AddNodeFromProto(const transform::Rigid3d& global_pose,
                                const proto::Node& node) = 0;

  // 从序列化数据中设置轨迹信息
  virtual void SetTrajectoryDataFromProto(
      const mapping::proto::TrajectoryData& data) = 0;

  // 向子图中添加节点信息
  virtual void AddNodeToSubmap(const NodeId& node_id,
                               const SubmapId& submap_id) = 0;

  // 添加约束
  virtual void AddSerializedConstraints(
      const std::vector<Constraint>& constraints) = 0;

  // 添加轨迹修剪器
  virtual void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) = 0;

  // 获取当前轨迹的连通关系
  virtual std::vector<std::vector<int>> GetConnectedTrajectories() const = 0;

  // 获取指定子图的全局位姿及其本体
  virtual SubmapData GetSubmapData(const SubmapId& submap_id) const = 0;

  // 将当前的 PoseGraph 转换为 proto 格式
  proto::PoseGraph ToProto() const override;

  // 获取 IMU 数据
  virtual sensor::MapByTime<sensor::ImuData> GetImuData() const = 0;

  // 获取里程计数据
  virtual sensor::MapByTime<sensor::OdometryData> GetOdometryData() const = 0;

  // 获取固定参考帧的位姿数据
  virtual sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData()
      const = 0;

  // 获取地标数据
  virtual std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
  GetLandmarkNodes() const = 0;

  // 设置初始轨迹位姿（用于多轨迹合并）
  virtual void SetInitialTrajectoryPose(int from_trajectory_id,
                                        int to_trajectory_id,
                                        const transform::Rigid3d& pose,
                                        const common::Time time) = 0;

SubmapData

• pose类型：cartographer::transform::Rigid3d表示子图的全局位姿，通常是通过图优化计算得到的。全局位姿是相对于全局坐标系的 3D 刚体变换。

  • 类型：cartographer::transform::Rigid3d

  • 表示子图的全局位姿，通常是通过图优化计算得到的。

  • 全局位姿是相对于全局坐标系的 3D 刚体变换。

• submap类型：std::shared_ptr<const Submap>子图本身的数据结构。Submap 是子图的具体表示，包含了扫描数据、状态信息（如是否完成）、以及栅格地图等。

  • 类型：std::shared_ptr<const Submap>

  • 子图本身的数据结构。

  • Submap 是子图的具体表示，包含了扫描数据、状态信息（如是否完成）、以及栅格地图等。

Submap

• 一个表示局部地图数据的核心数据结构。它通常是由 Cartographer 在 SLAM 过程中创建的，并且在地图构建的不同阶段，子图代表了一个小区域或一个时间段的扫描数据

• 包含以下信息：

  • 局部位姿（Local Pose）：表示子图在全局坐标系中的位置。

  • 栅格地图数据：如 ProbabilityGrid 或 HybridGrid，这两种数据结构用于存储空间中的占用情况或距离信息。

  • 扫描数据：表示该子图生成时所接收到的激光扫描数据。

• 这个只是接口类，看不到数据存在哪

• Submap2D 继承该类，数据存在

  std::unique_ptr<Grid2D> grid_;
  ValueConversionTables* conversion_tables_;

Grid2D

• Grid2D 是 Cartographer 中用于表示二维网格数据的类，通常用于表示地图、栅格图（grid maps）等二维数据结构。在 Cartographer 中，Grid2D 通常用于构建子图中的栅格地图，如概率网格（probability grid）或 TSDF（Truncated Signed Distance Field）等

• 主要成员变量

  • MapLimits limits_;
    这个成员表示地图的限制或边界，例如地图的尺寸、分辨率或坐标范围。MapLimits 可能包括地图的宽度、高度以及每个单元格的物理尺寸。

  • std::vector correspondence_cost_cells_;
    这是一个存储 uint16_t 类型的数组，用于存储地图中每个单元格的对应成本值（correspondence cost）。这些成本值通常用于表示该位置的置信度或与某些传感器数据的匹配度。例如，它可以表示在某些位置上传感器观测到的信号的强度或可信度。

  • float min_correspondence_cost_; 和 float max_correspondence_cost_;
    这两个变量分别表示对应成本的最小值和最大值。它们用于限制 correspondence_cost_cells_ 中存储的值范围，以确保成本不会低于或高于某个阈值。

  • std::vector update_indices_;
    这个成员变量用于存储已更新的单元格索引。每当地图数据发生变化时，相关的索引会被添加到 update_indices_ 中。这个数组通常用于增量更新的场景中，帮助追踪哪些区域发生了变化。

  • Eigen::AlignedBox2i known_cells_box_;
    这是一个 Eigen::AlignedBox2i 类型的对象，表示已知单元格的边界框（bounding box）。它用于高效地计算地图的裁剪范围，确保只处理已知区域，从而减少计算量。

• 主要成员函数

  • const MapLimits& limits() const { return limits_; }
    这个函数返回地图的限制（如尺寸和边界），即 limits_。

  • void FinishUpdate();
    这个函数标志着更新序列的完成，通常用于结束一次地图数据的更新过程。

  • float GetCorrespondenceCost(const Eigen::Array2i& cell_index) const;
    该方法返回给定单元格索引 cell_index 对应的成本值。cell_index 是一个二维数组，表示网格中的某个位置。

  • bool IsKnown(const Eigen::Array2i& cell_index) const;
    这个方法判断给定位置的单元格是否已知（即是否有有效数据）。通常，这用于检查该位置是否已被传感器观测到。

  • void ComputeCroppedLimits(Eigen::Array2i* const offset, CellLimits* const limits) const;
    这个方法用来计算一个包含所有已知单元格的子区域，并将该区域的偏移量和限制存储到 offset 和 limits 中。

  • void GrowLimits(const Eigen::Vector2f& point);
    这个方法用于根据给定的点 point 来扩展地图的限制。当新的数据点需要添加到地图中时，这个方法会更新地图的边界。

  • std::unique_ptr ComputeCroppedGrid() const = 0;
    这个纯虚函数返回一个裁剪后的 Grid2D 对象，通常用于创建一个新的只包含已知区域的网格。

  • proto::Grid2D ToProto() const;
    这个方法将 Grid2D 对象转换为一个协议缓冲区（proto::Grid2D），用于序列化和数据传输。

  • bool DrawToSubmapTexture(proto::SubmapQuery::Response::SubmapTexture* const texture, transform::Rigid3d local_pose) const = 0;
    这个纯虚函数将当前的 Grid2D 数据绘制到子地图的纹理中。它可能涉及图形渲染，并使用给定的姿态 local_pose 来确定纹理的位置。

class ProbabilityGrid : public Grid2D {}

ProbabilityGrid 是一个表示二维网格的概率分布的类，用于地图或图形处理

• 继承Grid2D。

class TSDF2D : public Grid2D {}

CollatorInterface

CollatorInterface 是一个抽象接口类，它定义了多轨迹数据处理和合并的基本框架。继承自 CollatorInterface 的类通常会实现这些方法，提供具体的逻辑来处理轨迹和传感器数据。

• AddTrajectory: 为轨迹 ID 设置回调函数，这样当数据添加到轨迹时，回调就会触发。
• FinishTrajectory: 当轨迹完成时，将其 ID 添加到已完成轨迹集合 finished_trajectories_。
• AddSensorData: 将传感器数据按顺序添加到指定轨迹中，并触发回调函数处理数据。
• Flush: 清空所有待处理的数据并调用相应的回调函数。
• GetBlockingTrajectoryId: 检查是否有轨迹尚未完成，若有则返回需要更多数据的轨迹 ID

class Collator : public CollatorInterface {...}
class TrajectoryCollator : public CollatorInterface {...}
// 等待看到所有传感器 ID 的至少一项数据，并按合并排序的顺序分发数据。
// 与 'Collator' 不同，它不等待其他轨迹的数据。
// 同样，与 'Collator' 的输出是确定性不同，数据分发的顺序不是排序的，因此非确定性的输入序列
// 将导致非确定性的输出。

OrderedMultiQueue

• OrderedMultiQueue 类用于维护多个已排序的传感器数据队列，并按合并排序的顺序（merge-sorted order）分发数据。它会确保在分发数据时，每个未完成的队列至少有一个数据项可用，才能继续合并并分发下一个排序的数据项。

简而言之，这个类通过合并多个已排序队列来生成一个按顺序排列的数据流，并保证每次分发的数据来自所有队列的最小项。

线程兼容性：此类设计为线程安全的，意味着它可以在多线程环境下安全地操作，而不会出现数据竞争问题。

• queues_：维护一个队列集合，键是 QueueKey，值是包含 Queue 对象的 map。Queue 包含了队列、回调和是否已完成的标志。
• last_dispatched_time_：记录上一个分发的时间，确保数据按时间顺序被处理。
• blocker_：指示哪个队列在当前时刻阻塞，需要更多的数据才能继续进行合并排序和分发。

LocalSlamResultCallback

//LocalSlamResultCallback 的目的是在局部SLAM过程中处理每次传感器数据的结果。该回调函数会返回处理后的信息，告知系统：
//    这笔数据是属于哪个轨迹的；
//    数据对应的时间戳；
//    局部位姿的估计（机器人或传感器在当前时刻的位置与姿态）；
//    在本地坐标系下的范围数据；
//    数据是否成功插入到子图中，若成功则返回插入结果
//数据插入到子图的情况：
//    如果这段传感器数据（sensor::RangeData）成功地被插入到一个子图（Submap）中，那么回调函数会报告该数据所对应的 NodeId。
//数据被过滤的情况：
//    如果传感器数据被过滤掉（比如，数据不符合要求或者不被插入到子图中），回调函数会返回 nullptr，表示没有插入数据。
  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>)>;

//原点：光线的起始位置，通常是激光雷达传感器的位置。
//返回点 (Returns)：光线遇到障碍物的位置，这些点用于生成环境的点云，帮助构建地图。
//未命中点 (Misses)：光线未检测到障碍物的地方，通常记录为最大距离，用于表示空旷区域或无障碍区域。
struct RangeData {
  Eigen::Vector3f origin;
  PointCloud returns;
  PointCloud misses;
};