講解關(guān)于slam一系列文章匯總鏈接:史上最全slam從零開(kāi)始，針對(duì)于本欄目講解(02)Cartographer源碼無(wú)死角解析-鏈接如下:
(02)Cartographer源碼無(wú)死角解析- (00)目錄_最新無(wú)死角講解：https://blog.csdn.net/weixin_43013761/article/details/127350885
?
$文末正下方中心提供了本人聯(lián)系方式，點(diǎn)擊本人照片即可顯示WX\to 官方認(rèn)證$
?

一、前言

通過(guò)前面一系列博客的分析，到目前為止，node.cc 文件中有關(guān)于數(shù)據(jù)發(fā)布的函數(shù)，只有 Node::PublishConstraintList() 函數(shù)沒(méi)有講解了。

// 每0.5s發(fā)布一次約束數(shù)據(jù)
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());
  }
}

這里就不用多說(shuō)了，其核心函數(shù)就是 MapBuilderBridge::GetConstraintList()，該函數(shù)返回的又是一個(gè)人 visualization_msgs::MarkerArray() 類(lèi)型的數(shù)據(jù)。就開(kāi)始進(jìn)入主題吧。

二、多種marker聲明

源碼中首先創(chuàng)建了一個(gè) visualization_msgs::MarkerArray 對(duì)象 constraint_list，且讓 marker_id 從零開(kāi)始，接著聲名了六種 marker。

$第一種:$ 為子圖內(nèi)約束 constraint_intra_marker，非全局約束, rviz中顯示的最多的約束。marker_id = 1，命名空間為 “Intra constraints”。constraint_intra_marker.header.frame_id = node_options_.map_frame 可知其是基于gloabal 系的。kConstraintMarkerScale 是設(shè)置線(xiàn)段縮放大小，且位姿設(shè)置為單位旋轉(zhuǎn)。注意 constraint_intra_marker.type = visualization_msgs::Marker::LINE_LIST 這個(gè)設(shè)置，其表示可以存儲(chǔ)多條線(xiàn)段，每條線(xiàn)段進(jìn)行連接。

$第二種:$ 源碼中的 residual_intra_marker，其在 constraint_intra_marker 的基礎(chǔ)上進(jìn)行修改， marker_id=2，命名空間為 “Intra residuals”，該 marker 先對(duì)于的其他的數(shù)量比較少，為了其容易被觀(guān)察到，將該標(biāo)記和其他數(shù)量較少的標(biāo)記設(shè)置z為略高于幀內(nèi)約束標(biāo)記, 對(duì)應(yīng)于源碼中的 residual_intra_marker.pose.position.z = 0.1，主要體現(xiàn)的是一個(gè)殘差關(guān)系，后續(xù)進(jìn)行分析。

$第三種:$ Inter constraints, 同1軌跡的外部約束 ，rviz 顯示的第二多的約束，命名空間為 “Inter constraints, same trajectory”，同樣 pose.position.z = 0.1。

$第四種:$ 基于第一種，marker_id=4，命名空間為 “Inter residuals, same trajectory”，也是用來(lái)顯示殘差的。

$第五種:$ 基于第一種，marker_id=5，命名空間為 “Inter constraints, different trajectories” 用來(lái)描述不同軌跡間的殘差。

$第六種:$ 基于第一種，marker_id=5，命名空間為 “Inter constraints, different trajectories” 用來(lái)描述不同軌跡間的子圖內(nèi)約束。這六種可以歸為3類(lèi)，

1.第一種與第二種表示不區(qū)分軌跡的子圖間約束及殘差
2.第三種與第四種表示相同軌跡的子圖間約束及殘差
3.第四種與第五種表示不同軌跡的子圖間約束及殘差

相關(guān)代碼注釋如下：

/**
 * @brief 獲取位姿圖中所有的約束,分成6種類(lèi)型,放入不同類(lèi)型的marker中
 * 
 * @return visualization_msgs::MarkerArray 返回6種marker的集合
 */
visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
  visualization_msgs::MarkerArray constraint_list;
  int marker_id = 0;

  // 6種marker的聲明

  // 1 內(nèi)部子圖約束, 非全局約束, rviz中顯示的最多的約束
  visualization_msgs::Marker constraint_intra_marker;
  constraint_intra_marker.id = marker_id++;
  constraint_intra_marker.ns = "Intra constraints";
  // note: Marker::LINE_LIST: 每對(duì)點(diǎn)之間畫(huà)一條線(xiàn), 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.
  // 將該標(biāo)記和其他數(shù)量較少的標(biāo)記設(shè)置z為略高于幀內(nèi)約束標(biāo)記, 以確保它們可見(jiàn).
  residual_intra_marker.pose.position.z = 0.1;

  // 3 Inter constraints, same trajectory, rviz中顯示的第二多的約束
  // 外部子圖約束, 回環(huán)約束, 全局約束
  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;

三、后端數(shù)據(jù)獲取

	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 系下軌跡節(jié)點(diǎn)位姿、子圖位姿。以及約束。隨后進(jìn)入到一個(gè)for循環(huán)，該循環(huán)主要就是把約束殘差的數(shù)據(jù)添加到 6種 marker 之中。每次遍歷之前，都會(huì)先創(chuàng)建兩個(gè) visualization_msgs::Marker 類(lèi)型的指針，一個(gè)用于存儲(chǔ)約束，一個(gè)用于存儲(chǔ)殘差。以及兩個(gè) std_msgs::ColorRGBA 實(shí)例，分別用于描述 *constraint_marker, *residual_marker 的顏色信息。

四、顏色透明度設(shè)置

$(1):$ 循環(huán)遍歷每一個(gè)越蘇，先判斷約束的類(lèi)型，如果為子圖內(nèi)約束，也就是條件 onstraint.tag ==cartographer::mapping::PoseGraphInterface::Constraint::INTRA_SUBMAP 成立，
首先把 constraint_marker、residual_marker 賦值成第1類(lèi) marker，接著設(shè)置顏色與透明圖，color_residual.a = 1.0 與 color_residual.r = 1.0 表示不透明，紅色。

$(2):$ 如果為子圖間約束，且子圖與節(jié)點(diǎn)軌跡相同，則設(shè)置為 Bright yellow 亮黃色，對(duì)應(yīng)前面的第2類(lèi)。

$(3):$ 如果為子圖間約束，且子圖與節(jié)點(diǎn)軌跡不相同，則設(shè)置為 Bright cyan 亮青色，對(duì)應(yīng)前面的第3類(lèi)。

$(4):$ 設(shè)置顏色信息，使用for循環(huán)添加了兩次，因?yàn)橐粭l線(xiàn)段有兩個(gè)點(diǎn)。

源碼注釋如下：

    // 根據(jù)不同情況,將constraint_marker與residual_marker 指到到不同的maker類(lèi)型上

    // 子圖內(nèi)部的constraint,對(duì)應(yīng)第一種與第二種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 {
      // 相同軌跡內(nèi),子圖外部約束, 對(duì)應(yīng)第三種與第四種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,對(duì)應(yīng)第五種與第六種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;
    }

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

五、構(gòu)建marker

無(wú)論那種情況，都會(huì)對(duì)應(yīng)一個(gè) constraint_marker 以及 一個(gè) residual_marker?，F(xiàn)在顏色信息已經(jīng)設(shè)置好了，下面就是設(shè)置線(xiàn)段的起始點(diǎn)與結(jié)束點(diǎn)了。

$(1):$ 先獲得約束對(duì)應(yīng)的子圖迭代器 submap_it，然后獲得子圖的 global 位姿，submap_pose。

$(2):$ 獲得約束對(duì)應(yīng)節(jié)點(diǎn)的迭代器 node_it，再獲得該節(jié)點(diǎn) global 系下的位姿 trajectory_node_pose。

$(3):$ 根據(jù)子圖的global位姿，結(jié)合約束(分支定界掃描匹配得到節(jié)點(diǎn)相對(duì)子圖的位姿)，求得越蘇的另一頭坐標(biāo)，constraint_pose。

$(4):$ 將global系下子圖原點(diǎn)(約束起點(diǎn))，以及約束的結(jié)束點(diǎn)連接起來(lái)，把這兩個(gè)點(diǎn)的global系下的位姿添加到 constraint_marker 之中。

$(5):$ 將global系下子圖原點(diǎn)(約束起點(diǎn))，以及約束的結(jié)束點(diǎn)連接起來(lái)，把這兩個(gè)點(diǎn)的global系下位置添加到 constraint_marker->points 之中。

$(6):$ constraint_pose.translation() 表示節(jié)點(diǎn)相對(duì)于子圖的位置，未進(jìn)行后端優(yōu)化，但是卻顯示再global系。trajectory_node_pose 表示經(jīng)過(guò)后端優(yōu)化時(shí)候的節(jié)點(diǎn)位姿。把兩者的位置都添加到 residual_marker->points 之中。兩種方式計(jì)算出的節(jié)點(diǎn)坐標(biāo)不會(huì)完全相同, 將這個(gè)差值作為殘差發(fā)布出來(lái)。

$(7):$ 最后就是把六種構(gòu)建好的 marker 全部添加到 constraint_list 之中，返回進(jìn)行話(huà)題發(fā)不。

六、結(jié)語(yǔ)

通過(guò)前面的講解，關(guān)于 Node::PublishConstraintList() 函數(shù)已經(jīng)講解完成了。關(guān)于約束的再 rviz 的查看這里就不說(shuō)了，但是提及以及殘差的查，如本人設(shè)置如下在在rviz的設(shè)置如下：

上面青色與紅色的線(xiàn)段就是約束殘差的可視化。通過(guò)連續(xù)幾篇的博客對(duì)源碼的分析，對(duì)于ROS話(huà)題的發(fā)布基本都比較清楚了，但是缺少一個(gè)很重要的東西，那就是地圖，地圖并不是以話(huà)題的形式發(fā)布的，而是通過(guò)服務(wù)的方式，具體過(guò)程下篇博客繼續(xù)為大家分析。文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-530288.html

到了這里，關(guān)于(02)Cartographer源碼無(wú)死角解析-(78) ROS數(shù)據(jù)發(fā)布→2D點(diǎn)云數(shù)據(jù)、tf、機(jī)器人tracking frame軌跡發(fā)布的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！