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

news/2024/12/13 17:48:35/

讲解关于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官方认证
 

一、前言

通过前面一系列博客的分析,到目前为止,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。

第一种 : \color{blue}第一种: 第一种: 为子图内约束 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 这个设置,其表示可以存储多条线段,每条线段进行连接。

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

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

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

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

第六种 : \color{blue}第六种: 第六种: 基于第一种,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-5constraint_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 residualsvisualization_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 trajectoryvisualization_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 trajectoriesvisualization_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 trajectoriesvisualization_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 ) : \color{blue}(1): (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 ) : \color{blue}(2): (2): 如果为子图间约束,且子图与节点轨迹相同,则设置为 Bright yellow 亮黄色,对应前面的第2类。

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

( 4 ) : \color{blue}(4): (4): 设置颜色信息,使用for循环添加了两次,因为一条线段有两个点。

源码注释如下:

    // 根据不同情况,将constraint_marker与residual_marker 指到到不同的maker类型上// 子图内部的constraint,对应第一种与第二种markerif (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 {// 相同轨迹内,子图外部约束, 对应第三种与第四种markerif (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,对应第五种与第六种markerelse {constraint_marker = &constraint_inter_diff_trajectory_marker;residual_marker = &residual_inter_diff_trajectory_marker;// Bright orangecolor_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 ) : \color{blue}(1): (1): 先获得约束对应的子图迭代器 submap_it,然后获得子图的 global 位姿,submap_pose。

( 2 ) : \color{blue}(2): (2): 获得约束对应节点的迭代器 node_it,再获得该节点 global 系下的位姿 trajectory_node_pose。

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

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

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

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

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

六、结语

通过前面的讲解,关于 Node::PublishConstraintList() 函数已经讲解完成了。关于约束的再 rviz 的查看这里就不说了,但是提及以及残差的查,如本人设置如下在在rviz的设置如下:
在这里插入图片描述
上面青色与红色的线段就是约束残差的可视化。通过连续几篇的博客对源码的分析,对于ROS话题的发布基本都比较清楚了,但是缺少一个很重要的东西,那就是地图,地图并不是以话题的形式发布的,而是通过服务的方式,具体过程下篇博客继续为大家分析。


http://www.ppmy.cn/news/802996.html

相关文章

MySQL:数据库的基本操作及数据库的三大数据类型

目录 一、创建数据库 二、删除数据库 三、查询数据 四、在数据库使用Linux命令 五、数据库的编码方式以及校验规则 1、查看MySQL支持的所有编码方式 2、查看某一种编码方式支持的所有校验规则 3、查看所有的编码方式和校验规则 4、查看本地数据库使用的编码方式和校验规…

Win7正传

人们都说7年之痒&#xff0c;貌似7不是个很吉利的数字&#xff0c;但我是个例外。 中国大陆的朋友&#xff0c;都叫我Win7&#xff0c;逢7就赢&#xff0c;好兆头也。 虽然 今日才是我继承大统的日子&#xff0c;但是江湖从来不缺少我的流言。微软的龙头棒我拿着&#xff0c;…

国产操作系统UOS一周年:桌面生态已具备Win7替代能力,曾被质疑“套壳”

本文转载自 AI前线 2020 年 1 月 15 日&#xff0c;统信 UOS 正式版本发布&#xff1b;到今天&#xff0c;UOS 已经走过了一周年。 统信 UOS 一周年 在最近的一次会议上&#xff0c;统信软件总经理刘闻欢表示&#xff1a;2020 年以来&#xff0c;统信软件建成了完整高效的生…

是否愿意使用WIN7?

WIN7就要到来。按新闻的报道&#xff0c;WIN7于10月22日上市&#xff0c;目前在压盘。微软官网给出的硬件要求配置为&#xff1a;h_ttp://windows.microsoft.com/zh-CN/windows7/products/windows-7/system-requirements如果要在电脑上运行 Windows 7&#xff0c;需要具备以下条…

倪光南炮轰Win10政府版没过审查 微软合作方回应

今年5月23日&#xff0c;微软在新品发布会上宣布&#xff0c;将发布专门针对中国定制的Windows 10政府版。微软介绍&#xff0c;Windows 10政府版由微软和神州网信合作开发&#xff0c;该系统满足“政府数据不出境、留在中国”的要求。 据悉&#xff0c;Windows 10政府版实现了…

[分享] 《步步为营封 Win7》--skyfree

[分享] 《步步为营封 Win7》--skyfree Skyfree 发表于 2009-9-13 05:51:32 https://www.itsk.com/thread-20957-1-25.html [分享] [0]《步步为营封 Win7》引言 《步步为营封 Win7》引言 WIN7 RTM发布已经有好几天了&#xff0c;其实我从最早VISTA时就研究过WIN6.X这一代系…

Win7屏幕键盘 在哪 使用

看微博玩Win7 学用Win7屏幕键盘 2012-03-20 10:07  来源&#xff1a;天极网软件频道 作者&#xff1a;Shiny 责任编辑&#xff1a;杨玲 yesky  评论(1) 小勇最近买了一台预装Win7系统的电脑&#xff0c;除了经常在IT网站学习一些Win7系统的教程和小技巧之外&#xff0c;他还…

Windows7的自述

人们都说7年之痒&#xff0c;貌似7不是个很吉利的数字&#xff0c;但我是个例外。 中国大陆的朋友&#xff0c;都叫我Win7&#xff0c;逢7就赢&#xff0c;好兆头也。 虽然10月23日才是我继承大统的日子&#xff0c;但是江湖从来不缺少我的流言。微软的龙头棒我拿着&#xff…