Cartographer源码阅读(4):Node和MapBuilder对象2
MapBuilder的成员变量sensor::Collator sensor_collator_;
再次阅读MapBuilder::AddTrajectoryBuilder方法。首先构造了mapping::GlobalTrajectoryBuilder实例,接着作为参数构造了CollatedTrajectoryBuilder实例。
trajectory_builders_.push_back(
common::make_unique<CollatedTrajectoryBuilder>(
&sensor_collator_, trajectory_id, expected_sensor_ids,
common::make_unique<mapping::GlobalTrajectoryBuilder<mapping_2d::LocalTrajectoryBuilder,mapping_2d::proto::LocalTrajectoryBuilderOptions,mapping_2d::PoseGraph>>
(trajectory_options.trajectory_builder_2d_options(),trajectory_id, pose_graph_2d_.get(),local_slam_result_callback)
)
);
这里sensor_collator_作为参数传入,参与CollatedTrajectoryBuilder构造。查看构造函数:
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(sensor::Collator* const sensor_collator, const int trajectory_id, const std::unordered_set<std::string>& expected_sensor_ids, std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder): sensor_collator_(sensor_collator), trajectory_id_(trajectory_id), wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)), last_logging_time_(std::chrono::steady_clock::now())
{sensor_collator_->AddTrajectory(trajectory_id, expected_sensor_ids,[this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data){HandleCollatedSensorData(sensor_id, std::move(data));});
}
这里是回调函数,std::unique_ptr是表示参数为智能指针。
[this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data){HandleCollatedSensorData(sensor_id, std::move(data));}
(1)查看sensor::Collator的AddTrajectory方法:
void Collator::AddTrajectory( const int trajectory_id, const std::unordered_set<std::string>& expected_sensor_ids, const Callback& callback)
{for (const auto& sensor_id : expected_sensor_ids){const auto queue_key = QueueKey{trajectory_id, sensor_id};queue_.AddQueue(queue_key, [callback, sensor_id](std::unique_ptr<Data> data){callback(sensor_id, std::move(data));});queue_keys_[trajectory_id].push_back(queue_key);}
}
for (const auto& sensor_id : expected_sensor_ids)用到了C++11的auto新特性。
(2)查看HandleCollatedSensorData方法。调用了data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());这里wrapped_trajectory_builder_是在CollatedTrajectoryBuilder构造函数中赋值的。为GlobalTrajectoryBuilder对象。因而查看sensor::Data的AddToTrajectoryBuilder() 方法。
virtual void AddToTrajectoryBuilder(mapping::TrajectoryBuilderInterface *trajectory_builder) = 0;是sensor::Data类的一个虚方法。内部执行了trajectory_builder->AddSensorData(sensor_id_, data_);
最后调用的是GlobalTrajectoryBuilder对象的AddSensorData(xx)方法。
1 void CollatedTrajectoryBuilder::HandleCollatedSensorData( const std::string& sensor_id, std::unique_ptr<sensor::Data> data) 2 { 3 auto it = rate_timers_.find(sensor_id); 4 if (it == rate_timers_.end()) 5 { 6 it = rate_timers_ .emplace( 7 std::piecewise_construct, std::forward_as_tuple(sensor_id), 8 std::forward_as_tuple(common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))) .first; 9 } 10 it->second.Pulse(data->GetTime()); 11 12 if (std::chrono::steady_clock::now() - last_logging_time_ > 13 common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) 14 { 15 for (const auto& pair : rate_timers_) 16 { 17 LOG(INFO) << pair.first << " rate: " << pair.second.DebugString(); 18 } 19 last_logging_time_ = std::chrono::steady_clock::now(); 20 } 21 22 data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get()); 23 } 24 25 }
CollatedTrajectoryBuilder::HandleCollatedSensorData
template <typename DataType>
class Dispatchable : public Data
{public:Dispatchable(const std::string &sensor_id, const DataType &data): Data(sensor_id), data_(data) {}common::Time GetTime() const override { return data_.time; }void AddToTrajectoryBuilder( mapping::TrajectoryBuilderInterface *const trajectory_builder) override {trajectory_builder->AddSensorData(sensor_id_, data_);}private:const DataType data_;
};
再以IMU数据为例,GlobalTrajectoryBuilder类的AddSensorData(xx):
void AddSensorData(const std::string& sensor_id, const sensor::ImuData& imu_data) override
{local_trajectory_builder_.AddImuData(imu_data);pose_graph_->AddImuData(trajectory_id_, imu_data);
}
再看一下激光点云的数据
1 void AddSensorData( const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) override 2 { 3 std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult> matching_result = 4 local_trajectory_builder_.AddRangeData( timed_point_cloud_data.time, 5 sensor::TimedRangeData {timed_point_cloud_data.origin, 6 timed_point_cloud_data.ranges, {}} 7 ); 8 if (matching_result == nullptr) 9 { 10 // The range data has not been fully accumulated yet. 11 return; 12 } 13 std::unique_ptr<mapping::NodeId> node_id; 14 if (matching_result->insertion_result != nullptr) 15 { 16 node_id = ::cartographer::common::make_unique<mapping::NodeId>( 17 pose_graph_->AddNode(matching_result->insertion_result->constant_data, 18 trajectory_id_, matching_result->insertion_result->insertion_submaps)); 19 CHECK_EQ(node_id->trajectory_id, trajectory_id_); 20 } 21 if (local_slam_result_callback_) 22 { 23 local_slam_result_callback_( trajectory_id_, matching_result->time, 24 matching_result->local_pose, 25 std::move(matching_result->range_data_in_local), std::move(node_id)); 26 } 27 }
这里有两个重要的步骤一个是local_trajectory_builder_.AddRangeData(xxx),一个是 pose_graph_->AddNode(xxx)方法。同时std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult> matching_result作为AddNode方法的参数。
1 mapping::NodeId PoseGraph::AddNode( 2 std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data, 3 const int trajectory_id, 4 const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) { 5 const transform::Rigid3d optimized_pose( 6 GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose); 7 8 common::MutexLocker locker(&mutex_); 9 AddTrajectoryIfNeeded(trajectory_id); 10 const mapping::NodeId node_id = trajectory_nodes_.Append( 11 trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose}); 12 ++num_trajectory_nodes_; 13 14 // Test if the 'insertion_submap.back()' is one we never saw before. 15 if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 || 16 std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap != 17 insertion_submaps.back()) { 18 // We grow 'submap_data_' as needed. This code assumes that the first 19 // time we see a new submap is as 'insertion_submaps.back()'. 20 const mapping::SubmapId submap_id = 21 submap_data_.Append(trajectory_id, SubmapData()); 22 submap_data_.at(submap_id).submap = insertion_submaps.back(); 23 } 24 25 // We have to check this here, because it might have changed by the time we 26 // execute the lambda. 27 const bool newly_finished_submap = insertion_submaps.front()->finished(); 28 AddWorkItem([=]() REQUIRES(mutex_) { 29 ComputeConstraintsForNode(node_id, insertion_submaps, 30 newly_finished_submap); 31 }); 32 return node_id; 33 }
PoseGraph::AddNode
PoseGraph::AddNode方法很重要,分析节点和子图的关系。
此处强调一下GlobalTrajectoryBuilder的两个关键对象local_trajectory_builder_和pose_graph_。
PoseGraph* const pose_graph_;LocalTrajectoryBuilder local_trajectory_builder_;
接下来按照准备安装ROS消息发布和处理的流程进行分析,即数据流。
参考资料:
http://blog.csdn.net/datase/article/details/78665862
http://blog.csdn.net/learnmoreonce/article/category/6989560
Cartographer源码阅读(4):Node和MapBuilder对象2相关推荐
- Cartographer源码阅读(1):程序入口
带着几个思考问题: (1)IMU数据的使用,如何融合,Kalman滤波? (2)图优化的具体实现,闭环检测的策略? (3)3D激光的接入和闭环策略? 1. 安装Kdevelop工具: http://b ...
- Cartographer源码阅读
(一) 定义的各种坐标系间的关系 1. map_frame = "map",cartographer中使用的全局坐标系,最好保持默认,否则ROS的Rviz不认识其它的定义,导致无法 ...
- sfm三维重建源码_OpenMVG源码阅读小记
"读一份好源码,就是和许多智慧的人谈话". 本文记录了笔者学习 openMVG 开源软件的一些初步经验和心得.如果你对计算机视觉和摄影测量有兴趣,需要用到相关技术,这篇文章正好就是 ...
- webpack源码阅读——npm脚本运行webpack与命令行输入webpack的区别
原文地址:webpack源码阅读--npm脚本执行webpack与命令行输入webpack执行的区别 如有错误,欢迎指正! webpack是目前被大家广为使用的模块打包器.从命令行输入webpack或 ...
- 【SeaJS】【3】seajs.data相关的源码阅读
在SeaJS官网上推荐了源码阅读顺序,本文并没有采用这个顺序,而是按个人习惯以调试官方示例的方式进行源码阅读.早期版本作者玉伯使用了几个闭包形式,本文源码版本为2.1.1,它的编码方式个人认为更加脚本 ...
- gin context和官方context_gin 源码阅读(二) 路由和路由组
" 上一篇讲的是gin 框架的启动原理,今天来讲一下 gin 路由的实现. 1 用法 还是老样子,先从使用方式开始: func main() { r := gin.Default() r.G ...
- koa源码阅读之koa-compose/application.js
koa源码阅读之koa-compose/application.js koa-Compose 为了理解方便特地把注释也粘进来 //这英语.我也来翻译一波 //大概就是把所有的中间件组合返回一个完整大块 ...
- Java8 Hashtable 源码阅读
一.Hashtable 概述 Hashtable 底层基于数组与链表实现,通过 synchronized 关键字保证在多线程的环境下仍然可以正常使用.虽然在多线程环境下有了更好的替代者 Concurr ...
- Java8 LinkedHashMap 源码阅读
如果你对 HashMap 的源码有了解的话,只需要一图就能知道 LinkedHashMap 的原理了,但是具体的实现细节还是需要去读一下源码. 一.LinkedHashMap 简介 1.1 继承结构 ...
最新文章
- 线上集群服务器性能参数查看命令
- 使用Denyhost来阻止恶意连接SSH的IP
- DATAGUARD 三种保护模式
- halcon ocr 生成样本变体_Halcon简单文字识别OCR的使用
- 多线程队列的算法优化
- 10 分钟,带你快速入门前端三大技术(HTML、CSS、JavaScript)
- 计算机二级考试题有之前的吗,国家计算机二级上机做对前两题为什么过不了
- LINUX报错:apt transaction returned result exit-failed
- c语言程序设计基础第三版答案,清华大学出版社-图书详情-《C语言程序设计上机指导与习题解答(第3版)》...
- mysql delphi_delphi 7 连接 MySql
- 华为oj题库分苹果JAVA_华为OJ机试训练(一)
- 黑客动画吧-古墓探险之过关题解
- java五子棋人机对战_java swing人机对战五子棋
- javascript 建模_使用JavaScript进行快速数据建模
- [原创]获取还原某音弹幕(web版)
- Vue项目中操作svg文件
- 100以内的质数(素数)
- 360 2013校园招聘笔试题(含参考答案)
- 重磅!华为宣布起诉美国政府
- 回文数函数的粗浅理解
热门文章
- 如何在Linux中使用netstat命令
- Android AlarmManager广播接收器和服务
- ROS的学习(二十)rosserial中的Publisher和Subscriber中的编程步骤
- 20道必须掌握的C++面试题
- C++基础教程之数组
- Node.js + Express 4+angularjs随笔小记,不供参考
- NetWare 5.1 Does Not Recognize CD-ROM Drive
- ubuntu vnc安装
- 【99JS手记】之一:nth-child选择器
- Mathematica颜色系,你喜欢哪一个?