(02)Cartographer源码无死角解析-(54) 2D后端优化→PoseGraphInterface、PoseGraph、PoseGraph2D::AddNode()

讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解:https://blog.csdn.net/weixin_43013761/article/details/127350885
 
文末正下方中心提供了本人 联系方式, 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式,点击本人照片即可显示WX官方认证
 

一、前言

通过上一篇博客,再次对 global_trajectory_builder.cc 文件中的 AddSensorData() 进行了分析,同时简要提及位姿图。在Cartographer 中,位姿图优化核心类为 src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.cc 文件中的 PoseGraph2D。其函数 PoseGraph2D::AddNode() 起到了前端与后端交互作用。

对于接下来的讲解,本人比较纠结,①先讲整体流程,再梳理细节;②按照源码执行过程,见招拆招,按顺序讲解;这两种方式,最终选择了后者。因为如果直接讲整体流程,没有基础的情况下是很难看懂的。所示,接下来按照源码执行过程,一一进行讲解,类似于本人分析代码的过程。等把细节都梳理清楚之后,再反过来做一个总结,这样应该理解就比较深刻了。这里复制一下 PoseGraph2D::AddNode() 被调用过程,即 GlobalTrajectoryBuilder::AddSensorData() 函数中代码段如下:

      // 将匹配后的结果 当做节点 加入到位姿图中
      auto node_id = pose_graph_->AddNode(
          matching_result->insertion_result->constant_data,  //子图相关的
          trajectory_id_, //轨迹id
          matching_result->insertion_result->insertion_submaps); //子图

该些变量都再前面进行分析过,其上 constant_data 为 const TrajectoryNode::Data 类型的智能指针:

  struct Data {
    common::Time time; //点云数据时间戳

    // Transform to approximately gravity align the tracking frame as
    // determined by local SLAM.
    Eigen::Quaterniond gravity_alignment; //重力校正旋转四元数

    // Used for loop closure in 2D: voxel filtered returns in the
    // 'gravity_alignment' frame.
    sensor::PointCloud filtered_gravity_aligned_point_cloud; //经过滤波之后的点云数据

    // Used for loop closure in 3D.用于3D回环检测,
    sensor::PointCloud high_resolution_point_cloud; //高分辨率点云数据
    sensor::PointCloud low_resolution_point_cloud; //低分辨率点云数据
    Eigen::VectorXf rotational_scan_matcher_histogram; //旋转扫描匹配直方图

    // The node pose in the local SLAM frame.
    transform::Rigid3d local_pose; //节点相对于local SLAM frame(可以理解为lcoal地图)的位姿,MatchingResult::local_pose是一致的
  };

在了解这些之后,下面就开始对PoseGrap2D继续进行一个详细分析了。首先其继承关系如下:

PoseGraph2D:PoseGraph:PoseGraphInterface

根据继承关系, 首先要理解PoseGraphInterface,其在 src/cartographer/cartographer/mapping/pose_graph_interface.h 定义。
 

二、PoseGraphInterface

首先其是定义了一个结构提体类型struct Constraint,代码注释之前,可以看下图,有助于后续的理解( 用于参考,并不一定完全正确 \color{red}用于参考,并不一定完全正确 用于参考,并不一定完全正确)
(02)Cartographer源码无死角解析-(54) 2D后端优化→PoseGraphInterface、PoseGraph、PoseGraph2D::AddNode()_第1张图片

图一

1、黑色坐标系: 表示Global全局坐标系,O表示原点
2、红色坐标系: 子图坐标系,O表示原点
3、紫色矩形: 子图
4、Submapx: 分别代表子图1、子图2、子图3
5、蓝色虚线:分别表示子图间约束、子图内约束

该图示中有些地方可能是带有疑问的,如Global全局坐标系是否在左上角?子图local坐标系是否在左上角?子图Submap1坐标系与Global全局坐标系是否相同?这些疑问暂时就不解答了,后续如果有时间,再来分析,到时候发现不对的地方再做纠正。

1、结构体Constrain

首先PoseGraphInterface中定义了结构体Constraint,其是用于描述一个约束,也就是图优化中的边,这里先贴一下代码注释:

  // 包含了子图的id, 节点的id, 节点j相对于子图i的坐标变换, 以及节点是在子图内还是子图外的标志
  struct Constraint {
    struct Pose {
      transform::Rigid3d zbar_ij;  
      double translation_weight;
      double rotation_weight;
    };

    SubmapId submap_id;  // 'i' in the paper.
    NodeId node_id;      // 'j' in the paper.

    // Pose of the node 'j' relative to submap 'i'.
    Pose pose;

    // Differentiates between intra-submap (where node 'j' was inserted into
    // submap 'i') and inter-submap constraints (where node 'j' was not inserted
    // into submap 'i').
    enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
  };

对应于论文中如下内容:
(02)Cartographer源码无死角解析-(54) 2D后端优化→PoseGraphInterface、PoseGraph、PoseGraph2D::AddNode()_第2张图片
在上一篇文中提到,简单的来说,可以直接把一个位姿变换看作一个约束。其上代码中定义的Constraint就是描述一个位姿变换: Constraint::submap_id对应的子图到Constraint::node_id对应的节点位姿变换, submap_id 等价于论文中 c i c_i ci,node_id等价于论文中 c j c_j cj,如果觉得这里所谓的节点不好理解,直接看成机器人位姿即可。总的来说 Constraint:pose表示节点 j 相对于子图 i 的位姿。

另外,其定义的一个枚举类型Tag,如图1所示,因为机器人位于Submap7中,其蓝色虚线INTRA_SUBMA表示 Submap7(原点)到机器人的位姿变换,等价于机器人在Submap7中的位姿,即机器人与在所属子图的位姿(等价于该子图到机器人位姿变换),称为INTER_SUBMAP 子图内约束。

除此之外,如图1还可以看到Submap2原点与机器人连接的蓝色虚线,其表示Submap2系到机器人系的位姿变换,可机器人并不处于子图Submap2中,所以该属于INTRA_SUBMAP子图间约束。

2、结构体LandmarkNode

除结构体结构体类型 struct Constrain,pose_graph_interface.h中还定义了结构体类型 struct LandmarkNode,其主要用来描述一个Landmark节点,首先机器人在某个时刻位姿下,可能同时观测到多个Landmark,这些被观测到的Landmark数据存储于成员变量 LandmarkNode::landmark_observations 之中,LandmarkNode::global_landmark_pose 表示这帧 Landmark 数据对应的 tracking_frame 在 globa l坐标系下的位姿,如下所示:

  struct LandmarkNode {
    // landmark数据是相对于tracking_frame的相对坐标变换
    struct LandmarkObservation {
      int trajectory_id;
      common::Time time;
      transform::Rigid3d landmark_to_tracking_transform;
      double translation_weight;
      double rotation_weight;
    };
    // 同一时刻可能会观测到多个landmark数据
    std::vector<LandmarkObservation> landmark_observations;
    // 这帧数据对应的tracking_frame在global坐标系下的位姿
    absl::optional<transform::Rigid3d> global_landmark_pose;
    bool frozen = false;
  };

可以看到单个 landmark 数据是使用 LandmarkNode::LandmarkObservation 类型结构体描述的,首先包含了trajectory_id、time。以及 landmark_to_tracking_transform,其表示单个landmark相对于tracking_frame(机器人)的位姿,另外包含了该位姿旋转与平移的权重 translation_weight、rotation_weight。最后关于参数LandmarkNode::frozen,其含义是冻结,这里猜测其是对 LandmarkNode 进行冻结,即表示其内的成员数据不会被改变了。

3、SubmapPose、SubmapData

  struct SubmapPose {
    int version;
    transform::Rigid3d pose;
  };

  struct SubmapData {
    std::shared_ptr<const Submap> submap;
    transform::Rigid3d pose;
  };

这两个结构体类型比较简单,分别用来表述子图的位姿与子图的数据,SubmapData 中实际上存储一个指向于 Submap 类型指针,可以注意到成员变量 SubmapData::pose,目前估计其也是子图的位姿→相对于全局坐标系。

4、TrajectoryData、TrajectoryState

  // tag: TrajectoryData
  struct TrajectoryData {
    double gravity_constant = 9.8; //重力常数
    std::array<double, 4> imu_calibration{{1., 0., 0., 0.}}; //初始默认值,单位四元数
    absl::optional<transform::Rigid3d> fixed_frame_origin_in_map; //GPS帧原点相对于map的位姿。
  };

  enum class TrajectoryState { ACTIVE, FINISHED, FROZEN, DELETED };

TrajectoryData 是用来秒速轨迹数据的、这里看起来不知道给来干嘛的,后续使用到了再做具体分析。枚举类型 TrajectoryState 定义了轨迹4种状态,分别为激活(ACTIVE)、完成(FINISHED)、冻结(FINISHED)、删除(DELETED)。

5、GlobalSlamOptimizationCallback

最后还定义了一个回调函数类型,该类型函数无返回值、输入类型分别为 std::map、 std::map 。该类函数的功能是做全局优化。

  using GlobalSlamOptimizationCallback =
      std::function<void(const std::map<int /* trajectory_id */, SubmapId>&,
                         const std::map<int /* trajectory_id */, NodeId>&)>;

下面都是一些虚函数了,后续对PoseGraph2D讲解时,再对其进行具体分析。
 

三、PoseGraph

前面以及提到PoseGraph由PoseGraphInterface派生而来,其声明于src/cartographer/cartographer/mapping/pose_graph.h 文件中,其中定义了结构体类型 struct InitialTrajectoryPose,用来秒速一个轨迹的初始位姿,不过值得注意的是,该初始位姿是一个先对值,相对于 to_trajectory_id 的位置,后续使用到了,再分析一遍,代码如下:

  // 相对于to_trajectory_id的初始位姿
  struct InitialTrajectoryPose {
    int to_trajectory_id;
    transform::Rigid3d relative_pose;
    common::Time time;
  };

 

四、PoseGraph2D::AddNode

那先现在就是关于PoseGraph2D部分了,其继承至PoseGraph。类声明于 src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.h 文件中,其主要声明一些重写的虚函数,后续一个一个的为大家讲解。该篇不可只对其中的PoseGraph2D::AddNode() 函数进行分析。上一篇博客中提到,该函数中在 src/cartographer/cartographer/mapping/internal/global_trajectory_builder.cc 文件中有被调用。PoseGraph2D::AddNode() 函数总体流程可以说是十分的简单、但是如果深入了解其调用函数,可以说是复杂至极。但是关于 PoseGraph2D::AddNode() 函数的逻辑还是十分简单的。其主要流程如下:

( 01 ) \color{blue}(01) (01) 首先其需要传递三个参数:①参数constant_data就是上面的struct Data类型,主要是关于插入子图中相关的固化信息;②trajectory_id表示轨迹id;③insertion_submaps去通常包含两个活跃子图。

( 02 ) \color{blue}(02) (02)首先调用GetLocalToGlobalTransform() 函数传入 trajectory_id 获得由local系到global系的坐标变换矩阵,然后左乘constant_data->local_pose,其目的是把local坐标系下的节点坐标(把扫描匹配出来的位姿看成一个节点)变换到global系下。

( 03 ) \color{blue}(03) (03) 把变换到global系下的节点(位姿)通过PoseGraph2D::AppendNode()函数加入到节点列表 PoseGraph2D::data_trajectory_nodes 之中,该成员变量的定义为:MapById trajectory_nodes;加入之后会获得一个NodeId node_id,其为整个函数的返回值。

( 04 ) \color{blue}(04) (04) 把计算约束的工作放入workitem中等待执行,需要注意的是,这里并还没有执行计算节点约束函数 ComputeConstraintsForNode(),仅仅是把该函数作为一个任务项添加至线程池→关于线程部分后续再进行详细分析。

/**
 * @brief 增加节点, 并计算跟这个节点相关的约束
 * 
 * @param[in] constant_data 节点信息
 * @param[in] trajectory_id 轨迹id
 * @param[in] insertion_submaps 子地图 active_submaps
 * @return NodeId 返回节点的ID
 */
NodeId PoseGraph2D::AddNode(
    std::shared_ptr<const TrajectoryNode::Data> constant_data,
    const int trajectory_id,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
  
  // 将节点在local坐标系下的坐标转成global坐标系下的坐标
  const transform::Rigid3d optimized_pose(
      GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);

  // 向节点列表加入节点,并得到节点的id
  const NodeId node_id = AppendNode(constant_data, trajectory_id,
                                    insertion_submaps, optimized_pose);

  // We have to check this here, because it might have changed by the time we
  // execute the lambda.
  // 获取第一个submap是否是完成状态
  const bool newly_finished_submap =
      insertion_submaps.front()->insertion_finished();

  // 把计算约束的工作放入workitem中等待执行
  AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    return ComputeConstraintsForNode(node_id, insertion_submaps,
                                     newly_finished_submap);
  });

  return node_id;
}

 

五、结语

到目前为止,对PoseGraph2D继承关系,以及其中包含的结构体定义与成员变量进行了简单讲解。那么下面就是对细节的分析。后续主要围绕 PoseGraph2D::AddNode() 函数进行展开。接下里要讲解的,首先就是其调用代码:

 // local系下的位姿变换到global系下
  const transform::Rigid3d optimized_pose(
      GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);

如何把local系下的位姿变换到global系下,其上的 constant_data->local_pose 就表示local系下的位姿,可以理解为机器人Robot位姿。

 
 
 

你可能感兴趣的:(#,机器人,自动驾驶,Cartographer,增强现实,slam)