(02)Cartographer源码无死角解析-(78) ROS数据发布→2D点云数据、tf、机器人tracking frame轨迹发布

news/2024/2/28 10:26:19

讲解关于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/761327.html

相关文章

Unity 血量显示 子弹显示 时间显示 敌人数显示

UI对象 效果 血量显示 时间显示 子弹显示 敌人数显示 核心代码 位于UIManager.cs脚本 //更新血条public void UpdateHealthBar(int curAmount,int maxAmount){healthBar.fillAmount (float)curAmount / (float)maxAmount;}//更新子弹public void UpdateBulletCount(int cu…

unity显示当前帧数的脚本

关于帧数的计算和显示&#xff0c;其实是比较简单的&#xff1b; 但是想写一个简洁&#xff0c;并且高内聚&#xff08;每个游戏拉过来就能用&#xff09;还是要花点心思的&#xff1b; 我这里用了一个协程实现循环计算&#xff0c;省去了update里边的代码&#xff1b;用OnGU…

手机切换ip地址会遇的问题以及解决方法汇总

手机切换IP地址是一种常见的操作&#xff0c;它可以帮助用户解决一些网络问题&#xff0c;提高网络连接速度&#xff0c;或者绕过某些限制。然而&#xff0c;在进行IP地址切换时&#xff0c;用户可能会遇到一些问题。本文将详细介绍手机切换IP地址可能遇到的问题&#xff0c;并…

Linux下ethtool指令详解

ethtool是一个强大的网络工具&#xff0c;用于配置和查询以太网接口的状态和参数。本文中将详细解释ethtool指令的前世今生、功能和用法&#xff0c;并提供示例和参考链接&#xff0c;帮助我们更好地理解和应用该指令。 文章目录 一、ethtool的前世今生二、ethtool的功能和用法…

小米4 miui6 android,小米4怎么刷miui6?小米4刷miui6三种方法详解

小编将向大家介绍三种小米4刷miui6的方法&#xff0c;分别是卡刷miui6、线刷miui6和Recovery下更新zip包升级miui6。 卡刷MIUI6完整包 下载必要的文件&#xff0c;为刷机过程做准备。 注意&#xff1a; MIUI完整包升级不需要清除数据&#xff0c;降级需要清除数据。 如果已经在…

新装idea的一些常用设置

新装idea的一些常用设置 新安装的idea常用配置 1、提示内容忽略大小写 File -> Settings -> Editor -> General -> Code Completion -> Match case 2、修改字体大小 File -> Settings -> Editor -> Font -> Size 3、取消启动时自动打开最后开…

CSS3:创造引人注目的Web界面

引言 CSS3作为最新版本的CSS标准&#xff0c;为Web界面设计师和开发人员带来了许多令人振奋的新特性和功能。本篇博客将带你深入了解CSS3的各个方面&#xff0c;包括选择器和样式属性、布局和盒模型、过渡和动画、响应式设计等。通过详细的代码示例和实用的技巧&#xff0c;我…

《源泉》节选:现实是多目标折衷

“凯蒂&#xff0c;我们为什么不结婚呢&#xff1f;” “我不知道。”她说&#xff0c;接着又匆忙地补充&#xff0c;只是因为她的心在咚咚作响&#xff0c;因为她不能保持沉默&#xff0c;因为她感到自己不能利用他&#xff0c;“我认为那是因为我们知道我们不必匆忙。” “但…

【硬核干货!】不刷面经,还想上岸大厂?AI算法篇(一)

文章目录 面试前的准备个人基本信息:专业技能:项目/实习/实验经历:强烈建议获奖情况:个人简单评价:校招时投简历1.内推。2.宣讲会。3.双选会。4.网投。笔试和面试过来人重点提醒大家好,我是cv君,开始面经,今天说说很重要的个人心得~ 这个专栏,应许多粉丝要求开通,希…

什么题目的暂时还没想好

序言 在8月底的一次微信谈话中&#xff0c;我定下了一个目标~要用一次性筷子搭起一座埃菲尔铁塔&#xff01; 其实也不一定是用一次性筷子&#xff0c;只是主要耗材是木料&#xff0c;当然我是不可能选择做木雕的&#xff0c;那东西你给我几个亿我都不想去碰&#xff0c;主要…

【网络协议趣谈】TCP协议可靠性保证

很多时候都是在公网上传输数据&#xff0c;而公网往往是不可靠的&#xff0c;因而需要很多的机制去保证传输的可靠性&#xff0c;这里面需要恒心&#xff0c;也即各种重传的策略&#xff0c;还需要有智慧&#xff0c;也就是说这里面包含着大量的算法 一、如何做个靠谱的人 TC…

图解神经网络:卷积、池化、全连接(通道数问题、kernel与filter的概念)

文章目录 卷积操作实际操作filter与kernel1x1的卷积层可视化的例子池化全连接 卷积操作 这个不难理解。我们知道图像在计算机中是由一个个的像素组成的&#xff0c;可以用矩阵表示。 假设一个5x5的输入图像&#xff0c;我们定义一个3x3的矩阵&#xff08;其中的数值是随机生成…

分布式限流的主流方案

前言 详看文章,O(∩_∩)O哈哈~ 常见的分布式限流方案 细数一下分布式限流都有哪些常见方案。话说条条大道通罗马,实现分布式限流的方案之多,两只手加起来都数不过来。 说起Guava大家一定不陌生,它是Google出品的一款工具包(com.google.guava),我们经常用它做一些集合…

物联网应用开发实践案例-智慧农业

1. 设计需求、硬件环境介绍 1.1 项目背景 近几年,物联网、智能家居、AI人工智能技术发送非常迅速。在物联网技术的支撑下,如今农业逐渐走向现代化,自动化、现在智能化的农业生产成为了主流。告别“刀耕火种”的传统农业后,现代农业也正在向智慧型转变,当前智慧农业模式已…

短信接口被恶意调用,瞬间损失数万元,怎么解决?

一、 事件简述 这是一件发生在前段时间的事情&#xff0c;当时的情况是这样的&#xff1a; 一个新的功能模块上线之后&#xff0c;出现短信接口被恶意访问调用的情况&#xff0c;请求数量很大&#xff0c;而且通过查看短信服务商控制台也发现&#xff0c;短信发送量在飙升&…

最新版校园招聘进大厂系列----------(5)百度篇 -----未完待续

&#x1f4e2;&#x1f4e2;&#x1f4e2;&#x1f4e3;&#x1f4e3;&#x1f4e3; 哈喽&#xff01;大家好&#xff0c;我是「奇点」&#xff0c;江湖人称 singularity。刚工作几年&#xff0c;想和大家一同进步&#x1f91d;&#x1f91d; 一位上进心十足的【Java ToB端大厂…

短信接口被恶意盗刷(验证码短信被盗刷)怎么办?

短信验证码被刷怎么办&#xff1f; 一 事件简述二 问题分析三 应急解决方案1 黑名单模式拦截2 请求验证拦截3 应急方案总结 四 最终解决方案第一步&#xff1a;获取防火墙帐号密钥第二步&#xff1a;下载防火墙服务器第三步&#xff1a;前后端接入查看防火墙数据 一 事件简述 …

Mysql是怎么运行的读书笔记

前言 第1章 装作自己是个小白–初识Mysql Mysql分为客户端和服务端&#xff0c;客户端有很多种比如&#xff1a;手机 App、桌面端的软件或者网页版的微信。客户端发送请求&#xff0c;服务端处理请求&#xff0c;服务端将处理结果发送客户端。 环境变量PATH&#xff1a;环境变…

Java学习day12-13抽象类和接口

抽象类和接口 今日目标一.抽象类1.如何理解抽象方法2.抽象类的语法规则 二.接口&#xff08;day13&#xff09;1.接口的语法规则 2.深入思考&#xff1a;3.关于default作业&#xff1a; 今日目标 抽象类 接口 一.抽象类 1.如何理解抽象方法 1.抽象方法衍生背景 “用进废退”…

教你接馈线

馈线是业余电台的三大件之一&#xff0c;馈线的好坏直接影响电波能量的传输。我们所能接触的都是厂制成品馈线&#xff0c;想要DIY几乎不可能。但是&#xff0c;馈线两端连接机器和天线的接头&#xff0c;通常是需要我们自己焊接制作的。 首先是馈线公头&#xff0c;在HF频段和…
最新文章