Cartographer激光SLAM2D源码分析
本文目的
本文主要记录调试运行demo中cartographer_paper_deutsches_museum.bag中代码的流程,不具体分析每个函数的功能而是理清调用关系.
首先cartographer ros入口
1.node_main.cc
在node_main中, 主要定义了一个Node node 和 map_builder, 这两个则主要就是为了链接cartographer_ros和cartographer.
auto map_builder = cartographer::common::make_unique<cartographer::mapping::MapBuilder>(node_options.map_builder_options); Node node(node_options, std::move(map_builder), &tf_buffer); if (!FLAGS_load_state_filename.empty()) { node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); } if (FLAGS_start_trajectory_with_default_topics) { node.StartTrajectoryWithDefaultTopics(trajectory_options); } ::ros::spin(); // ros一直循环,直到ctrl+c或者ROS主线程退出才会执行后面的语句 node.FinishAllTrajectories(); node.RunFinalOptimization(); if (!FLAGS_save_state_filename.empty()) { node.SerializeState(FLAGS_save_state_filename); // 序列化 } 下面就主要从这node中进行分析如何一步步的从cartographer_ros到cartographer.
实际运行roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=/home/lee/Downloads/cartographer_paper_deutsches_museum.bag之后除了获取一些配置文件信息之后最先输出的就是添加一条轨迹ID=0这一步是怎么来的呢,下面进行分析:
2. node.h && node.cc
主要功能就是wires up ROS topic to SLAM , 处理激光,里程计, IMU等传感器数据, 开始建图,结束建图,以及最后优化.
1, node.h
定义了几个关键变量基本都是map或者set类型, 表示一个轨迹一个相对应的变量
MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_); std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_; std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_; std::unordered_map<int, std::vector<Subscriber>> subscribers_; std::unordered_set<std::string> subscribed_topics_; std::unordered_map<int, bool> is_active_trajectory_ GUARDED_BY(mutex_); 1, StartTrajectoryWithDefaultTopics 建立轨迹
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { carto::common::MutexLocker lock(&mutex_); CHECK(ValidateTrajectoryOptions(options)); AddTrajectory(options, DefaultSensorTopics()); } 这里面有一个AddTrajectory函数主要功能:
trajectory_builder_interface.h
1, ComputeExpectedSensorIds:返回订阅的传感器集合, RANGE IMU ODOMETRY LANDMARK等
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId> 2, map_builder_bridge_.AddTrajectory
map_builder_bridge
map_builder_bridge顾名思义就是cartographer_ros和cartographer中map_builder的一个桥梁,由它引入到cartographer的map_builder.c中, 第一幅图中打印的消息就是由LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";得到
3, AddExtrapolator
4,AddSensorSamplers
5,LaunchSubscribers
4. map_builder.cc
int MapBuilder::AddTrajectoryBuilder(const std::set<SensorId>& expected_sensor_ids, const proto::TrajectoryBuilderOptions& trajectory_options, LocalSlamResultCallback local_slam_result_callback) AddTrajectoryBuilder这个函数中包含3D和2D的部分,本文主要看2D的情况所以只提取2D中的代码段:
{ std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder; if (trajectory_options.has_trajectory_builder_2d_options()) { local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder2D>(trajectory_options.trajectory_builder_2d_options(),SelectRangeSensorIds(expected_sensor_ids)); } DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get())); trajectory_builders_.push_back(common::make_unique<CollatedTrajectoryBuilder>( sensor_collator_.get(), trajectory_id, expected_sensor_ids, CreateGlobalTrajectoryBuilder2D( std::move(local_trajectory_builder), trajectory_id, static_cast<PoseGraph2D*>(pose_graph_.get()), local_slam_result_callback))); if (trajectory_options.has_overlapping_submaps_trimmer_2d()) { const auto& trimmer_options = trajectory_options.overlapping_submaps_trimmer_2d(); pose_graph_->AddTrimmer(common::make_unique<OverlappingSubmapsTrimmer2D>( trimmer_options.fresh_submaps_count(), trimmer_options.min_covered_area() / common::Pow2(trajectory_options.trajectory_builder_2d_options() .submaps_options() .grid_options_2d() .resolution()), trimmer_options.min_added_submaps_count())); } } map_builder的主要功能就是综合TrajectoryBuilders (for local submaps) 和 a PoseGraph (for loop closure).从上面代码中可以看到几个变量和函数
1,local_trajectory_builder: LocalTrajectoryBuilder2D
2,trajectory_builders_ : std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
3,CollatedTrajectoryBuilder
4,CreateGlobalTrajectoryBuilder2D
5,pose_graph_: PoseGraph2D
接下来就需要对这几个进行分析了即进入到第二个部分cartographer.
cartographer模块
上面我们已经知道map_builder的功能就是同时处理前端和后端, 主要利用其中trajectory_builders_,一条轨迹就是一次建图的过程, 比如推着小车走一遍建立地图就是一轨迹, 如果需要再走一遍就可以再开启一条轨迹,ID++
trajectory_builders_.push_back(common::make_unique<CollatedTrajectoryBuilder>(sensor_collator_.get(), trajectory_id,expected_sensor_ids, CreateGlobalTrajectoryBuilder2D( std::move(local_trajectory_builder), trajectory_id, static_cast<PoseGraph2D*>(pose_graph_.get()), local_slam_result_callback))); 我们来扒皮抽筋
其中有一个CreateGlobalTrajectoryBuilder2D()函数, 一个CollatedTrajectoryBuilder类,我们可以逐一跟进
第一CreateGlobalTrajectoryBuilder2D()是为了返回一个GlobalTrajectoryBuilder
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D( std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder, const int trajectory_id, mapping::PoseGraph2D* const pose_graph, const TrajectoryBuilderInterface::LocalSlamResultCallback& local_slam_result_callback) { return common::make_unique< GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>( std::move(local_trajectory_builder), trajectory_id, pose_graph, local_slam_result_callback); } GlobalTrajectoryBuilder和CollatedTrajectoryBuilder都继承于TrajectoryBuilderInterface,其中在collated_trajectory_builder.h
CollatedTrajectoryBuilder( sensor::CollatorInterface* sensor_collator, int trajectory_id, const std::set<SensorId>& expected_sensor_ids, std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder); wrapped_trajectory_builder就通过为用子类指针赋值基类,使得最终调用为GlobalTrajectoryBuilder的方法,我们需要进入到这个类中进行分析