Category: Software Engineering Practices

这篇文章将对cartographer的系统做一个简要总结，前面系列文章已经对各个子模块做了较为详细的分析和介绍。这里再做一点补充说明：全局优化的结果不会反馈到局部优化的过程，所有子地图构建的时候其栅格的坐标系和轨迹的起始点标准坐标系对齐，点云帧的origin作为初始地图的中心。前面总体的分析内容主要基于二维建图过程展开，具体三维建图的逻辑框架基本和二维建图保持一致，这里暂不准备继续在三维建图方面做进一步详细的介绍。 如下图为cartographer系统的结构图，主要有传感器数据模块，局部SLAM和全局SLAM等关键模块， 在这里将对cartographer系统做一下简要的总结： 1、多传感器基于sensor_collator的缓存并实现同步和分发[2][3]；2、点云数据的数据流转和变换，以及同步器range_collator实现多lidar传感器的融合及坐标变换支持(对定义的多个点云帧数据进行聚合并按照时间排序，多个点云传感器的origin也加以记录，以便进行range过滤)[4]；3、基于extrapolator的点云点级别位姿估计，基于点云点时刻位姿和点云点局部坐标变换到trajectory统一坐标系下实现点云点去畸变[5][6]；4、子地图的表示基于元数据limits和具体的栅格数据grid相结合，子地图的坐标系和轨迹坐标原点坐标系方向保持一致。地图创建的中心点为创建时点云帧的原点坐标，在更新的过程中，每一个点云帧的坐标都会对齐到轨迹坐标轴方向后进行更新，更新的时候基于当前点云点的原点作为射线起始点，在地图表示方面，二维概率栅格地图和tsdf表示方法具有不同的含义，但处理流程类似，三维地图构建扩展主要在地图表示和位姿估计时的自由度的扩展上，其主体流程也基本相同，框架也是复用[7]][8][9]；5、基于局部扫描匹配算法实现点云帧位姿更精确估计并更新当前活动子地图[10]；6、全局位姿优化，基于点云帧位姿和子地图位姿实现基于位姿图的全局优化，优化的结果不返回更新到子地图。可视化渲染或地图融合时，可以根据submap的全局位姿优化结果和submap对应的和轨迹坐标轴对齐的地图栅格数据做对应旋转变换实现不同子地图的对齐统一表示[11][12]。 以上为对cartographer的系统的部分关键内容做的一个小结，由于其良好的结构设计，cartographer的工程化的设计和实现在应用落地方面具有较为明显的优势，也可以在此现有架构基础之上做扩展开发考虑将当前的一些先进的SLAM算法集成进来。关于cartographer系统的系列介绍暂时就到这里，有疑问的地方欢迎联系沟通确认或勘误，谢谢。 References

上篇文章主要向大家分析介绍了cartographer系统中基于ceres的位姿图构建的相关方面的细节，本篇将继续cartographer系统中关于全局优化的流程处理中的更多细节问题。也将继续以问答的形式向大家做分析介绍。 问题1：cartographer的实现中，一般一个点云帧会对应到两个子图中，在动态构建位姿图的过程中，是否是两个子图的全局位姿节点和点云帧位姿节点都要建立约束呢？ 回答：计算约束的函数为PoseGraph2D::ComputeConstraintsForNode，在该函数中根据三种情况来计算约束：(1)、根据当前点云帧节点(简称节点)和其相关的正在构建的两个局部地图的全局位姿参数块节点(简称子图节点)之间的约束(INTRA_SUBMAP)；(2)、当前点云帧节点和已经构建完成的子图(标记为完成状态)之间可能的回环约束;（3）、如果当前的子图也刚刚标记为结束状态，则需要更新已有的位姿图点云帧位姿节点和当前子图的约束，其中后面两种约束为INTER_MAP回环约束，将在下一个问题中加以具体的描述。 问题2：回环检测和约束如何实现？ 回环检测的算法在PoseGraph2D::ComputeConstraint函数中进行的实现，其计算涉及到了轨迹内部和轨迹之间的可能的回环检测。具体会根据条件选择局部匹配搜索和全局匹配搜索。关于其约束构建的具体计算参考ComputeConstraint，下面对其具体的细节做相关分析说明。 其中计算这些constraints的过程也是作为异步背景线程中来运行的。具体更多的实现可以参考ConstraintBuilder2D类中的更多的代码实现细节。 问题3：全局地图的表示以及更新的方法细节如何实现？位姿图进行全局优化后如何更新地图的表示，以及如何更新子地图和点云帧的全局位姿信息？ 回答：cartographer_node是cartographer在ROS中最主要的运行节点，在其内部运行了cartographer核心库的实现，包括SLAM的前端和后端，核心库本身不维护全局唯一的大图，但是其会已优化好的子图数据（包括每个子图的网格内容及其最终优化后的全局姿态）以二进制流（pbstream 格式）的形式发送给接收融合节点，如cartographer_occupancy_grid_node，在接收融合节点内会创建一个足够大的消息nav_msgs/OccupancyGrid，并将子地图进行融合后，以/map的ros topic发布出来。 问题4：位姿图优化一般作为背景线程，在什么时候被触发？如何支持多轨迹联合全局优化？ 回答：在cartographer的内部实现中，基于线程池和任务队列的方式进行位姿图节点的更新，约束的计算等计算任务较重的计算逻辑，在cartographer中节点(点云帧和子图都有对应的trajectory_id属性，在进行全局优化的时候，不同轨迹的点云帧节点和子图数据更新插入到位姿图的逻辑是统一的接口。位姿图优化的是所有轨迹的节点和子子图节点和约束边的整体全局多轨迹位姿图。在全局的多轨迹地图点云节点联合位姿优化中，已经冻结的轨迹中的点云帧节点和子地图节点在位姿图节点中将会被设置为常量，不参与进一步的优化，也有利于效率的进一步提高。 References

这篇文章将向大家介绍cartographer系统中2D SLAM中的扫描匹配算法，该算法基于姿态外推器的车体位姿估计初始值和点云帧信息实现了位姿估计的进一步优化。下面将对其实现代码和注释做具体的说明分析。 首先是ScanMatch算法调用前的一些预处理，其代码和注释说明如下。 关于ceres的非线性优化方法的一点说明，其采用了自定义的损失函数和正则化的约束损失函数相结合的损失函数定义，自定义的损失函数的总体思路是对点云点基于位姿做坐标变换后和较为准确的地图数据做对照计算，如点云点总体上处理在概率值较大的或TSDF值接近0的栅格位置附近，则损失函数较小，否则偏移越大损失越大，通过调整位姿使得损失函数较小时的位姿为更加和地图匹配和对齐的位姿，具体实现上通过位姿损失函数和正则化约束函数采用ceres非线性优化库实现对位姿的优化求解。代码片段示意如下： 基于占用概率的loss function定义的部分关键代码片段如下。 References

本篇文章将向大家介绍TSDF(Truncated Signed Distance Field，截断符号距离场)表示的二维栅格建图实现，TSDF2D和ProbabilityGrid是两种不同的2D建图的地图表示方法，TSDF 对于三维重建和平滑表面表示特别有用，TSDF用在平面应用场景的作用（以及和probabitliy的应用对比待分析）。接下来也将以问答的形式来介绍相关的技术实现原理。 问题1：如何更加直观理解TSDF的定义，其表示二维地图的几何含义和基于occupancy的概率栅格地图表示法有什么不同？最好有可视化的直觉感官方面的图示解释，以及相关关键代码和注释的说明。 回答：TSDF定义示意图如图1所示，origin为点云坐标定义的原点(具体的定义参考之前的激光点云数据流转和变换分析一文)，示意图中假设橙色的曲线为物体的二维截面的表面(如室内的桌腿)。紫色的线条为origin到点云点的射线，其中会穿越物体的表面一段距离(truncation_distance，图中的surface back双线箭头所指的ray部分)。更新时需要计算权重，这里一般权重为综合度量，第一个因素为ray的方向和物体表面法线的夹角，如果重叠说明ray垂直扫描物体的表面，否则如果夹角加大(如掠射的时候)，则点云点返回的强度(intensities)一般也会较小，一般精确度也会受影响；第二个因素为距离因素，一般距离越小精度也越高。综合因素可以考虑设计为两个因素的乘积(如上图中两条紫色线条的粗细代表了权重，粗线条由于是垂直扫描，且距离也更近一点，因此权重高)。设置物体背面的负值区域的原因在于由于物体表面的背面的具体几何形态未知，如系统之前运行没有经过，或者基于动态变化的场景，因此设置truncation_distance的半径区域为默认的物体的背面，设置一定的安全空间以便后续更新，也为导航提供更加安全的路径规划。 问题2：二维平面的点云点对应的法线如何定义和求解？ 回答：在单个点云点所对应的物体或障碍物外表的水平曲线的切线对应的垂线为法线，如基于点云点的法线估计前，通常会先对点云点按相对于传感器的角度进行排序。这使得邻近点的查找非常高效，因为角度上相邻的点通常在空间上也相邻。 问题3：整个建图的整体思路流程以及关键实现算法？ 回答：在点云点的坐标，坐标原点以及点云点法线估计值的基础上进行地图的更新逻辑的思路为基于权重的更新，栅格已有TSDF的值和权重和当前基于点云点信息计算出的TSDF的值和权重进行加权和更新。 这里权重的计算逻辑如下： 更新TSDF的更新方法代码和注释片段如下： 关于全局地图信息的维护等等，由于cartographer系统中面向对象的代码可复用良好的设计模式的使用，整个局部SLAM和全局SLAM的框架结构是复用的，和ProbabilityGrid的地图方法保持一致。具体可以参考前面文章的描述。 References

这篇文章主要的内容为cartographer系统中2D建图相关代码类图关系结构和应用功能的简要分析说明，以总览性的方式向大家介绍2D建图基础类的定义，应用功能及类对象间相互通信和依赖关系。具体的2D建图表示方法和详细逻辑将在后续专门的文章中加以详细分析阐述，欢迎大家关注并期待任何形式的意见和建议，谢谢。 如室内地面等二维应用场景中，cartographer采用基于Grid栅格的形式来表示地图，如上类结构图所示，下面对相关类的定义，实现和相互通信和依赖关系做相关说明。 1、二维地图的表示方法所涉及的相关类中，Grid2D为表示栅格结构的基础类，其中：(1)、MapLimits类型变量limits_表示了地图结构基本信息的元数据，该对象中的resolution变量表示每个cell单元格的物理分辨率尺寸，如0.05则表示每个单元格的长宽物理尺寸为0.05米(0.05米/像素，类似地图中的scale比例尺度），max_代表MapLimits类对象x，y方向上的的最大物理尺寸，CellLimits类为单元格数目的简单封装类，cell_limits对象保存了栅格地图横向和纵向单元格数目。以上三个变量的关系通过公式表达为：max_.x() = min_x + cell_limits_.num_x_cells * resolution_。(2)、Grid2D类的成员函数GrowLimits实现地图的动态增长，如果给定点超出了当前地图的边界时，通过扩展地图的方法将给定点纳入到新的地图边界内；(3)、correspondence_cost_cells_为保存的具体栅格值变量，其类型为uint16短整型数组，具体该值的表示含义将会根据具体的地图类型不同而不同，后面将会根据地图类型做专门具体的介绍。 2、实际中具体会用到的二维地图类别有两种，其中：(1)、ProbabilityGrid为概率栅格地图类，表示栅格地图中单元格位置是否存在物体或障碍物某部分的表面的概率；(2)、TSDF2D为截断有符号性距离场(Truncated Signed Distance Field, TSDF)栅格地图类，其表示栅格里物体表面的距离值，关于具体的表示方法和含义将会在专门的文章中做相关分析说明。 3、2D建图的入口类为ActiveSubmaps2D，如图1所示，其主要的成员变量和成员函数说明如下：(1)、submaps_为Submap2D类型的指针数组，Submap2D为2D子地图的具体的实例类别，其grid_成员变量指向Grid2D基础类，在实际的系统运行时，会根据配置初始化具体的地图类别；(2)、range_data_inserter_成员变量的类型为接口类型RangeDataInserterInterface指针(C++里接口为纯虚类)，该接口可实例化继承类重载了成员变量函数Insert，以实现具体的地图更新逻辑，该变量实例化类型会和地图实例化类型相匹配，trajectory_builder_2d.lua文件中就对上述两个变量类型进行了设置，这里截取相关的一段如下: ActiveSubmaps2D中最多维护了两个活跃的子地图，两个子地图有一定的重叠，可以参考如下的代码和注释： References

上篇文章介绍到传感器消息在到达SLAM核心算法模块之前的数据预处理的方法和过程，这篇文章介绍激光点云数据在cartographer系统中统一适配处理的方法细节。 由于精心设计的代码框架，cartographer系统能较好适配多种机器人不同传感器组合。如支持兼容LaserScan 2D激光雷达(室内地面平面空间轮式机器人的配置，如仓库AGV Automated Guided Vehicle，小车自动导引运输系统小车，扫地机器人等)和PointCloud 3D激光雷达(空中无人机3D建图，室外复杂地面3维空间场景自主导航机器人，如自动泊车应用等)，而且系统自动适配机器人本体设备上多个3D Lidar传感器的点云数据的融合以支持更高精度建图(RangeDataCollator类的成员变量expected_sensor_ids_的支持，可以通过配置文件定义激光雷达的数目)。 当数据流转到LocalTrajectoryBuilderND(先以LocalTrajectoryBuilder2D类为例进行介绍)的AddRangeData函数时，就到达了SLAM算法处理的核心部分。在该函数里，主要处理逻辑有： 1、首先调用range_data_collator_.AddRangeData()函数实现一到多个Lidar传感器的点云数据的同步和融合，返回同步后的逻辑点云帧。该函数内部主要调用了CropAndMerge函数将不同来源点云传感器的数据在时间重叠区域外进行切割裁剪，在时间重叠区域内进行合并融合。裁剪融合后逻辑点云帧中部分点云点由于截至时间的变化，其相对时间信息需要做一个校正，具体的代码和文字注释如下： 下面部分的代码实现了融合区间之外的点云帧的数据的切分，其中data为引用数据类型，直接修改了原始电源帧的数据，保留裁剪剩下的部分供和下一次融合使用。最后还对多个传感器融合后的点云数据结果根据时间进行了严格的排序。 2、在对点云数据帧进行裁剪融合同步后，对返回的逻辑点云帧的每一个点云点对应的时间启用姿态外推器进行位姿估计(关于姿态外推器的实现原理将在专门的文章中进行介绍[1])，将估计的结果存入到range_data_poses的数组里。紧接着将基于估计出的点云点时刻的位姿对点云点进行坐标变换，变换到统一的全局坐标系下的坐标以解决点云扫描时间的不同形成的点云位置畸变。统一坐标后将点云点测得的距离按range区间进行过滤。具体的代码和分析注释如下： 在对当前逻辑点云帧进行了数据处理后，会将状态变量num_accumulated_进行递增，当满足条件num_accumulated_ >= options_.num_accumulated_range_data()时，就将多个逻辑点云帧的数据进行聚合并调用局部位姿优化得出局部位姿优化结果。具体的形成聚合数据的方法参见下面的代码和分析。 这里对TransformToGravityAlignedFrameAndFilter变换做一下具体说明。在进行变换之前，所有点云点accumulated_range_data_的坐标已经变换到全局的坐标系下(通过点云点时刻time车体相对于起始世界坐标的位姿对点云点相对于车体坐标系下的坐标点进行坐标变换实现，既点云点去畸变的实现，详见第2部分的说明)。这里的变换为两个变换的组合变换，关于这两个变换分别说明如下。 1、range_data_poses.back().inverse()为最后一个点云点对应的车体位姿的逆变换，其作用于当前逻辑点云帧的所有点云点上，将最后一个点云点的坐标定义坐标原点且坐标轴为标准方向，基于此坐标系定义，将点云点坐标进行变换到局部的坐标系下。range_data_poses基于ScanMatch算法获得较为精确的基础位姿，和局部时间段基于ImuTracker估计所得位姿变化量叠加实现点云点时刻的位姿估计。 2、gravity_alignment.cast<float>() 为重力对齐变换，其为ImuData数据根据陀螺仪的角速度和加速度计加速度进行积分和双重校正得以实现，返回的是time时刻预估的方向旋转四元数估计orientation_(和标准的坐标轴方向不一致，定义了车身的姿态的方向角度部分以及重力相对于垂直方向的角度偏移)。gravity_alignment的估计只用到了ImuData的数据，比range_data_poses.back()中的重力方向的估计更加的鲁棒和稳定，所以用gravity_alignment变换去和重力方向做对齐操作。 gravity_alignment.cast<float>() * range_data_poses.back().inverse()为两者变换的叠加，实现了将点云点转换到点云帧局部标准坐标系下后再根据当时的重力方向进行对齐(相当于将标准坐标z轴对齐到重力方向后点云点的坐标值的变化）。 将点云点坐标进行上述的变换后，AddAccumulatedRangeData函数将实现具体SLAM算法的核心部分，主要包括：1、基于上述变换后的点云点和时间通过调用函数ScanMatch进行局部位姿优化；2、InsertIntoSubmap函数基于点云数据对局部地图进行更新；3、返回匹配优化和更新后的地图结果的综合信息。关于位姿优化和局部地图更新的具体实现将在后续文章中加以详细介绍，欢迎关注。 References

本篇文章将向大家介绍cartographer中的姿态外推器PoseExtrapolater的实现，姿态外推器主要基于ImuData和/或OdometryData以及局部SLAM优化后的位姿实现较近的未来时刻的车体(机器人本体)的姿态估计。下面将和代码相结合进行较为详细具体的分析介绍。 姿态外推器的主要作用体现在：1、在同步后的点云数据的每一个点云点上都会调用ExtrapolatePose函数预估出即时姿态，以便基于点云坐标的去畸变操作(将点云点相对于车体坐标系的坐标和车体姿态叠加获取点云点在统一坐标系下的坐标从而实现了去畸变的操作，和FAST-LIVO2中的运动补偿实现去畸变的思路和实现方法不同)；2、当点云帧的最后一个点的坐标校正完成后调用ScanMatch函数对基于外推器预测的姿态和点云数据基础上进行位姿优化获得更加精确的姿态，同时调用extrapolator_->AddPose函数将优化后的姿态更新到姿态外推器，姿态外推器基于更加准确的姿态的基础上进行后续时间点的位姿估计。如下图所示，在帧内和帧的边界分别调用了姿态外推器的上述两个函数。 下面就具体的实现代码细节做一些说明。 PoseExtrapolater的成员变量及其含义和作用见下面的代码和对应的注释说明。 AddPose函数在局部SLAM算法优化位姿后调用，通过ScanMatch算法优化后的位姿和对应的时间作为参数调用AddPose函数，具体关于AddPose函数的实现细节及分析请参考如下的代码和注释。 关于姿态外推器中的旋转和位移估计的代码实现，下面的段落文字给出具体的代码和分析注释。 References

这篇文章将向大家介绍cartographer系统中姿态外推器PoseExtrapolator实现分析。姿态外推器可以看成是一种软件逻辑实现的里程计，但是其利用了局部位姿优化的结果及时更新更高精度的位姿，从而有效防止出现位姿的严重累积漂移现象。本篇文章将从两个方面进行介绍，第一部分介绍IMU传感器的基本作用及采用高频ImuData进行预积分估计姿态的方法(主要体现在ImuTracker类里)；第二部分将介绍姿态外推器和局部位姿优化的结果如何结合实现更精确的位姿估计(主要体现在PoseExtrapolator类里)。 惯性测量单元（IMU, Inertial Measurement Unit）传感器包含陀螺仪(主要测量角速度angular_velocity，频率高)和加速度计(主要测量linear_acceleration，频率低)。但在IMU的硬件驱动层一般会对低频率的线性加速度值进行插值或直接填充，以保持和角速度的测量同样的频率输出，因此输出的消息同时有这两个测量值。IMU的消息标准中msg->header.frame_id 是 sensor_msgs::Imu 消息中声明的IMU数据所在的坐标系名称（例如 “imu_link”）,IMU和 tracking_frame 必须共址（The IMU frame must be colocated with the tracking frame，否则由于imu和tracking_frame的位置上平移关系的存在导致车体的线性加速度的估计变得困难），以便IMU相关算法简化计算。 IMU的线性加速度的读数值为一个三维的向量，测量的是比力（specific force），加速度计的测量值里天然包含重力方向的加速度分量(比如车体在平面上静止时IMU的加速度计读数为[0,0,9.8]，当车体做自由落体运动时，加速度计的读数为0。 Imu在做time时刻的位姿估计的时候，其主要估计的是旋转的部分(位姿估计的平移部分主要在姿态外推器中利用预估的线速度和时间差进行估计)。 如上ImuData所述，ImuTracker一般会同时接收到角速度和线性加速度的消息，接收角速度消息时，直接更新角速度变量的值即可(用于后续基于此进行角度增量预估)，在接收线性加速度时的处理稍微复杂一些，下面是具体的分析。 一般的使用方式如下(在姿态外推器中调用），通过旋转姿态估计函数Advance和更新线性加速度观测值值函数AddImuLinearAccelerationObservation实现了旋转角度和重力向量的双重校正。实现了旋转姿态和重力向量的双重校正，使用IMU的陀螺仪测得的角速度和线性加速度计测得的线性加速度两个观测值相互校正和纠偏，实现更加稳定的姿态估计。 References