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

news/2024/10/15 14:04:29/

讲解关于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;“我认为那是因为我们知道我们不必匆忙。” “但…