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的方法,我们需要进入到这个类中进行分析

1. collated_trajectory_builder