Cartographer的总体框架与安装试用

Cartographer总体上可以看作是由局部地图更新和全局回环检测两个部分构成。在局部地图更新过程中，通过最优位姿估计，把激光扫描数据插入到当前维护的子图(submap)中。 由于局部地图只使用最近一段时间的传感器数据，所以存在累积误差的问题。这一问题通过全局的闭环检测来加以修正，这还是一个优化问题，Cartographer通过分支定界的方式提高了算法的运行效率。

Cartographer本身是一个C++的库，虽然可以不依赖ROS的环境运行，但为了快速的上手，google还是提供了一个ROS环境下的封装。 关于ROS我们并不陌生，在机器人操作系统之ROS中已经介绍了ROS的基本环境和常用工具。 也通过Turtlebot的建图导航之旅了解了ROS下常用的建图导航技术栈Navigation。 我们的cartographer之旅也将从ROS开始。

本文中，我根据自己的理解解释一下官方的系统框图，然后介绍如何快速的安装试用Cartographer，并简单的分析一下系统运行时的节点和主题。

1. 总体框架

右图是从官方文档上抠的系统框图，Cartographer以激光雷达的扫描数据、里程计位姿、 IMU测量数据、固定坐标系位姿作为输入的传感器数据。

其中，激光雷达的扫描数据是Cartographer的主要处理对象。整个定位建图过程就是根据扫描数据，来优化机器人的位姿估计和地图占用栅格概率。 它通过两个体素滤波器输入到Local SLAM中完成局部地图的构建，生成一个个子图(Submaps)。

剩下的三个传感器数据主要是为了给寻优求解过程提供一个较好的初值，IMU数据是它们的主力。通过IMU跟踪器(IMU Tracker)和位姿推理器(PoseExtrapolator)，为Local SLAM提供初始的位姿估计。 它们的原始数据也会作用于Global SLAM中的稀疏位姿调节器(Sparse Pose Adjustment)。

里程计位姿一般是指根据移动机器人纶组上的编码器估计的机器人位姿，它是一种积分性质的位姿估计，如果机器人在运动过程中出现了打滑、碰撞等无法或者很难建模的情况时，会出现很大的误差， 而且随着时间的流逝不断累积。IMU的测量数据主要是指加表和陀螺测量的线加速度和角速度。根据惯导的理论知识，我们知道IMU是可以比较准确的估计出机器人位姿的。 但是同样会存在累积误差，系统长时间运行时如果不能及时修正就会爆炸。固定坐标系位姿(Fixed Frame Pose)，应该是指类似GPS这样具有全局定位能力的传感器数据。 这类传感器虽然能够提供全局的定位信息，但是精度和动态特性往往比较差。

Cartographer的主体则是由Local SLAM和Global SLAM两部分构成。一些资料和博客中称Local SLAM为前端，Global SLAM为后端。一般SLAM系统中的前端都只使用最近一段时间内的数据， 对算力的需求较小，可以实时计算，但存在累积误差。而后端则主要是为了解决累积误差的问题而存在的，引入了闭环检测等机制，往往是对整个地图和历史轨迹的优化，数据量和计算量上都要大很多， 但好在实时性上没有前端那么高的要求，可以在后台慢慢计算。但也不能慢的太过分，所以Cartogapher针对Global SLAM使用了分支定界的方式进行了优化一定程度上减少了算力的需求。

2. 安装试用

Cartographer本身是一个独立的C++库，但为了快速上手，官方推荐使用ROS环境下的封装。 此外，官方还提供了一个ROS环境下运行Cartographer的文档。关于ROS，我们并不陌生。曾在机器人操作系统之ROS中介绍了ROS的基本环境和常用工具。 也通过Turtlebot的建图导航之旅了解了ROS下常用的建图导航技术栈Navigation。

目前(2020.01.18)ROS官方已经发布了12个版本了，最新的是Melodic。 但由于写作机器人操作系统之ROS和Turtlebot的建图导航之旅时的环境都是第十个发行版Kinetic， 所以这里我们仍然选用Kinetic，其安装过程可以参考ROS的安装和初体验以及官方教程。 想用Melodic的，可以参考官方教程，其安装和使用方式都与Kinetic没有太大的区别。在确认安装好ROS系统之后，我们就可以安装cartographer了。

cartographer官方推荐使用wstool和rosdep构建其ROS运行环境，为了提高构建速度，推荐使用Ninja系统，下面先检查安装相关工具:

        $ sudo apt-get update
        $ sudo apt-get install -y python-wstool python-rosdep ninja-build

接着按照ROS的套路创建工作空间，并使用wstool下载相关源码，下面的指令可能需要科学上网才能正常执行。

        $ cd ~
        $ mkdir catkin_cartographer_ws
        $ cd catkin_cartographer_ws
        $ wstool init src
        $ wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall
        $ wstool update -t src

然后通过rosdep工具解决软件包的依赖问题。下面第二行的指令"sudo rosdep init"如果在安装ROS的时候执行过了，这里再次执行可能会报错，但是我们不用管它。

        $ src/cartographer/scripts/install_proto3.sh
        $ sudo rosdep init
        $ rosdep update
        $ rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y

最后编译安装即可。

        $ catkin_make_isolated --install --use-ninja

但是我在编译过程中，遇到了一个编译错误，按照打印出来的日志信息来看应该是gtest和gmock库的路径出现了问题。 后来根据一个issues的回帖，修改cartographer中FindGMock.cmake文件， 添加gtest和gmock的库搜索

        find_library(GTEST_LIBRARIES
            NAMES gtest
            PATH_SUFFIXES lib
        )
        find_library(GMOCK_A_LIBRARIES
            NAMES gmock
            PATH_SUFFIXES lib
        )

同时修改GMOCK_LIBRARIES列表：

        # list(APPEND GMOCK_LIBRARIES ${CMAKE_THREAD_LIBS_INIT})
        list(APPEND GMOCK_LIBRARIES ${GMOCK_A_LIBRARIES} ${GTEST_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT})

如果编译成功，那么我们就可以加载cartographer的运行环境，并下载bag数据包运行demo了。

        $ source install_isolated/setup.bash
        $ wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag
        $ roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag

如果一切进行的比较顺利，就可以看到如下的动图了。

        $ cd ~/catkin_cartographer_ws/src
        $ ls
        cartographer  cartographer_ros  ceres-solver  CMakeLists.txt

        $ rosnode list
        /cartographer_node
        /cartographer_occupancy_grid_node
        /playbag
        /robot_state_publisher
        /rosout
        /rviz
        $ rosrun rqt_graph rqt_graph