(02)Cartographer源码无死角解析-(78) ROS数据发布→子图内、子图间、约束与残差发布

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

一、前言

通过前面一系列博客的分析，到目前为止，node.cc 文件中有关于数据发布的函数，只有 Node::PublishConstraintList() 函数没有讲解了。

// 每0.5s发布一次约束数据
void Node::PublishConstraintList(
    const ::ros::WallTimerEvent& unused_timer_event) {
  if (constraint_list_publisher_.getNumSubscribers() > 0) {
    absl::MutexLock lock(&mutex_);
    constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList());
  }
}

这里就不用多说了，其核心函数就是 MapBuilderBridge::GetConstraintList()，该函数返回的又是一个人 visualization_msgs::MarkerArray() 类型的数据。就开始进入主题吧。

二、多种marker声明

源码中首先创建了一个 visualization_msgs::MarkerArray 对象 constraint_list，且让 marker_id 从零开始，接着声名了六种 marker。

$第一种:$ 为子图内约束 constraint_intra_marker，非全局约束, rviz中显示的最多的约束。marker_id = 1，命名空间为 “Intra constraints”。constraint_intra_marker.header.frame_id = node_options_.map_frame 可知其是基于gloabal 系的。kConstraintMarkerScale 是设置线段缩放大小，且位姿设置为单位旋转。注意 constraint_intra_marker.type = visualization_msgs::Marker::LINE_LIST 这个设置，其表示可以存储多条线段，每条线段进行连接。

$第二种:$ 源码中的 residual_intra_marker，其在 constraint_intra_marker 的基础上进行修改， marker_id=2，命名空间为 “Intra residuals”，该 marker 先对于的其他的数量比较少，为了其容易被观察到，将该标记和其他数量较少的标记设置z为略高于帧内约束标记, 对应于源码中的 residual_intra_marker.pose.position.z = 0.1，主要体现的是一个残差关系，后续进行分析。

$第三种:$ Inter constraints, 同1轨迹的外部约束 ，rviz 显示的第二多的约束，命名空间为 “Inter constraints, same trajectory”，同样 pose.position.z = 0.1。

$第四种:$ 基于第一种，marker_id=4，命名空间为 “Inter residuals, same trajectory”，也是用来显示残差的。

$第五种:$ 基于第一种，marker_id=5，命名空间为 “Inter constraints, different trajectories” 用来描述不同轨迹间的残差。

$第六种:$ 基于第一种，marker_id=5，命名空间为 “Inter constraints, different trajectories” 用来描述不同轨迹间的子图内约束。这六种可以归为3类，

1.第一种与第二种表示不区分轨迹的子图间约束及残差
2.第三种与第四种表示相同轨迹的子图间约束及残差
3.第四种与第五种表示不同轨迹的子图间约束及残差

相关代码注释如下：

/**
 * @brief 获取位姿图中所有的约束,分成6种类型,放入不同类型的marker中
 * 
 * @return visualization_msgs::MarkerArray 返回6种marker的集合
 */
visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
  visualization_msgs::MarkerArray constraint_list;
  int marker_id = 0;

  // 6种marker的声明

  // 1 内部子图约束, 非全局约束, rviz中显示的最多的约束
  visualization_msgs::Marker constraint_intra_marker;
  constraint_intra_marker.id = marker_id++;
  constraint_intra_marker.ns = "Intra constraints";
  // note: Marker::LINE_LIST: 每对点之间画一条线, eg: 0-1, 2-3, 4-5
  constraint_intra_marker.type = visualization_msgs::Marker::LINE_LIST;
  constraint_intra_marker.header.stamp = ros::Time::now();
  constraint_intra_marker.header.frame_id = node_options_.map_frame;
  constraint_intra_marker.scale.x = kConstraintMarkerScale;
  constraint_intra_marker.pose.orientation.w = 1.0;

  // 2 Intra residuals
  visualization_msgs::Marker residual_intra_marker = constraint_intra_marker;
  residual_intra_marker.id = marker_id++;
  residual_intra_marker.ns = "Intra residuals";
  // This and other markers which are less numerous are set to be slightly
  // above the intra constraints marker in order to ensure that they are
  // visible.
  // 将该标记和其他数量较少的标记设置z为略高于帧内约束标记, 以确保它们可见.
  residual_intra_marker.pose.position.z = 0.1;

  // 3 Inter constraints, same trajectory, rviz中显示的第二多的约束
  // 外部子图约束, 回环约束, 全局约束
  visualization_msgs::Marker constraint_inter_same_trajectory_marker =
      constraint_intra_marker;
  constraint_inter_same_trajectory_marker.id = marker_id++;
  constraint_inter_same_trajectory_marker.ns =
      "Inter constraints, same trajectory";
  constraint_inter_same_trajectory_marker.pose.position.z = 0.1;

  // 4 Inter residuals, same trajectory
  visualization_msgs::Marker residual_inter_same_trajectory_marker =
      constraint_intra_marker;
  residual_inter_same_trajectory_marker.id = marker_id++;
  residual_inter_same_trajectory_marker.ns = "Inter residuals, same trajectory";
  residual_inter_same_trajectory_marker.pose.position.z = 0.1;

  // 5 Inter constraints, different trajectories
  visualization_msgs::Marker constraint_inter_diff_trajectory_marker =
      constraint_intra_marker;
  constraint_inter_diff_trajectory_marker.id = marker_id++;
  constraint_inter_diff_trajectory_marker.ns =
      "Inter constraints, different trajectories";
  constraint_inter_diff_trajectory_marker.pose.position.z = 0.1;

  // 6 Inter residuals, different trajectories
  visualization_msgs::Marker residual_inter_diff_trajectory_marker =
      constraint_intra_marker;
  residual_inter_diff_trajectory_marker.id = marker_id++;
  residual_inter_diff_trajectory_marker.ns =
      "Inter residuals, different trajectories";
  residual_inter_diff_trajectory_marker.pose.position.z = 0.1;

三、后端数据获取

	const auto trajectory_node_poses =map_builder_->pose_graph()->GetTrajectoryNodePoses();
	const auto submap_poses = map_builder_->pose_graph()->GetAllSubmapPoses();
	const auto constraints = map_builder_->pose_graph()->constraints();
	
	 // 将约束信息填充到6种marker里
	for (const auto& constraint : constraints) {
		visualization_msgs::Marker *constraint_marker, *residual_marker;
		std_msgs::ColorRGBA color_constraint, color_residual;
		......
	}

在定义好6种 marker 之后，其首先获得基于 global 系下轨迹节点位姿、子图位姿。以及约束。随后进入到一个for循环，该循环主要就是把约束残差的数据添加到 6种 marker 之中。每次遍历之前，都会先创建两个 visualization_msgs::Marker 类型的指针，一个用于存储约束，一个用于存储残差。以及两个 std_msgs::ColorRGBA 实例，分别用于描述 *constraint_marker, *residual_marker 的颜色信息。

四、颜色透明度设置

$(1):$ 循环遍历每一个越苏，先判断约束的类型，如果为子图内约束，也就是条件 onstraint.tag ==cartographer::mapping::PoseGraphInterface::Constraint::INTRA_SUBMAP 成立，
首先把 constraint_marker、residual_marker 赋值成第1类 marker，接着设置颜色与透明图，color_residual.a = 1.0 与 color_residual.r = 1.0 表示不透明，红色。

$(2):$ 如果为子图间约束，且子图与节点轨迹相同，则设置为 Bright yellow 亮黄色，对应前面的第2类。

$(3):$ 如果为子图间约束，且子图与节点轨迹不相同，则设置为 Bright cyan 亮青色，对应前面的第3类。

$(4):$ 设置颜色信息，使用for循环添加了两次，因为一条线段有两个点。

源码注释如下：

    // 根据不同情况,将constraint_marker与residual_marker 指到到不同的maker类型上

    // 子图内部的constraint,对应第一种与第二种marker
    if (constraint.tag ==
        cartographer::mapping::PoseGraphInterface::Constraint::INTRA_SUBMAP) {
      constraint_marker = &constraint_intra_marker;
      residual_marker = &residual_intra_marker;
      // Color mapping for submaps of various trajectories - add trajectory id
      // to ensure different starting colors. Also add a fixed offset of 25
      // to avoid having identical colors as trajectories.
      // 各种轨迹的子图的颜色映射-添加轨迹ID以确保不同的起始颜色 还要添加25的固定偏移量, 以避免与轨迹具有相同的颜色. 
      color_constraint = ToMessage(
          cartographer::io::GetColor(constraint.submap_id.submap_index +
                                     constraint.submap_id.trajectory_id + 25));
      color_residual.a = 1.0;
      color_residual.r = 1.0;
    } 
    else {
      // 相同轨迹内,子图外部约束, 对应第三种与第四种marker
      if (constraint.node_id.trajectory_id ==
          constraint.submap_id.trajectory_id) {
        constraint_marker = &constraint_inter_same_trajectory_marker;
        residual_marker = &residual_inter_same_trajectory_marker;
        // Bright yellow 亮黄色
        color_constraint.a = 1.0;
        color_constraint.r = color_constraint.g = 1.0;
      } 
      // 不同轨迹间的constraint,对应第五种与第六种marker
      else {
        constraint_marker = &constraint_inter_diff_trajectory_marker;
        residual_marker = &residual_inter_diff_trajectory_marker;
        // Bright orange
        color_constraint.a = 1.0;
        color_constraint.r = 1.0;
        color_constraint.g = 165. / 255.;
      }
      // Bright cyan 亮青色
      color_residual.a = 1.0;
      color_residual.b = color_residual.g = 1.0;
    }

    // 设置颜色信息
    for (int i = 0; i < 2; ++i) {
      constraint_marker->colors.push_back(color_constraint);
      residual_marker->colors.push_back(color_residual);
    }

五、构建marker

无论那种情况，都会对应一个 constraint_marker 以及 一个 residual_marker。现在颜色信息已经设置好了，下面就是设置线段的起始点与结束点了。

$(1):$ 先获得约束对应的子图迭代器 submap_it，然后获得子图的 global 位姿，submap_pose。

$(2):$ 获得约束对应节点的迭代器 node_it，再获得该节点 global 系下的位姿 trajectory_node_pose。

$(3):$ 根据子图的global位姿，结合约束(分支定界扫描匹配得到节点相对子图的位姿)，求得越苏的另一头坐标，constraint_pose。

$(4):$ 将global系下子图原点(约束起点)，以及约束的结束点连接起来，把这两个点的global系下的位姿添加到 constraint_marker 之中。

$(5):$ 将global系下子图原点(约束起点)，以及约束的结束点连接起来，把这两个点的global系下位置添加到 constraint_marker->points 之中。

$(6):$ constraint_pose.translation() 表示节点相对于子图的位置，未进行后端优化，但是却显示再global系。trajectory_node_pose 表示经过后端优化时候的节点位姿。把两者的位置都添加到 residual_marker->points 之中。两种方式计算出的节点坐标不会完全相同, 将这个差值作为残差发布出来。

$(7):$ 最后就是把六种构建好的 marker 全部添加到 constraint_list 之中，返回进行话题发不。

六、结语

通过前面的讲解，关于 Node::PublishConstraintList() 函数已经讲解完成了。关于约束的再 rviz 的查看这里就不说了，但是提及以及残差的查，如本人设置如下在在rviz的设置如下：

上面青色与红色的线段就是约束残差的可视化。通过连续几篇的博客对源码的分析，对于ROS话题的发布基本都比较清楚了，但是缺少一个很重要的东西，那就是地图，地图并不是以话题的形式发布的，而是通过服务的方式，具体过程下篇博客继续为大家分析。