(02)Cartographer源码无死角解析-(28) GlobalTrajectoryBuilder构建过程与数据转发前后端
创始人
2024-02-07 07:24:56
0

讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解:https://blog.csdn.net/weixin_43013761/article/details/127350885
 
文末正下方中心提供了本人联系方式,点击本人照片即可显示WX→官方认证{\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证}文末正下方中心提供了本人联系方式,点击本人照片即可显示WX→官方认证
 

一、前言

通过上一篇博客的复盘,已经可以很清晰的知道数据流动过程,总的来说,会依照时间的先后顺序,把数据传送给 GlobalTrajectoryBuilder。当然,对于 GPS 与 landmark 数据可以通过参数 collate_fixed_frame_,collate_landmarks_ 进行控制,是否对数据进行排序之后,再添加到 GlobalTrajectoryBuilder,默认配置 trajectory_builder.lua 中:

  collate_fixed_frame = true,  --是否将GPS数据放入阻塞队列中,按时间排序再进行分发collate_landmarks = false,  --是否将landmarks数据放入阻塞队列中,按时间排序再进行分发

在对 GlobalTrajectoryBuilder 进行深入分析之前,先来看看其构建过程。
 

二、GlobalTrajectoryBuilder实例

在 src/cartographer/cartographer/mapping/map_builder.cc 文件中的 MapBuilder::AddTrajectoryBuilde() 函数

	3D追踪trajectory_builders_.push_back(absl::make_unique(trajectory_options, sensor_collator_.get(), trajectory_id,expected_sensor_ids,// 将3D前端与3D位姿图打包在一起, 传入CollatedTrajectoryBuilderCreateGlobalTrajectoryBuilder3D(std::move(local_trajectory_builder), trajectory_id,static_cast(pose_graph_.get()),local_slam_result_callback, pose_graph_odometry_motion_filter)));
	2D追踪trajectory_builders_.push_back(absl::make_unique(trajectory_options, sensor_collator_.get(), trajectory_id,expected_sensor_ids,// 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilderCreateGlobalTrajectoryBuilder2D(std::move(local_trajectory_builder), trajectory_id,static_cast(pose_graph_.get()),local_slam_result_callback, pose_graph_odometry_motion_filter)));

其上可以看到,在对3D或者2D追踪,实例化 CollatedTrajectoryBuilder 对象的时候, 其需要一个 std::unique_ptr wrapped_trajectory_builder 参数,这里以2D为例,也就是如下函数的返回值:

 CreateGlobalTrajectoryBuilder2D(std::move(local_trajectory_builder), trajectory_id,static_cast(pose_graph_.get()),local_slam_result_callback, pose_graph_odometry_motion_filter)

CreateGlobalTrajectoryBuilder2D 是一个工厂函数,在前面提到过,类模板成员函数没有自动推演模板参数的功能,所以单独写了出来,
实现过程如下:

// 2d的完整的slam
std::unique_ptr CreateGlobalTrajectoryBuilder2D(std::unique_ptr local_trajectory_builder,const int trajectory_id, mapping::PoseGraph2D* const pose_graph,const TrajectoryBuilderInterface::LocalSlamResultCallback&local_slam_result_callback,const absl::optional& pose_graph_odometry_motion_filter) {return absl::make_unique>(std::move(local_trajectory_builder), trajectory_id, pose_graph,local_slam_result_callback, pose_graph_odometry_motion_filter);
}

初步看起来比较复杂,其实不然,直接看return,其就是构建一个 GlobalTrajectoryBuilder 类型智能指针返回,共需要五个参数。
 

参数local_trajectory_builder

该参数在 MapBuilder::AddTrajectoryBuilder() 中创建,主要用于前端优化代码如下:

    // local_trajectory_builder(前端)的初始化std::unique_ptr local_trajectory_builder;if (trajectory_options.has_trajectory_builder_3d_options()) {local_trajectory_builder = absl::make_unique(trajectory_options.trajectory_builder_3d_options(),SelectRangeSensorIds(expected_sensor_ids));} 

 

参数 pose_graph_

该参数在 MapBuilder 的构造函数中根据配置信息创建,主要用于后端优化

  // 2d位姿图(后端)的初始化根据if (options.use_trajectory_builder_2d()) {//如果使用2d追踪pose_graph_ = absl::make_unique(options_.pose_graph_options(),absl::make_unique(options_.pose_graph_options().optimization_problem_options()),&thread_pool_);}// 3d位姿图(后端)的初始化if (options.use_trajectory_builder_3d()) {//如果使用3d追踪pose_graph_ = absl::make_unique(options_.pose_graph_options(),absl::make_unique(options_.pose_graph_options().optimization_problem_options()),&thread_pool_);} 

 

参数 local_slam_result_callback

local_slam_result_callback 是一个回调函数,该回调函数是src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 的 MapBuilderBridge::AddTrajectory 函数中的一个lambda 表达式:

      // lambda表达式 local_slam_result_callback_[this](const int trajectory_id, const ::cartographer::common::Time time, const Rigid3d local_pose,::cartographer::sensor::RangeData range_data_in_local,const std::unique_ptr) {// 保存local slam 的结果数据 5个参数实际只用了4个OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);}

总的来说呢,local_slam_result_callback 等价于函数 MapBuilderBridge::OnLocalSlamResult() 函数,该函数的内容后面再讲解。
 

参数 pose_graph_odometry_motion_filter

参数 pose_graph_odometry_motion_filter 是再 MapBuilder::AddTrajectoryBuilde() 中创建,主要涉及代码如下:

  // 运动过滤器, 运动太小没必要进行更新// 配置文件中没有 pose_graph_odometry_motion_filteabsl::optional pose_graph_odometry_motion_filter;if (trajectory_options.has_pose_graph_odometry_motion_filter()) {LOG(INFO) << "Using a motion filter for adding odometry to the pose graph.";pose_graph_odometry_motion_filter.emplace(MotionFilter(trajectory_options.pose_graph_odometry_motion_filter()));}

其主要功能是一个滤波器。
 

提示

重点:\color{red}重点:重点:这里先不要深究,主要是回顾一下 GlobalTrajectoryBuilder 的构建过程。这里呢,额外介绍一点东西。src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 是 包 cartographer_ros 中的代码,简单的说,map_builder_bridge.cc 负责调用 Cartographer 的算法代码。

MapBuilderBridge 可以理解为 ros 侧封装的代码,其中的函数 MapBuilderBridge::AddTrajectory() 添加一条轨迹的时候会调用到 MapBuilder::AddTrajectoryBuilder() 函数,那么很显然,MapBuilder 也是 Cartographer 暴漏在外面接口,或者说共客户使用的类。

前面我们知道 CollatedTrajectoryBuilder 也是 Cartographer 暴露给客户的接口,那么问题就出现了,CollatedTrajectoryBuilder 与 MapBuilder 是怎么联系起来的?注意,在 MapBuilder 中有一个成员变量

std::vector>trajectory_builders_;

其存储的,就是所有创建轨迹是生成的 CollatedTrajectoryBuilder 实例对象。也就是说,对于 Cartographer 算法的使用,暂时只需要着重这 MapBuilder 与 CollatedTrajectoryBuilder 这两个类就行了,后续其他的在后面也会陆续讲解。
 

三、GlobalTrajectoryBuilder构造函数

GlobalTrajectoryBuilder.h 的头文件,就是声明了两个工厂函数,前面已经进行讲解。再次提及一下 GlobalTrajectoryBuilder 的实例对象,是存储在 CollatedTrajectoryBuilder::wrapped_trajectory_builder_ 之中。

下面来看看 GlobalTrajectoryBuilder 的构造函数,函数所示:

  GlobalTrajectoryBuilder(std::unique_ptr local_trajectory_builder,const int trajectory_id, PoseGraph* const pose_graph,const LocalSlamResultCallback& local_slam_result_callback,const absl::optional& pose_graph_odometry_motion_filter): trajectory_id_(trajectory_id),pose_graph_(pose_graph),local_trajectory_builder_(std::move(local_trajectory_builder)),local_slam_result_callback_(local_slam_result_callback), ///主要用于保存结果的回调函数pose_graph_odometry_motion_filter_(pose_graph_odometry_motion_filter) {}

与前面创建实例的时候,传入的参数一一对应的,不过没有做什么复杂的操作,就是一个初始化列表,对如下的几个成员变量进行了赋值:

  const int trajectory_id_; //轨迹idPoseGraph* const pose_graph_;     // 模板参数, 可以指向PoseGraph2D也可以指向PoseGraph3Dstd::unique_ptr local_trajectory_builder_;  //前端LocalSlamResultCallback local_slam_result_callback_; //主要用于保存结果的回调函数absl::optional pose_graph_odometry_motion_filter_; //里程计过滤器,没有启用

 

四、AddSensorData() 重载函数

可以很明显的可以看到,实现了多个 AddSensorData() 重载函数,如下所示:

1、imu数据处理

  // imu数据的处理, 数据走向有两个,一个是进入前端local_trajectory_builder_,一个是进入后端pose_graph_void AddSensorData(const std::string& sensor_id,const sensor::ImuData& imu_data) override {if (local_trajectory_builder_) { //如果存在前端local_trajectory_builder_->AddImuData(imu_data); //把数据发送给前端}pose_graph_->AddImuData(trajectory_id_, imu_data); //把数据发送给后端}

 

2、里程计数据处理

  // 里程计数据的处理, 数据走向有两个,一个是进入前端local_trajectory_builder_, 一个是进入后端pose_graph_// 加入到后端之前, 先做一个距离的计算, 只有时间,移动距离,角度 变换量大于阈值才加入到后端中void AddSensorData(const std::string& sensor_id,const sensor::OdometryData& odometry_data) override {//判断数据是否有值,如果没有值,则报错,且输出里程计数据                 CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;if (local_trajectory_builder_) { //如果存在前端local_trajectory_builder_->AddOdometryData(odometry_data);}// TODO(MichaelGrupp): Instead of having an optional filter on this level,// odometry could be marginalized between nodes in the pose graph.// Related issue: cartographer-project/cartographer/#1768//如果设置则进行过滤if (pose_graph_odometry_motion_filter_.has_value() &&pose_graph_odometry_motion_filter_.value().IsSimilar(odometry_data.time, odometry_data.pose)) {return;}//把数据分发给后端pose_graph_->AddOdometryData(trajectory_id_, odometry_data);}

 

3、GPS数据处理

  // gps数据只在后端中使用void AddSensorData(const std::string& sensor_id,const sensor::FixedFramePoseData& fixed_frame_pose) override {if (fixed_frame_pose.pose.has_value()) {CHECK(fixed_frame_pose.pose.value().IsValid())<< fixed_frame_pose.pose.value();}pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);}

 

4、Landmark数据处理

  // Landmark的数据只在后端中使用void AddSensorData(const std::string& sensor_id,const sensor::LandmarkData& landmark_data) override {pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);}

 

5、local slam 数据处理

  // 将local slam的结果加入到后端中, 作为位姿图的一个节点void AddLocalSlamResultData(std::unique_ptrlocal_slam_result_data) override {CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with ""local_trajectory_builder_ present.";local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_);}

 

五、雷达数据处理

下面来重点讲解一下雷达数据的处理,代码如下:

 /*** @brief 点云数据的处理, 先进行扫描匹配, 然后将扫描匹配的结果当做节点插入到后端的位姿图中* * @param[in] sensor_id topic名字* @param[in] timed_point_cloud_data 点云数据*/void AddSensorData(const std::string& sensor_id, //订阅的话题const sensor::TimedPointCloudData& timed_point_cloud_data) override {CHECK(local_trajectory_builder_)//检测是否存在前端,没有前端则报错<< "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";// 通过前端进行扫描匹配, 然后返回匹配后的结果std::unique_ptrmatching_result = local_trajectory_builder_->AddRangeData(sensor_id, timed_point_cloud_data);if (matching_result == nullptr) { //如果失败了,直接返回// The range data has not been fully accumulated yet.return;}kLocalSlamMatchingResults->Increment();std::unique_ptr insertion_result;// matching_result->insertion_result 的类型是 LocalTrajectoryBuilder2D::InsertionResult// 如果雷达成功插入到地图中if (matching_result->insertion_result != nullptr) {kLocalSlamInsertionResults->Increment();// 将匹配后的结果 当做节点 加入到位姿图中auto node_id = pose_graph_->AddNode(matching_result->insertion_result->constant_data, trajectory_id_,matching_result->insertion_result->insertion_submaps);CHECK_EQ(node_id.trajectory_id, trajectory_id_);// 这里的InsertionResult的类型是 TrajectoryBuilderInterface::InsertionResultinsertion_result = absl::make_unique(InsertionResult{node_id, matching_result->insertion_result->constant_data,std::vector>(matching_result->insertion_result->insertion_submaps.begin(),matching_result->insertion_result->insertion_submaps.end())});}// 将结果数据传入回调函数中, 进行保存if (local_slam_result_callback_) {local_slam_result_callback_(trajectory_id_, matching_result->time, matching_result->local_pose,std::move(matching_result->range_data_in_local),std::move(insertion_result));}}

如果看得不是是明白没有关系,下一篇博客会针对该函数进行十分详细的讲解。

六、结语

通过篇博客,可以知道,GlobalTrajectoryBuilder 中除了对雷达数据做了比较复杂的处理,对于其他的数据集,基本就是做了个转发工作。值得注意的是,GlobalTrajectoryBuilder::local_slam_result_callback_ 回调函数,是一个十分主要点,其主要起到保存结果的作用,具体的内容后面再进行讲解。

 
 
 

相关内容

热门资讯

富榜|2025年上半年公募基金... (转自:富榜)2025年上半年,公募基金市场整体呈现出鲜明的分化态势。在市场赚钱效应显著,多数基金收...
宁德时代新能源科技股份有限公司... 来源:中访网财观中访网数据  宁德时代新能源科技股份有限公司(股票代码:3750.HK)宣布,根据香...
大金重工:完成欧洲波罗的海海上... 中证智能财讯大金重工(002487)7月8日晚间公告,公司全资子公司蓬莱大金海洋重工有限公司为欧洲波...
借钱养老?湖南多家银行推出“养...   近日,湖南省多家农商行推出以补缴养老保险等为专属用途的“养老贷”,包括长沙农商行、华容农商行、临...
新朋股份:全资子公司参与投资的... 新朋股份晚间公告,公司所属全资子公司瀚娱动从南京金浦新潮新兴产业股权投资基金合伙企业(有限合伙)(简...
“丹娜丝”将在浙闽二次登陆,国... 转自:上观新闻今年第4号台风“丹娜丝”预计将于今天傍晚到夜间在浙江台州至福建宁德一带沿海登陆。受台风...
国办:加强全流程安全管理 做好... 格隆汇7月8日|国务院办公厅印发《关于健全“高效办成一件事”重点事项常态化推进机制的意见》。《意见》...
赛象科技:收到5.33亿元空客... 中证智能财讯赛象科技(002337)7月8日晚间公告,公司近日收到 “空客天津A320系列飞机第二条...
反内卷暴击!政策铁拳淘汰内耗,... (转自:ETF万亿指数)卷死同行?不如让政策卷走对手! 光伏减产30%逼退小厂、钢铁水泥清退低价产能...
“环青赛牵动青海人的心” 精彩... 转自:中国体育报  骑行在蜿蜒美丽的十二盘,畅游于黄色油菜花海洋之间……你能看到的,是一场自行车运动...