一、前言回顾一下在哪里见过MapBuilder先回忆一下这个类的创建在src/cartographer _ros/cartographer_ros/cartographer_ros/node_main.cc 的 run 函数中, 可看到下代码:1.初始构造过程node_main.cc// MapBuilder类是完整的SLAM算法类 // 包含前端(TrajectoryBuilders,scan to submap) 与后端(用于查找回环的PoseGraph) auto map_builder cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);其调用了一个工厂函数创建一个MapBuilder类对象该函数的实现于src/cartographer/cartographer/mapping/map builder.cc文件中实现,代码如下:std::unique_ptrMapBuilderInterface CreateMapBuilder( const proto::MapBuilderOptions options) { return absl::make_uniqueMapBuilder(options); }所谓的工厂函数简单的说就是根据传入的参数返回不同的类对象如上面代码就是表示根据传入的options参数构建一个MapBuilder实例返回其指针注意这里返回的指针指向MapBuilder实例但是返回的类型为其父类std.unique ptrMapBuilderinterface返回的实例对象指针变量为auto map builder.然后node_main.cc的run函数,利用map builder变量构建Node类对象node,代码如下:// Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder Node node(node_options, std::move(map_builder), tf_buffer, FLAGS_collect_metrics);2. Node类的构造函数Node::Node( const NodeOptions node_options, std::unique_ptrcartographer::mapping::MapBuilderInterface map_builder, tf2_ros::Buffer* const tf_buffer, const bool collect_metrics) : node_options_(node_options), map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) // step:1 声明需要发布的topic // step:2 声明发布对应名字的Ros服务并将服务的发布器放入到vector容器中 // step:3处理之后的点云的发布器 // step:4 进行定时器与函数的绑定定时发布数据tf2_ros::Buffer* const tf_buffer用于监听tf,数据预预处理SensorBridge类经常有用到。const bool collect metrics是否进行评估。对于这两个参数还是很好理解但是传入的map builder变量用于构建MapBuilderBridge对象map_builder_bridge_而并不是直接赋值给Node的成员变量了。总的来说auto map_builder参数用于构建MapBuilderBridge对象其构建的对象为Node中的成员变量MapBuilderBridge map_builder_bridge_。3.MapBuilderBridge类的构造函数该构造函数位于 src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 文件中/** * brief 根据传入的node_options, MapBuilder, 以及tf_buffer 完成三个本地变量的初始化 * * param[in] node_options 参数配置 * param[in] map_builder 在node_main.cc中传入的MapBuilder * param[in] tf_buffer tf_buffer */ MapBuilderBridge::MapBuilderBridge( const NodeOptions node_options, std::unique_ptrcartographer::mapping::MapBuilderInterface map_builder, tf2_ros::Buffer* const tf_buffer) : node_options_(node_options), map_builder_(std::move(map_builder)), tf_buffer_(tf_buffer) {}从这里可以看到初始化列表中的map_builder_(std:.move(map_builder))代码。总的来说一路下来map_builder实际被赋值给了Node.map_builder_bridge_.map_builder_变量。二、MapBuilderInterface复习了前面的内容。下面看一下MapBuilder通过map_builder.h文件可以知道其继承于MapBuilderInterface先学习这里class MapBuilder : public MapBuilderInterfaceMapBuilderInterface这个类实现于src/cartographer/cartographer/mapping/map_builder_interface.h从命名上可以明显的看出这是一个接口类声明了大量纯虚拟函数参数后面加上0是为了告诉编译系统这是纯虚函数这里简单描述一下纯虚函数和与函数的区别1.纯虚函数只有定义没有实现虚函数既有定义也有实现的代码。2.报春纯虚函数的类不能定义其对象而包含虚函数的则可以另外看到下面代码using LocalSlamResultCallback TrajectoryBuilderInterface::LocalSlamResultCallback; using SensorId TrajectoryBuilderInterface::SensorId;这里的using是C11的新用法简单理解为取别名注意这里的LocalSlamResultCallback为一个函数指针具体定义src/cartographer/cartographer/mapping/trajectory_builder_interface.h里如下所示后续在详细讲解using LocalSlamResultCallback std::functionvoid(int /* trajectory ID */, common::Time, transform::Rigid3d /* local pose estimate */, sensor::RangeData /* in local frame */, std::unique_ptrconst InsertionResult);三、proto简介现在再回到头文件src/cartographer/cartographer/mapping/mapbuilder.h文件,来查看 class MapBuilder,该类包含前端(TrajectoryBuilders,scan to submap)与后端(用于查找回环的PoseGraph)的完整的SLAM。不过该头文件需要讲解的东西太多了这样一看不知道从哪里下手那么就来看map_builder.cc 文件吧.该头文件开头部分可以看的如下Cartographer 工程的命名空间使用是十分规范的比如之前分析 src/cartographer_ros 下代码的时候其最外层的命名空间为cartographer_ros现在分析 src/cartographer 的代码可以看到其命名空间为cartographer另外其二级命名空间跟文件夹路径相关如src/cartographer/cartographer/mapping 下的代码都是使用namespace cartographer { namespace mapping { }}作为命名空间另外这里还使用了匿名空间表示再该命名空间内的变量与函数等只能在本文件中使用。另外这里提及两个内容:1在src/cartographer/cartographer/mapping/prot目录下可以看到很多.proto后最的文件其存储的都是参数信息如其下的map_builder_options.proto文件内容如下:syntax proto3; //定义这个文件的语法是proto3 import cartographer/mapping/proto/pose_graph_options.proto; //包含内容 package cartographer.mapping.proto; //命名空间 //参数编写为【字段类型 字段名称 字段序号】 message MapBuilderOptions { //类名 bool use_trajectory_builder_2d 1; // bool use_trajectory_builder_3d 2; // Number of threads to use for background computations. int32 num_background_threads 3; PoseGraphOptions pose_graph_options 4; // Sort sensor input independently for each trajectory. bool collate_by_trajectory 5; }在编译过程中其会被生成c类文件如该文件对应生成的类文件为build_isolated/cartographer/install/cartographer/mapping/proto/map_builder_options.pb.h, 其为google::protobuf.:Message的派生类且生成过程中会为每个.proto定义的参数自动生成三个函数举例如下:clear_num_background_threads() //恢复变量num_background_threads默认设置 num_background_threads() //获取变量num_background_threads内容 set_num_background_threads(::google::protobuf::int32 value)//设置变量num_background_threads内容总的来说就是把配置文件使用.proto文件编写只需要配置变量名然后通过指令就可以生成c(java,python)的类文件了其包含了对于配置参数的一些基本操作。这里只做一个简单的介绍有兴趣深入了解的朋友可以查阅相关的一些资料深入了解。三、MapBuilder类的构造函数MapBuilder构造函数实现于 src/cartographer/cartographer/mapping/map_builder.cc 文件中,注释下:/** * brief 保存配置参数, 根据给定的参数初始化线程池, 并且初始化pose_graph_与sensor_collator_ * * param[in] options proto::MapBuilderOptions格式的 map_builder参数 */ MapBuilder::MapBuilder(const proto::MapBuilderOptions options) : options_(options), // 根据num_background_threads参数构建一个线程池 thread_pool_(options.num_background_threads()) { // param: num_background_threads // 异或操作相同为1不同为1:表示2d或者3d追踪有且只有一个启动 CHECK(options.use_trajectory_builder_2d() ^ options.use_trajectory_builder_3d()); // 2d位姿图(后端)的初始化 if (options.use_trajectory_builder_2d()) { pose_graph_ absl::make_uniquePoseGraph2D( options_.pose_graph_options(), absl::make_uniqueoptimization::OptimizationProblem2D( options_.pose_graph_options().optimization_problem_options()), thread_pool_); } // 3d位姿图(后端)的初始化 if (options.use_trajectory_builder_3d()) { pose_graph_ absl::make_uniquePoseGraph3D( options_.pose_graph_options(), absl::make_uniqueoptimization::OptimizationProblem3D( options_.pose_graph_options().optimization_problem_options()), thread_pool_); } // 在 cartographer/configuration_files/map_builder.lua 中设置 // param: MAP_BUILDER.collate_by_trajectory 默认为false if (options.collate_by_trajectory()) { sensor_collator_ absl::make_uniquesensor::TrajectoryCollator(); } else { // sensor_collator_初始化, 实际使用这个 sensor_collator_ absl::make_uniquesensor::Collator(); } }该构造函数首先进行了线程池thread_pool_的创建然后对位姿图(后端)初始化最后返回一个sensor:.Collator类型指针因为collate_by_trajectory参数默认为false。四、线程池构建线程池的代码为 thread_pool_(options.num_background_threads(),在头文件src\cartographer\cartographer\mapping\map_builder.h里thread_pool_的定义private: const proto::MapBuilderOptions options_; common::ThreadPool thread_pool_; // 线程池thread_pool_为ThreadPool的实例化,ThreadPool成员函数定义在 src/cartographer/cartographer/common/thread_pool.cc 文件中其构造函数如下// 根据传入的数字, 进行线程池的构造, DoWork()函数开始了一个始终执行的for循环 ThreadPool::ThreadPool(int num_threads) { CHECK_GT(num_threads, 0) ThreadPool requires a positive num_threads!; absl::MutexLock locker(mutex_); for (int i 0; i ! num_threads; i) { pool_.emplace_back([this]() { ThreadPool::DoWork(); }); } } 其中的pool_ // 线程池 std::vectorstd::thread pool_ GUARDED_BY(mutex_);代码比较简单就是根据num_threads创建线程存储于pool_变量之中这里需要注意ThreadPool::DoWork()内部为一个循环函数也就是说线程被创建出来之后就已经再运行了当检检测到标志位为 cartographer::common::ThreadPool::running_false, 则会跳出循环。这里的pool_变量的定义分为两部分基础部分std::vectorstd::thread pool_表明pool_是vector容器里面存储着thread类的线程对象。注释部分GUARDED_BY(mutex_):GUARDED_BY注解关键字表达「受... 保护」的语义,mutex_是保护pool_的互斥锁线程池的pool_是多线程共享的变量比如主线程可能添加线程、工作线程可能被析构函数 join如果不加锁直接访问。本篇小节这里再提及一个MapBuilder 的构造函数是在执行node_main.cc文件中如下代码:// MapBuilder类是完整的SLAM算法类 // 包含前端(TrajectoryBuilders,scan to submap) 与 后端(用于查找回环的PoseGraph) auto map_builder cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);构建的因为这里创建了一个MapBuilder对象也就是同时完成了线程池的初始化。接下来会讲解-下Cartographer 的追踪时如何开始的。-------------------------------------------------------------------------------------------------------MapBuilder::AddTrajectoryBuilder()上面简单介绍了MapBuilder的构造函数该篇博客主要讲解的是 MapBuilder::AddTrajectoryForDeserialization() 函数首先来回忆一下其是怎么被调用的在src/cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc创建完map_builder 之后// 使用默认topic 开始轨迹 if (FLAGS_start_trajectory_with_default_topics) { node.StartTrajectoryWithDefaultTopics(trajectory_options); }这里StartTrajectoryWithDefaultTopics函数在src/cartographer_ros/cartographer_ros/cartographer_ros/node.cc里// 使用默认topic名字开始一条轨迹,也就是开始slam void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions options) { absl::MutexLock lock(mutex_); // 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息 CHECK(ValidateTrajectoryOptions(options)); // 添加一条轨迹 AddTrajectory(options); }int Node::AddTrajectory(const TrajectoryOptions options) { const std::setcartographer::mapping::TrajectoryBuilderInterface::SensorId expected_sensor_ids ComputeExpectedSensorIds(options); // 调用map_builder_bridge的AddTrajectory, 添加一个轨迹 const int trajectory_id map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); // 新增一个位姿估计器 AddExtrapolator(trajectory_id, options); ........................................................................在这里map_builder_bridge_.AddTrajectory 函数首先调用了 map_builder_-AddTrajectoryBuilder() 创建一条轨迹我们回顾下Node类的map_builder_bridge_在哪里来的// Node类的构造函数声明参数列表 Node::Node( const NodeOptions node_options, // 选项参数const引用避免拷贝 std::unique_ptrcartographer::mapping::MapBuilderInterface map_builder, // 独占指针所有权需要转移 tf2_ros::Buffer* const tf_buffer, // 指针参数const指针指向的对象不可通过该指针修改 const bool collect_metrics) // 布尔参数是否收集指标 // 构造函数初始化列表 : node_options_(node_options), // 初始化node_options_成员 map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) { // 初始化map_builder_bridge_成员 ---------- }用构造函数的参数node_optionsconst NodeOptions类型初始化Node类的成员变量node_options_map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer)用三个参数node_options_、std::move(map_builder)、tf_buffer初始化Node类的成员变量map_builder_bridge_推测是MapBuilderBridge类的对象。------------------------------------------------------------------------------------------------然后看一下map_builder_-AddTrajectoryBuilder()src\cartographer\cartographer\mapping\map_builder.cc/** * brief 创建一个新的 TrajectoryBuilder 并返回它的 trajectory_id * * param[in] expected_sensor_ids 所有需要的topic的名字的集合 * param[in] trajectory_options 轨迹的参数配置 * param[in] local_slam_result_callback 需要传入的回调函数 * 实际上是map_builder_bridge.cc 中的 OnLocalSlamResult() 函数 * return int 新生成的轨迹的id */ int MapBuilder::AddTrajectoryBuilder( const std::setSensorId expected_sensor_ids, //根据配置参数获得期待的传感器类型主要为订阅topic名字 const proto::TrajectoryBuilderOptions trajectory_options, //追踪的配置参数 LocalSlamResultCallback local_slam_result_callback) { //回调函数在介绍之前先了解一下c11的一些知识点/** * c11: static_cast关键字编译时类型检查: static_cast type-id ( expression ) * 该运算符把expression转换为type-id类型, 但没有运行时类型检查来保证转换的安全性 1用于基本数据类型之间的转换, 如把int转换为char, 把int转换成enum, 2把空指针转换成目标类型的空指针 3把任何类型的表达式类型转换成void类型 4用于类层次结构中父类和子类之间指针和引用的转换. * c11: dynamic_cast关键字运行时类型检查: dynamic_cast type-id ( expression ) 该运算符把 expression 转换成 type-id 类型的对象. Type-id必须是类的指针、类的引用或者void * 如果type-id是类指针类型, 那么expression也必须是一个指针 如果type-id是一个引用, 那么expression也必须是一个引用 dynamic_cast主要用于类层次间的上行转换子类到父类和下行转换父类到子类, 还可以用于类之间的交叉转换. 在类层次间进行上行转换时, dynamic_cast和static_cast的效果是一样的 在进行下行转换时, dynamic_cast具有类型检查的功能, 比static_cast更安全. */函数的内部主要分为如下几步(1) 获得轨迹 id因为每条轨迹都会创建一个 CollatedTrajectoryBuilder 对象存储于trajectory_builders_之中其size()就可以用作为 trajectory_id。另外其没有是个设置 pose_graph_odometry_motion_filter 相关参数所以 MotionFilter() 函数未执行。(2) 如果使用3d轨迹①首先创建一个 LocalTrajectoryBuilder3D(前端) 类型智能指针其主要为3D前端的初始化。②尝试通过dynamic_cast函数把 pose_graph_ 原 PoseGraph 类型转换成 PoseGraph3D类型PoseGraph3D为后端优化。③利用前端LocalTrajectoryBuilder3D与后端PoseGraph3D通过CreateGlobalTrajectoryBuilder3D函数构建一个TrajectoryBuilderInterface智能指针对象④结合TrajectoryBuilderInterface智能指针对象与trajectory_options、 sensor_collator_.get()、trajectory_id等参数构建一个std::unique_ptrmapping::TrajectoryBuilderInterface指针对象添加到trajectory_builders_之中。(3) 如果使用2d轨迹①首先创建一个 LocalTrajectoryBuilder2D(前端) 类型智能指针其主要为2D前端的初始化。②尝试通过dynamic_cast函数把 pose_graph_ 原 PoseGraph 类型转换成 PoseGraph2D类型PoseGraph2D为后端优化。③利用前端LocalTrajectoryBuilder2D与后端PoseGraph2D通过CreateGlobalTrajectoryBuilder2D函数构建一个TrajectoryBuilderInterface智能指针对象④结合TrajectoryBuilderInterface智能指针对象与trajectory_options、 sensor_collator_.get()、trajectory_id等参数构建一个std::unique_ptrmapping::TrajectoryBuilderInterface指针对象添加到trajectory_builders_之中。(4) 判断是否为纯定位模式如果是则只保存最近3个submap(老版本默认)通过参数 pure_localization 参数控制。新版本可以通过设置 src/cartographer/configuration_files/trajectory_builder.lua文件中的-- pure_localization_trimmer { -- max_submaps_to_keep 3, -- },参数进行配置先有 pure_localization_trimmer 这个参数然后再配置其中的max_submaps_to_keep默认设置依旧为3。其本质是通过PoseGraph::AddTrimmer() 函数中的 PureLocalizationTrimmer 的实例对象进行控制的。(5)如果给了初始位姿通过 pose_graph_-SetInitialTrajectoryPose 在位姿图中设置初始位姿。(6) 保存轨迹的配置文件每条轨迹都对应一个配置文件 proto::TrajectoryBuilderOptionsWithSensorIds 对象。
cartographer源码阅读四-MapBuilder
一、前言回顾一下在哪里见过MapBuilder先回忆一下这个类的创建在src/cartographer _ros/cartographer_ros/cartographer_ros/node_main.cc 的 run 函数中, 可看到下代码:1.初始构造过程node_main.cc// MapBuilder类是完整的SLAM算法类 // 包含前端(TrajectoryBuilders,scan to submap) 与后端(用于查找回环的PoseGraph) auto map_builder cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);其调用了一个工厂函数创建一个MapBuilder类对象该函数的实现于src/cartographer/cartographer/mapping/map builder.cc文件中实现,代码如下:std::unique_ptrMapBuilderInterface CreateMapBuilder( const proto::MapBuilderOptions options) { return absl::make_uniqueMapBuilder(options); }所谓的工厂函数简单的说就是根据传入的参数返回不同的类对象如上面代码就是表示根据传入的options参数构建一个MapBuilder实例返回其指针注意这里返回的指针指向MapBuilder实例但是返回的类型为其父类std.unique ptrMapBuilderinterface返回的实例对象指针变量为auto map builder.然后node_main.cc的run函数,利用map builder变量构建Node类对象node,代码如下:// Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder Node node(node_options, std::move(map_builder), tf_buffer, FLAGS_collect_metrics);2. Node类的构造函数Node::Node( const NodeOptions node_options, std::unique_ptrcartographer::mapping::MapBuilderInterface map_builder, tf2_ros::Buffer* const tf_buffer, const bool collect_metrics) : node_options_(node_options), map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) // step:1 声明需要发布的topic // step:2 声明发布对应名字的Ros服务并将服务的发布器放入到vector容器中 // step:3处理之后的点云的发布器 // step:4 进行定时器与函数的绑定定时发布数据tf2_ros::Buffer* const tf_buffer用于监听tf,数据预预处理SensorBridge类经常有用到。const bool collect metrics是否进行评估。对于这两个参数还是很好理解但是传入的map builder变量用于构建MapBuilderBridge对象map_builder_bridge_而并不是直接赋值给Node的成员变量了。总的来说auto map_builder参数用于构建MapBuilderBridge对象其构建的对象为Node中的成员变量MapBuilderBridge map_builder_bridge_。3.MapBuilderBridge类的构造函数该构造函数位于 src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 文件中/** * brief 根据传入的node_options, MapBuilder, 以及tf_buffer 完成三个本地变量的初始化 * * param[in] node_options 参数配置 * param[in] map_builder 在node_main.cc中传入的MapBuilder * param[in] tf_buffer tf_buffer */ MapBuilderBridge::MapBuilderBridge( const NodeOptions node_options, std::unique_ptrcartographer::mapping::MapBuilderInterface map_builder, tf2_ros::Buffer* const tf_buffer) : node_options_(node_options), map_builder_(std::move(map_builder)), tf_buffer_(tf_buffer) {}从这里可以看到初始化列表中的map_builder_(std:.move(map_builder))代码。总的来说一路下来map_builder实际被赋值给了Node.map_builder_bridge_.map_builder_变量。二、MapBuilderInterface复习了前面的内容。下面看一下MapBuilder通过map_builder.h文件可以知道其继承于MapBuilderInterface先学习这里class MapBuilder : public MapBuilderInterfaceMapBuilderInterface这个类实现于src/cartographer/cartographer/mapping/map_builder_interface.h从命名上可以明显的看出这是一个接口类声明了大量纯虚拟函数参数后面加上0是为了告诉编译系统这是纯虚函数这里简单描述一下纯虚函数和与函数的区别1.纯虚函数只有定义没有实现虚函数既有定义也有实现的代码。2.报春纯虚函数的类不能定义其对象而包含虚函数的则可以另外看到下面代码using LocalSlamResultCallback TrajectoryBuilderInterface::LocalSlamResultCallback; using SensorId TrajectoryBuilderInterface::SensorId;这里的using是C11的新用法简单理解为取别名注意这里的LocalSlamResultCallback为一个函数指针具体定义src/cartographer/cartographer/mapping/trajectory_builder_interface.h里如下所示后续在详细讲解using LocalSlamResultCallback std::functionvoid(int /* trajectory ID */, common::Time, transform::Rigid3d /* local pose estimate */, sensor::RangeData /* in local frame */, std::unique_ptrconst InsertionResult);三、proto简介现在再回到头文件src/cartographer/cartographer/mapping/mapbuilder.h文件,来查看 class MapBuilder,该类包含前端(TrajectoryBuilders,scan to submap)与后端(用于查找回环的PoseGraph)的完整的SLAM。不过该头文件需要讲解的东西太多了这样一看不知道从哪里下手那么就来看map_builder.cc 文件吧.该头文件开头部分可以看的如下Cartographer 工程的命名空间使用是十分规范的比如之前分析 src/cartographer_ros 下代码的时候其最外层的命名空间为cartographer_ros现在分析 src/cartographer 的代码可以看到其命名空间为cartographer另外其二级命名空间跟文件夹路径相关如src/cartographer/cartographer/mapping 下的代码都是使用namespace cartographer { namespace mapping { }}作为命名空间另外这里还使用了匿名空间表示再该命名空间内的变量与函数等只能在本文件中使用。另外这里提及两个内容:1在src/cartographer/cartographer/mapping/prot目录下可以看到很多.proto后最的文件其存储的都是参数信息如其下的map_builder_options.proto文件内容如下:syntax proto3; //定义这个文件的语法是proto3 import cartographer/mapping/proto/pose_graph_options.proto; //包含内容 package cartographer.mapping.proto; //命名空间 //参数编写为【字段类型 字段名称 字段序号】 message MapBuilderOptions { //类名 bool use_trajectory_builder_2d 1; // bool use_trajectory_builder_3d 2; // Number of threads to use for background computations. int32 num_background_threads 3; PoseGraphOptions pose_graph_options 4; // Sort sensor input independently for each trajectory. bool collate_by_trajectory 5; }在编译过程中其会被生成c类文件如该文件对应生成的类文件为build_isolated/cartographer/install/cartographer/mapping/proto/map_builder_options.pb.h, 其为google::protobuf.:Message的派生类且生成过程中会为每个.proto定义的参数自动生成三个函数举例如下:clear_num_background_threads() //恢复变量num_background_threads默认设置 num_background_threads() //获取变量num_background_threads内容 set_num_background_threads(::google::protobuf::int32 value)//设置变量num_background_threads内容总的来说就是把配置文件使用.proto文件编写只需要配置变量名然后通过指令就可以生成c(java,python)的类文件了其包含了对于配置参数的一些基本操作。这里只做一个简单的介绍有兴趣深入了解的朋友可以查阅相关的一些资料深入了解。三、MapBuilder类的构造函数MapBuilder构造函数实现于 src/cartographer/cartographer/mapping/map_builder.cc 文件中,注释下:/** * brief 保存配置参数, 根据给定的参数初始化线程池, 并且初始化pose_graph_与sensor_collator_ * * param[in] options proto::MapBuilderOptions格式的 map_builder参数 */ MapBuilder::MapBuilder(const proto::MapBuilderOptions options) : options_(options), // 根据num_background_threads参数构建一个线程池 thread_pool_(options.num_background_threads()) { // param: num_background_threads // 异或操作相同为1不同为1:表示2d或者3d追踪有且只有一个启动 CHECK(options.use_trajectory_builder_2d() ^ options.use_trajectory_builder_3d()); // 2d位姿图(后端)的初始化 if (options.use_trajectory_builder_2d()) { pose_graph_ absl::make_uniquePoseGraph2D( options_.pose_graph_options(), absl::make_uniqueoptimization::OptimizationProblem2D( options_.pose_graph_options().optimization_problem_options()), thread_pool_); } // 3d位姿图(后端)的初始化 if (options.use_trajectory_builder_3d()) { pose_graph_ absl::make_uniquePoseGraph3D( options_.pose_graph_options(), absl::make_uniqueoptimization::OptimizationProblem3D( options_.pose_graph_options().optimization_problem_options()), thread_pool_); } // 在 cartographer/configuration_files/map_builder.lua 中设置 // param: MAP_BUILDER.collate_by_trajectory 默认为false if (options.collate_by_trajectory()) { sensor_collator_ absl::make_uniquesensor::TrajectoryCollator(); } else { // sensor_collator_初始化, 实际使用这个 sensor_collator_ absl::make_uniquesensor::Collator(); } }该构造函数首先进行了线程池thread_pool_的创建然后对位姿图(后端)初始化最后返回一个sensor:.Collator类型指针因为collate_by_trajectory参数默认为false。四、线程池构建线程池的代码为 thread_pool_(options.num_background_threads(),在头文件src\cartographer\cartographer\mapping\map_builder.h里thread_pool_的定义private: const proto::MapBuilderOptions options_; common::ThreadPool thread_pool_; // 线程池thread_pool_为ThreadPool的实例化,ThreadPool成员函数定义在 src/cartographer/cartographer/common/thread_pool.cc 文件中其构造函数如下// 根据传入的数字, 进行线程池的构造, DoWork()函数开始了一个始终执行的for循环 ThreadPool::ThreadPool(int num_threads) { CHECK_GT(num_threads, 0) ThreadPool requires a positive num_threads!; absl::MutexLock locker(mutex_); for (int i 0; i ! num_threads; i) { pool_.emplace_back([this]() { ThreadPool::DoWork(); }); } } 其中的pool_ // 线程池 std::vectorstd::thread pool_ GUARDED_BY(mutex_);代码比较简单就是根据num_threads创建线程存储于pool_变量之中这里需要注意ThreadPool::DoWork()内部为一个循环函数也就是说线程被创建出来之后就已经再运行了当检检测到标志位为 cartographer::common::ThreadPool::running_false, 则会跳出循环。这里的pool_变量的定义分为两部分基础部分std::vectorstd::thread pool_表明pool_是vector容器里面存储着thread类的线程对象。注释部分GUARDED_BY(mutex_):GUARDED_BY注解关键字表达「受... 保护」的语义,mutex_是保护pool_的互斥锁线程池的pool_是多线程共享的变量比如主线程可能添加线程、工作线程可能被析构函数 join如果不加锁直接访问。本篇小节这里再提及一个MapBuilder 的构造函数是在执行node_main.cc文件中如下代码:// MapBuilder类是完整的SLAM算法类 // 包含前端(TrajectoryBuilders,scan to submap) 与 后端(用于查找回环的PoseGraph) auto map_builder cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);构建的因为这里创建了一个MapBuilder对象也就是同时完成了线程池的初始化。接下来会讲解-下Cartographer 的追踪时如何开始的。-------------------------------------------------------------------------------------------------------MapBuilder::AddTrajectoryBuilder()上面简单介绍了MapBuilder的构造函数该篇博客主要讲解的是 MapBuilder::AddTrajectoryForDeserialization() 函数首先来回忆一下其是怎么被调用的在src/cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc创建完map_builder 之后// 使用默认topic 开始轨迹 if (FLAGS_start_trajectory_with_default_topics) { node.StartTrajectoryWithDefaultTopics(trajectory_options); }这里StartTrajectoryWithDefaultTopics函数在src/cartographer_ros/cartographer_ros/cartographer_ros/node.cc里// 使用默认topic名字开始一条轨迹,也就是开始slam void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions options) { absl::MutexLock lock(mutex_); // 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息 CHECK(ValidateTrajectoryOptions(options)); // 添加一条轨迹 AddTrajectory(options); }int Node::AddTrajectory(const TrajectoryOptions options) { const std::setcartographer::mapping::TrajectoryBuilderInterface::SensorId expected_sensor_ids ComputeExpectedSensorIds(options); // 调用map_builder_bridge的AddTrajectory, 添加一个轨迹 const int trajectory_id map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); // 新增一个位姿估计器 AddExtrapolator(trajectory_id, options); ........................................................................在这里map_builder_bridge_.AddTrajectory 函数首先调用了 map_builder_-AddTrajectoryBuilder() 创建一条轨迹我们回顾下Node类的map_builder_bridge_在哪里来的// Node类的构造函数声明参数列表 Node::Node( const NodeOptions node_options, // 选项参数const引用避免拷贝 std::unique_ptrcartographer::mapping::MapBuilderInterface map_builder, // 独占指针所有权需要转移 tf2_ros::Buffer* const tf_buffer, // 指针参数const指针指向的对象不可通过该指针修改 const bool collect_metrics) // 布尔参数是否收集指标 // 构造函数初始化列表 : node_options_(node_options), // 初始化node_options_成员 map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) { // 初始化map_builder_bridge_成员 ---------- }用构造函数的参数node_optionsconst NodeOptions类型初始化Node类的成员变量node_options_map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer)用三个参数node_options_、std::move(map_builder)、tf_buffer初始化Node类的成员变量map_builder_bridge_推测是MapBuilderBridge类的对象。------------------------------------------------------------------------------------------------然后看一下map_builder_-AddTrajectoryBuilder()src\cartographer\cartographer\mapping\map_builder.cc/** * brief 创建一个新的 TrajectoryBuilder 并返回它的 trajectory_id * * param[in] expected_sensor_ids 所有需要的topic的名字的集合 * param[in] trajectory_options 轨迹的参数配置 * param[in] local_slam_result_callback 需要传入的回调函数 * 实际上是map_builder_bridge.cc 中的 OnLocalSlamResult() 函数 * return int 新生成的轨迹的id */ int MapBuilder::AddTrajectoryBuilder( const std::setSensorId expected_sensor_ids, //根据配置参数获得期待的传感器类型主要为订阅topic名字 const proto::TrajectoryBuilderOptions trajectory_options, //追踪的配置参数 LocalSlamResultCallback local_slam_result_callback) { //回调函数在介绍之前先了解一下c11的一些知识点/** * c11: static_cast关键字编译时类型检查: static_cast type-id ( expression ) * 该运算符把expression转换为type-id类型, 但没有运行时类型检查来保证转换的安全性 1用于基本数据类型之间的转换, 如把int转换为char, 把int转换成enum, 2把空指针转换成目标类型的空指针 3把任何类型的表达式类型转换成void类型 4用于类层次结构中父类和子类之间指针和引用的转换. * c11: dynamic_cast关键字运行时类型检查: dynamic_cast type-id ( expression ) 该运算符把 expression 转换成 type-id 类型的对象. Type-id必须是类的指针、类的引用或者void * 如果type-id是类指针类型, 那么expression也必须是一个指针 如果type-id是一个引用, 那么expression也必须是一个引用 dynamic_cast主要用于类层次间的上行转换子类到父类和下行转换父类到子类, 还可以用于类之间的交叉转换. 在类层次间进行上行转换时, dynamic_cast和static_cast的效果是一样的 在进行下行转换时, dynamic_cast具有类型检查的功能, 比static_cast更安全. */函数的内部主要分为如下几步(1) 获得轨迹 id因为每条轨迹都会创建一个 CollatedTrajectoryBuilder 对象存储于trajectory_builders_之中其size()就可以用作为 trajectory_id。另外其没有是个设置 pose_graph_odometry_motion_filter 相关参数所以 MotionFilter() 函数未执行。(2) 如果使用3d轨迹①首先创建一个 LocalTrajectoryBuilder3D(前端) 类型智能指针其主要为3D前端的初始化。②尝试通过dynamic_cast函数把 pose_graph_ 原 PoseGraph 类型转换成 PoseGraph3D类型PoseGraph3D为后端优化。③利用前端LocalTrajectoryBuilder3D与后端PoseGraph3D通过CreateGlobalTrajectoryBuilder3D函数构建一个TrajectoryBuilderInterface智能指针对象④结合TrajectoryBuilderInterface智能指针对象与trajectory_options、 sensor_collator_.get()、trajectory_id等参数构建一个std::unique_ptrmapping::TrajectoryBuilderInterface指针对象添加到trajectory_builders_之中。(3) 如果使用2d轨迹①首先创建一个 LocalTrajectoryBuilder2D(前端) 类型智能指针其主要为2D前端的初始化。②尝试通过dynamic_cast函数把 pose_graph_ 原 PoseGraph 类型转换成 PoseGraph2D类型PoseGraph2D为后端优化。③利用前端LocalTrajectoryBuilder2D与后端PoseGraph2D通过CreateGlobalTrajectoryBuilder2D函数构建一个TrajectoryBuilderInterface智能指针对象④结合TrajectoryBuilderInterface智能指针对象与trajectory_options、 sensor_collator_.get()、trajectory_id等参数构建一个std::unique_ptrmapping::TrajectoryBuilderInterface指针对象添加到trajectory_builders_之中。(4) 判断是否为纯定位模式如果是则只保存最近3个submap(老版本默认)通过参数 pure_localization 参数控制。新版本可以通过设置 src/cartographer/configuration_files/trajectory_builder.lua文件中的-- pure_localization_trimmer { -- max_submaps_to_keep 3, -- },参数进行配置先有 pure_localization_trimmer 这个参数然后再配置其中的max_submaps_to_keep默认设置依旧为3。其本质是通过PoseGraph::AddTrimmer() 函数中的 PureLocalizationTrimmer 的实例对象进行控制的。(5)如果给了初始位姿通过 pose_graph_-SetInitialTrajectoryPose 在位姿图中设置初始位姿。(6) 保存轨迹的配置文件每条轨迹都对应一个配置文件 proto::TrajectoryBuilderOptionsWithSensorIds 对象。
