啃一下谷歌优秀的激光SLAM开源框架-Cartographer. 这个框架算法简单,但是程序部分太多需要学习的地方了.不论是整体框架的结构,还是数据的使用,都是非常优美的.不愧是大公司啊.接下来记录一下每天学习的内容和心得,督促自己坚持下去!

node_main.cc是整个Cartographer程序的入口，用来调用整个Cartographer进程。以最基础的单线雷达和轮速计为例。

整体的代码开始是在Run函数中实现的。

1. Run函数

void Run() {constexpr double kTfBufferCacheTimeInSeconds = 10.;tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};// 开启监听tf的独立线程tf2_ros::TransformListener tf(tf_buffer);NodeOptions node_options;TrajectoryOptions trajectory_options;// c++11: std::tie()函数可以将变量连接到一个给定的tuple上,生成一个元素类型全是引用的tuple// 读取Lua文件内容,把Lua文件内容给到node_options和trajectory_optionsstd::tie(node_options, trajectory_options) =LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);// MapBuilder类是完整的SLAM算法类// 包含前端(TrajectoryBuilders,scan to submap) 与 后端(用于查找回环的PoseGraph) auto map_builder =cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);//在map_builder.cc中实现，工厂函数//在这里,实例化一个MapBuilder, 而MapBuilder是MapBuilderInterface的子类//MapBuilder的AddTrajectoryBuilder实例化了CollatedTrajectoryBuilder // c++11: std::move 是将对象的状态或者所有权从一个对象转移到另一个对象, // 只是转移, 没有内存的搬迁或者内存拷贝所以可以提高利用效率,改善性能..// 右值引用是用来支持转移语义的.转移语义可以将资源 ( 堆, 系统对象等 ) 从一个对象转移到另一个对象, // 这样能够减少不必要的临时对象的创建、拷贝以及销毁, 能够大幅度提高 C++ 应用程序的性能.// 临时对象的维护 ( 创建和销毁 ) 对性能有严重影响.// Node类的初始化, 开启订阅,发布topic和service,将ROS的topic传入SLAM, 也就是MapBuilderNode node(node_options, std::move(map_builder), &tf_buffer,FLAGS_collect_metrics);// 如果加载了pbstream文件, 就执行这个函数,为定位if (!FLAGS_load_state_filename.empty()) {node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);}// 使用默认topic 开始轨迹if (FLAGS_start_trajectory_with_default_topics) {node.StartTrajectoryWithDefaultTopics(trajectory_options);}::ros::spin();// 结束所有处于活动状态的轨迹node.FinishAllTrajectories();// 当所有的轨迹结束时, 再执行一次全局优化node.RunFinalOptimization();// 如果save_state_filename非空, 就保存pbstream文件if (!FLAGS_save_state_filename.empty()) {node.SerializeState(FLAGS_save_state_filename,true /* include_unfinished_submaps */);}
}}  // namespace
}  // namespace cartographer_ros

Run函数主要做了一下几件事：

1. 读取Lua配置文件中的内容，确定节点构造的方式和轨迹构造的方式与参数。

2. 实例化map_builder，map_builder是完整的SLAM算法类，包含了前端和后端。具体时间方式是通过工厂模式。

3. 初始化Node，通过初始化Node，开启订阅，发布topic与service，还将topic带的传感器数据传入MapBuilder。

4. 判断是否为定位还是建图，并开启轨迹

5. 死循环，不停地接受topic并运行Cartographer

6. 结束时停止所用传感器数据的订阅，并且执行一次全局优化，保存pbstream地图文件

2. 读取配置参数

其中std::tie很有意思,可以实现多个不同类型的返回值. 很多时候我们想通过一个函数丢出去多个结果,但一个函数只能有一个返回值,于是我们可以用std::make_tuple把多个返回值打包成std::tuple类型的数据,这时候返回值只是tuple类型了,所以没有违反只能返回一个返回值的规定.这点很类似Python中的pickle和tuple,啥都可以装在一起丢出去. 实现文件在node_options.cc

/*** @brief 加载lua配置文件中的参数* * @param[in] configuration_directory 配置文件所在目录* @param[in] configuration_basename 配置文件的名字* @return std::tuple 返回节点的配置与轨迹的配置*/
std::tuple LoadOptions(const std::string& configuration_directory,const std::string& configuration_basename) {// 获取配置文件所在的目录auto file_resolver =absl::make_unique(std::vector{configuration_directory});// 读取配置文件内容到code中const std::string code =file_resolver->GetFileContentOrDie(configuration_basename);// 根据给定的字符串, 生成一个lua字典cartographer::common::LuaParameterDictionary lua_parameter_dictionary(code, std::move(file_resolver));// 创建元组tuple,元组定义了一个有固定数目元素的容器, 其中的每个元素类型都可以不相同// 将配置文件的内容填充进NodeOptions与TrajectoryOptions, 并返回return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary),CreateTrajectoryOptions(&lua_parameter_dictionary));
}

3. 构建地图构建器

Cartographer_ros和Cartographer是两个部分,一个是数据处理与分配,一个才是真正的Cartographer算法代码的部分,代码上把ros和算法库分得很开,让我们移植和开发很容易.那么如何让ros数据和Cartographer算法建立联系呢?第一步就是地图构建器.

地图构建器的大致作用是调用Cartographer的算法.

地图构建器通过配置文件中node_options中map_builder_options部分去初始化一个地图.这个地图构建器的作用以后再说.先来看看他是怎么实现的.

由node_main.cc调用map_builder中的CreateMapBuilder函数,这个函数只有一个参数,就是上一行从lua中读取的配置文件内容. 进入map_builder.cc中:

// 工厂函数,生成接口API
std::unique_ptr CreateMapBuilder(const proto::MapBuilderOptions& options) {return absl::make_unique(options);
}

发现这个就是一个接口函数. 但这个函数也有用到一些cpp的技巧,值得学习:

返回值是一个unique_ptr的MapBuilder类型的类,而返回类型却定于为MapBuilder的父类MapBuilderInterface类,这在cpp中是允许的,而且这样做更能让返回值类型更加有包容性,实现工厂模式.

MapBuilder这个类是SLAM算法的入口类十分重要,用来初始化pose_graph,创建轨迹等.会在另一篇中详细介绍.

4. Node类的初始化:

Node类的作用主要是传感器数据的获取和处理,让数据与MapBuilder构建联系,从而使获取的raw sensor data能够灌入Cartographer算法库,实现定位建图等功能.

在node_main.cc中初始化方式如下:

  // Node类的初始化, 开启订阅,发布topic和service,将ROS的topic传入SLAM, 也就是MapBuilderNode node(node_options, std::move(map_builder), &tf_buffer,FLAGS_collect_metrics);

这一行代码也有值得学习的地方,就是std::move这个函数,他通过把某个实例化的类变为右值引用然后直接转移给某个对象,从而实现高效的"转移".

举个简单的不太恰当的例子,你想要我的西瓜,有两种方式,一个是我不远千里坐车给你,还有一种是给西瓜贴上你的名字,别人问我就说我说了不算,问你去. std::move就是后者(如有错请指出哈).所以这样可以直接从一个对象转移到另一对象(贴名字),取消了不必要的临时对象的创建拷贝与销毁(运输西瓜需要位子还要搬上搬下). 对于占用很大的类的转移就很节约开销(一亿吨西瓜咋运啊).大致就这个意思.

Node类的内容在node.cc中,主要作用是实现传感器数据的订阅发布以及初始处理, 以及传递给mapbuilder.具体内容在后面会详细介绍.

5. 开始轨迹与结束轨迹

在上面实例化了Node类之后,我们就可以调用node中的方法去建图. 建图就不用加载地图了,毕竟是建图,所以直接调用node开始轨迹,然后在进入ros中的死循环,不停地接受新的数据,处理并运算,输出结果, 直到按下ctrl+c去终止程序,跳出死循环,执行结束输入数据和进行最终优化.

其实看程序就可以知道,Cartographer的建图和定位是一样的,只是建图的时候不加载地图并且在结束的时候保存地图,定位的时候加载地图,可以不保存地图,也可不进行最终优化.其实我测试的不进行最终优化也是可以的,毕竟定位是实时的,就算最终优化使之前的定位结果有变化,机器人也回不去了.所以我认为是可以去掉的.

  // 如果加载了pbstream文件, 就执行这个函数,为定位if (!FLAGS_load_state_filename.empty()) {node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);}// 使用默认topic 开始轨迹if (FLAGS_start_trajectory_with_default_topics) {node.StartTrajectoryWithDefaultTopics(trajectory_options);}::ros::spin();// 结束所有处于活动状态的轨迹node.FinishAllTrajectories();// 当所有的轨迹结束时, 再执行一次全局优化node.RunFinalOptimization();// 如果save_state_filename非空, 就保存pbstream文件if (!FLAGS_save_state_filename.empty()) {node.SerializeState(FLAGS_save_state_filename,true /* include_unfinished_submaps */);}

LoadState作用是加载地图文件.这个地图不同于可以可视化的地图,这个地图里面包含了位姿图pose_graph,传感器数据和landmark_pose等其他信息,不单单是一个地形图一样的地图.调用的最终函数是Cartographer算法部分的map_builder.cc中的同名函数,调用流程一环套一环(Cartographer整体框架就是这样,复杂但都是必要的).调用的流程如下:

只有最后一层的map_builder.cc才是Cartographer算法部分的内容,才是真正实现加载地图的功能. 这部分程序又臭又长,大家可以自己看看,实现功能加载posegraph和旧地图的传感器数据与landmark.

StartTrajectoryWithDefaultTopics实际上是调用了node.cc的AddTrajectory,去让map_builder创建一个轨迹,并且新增位姿估计器,传感器数据采样器,订阅topic以及调用回调函数的功能. 这个函数建立了数据与算法的统一. 详细会在Node中解析.

FinishAllTrajectories调用node.cc中的FinishTrajectoryUnderLock去结束传感器订阅,然后调用map_builder的FinishTrajectory()进行轨迹的结束

node::RunFinalOptimization调用map_builder的pose_graph的RunFinalOptimization实现结束建图后所有位姿图的最终优化.

由此可见, Node类通过类方法,实现了传感器数据的处理与使用.具体的方式是用了sensor_bridge和map_builder_bridge,把传感器数据转换并且给了Cartographer的算法部分, 实现了建图与定位.