源码阅读:使用Cartogrpaher进行毫米波雷达栅格SLAM(三)MapBuilderBridge

cartographer_node这个节点的所有操作,都交给了MapBuilderBridge的一个对象map_builder_bridge_来完成。因此,接下来我们要来看看在MapBuilderBridge中都进行了哪些操作

MapBuilderBridge定义在/src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.h中,实现由.../mapbuild_brige.cc完成。

包含以下成员:逐块儿查看一下.

1:跟显示相关。

维持原样就好了。这几个函数也不是MapBuilderBridge的成员函数,而只是一些工具函数


2. 与LandMark相关的


一般参数设置里,use_landmarks = false,所以,landmar这部分功能在目前的代码中是没有起作用的。但cartographer已经考虑了加入landmark的途径。调用map_builder_->pose_graph()->GetLandmarkPoses()可获取Landmark的Pose;



3、构造函数


只是做了一下实例化赋值


4、LoadState函数

调用了map_builder_的成员函数LoadState来加载一个.pbstream文件。map_builder_是接口MapBuilderInterface的实例化对象,而根据是2d还是3d情况,其具体实现会略有不同。


5、AddTrajectory

比较重要的函数:AddTrajectory。添加一条Trajectory


调用了map_builder_->AddTrajectoryBuilder,这已经是cartographer项目中的代码了。也就是说我们快把cartographer_ros的衣服剥的差不多了。可以看到传入的数据有传感器的id,trajectory的一些配置参数,以及一个仿函数对象

这里这个map_builder变量就是cartographer::mapping::MapBuilderInterface的一个实例


其中,MapBuilderBridge::OnLocalSlamResult则如下定义


6. 同样地,接下来的这三个函数也都是调用了map_builder_的成员函数来处理


7. HandleSubmapQuery

处理submap查询的,在cartographer_node中被kSubmapQueryServiceName这个Service调用。


调用map_builder_->SubmapToProto(submap_id, &response_proto);这个函数查询信息,结果放到response这个变量中。

8. GetSubmaplist

这个函数是在往kSubmapListTopic这个Topic上发布数据时被Node::PublishSubmapList调用的,用来获取Submap的列表:

该函数主要也是通过调用map_builder_->pose_graph()->GetAllSubmapPoses()来获取列表信息。

9.GetTrajectoryStates


GetTrajectoryStates用来返回一个TrajectoryStates变量组成的unordered_map这个容器。TrajectoryStates这个结构体的定义如上,见MapBuilderBridge的成员函数。


10.GetTrajectoryNodeList

通过map_builder_->pose_graph()->GetTrajectoryNodePoses()来获取TrajectoryNode的pose的。map_builder_->pose_graph()->constraints()获取Constraints


太长 286到384行 就不放截图了

11.GetConstraintList()

其他跳过


13.sensor_bridge

返回一个SensorBridge的指针。

下面来看一下sensor_bridge这个类

在处理传感器的数据时,都是通过调用map_builder_bridge_的一个类成员sensor_bridge_ptr来处理的。在阅读MapBuilderBridge的源码后我们可以看到跟我们最初的理解略有出入,sensor_bridge_ptr是在MapBuilderBridge中定义的一个SensorBridge类型的局部变量,MapBuilderBridge中一个函数MapBuilderBridge::sensor_bridge可以返回指定trajectory_id的SensorBridge变量。然后在SensorBridge中处理相关传感器的信息。以IMU为例,在cartographer_node中由Node::HandleImuMessage函数处理,该函数实际调用的是map_builder_bridge_->sensor_bridge_ptr->HandleImuMessage。

接下来看一下SensorBridge中具体处理各个传感器的函数

1. 构造函数等

构造函数的参数赋值给成员变量。

但需要注意的是成员变量中有一个TrajectoryBuilderInterface型的一个指针变量。继续跟踪代码我们可以发现,cartographer中各种消息都统一调用了这个成员类的虚函数AddSensorData(). 

实际上是虚函数加重载

而CollatedTrajectoryBuilder继承了这个类并实现了AddSensorData()函数。这两个类都定义在cartographer中的mapping文件夹下。CollatedTrajectoryBuilder的构造函数说明通过统一调用HandleCollatedSensorData()函数,来轮询处理kImu(IMU消息)、kRangefinder(测距消息,不仅仅是激光)、kOdometer(里程计消息)等。所以,我们到后面可以再看这两个类

2. 处理里程计的函数


一个预处理的工具函数,并非SensorBridge的成员变量;

需要注意的是TfBridge这个类,他的实例化对象是tf_bridge_,tf_bridge_是SensorBridge的一个成员变量。我们通过查看TfBridge可以看到,作者在设计的时候,通过TfBridge把不同传感器的差异进行了一下屏蔽,不管什么传感器,都会对每一帧数据的位姿进行估计,而这个历史的位姿数据就以TfBridge的形式存起来。这样在使用的时候也可以通过TfBridge查询某一个传感器在某一个历史时刻的位姿

这里同样也是,代码通过TfBridge查询了一下历史数据(54行:const auto sensor_to_tracking = tf_bridge_.LookupToTracking()),然后把这个作为参数传给了TrajectoryBuilder的AddSensorData来做处理(68行: trajectory_builder_->AddSensorData())。前面我们已经介绍过了,TrajectoryBuilder也是为不同的传感器提供了统一的处理接口。我们可以在TrajectoryBuilder的具体实现里再看具体做了什么操作。但我们可以合理猜测,比如,里程计的数据是在原来数据的基础上再做一个累加,作为新的值同样保存到TfBridge里。

以里程计数据为例,我们可以梳理一下传感器数据整个的处理流程:

传感器的ROS节点/Playbag广播到Topic上相关数据的Message---->cartographer_node中启动的StartTrajectory这个服务会订阅传感器数据---->接收到该数据由相应的处理函数处理,比如Node::HandleImuMessage---->该处理函数实际调用是MapBuilderBridge中的一个SensorBridge变量进行处理---->调用了TrajectoryBuilder的虚函数AddSensorData()---->CollatedTrajectoryBuilder继承TrajectoryBuilder并具体实现AddSensorData()函数

3、4、分别处理GPS和IMU的数据 和上面类似,跳过


5、处理激光、多激光、点云数据

初看似乎没有调用trajectory-->AddSensorData函数。但仔细研究会发现他们之间彼此存在调用关系。最根上是HandleRangefinder这个函数:这个函数调用了trajectory_builder_->AddSensorData来处理, 如:



最后编辑于
©著作权归作者所有,转载或内容合作请联系作者
  • 序言:七十年代末,一起剥皮案震惊了整个滨河市,随后出现的几起案子,更是在滨河造成了极大的恐慌,老刑警刘岩,带你破解...
    沈念sama阅读 216,544评论 6 501
  • 序言:滨河连续发生了三起死亡事件,死亡现场离奇诡异,居然都是意外死亡,警方通过查阅死者的电脑和手机,发现死者居然都...
    沈念sama阅读 92,430评论 3 392
  • 文/潘晓璐 我一进店门,熙熙楼的掌柜王于贵愁眉苦脸地迎上来,“玉大人,你说我怎么就摊上这事。” “怎么了?”我有些...
    开封第一讲书人阅读 162,764评论 0 353
  • 文/不坏的土叔 我叫张陵,是天一观的道长。 经常有香客问我,道长,这世上最难降的妖魔是什么? 我笑而不...
    开封第一讲书人阅读 58,193评论 1 292
  • 正文 为了忘掉前任,我火速办了婚礼,结果婚礼上,老公的妹妹穿的比我还像新娘。我一直安慰自己,他们只是感情好,可当我...
    茶点故事阅读 67,216评论 6 388
  • 文/花漫 我一把揭开白布。 她就那样静静地躺着,像睡着了一般。 火红的嫁衣衬着肌肤如雪。 梳的纹丝不乱的头发上,一...
    开封第一讲书人阅读 51,182评论 1 299
  • 那天,我揣着相机与录音,去河边找鬼。 笑死,一个胖子当着我的面吹牛,可吹牛的内容都是我干的。 我是一名探鬼主播,决...
    沈念sama阅读 40,063评论 3 418
  • 文/苍兰香墨 我猛地睁开眼,长吁一口气:“原来是场噩梦啊……” “哼!你这毒妇竟也来了?” 一声冷哼从身侧响起,我...
    开封第一讲书人阅读 38,917评论 0 274
  • 序言:老挝万荣一对情侣失踪,失踪者是张志新(化名)和其女友刘颖,没想到半个月后,有当地人在树林里发现了一具尸体,经...
    沈念sama阅读 45,329评论 1 310
  • 正文 独居荒郊野岭守林人离奇死亡,尸身上长有42处带血的脓包…… 初始之章·张勋 以下内容为张勋视角 年9月15日...
    茶点故事阅读 37,543评论 2 332
  • 正文 我和宋清朗相恋三年,在试婚纱的时候发现自己被绿了。 大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
    茶点故事阅读 39,722评论 1 348
  • 序言:一个原本活蹦乱跳的男人离奇死亡,死状恐怖,灵堂内的尸体忽然破棺而出,到底是诈尸还是另有隐情,我是刑警宁泽,带...
    沈念sama阅读 35,425评论 5 343
  • 正文 年R本政府宣布,位于F岛的核电站,受9级特大地震影响,放射性物质发生泄漏。R本人自食恶果不足惜,却给世界环境...
    茶点故事阅读 41,019评论 3 326
  • 文/蒙蒙 一、第九天 我趴在偏房一处隐蔽的房顶上张望。 院中可真热闹,春花似锦、人声如沸。这庄子的主人今日做“春日...
    开封第一讲书人阅读 31,671评论 0 22
  • 文/苍兰香墨 我抬头看了看天上的太阳。三九已至,却和暖如春,着一层夹袄步出监牢的瞬间,已是汗流浃背。 一阵脚步声响...
    开封第一讲书人阅读 32,825评论 1 269
  • 我被黑心中介骗来泰国打工, 没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留,地道东北人。 一个月前我还...
    沈念sama阅读 47,729评论 2 368
  • 正文 我出身青楼,却偏偏与公主长得像,于是被迫代替她去往敌国和亲。 传闻我的和亲对象是个残疾皇子,可洞房花烛夜当晚...
    茶点故事阅读 44,614评论 2 353