Category: Software Engineering Practices

在cartographer系统中，激光点云作为定位和建图的核心数据，在系统中不同的类对象中的流转和变换(包括坐标变换和数据类型的变换)是最频繁和复杂的，这篇文章将向大家梳理一下cartographer系统中多种类型传感器数据的统一预处理，包括传感器数据类型从标准ROS消息到cartographer内部处理所需数据类型的转换，消息多队列缓存和分发派遣机制等，如对本文理解和分析的过程有疑问或发现有问题的地方欢迎联系作者讨论并校正，谢谢。 1、首先在cartographer_ros的节点中，接收到的点云消息为标准通用的点云消息定义（根据物理传感器的不同，点云的标准消息有三类: sensor_msgs::PointCloud2，sensor_msgs::LaserScan，sensor_msgs::MultiEchoLaserScan，sensor_msgs 包用来专门存放消息的标准定义格式，其中msg.header.stamp定义了点云帧扫描的起始时间)，ROS节点收到点云消息后，在SensorBridge类对象中调用了三个函数名相同(函数名均为ToPointCloudWithIntensities，具体的实现参考msg_conversion.cc代码)但参数不同的重载函数将上述三种不同的点云数据转换成为一致的cartographer内部的PointCloudWithIntensities类型对象，该类型对象结构含有成员变量points和intensities，其中points的每一个点云点含有时间信息（TimedCloudPoint，时间为相对于点云扫描帧结束时间的偏移，为负值），intensities为点云点对应的强度信息，为浮点值数组。 以sensor_msgs::PointCloud2点云消息为例，SensorBridge类的函数HandlePoint2Message对接收到的消息进行处理，其中会通过函数调用ToPointCloudWithIntensities(*msg)进行的转换，在sensor_msgs::PointCloud2类型转换的工程中，用到了较为常用的pcl点云处理库，先将数据转换成pcl::PointCloud<PointXYZT>数据类型（在pcl::PointCloud数据类型汇总，点云点的时间戳为相对于点云帧扫描开始的时间偏移，为正值），然后基于该类型数据转换为PointCloudWithIntensities类型。 2、SensorBridge中通过函数HandleRangefinder将TimedPointCloud数据（PointCloudWithIntensities的points成员变量）转化为TimedPointCloudData类型的数据对象，该转换涉及到了点云数据从传感器坐标系到机器人本体坐标系（车身坐标系）的坐标变换，该变换定义为sensor_to_tracking->cast<float>()，而tracking_frame一般在lua配置文件中定义为“base_link”（一般为车体底盘后面中间位置）。同时TimedPointCloudData的time成员变量值为点云帧扫描结束时间，其origin成员变量定义了Lidar传感器相对于tracking_frame的位置偏移（ sensor_to_tracking->translation().cast<float>()）。将转换后的数据通过调用trajectory_builder_->AddSensorData转入到cartographer的核心库进行后续的算法处理。具体的变换代码为： 3、上面的trajectory_builder_变量的类型为CollatedTrajectoryBuilder，在MapBuilderBridge::AddTrajectory函数里通过调用map_builder_->AddTrajectoryBuilder函数来生成，在map_builder.cc代码文件里的实现片段可供参考： 其中map_builder对象在node_main.cc文件里通过调用cartographer::mapping::CreateMapBuilder(node_options.map_builder_options)进行的初始化），CollatedTrajectoryBuilder类为local_trajectory_builder（局部SLAM，前端）和PoseGraphND（全局SLAM，后端）的对象的集成封装，为SLAM前后端的集成入口。trajectory_builders_为vector数组容器，因此cartographer系统默认支持多个trajectory的构建。在cartographer核心库的实现中，不同的传感器数据将会在一起集中进行队列缓存管理和进一步分发。相关的数据队列缓存管理和分发的对象为sensor_collator_ ，其内部实现中使用了多个队列来分别缓存不同的传感器消息，并基于类似归并排序的思想从多个队列中取出最早的sensor数据进行分发(Dispatch)。 综合上述的描述，数据的处理流转主要表现为：CollatedTrajectoryBuilder::AddSensorData(…)->CollatedTrajectoryBuilder::AddData(…)->sensor_collator_->AddSensorData(…)->OrderedMultiQueue::Add()->OrderedMultiQueue::Dispatch()->HandleCollatedSensorData()->data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get())->trajectory_builder->AddSensorData(sensor_id_, data_);->local_trajectory_builder_->AddRangeData()。 当数据流转到LocalTrajectoryBuilderND的AddRangeData函数时，就基本到达了SLAM算法的核心部分，包括外推器的姿态估计，ScanMatch的局部位姿优化(Local SLAM)，以及全局位姿优化(PoseGraph )等过程中的Lidar数据的进一步的流转和变换，限于篇幅，将在下一篇文章中向大家介绍。 References

本篇文章将向大家介绍cartographer系统的sensor模块的功能的代码实现，希望对感兴趣的读者朋友们有所帮助，欢迎读者朋友阅读并对说明描述不足之处提出意见和建议。 首先，cartographer库是SLAM的算法处理核心部分，其关于sensor的数据的定义主要用于内部算法的处理，而从智能设备本体接受的sensor原始数据会由cartographer_ros的节点进行接收和发送，这里将对cartographer_ros和cartographer算法库这两部分的sensor的数据表示在一起做一下说明。 如下图所示为cartographer系统（包括ros节点）代码中数据相关的类图。其中左边一列为IMU，Odometry等其他传感器的类图，右边两列为Lidar传感器相关类图，其中右上方描述了Lidar数据在系统中的流转转换流程过程。 在这里需要特别加以说明的有：在ROS节点接收通用点云数据ROS消息在SensorBridge类中转换为TimedPointCloudData类型的数据后发送给cartographer库进一步处理，TimedPointCloudData类对象数据的time成员表示为当前扫描帧点云的最晚时间，原点坐标为lidar传感器相对于tracking_frame的相对位置（一般tracking_frame定义为base_link，lua中有定义tracking_frame = “base_link”，原点定义为lidar传感器相对于base_link的位移大小，为固定的标定的值)。cartographer系统统一支持2D的LaserScan Lidar（较低成本的平面Lidar传感器）和3D Lidar传感器，对于2D LaserScan点云数据，一般会在预处理时进行切分处理(如backpack_2d.lua中定义了参数num_subdivisions_per_laser_scan = 10)，以便充分利用时间及时进行点云去畸变的计算。 在cartographer库内部，RangeDataCollator类实现了兼容多个Lidar(如backpack_3d.lua中定义了num_point_clouds = 2)传感器数据的同步融合，融合后的数据类型为TimedPointCloudOriginData，保留传感器相对于机器人本体坐标系的位移origin信息，可以计算range的大小以便进行点云的过滤操作。经过进一步的处理后的点云数据转换为RangeData，基于该数据调用ScanMatch算法优化局部位姿后对局部地图进行更新，关于整个SLAM算法流程中点云数据的坐标变换过程，ScanMatch算法实现局部位姿优化以及PoseGraph算法实现全局位姿优化等细节将在后续文章中做更加具体的说明分析。 ImuData为高频数据（频率几百到上千hz），PoseExtrapolate类实现了基于ImuData和/或OdometryData实现的机器人的位姿的较为精确的初始估计值。其中预估的大体方法为基于线速度和时间差估计位移，基于角速度和时间差积分出旋转增量估计，并且姿态外推器也基于ScanMatch的局部位姿优化结果更新time时刻更加准确的位姿，并预估出两个pose之间的匀速速度用于位移估计，在cartographer中没有采用IMU的线性加速度去进行预积分计算位姿中的translation部分(IMU的线性加速度的噪声较大，容易导致位姿漂移，而基于较为准确的位姿的变化去估计速度则较为稳定和准确，也可以基于OdometryData进行位移估计，如果有提供且数据量组足够时）。 后续文章将对cartographer的细节算法，系统架构等做进一步的介绍，欢迎关注。 References

本篇文章将向大家介绍RLBench，一个针对机器人研发的基准平台，旨在推动机器人操作常用任务的研发和对比评估。下面将以问答的形式来对RLBench进行介绍(RLBench的安装使用参考引文[1])。 问题1：RLBench和Gym提供的机器人模拟器环境对比，各有什么特色？ 回答：RLBench为机器人强化学习等提供了一个基准平台，特别是单臂机器人的常用操作任务提供高保真仿真模拟环境，而gym是强化学习中更加通用的平台，提供了众多基础的物理仿真环境（如小车平衡杆CartPole-v0等，参考引文[2]）进行强化学习基础算法实验。RLBench默认使用的是Franka Panda机械臂，也可以支持其他机器人，有人已经移植了UR5等，仿真环境要和真实的机器人尽可能保持一致性（包括外形等几何结构一致性，动力学运动一致性以及仿真控制操作接口一致性），这样Sim2Real时会保持最大限度的兼容，迁移也更为方便和有效。RLBench的源代码里有一个gym.py文件，通过RLBenchEnv Wrapper类将RLBench的物理仿真环境进和Gym仿真环境进行了适配，如果研发人员熟悉了gym的环境，就可以采样gym的接口实现RLBench里定义的任务相关的强化学习等模型的研发。Issac Gym（引文[4]）是Nvidia提出的模拟仿真环境，其利用GPU强大的计算能力基于大量并行数据训练人形机器人的运动功能的强化学习模型算法上有较大优势，但CoppeliaSim的仿真度更高，适合高精度操作的工业机器人，更加适合sim2real的实验。具体的更多的信息可以参考引文[3]中的问题1的详细说明。 问题2：RLBench源代码结构及实现上关键点介绍 回答：关于RLBench的核心组件主要有：Scene，Observation，Task，Robot，ActionMode，Enrionment等等。具体每个部分的详细介绍和代码示例请参考引文[3]中的问题2的详细说明。 问题3：具体个性化任务的定义 回答：主要重载Task类中的几个关键函数：init_task，init_episode，variation_count等。具体关于每一个函数的作用及示例介绍请参考引文[3]中问题3的详细说明。 问题4：模拟仿真数据采集过程介绍 回答：主要通过TaskEnvrionment的get_demos函数获取多个episodes的rollout数据，具体的代码可以参考引文[3]中的问题4的详细说明。 References

这篇文章将向大家简要介绍FAST-LIVO2开源系统中的VIO部分实现的思路，具体的较为细节的内容在参考引文的链接里，这里对相关思路做一下汇总说明。VIO的实现采用直接法视觉里程计（Direct Visual Odometry）的核心概念，用光度残差衡量同一3D点在当前帧与参考帧的像素亮度差异。 在该系统的VIO算法中，基于ESKF（error state kalman filter）的误差测量方程的相关公式定义和基于光度残差和状态参数（误差状态，如基于旋转的李代数扰动量，平移噪声等）的雅可比矩阵的计算及推导参见[1]，其中链式计算中的图像梯度的具体计算方法参考[2]。ESKF的状态更新公式及相关代码说明参考[3]。其中需要说明的是每一个可视点的测量光度误差的计算时，其参考图像帧可能不属于同一个参考帧（每个点云点根据光度一致性，几何约束或其他等条件选择最优的参考帧），而ESKF的每一次迭代都是基于当前的所有的用来优化位姿的地图点集合的光度误差总和进行的。具体的可以参考代码[4]中的详细实现。 函数retrieveFromVisualSparseMap基于当前处理逻辑点云帧（经时间对齐在原始点云帧基础上处理过的点云集合）中的点云集合信息动态提取当前帧可见的特征点，并构建高质量的局部子地图(visual_submap)，其中稀疏地图中的可视点会基于点的多个观测，选择最优的patch参考（不同点可能会选择不同的参考图像帧），并根据姿态变换对图像做出对应的放射变换以便后续的光度残差的计算，ESKF基于子地图的可视点的观测状态（光度残差）和IMU的位姿误差运动方程紧耦合优化位姿； 问题：vio中关于地图的构建思路，和lidar的体素地图的关系？ 回答：局部地图visual_submap主要用于优化位姿，在函数retrieveFromVisualSparseMap中构建局部稀疏地图的开始时进行重置，而全局地图feat_map为基于体素索引值和视觉点（VisualPoint）集合的hash表结构(unordered_map)。 References

这篇文章将向大家介绍FAST-LIVO2开源系统中的数据预处理部分。包括多传感器数据同步、IMU预积分与激光点云去畸变（运动补偿）等。关于激光点云和IMU传感器的数据特性的详细说明请参考附录（基于像素表示的摄像头RGB图像数据比较普及不做介绍）。 其中多传感器数据同步的实现在sync_packages函数里，这里主要分析slam模式在LIVO下的数据同步方法。其主要的思想是以图片帧作为处理的时间基准(从lidar_header_time_buffer找出小于当前图片基准时间戳的所有点云原始帧数据)，将原始的pcl数据帧按照时间关系重新组织到两个逻辑点云帧结构体（pcl_proc_cur和pcl_proc_next）中。 激光点云数据的时间戳是以point为粒度，扫描时间不同，激光雷达传感器的帧率相比图像和IMU是最低的（大概10-20Hz)，在一帧图像数据中，点云之间被扫描到的时间点各异（因为扫描有先后，位置在时间上没有对齐），可以根据IMU的预积分的结果进行运动补偿。一般将这个过程分为两步：1、IMU预积分，也称前向传播(propogation of IMU); 2、基于IMU的预积分的结果对PointCloudXYZI类型的待处理的点云数据用反向传播的方式进行运动补偿。 附录： 附录A:关于激光点云的基本数据结构的介绍 1、激光点云原始数据帧PointCloudXYZI类，包含XYZ坐标和强度值（Intensity），为激光点的容器集合。在本开源系统中，点云数据等传感器数据通过ROS的message发出来，并进行预处理，激光点云信息中，将curvature属性作为当前点云点扫描时的时间信息加以存储（相对于原始点云帧的起始点时间差）。激光点云数据的一帧数据一般定义为一个完整的扫描周期内所获取的所有点云的集合，而且以最先扫描到的点的时间戳作为帧起始时间。激光雷达传感器也分为传统的激光的旋转式机械激光扫描和livox非重复式较高密度点云点采样扫描。有时如果点云分辨率过高，会需要更多的计算资源，点云处理库pcl中也有针对点云进行降采样的相关类pcl::VoxelGrid，直接调用相关函数即可实现点云降采样。 2、Voxel，体素，相对于二维平面的像素而言，为三维空间的细分表示单元，通常表示为一个小的立方体，存储的信息可能包括：1、该空间是否被占据；2、颜色或强度值；3、法向量；4、置信度。在体素地图(Voxel map)中，三维空间被划分成类似于魔方结构的多个体素单元，体素地图用于点云建图，三维重建等任务； 3、Octree（八叉树），满足一般的树结构的递归定义，为一种八叉空间划分树，如一开始的L*L*L尺寸的空间，第一级划分将空间的“长宽高”各对半切分，上下各四个子体素（八个子节点），以此类推进行递归再划分。八叉树的表示空间的好处在于对于稀疏的区域表示，可以用大体素（对应的叶子节点的层次比较低，可以提高存储效率），对于密集表面比较丰富的区域，可以用小体素（可以有更高的精细度），这种表示方法又称为自适应体素分割。 4、Octree Voxel Map（八叉树体素地图），体素的位置按照整数坐标存储，在空间上对应三个坐标轴上的整数索引x，y，z。某个点云的索引为点云的三个维度世界坐标除以根体素的大小(max_voxel_size)。八叉树体素地图可以基于字典结构（unordered_map，哈希表）实现体素的快速索引（map的key为三维整数坐标缩影，value为八叉树的根节点，可以存储多个点云数据）。一般如果max_voxel_size=0.5，则表示根体素的分辨率为0.5m*0.5m*0.5m。在该体素内部一般也会有几个到几十上百个点云点，具体要看采样的设备和是否进行了降采样处理。体素地图基于世界坐标构建，在系统运行过程中其体量可能较大。通过八叉树体素地图可以动态构建和更新全局地图。同时该体素地图支持点云的自适应体素分割，在代码实现中max_layer和voxel_size密切相关，如max_layer>1，则当原始体素内的点云点如果不满足拟合出平面的条件（说明表面为较为丰富的曲面），则会递归的进行八叉树子节点创建以支持构建更加丰富的表面信息。 5、Voxel Plane，代表了体素对应的局部平面信息，常用于基于八叉树的三维环境建模，对于体素内的多个点云，通过拟合平面模型，提取点云在该体素单元内的主要几何结构信息（如物体所表现的在该位置处的主体表面信息）。如果拟合平面成功，说明该体素对应的区域为平面如地面，否则可能对应着丰富的曲面表面，需要再进行细粒度的分割，用更小的平面去拟合曲面。 附录B：IMU数据基本结构介绍 IMU（Inertial Measurement Unit），即惯性测量单元，一般提供智能体运动的线性加速度（linear_acceleration，通过加速度计获取），角速度（angular_velocity，通过陀螺仪获取）等信息，通过线速度和角速度以及时间戳信息就可以通过预积分获取智能体的在时间戳上累积运动的位姿。 References

这篇文章将向大家总体介绍一下ORB-SLAM3系统的设计和实现，其中每个模块的实现逻辑和思路已经在主题文章中向大家做了介绍。 ORB-SLAM3实现了多种传感器配置下的同步定位（相机位姿的实时跟踪定位）和建图（基于稀疏三维地图点的三维地图构建）功能，且通过Atlas类实现了多地图集（跟踪失效时会重新创新新地图）的管理，以及包括地图的回环检测和矫正以及地图的合并。整个系统大体由单帧实时跟踪模块（Tracking.cc），局部建图模块(LocalMapping.cc)和回环检测和矫正（LoopClosing.cc）模块以及可视化模块（View.cc）等构成。其中各种传感器配置场景在代码实现时是耦合在一起的，根据配置的条件的不同对其逻辑的差异性分别按条件进行处理。 Tracking模块需要处理频率较高的视频帧，采用参考关键帧或基于上一帧的运动跟踪模式对特征点进行匹配和相机位姿进行优化等。一般如果处理一帧的tracking较为耗时，可能要降低帧率或丢弃部分帧，一般来说，tracking一帧的耗时可以在帧率的一半，而仅有部分帧将作为关键帧来加入到地图中，关键帧帧率大概在0.5-2秒左右，因此关键帧占总帧数的比率较低，即使在tracking的全过程中使用地图更新访问加锁，其他模块如局部建图和回环检测和矫正模块都会有较为充裕的时间来调度执行。 这里对于模块涉及地图更新的操作的地方做一下说明。首先在Tracking过程中涉及地图更新的操作的地方有：（1）单目初始化函数MonocularInitialization里调用了初始地图的创建，将当前帧和初始帧作为两个关键帧插入了初始地图里；（2）TrackReferenceKeyFrame或TrackWithMotionModel函数里对相机位姿进行了优化（也更新了地图点的信息）；（3）重定位时从候选的关键帧中计算匹配点并优化位姿；（4）、TrackLocalMap利用了关键帧邻近关键帧的更多的匹配地图点进行位姿优化；（5）最后判断是否将当前帧作为关键帧插入，如果插入关键帧，同样也需要更新地图。 问题：ORBSLAM3中的LocalMapping的函数SetAcceptKeyFrames(false);只限制了Tracking模块不能插入新的关键帧，但如果在LocalMapping的过程中如实现冗余关键帧剔除（KeyFrameCulling）或者地图点的剔除操作的同时，有没有可能和Tracking的访问地图操作存在着数据访问竞争的问题，因为在LocalMapping的处理过程中好像没有加锁访问地图更新？ 回答：在局部建图的处理过程中虽然没有使用当前地图的互斥量mMutexMapUpdate进行加锁，但是在地图的细粒度的访问中都加了细粒度的互斥量的定义，同时ORB-SLAM3不追求严格的实时一致性，而是通过以下设计实现最终一致性：（1）闭环校正的全局同步：当检测到闭环时，系统通过全局BA或位姿图优化对所有关键帧和地图点进行全局调整，此时所有线程暂停，确保全局状态的一致性。（2）高频跟踪，低频优化：如Tracking线程以相机帧率（30Hz）运行，而LocalMapping和LoopClosing线程以更低频率（约10Hz）执行优化。优化结果对Tracking的影响是“延迟生效”的，但系统通过BA和闭环校正逐步收敛到全局一致状态，如在Tracking的高频计算过程中，低频的局部建图和回环检测运行的数据是基于时间线上靠前面的数据，从而也减少了冲突访问的可能性。（3）、采用一些鲁棒的核函数和优化算法，即使存在着临时的量的不一致性，也能足够健壮得出较为理想的优化结果。（4）、局部BA等优化过程更新相机位姿和地图点时采用了地图更新互斥量这种粒度较大的锁，实现了和tracking过程的互斥访问。 关于ORB-SLAM3系统中可能存在的bug问题，可以参考文献[7]中作者给出的修正。ORB-SLAM3系统主要的功能是定位和建图，在机器人无人机等系统中作为感知的大模块的一部分，也需要和PNC（Planning and Control）等模块集成形成一个完整的自主导航系统。 References

本篇文章将向大家介绍在SLAM中使用的相关优化算法及其在ORB-SLAM3中相关模块中的具体应用案例。 首先，SLAM系统的核心功能之一为视觉里程计（Visual Odometry, VO，也通常称为前端，记录智能体随时间变化在场景中运动的姿势方向和位移，由于前端长时间运行会出现累积漂移和误差，因此也还有回环和地图合并检测等后端模块来进行误差矫正和消除）. 在系统初始化或tracking跟踪丢失创新新地图时，地图初始化时的第一帧的位姿设定为单位矩阵，第二帧的位姿以及两个连续帧的匹配特征点对应的地图点初始值通过Camera类的ReconstructWithTwoViews函数进行计算实现，具体是通过对极约束和三角测量来分别估计相机运动位姿和求解像素的深度信息和对应的物理世界的地图点坐标）。 MLPnPSolver优化器，该优化器的作用为基于最大似然的PnP（Perspective-n-Point）问题求解器，PnP问题的一种典型应用场景为基于匹配的前后帧的3D坐标和2D图像特征点集合来估计相机运动的位姿（旋转+平移），在Tracking的重定位算法中用于快速找出相机位姿的初始解供后续进一步优化。MLPnPSolver基于最小二乘的高斯牛顿法迭代求解逐步减小重投影误差来优化相机位姿，其中MLE(最大似然估计）是一种基于统计概率的参数估计方法，在SLAM应用场景中，参数为相机位姿，观察值为图像中ORB特征点的像素坐标，通过MLE算法，找到最优的参数解等价于使得重投影误差最小化。在算法的实现过程中，由于图像的位姿如旋转矩阵的参数对于误差函数不方便直接求导，而是通过罗德里格斯公式将旋转矩阵转化为旋转向量，和位移一起构成6维向量根据李代数的扰动模型进行求导，而在进行优化时，误差项e为图像uv平面的二维向量。因此目标函数将表现为向量的形式，基于雅可比的矩阵导数的优化比较直观，在大规模问题场景中（如基于地图的全局位姿和地图点优化），利用雅可比矩阵的稀疏性能更高效求解。在MLPnPSolver中，同时采用了RANSAC（Random Sample Consensus，重采样一致性算法）算法，RANSAC是一种鲁棒估计方法，其也采用了多次迭代求解，每次迭代随机选择一定数量的匹配3D2D点对进行求解，并统计内点（求得的位姿进行匹配点映射后误差在一定小的范围内）的数量，最后选择内点数量最多的为最后的位姿优化结果。 Optimizer类的相关优化函数；（1）、PoseOptimization，基于3D2D匹配点对和已经根据Tracking的过程优化的初始位姿，采用g2o的图优化方法来优化位姿，其中g2o的图优化中，节点有基于位姿的g2o::VertexSE3Expmap，边有ORB_SLAM3::EdgeSE3ProjectXYZOnlyPose（顾名思义，这条边仅仅通过相机位姿SE3将地图点坐标映射到图像像素且仅仅优化位姿Pose），以及设置观测值（像素坐标），核函数等，（2）、LocalBundleAdjustment，基于传入的关键帧及其共视关键帧集合对多帧位姿和地图点进行联合优化，同样基于g2o的图优化算法，这里有一个需要注意的地方在于地图点的观测帧不属于共视关键帧的观测帧将作为位姿固定的节点（不参与位姿优化的节点）加入到优化的graph里。这里用的边为EdgeSE3ProjectXYZ，将同时优化位姿和地图点；（3）、GlobalBundleAdjustemnt和BundleAdjustment将以关键帧数组和地图点数组来构建g2o优化图进行联合位姿和地图点优化。(4)，Optimizer还有一些和IMU相关的联合优化方法实现，具体可以参考文献2中的代码实现[2]。 Sim3Solver：Horn’s 四元数方法（Horn’s Method）是一种用于最优对齐两个三维点集的方法，广泛用于位姿估计、点云配准、结构光扫描等场景。它通过最小化均方误差（Least-Squares Error），利用四元数来高效求解旋转矩阵R，在ORB-SLAM3系统中，其主要用于（1）、回环匹配关键帧和当前关键帧的变换求解；求解的参数有旋转、平移和尺度信息，为七个自由度；（2）、合并匹配关键帧和当前关键帧的变换求解。 References

这篇文章将向大家介绍LoopClosing模块中的地图回环矫正和地图合并和矫正部分的内容，其函数分别为CorrectLoop和MergeLocal（或MergeLocal2），其根据从回环检测过程中求解出的回环匹配关键帧或合并匹配关键帧到当前帧的Sim(3)变换进行回环和地图合并及矫正。 关于回环矫正的几个问题： 问题1： 在ORB-SLAM3中CorrectLoop的函数[1]的功能中根据前面优化求得的变换mg2oLoopScw应用到当前关键帧及其共视帧（统称为连接帧mvpCurrentConnectedKFs）上，然后进行OptimizeEssentialGraph进行优化，请详细分析一下具体的步骤及思路。 回答：mg2oLoopScw是 Sim3Solver计算得到的回环帧到当前帧的 Sim(3) 变换所得矫正后位姿，用于修正关键帧及其连接帧集合（当前关键帧的所有共视关键帧）的位姿（将该位姿和调整关键帧和当前关键帧的相对变换进行叠加及可以对每一个共视关键帧的位姿进行初步矫正），使整个局部关键帧集合做统一一致的初步矫正。然后调用OptimizeEssentialGraph进行通常在闭环检测后调整关键帧的位姿，保持整体一致性。这里使用sim(3)变换，因为闭环可能涉及尺度漂移，需要调整尺度。(而GlobalBundleAdjustment是全局BA，优化所有关键帧和地图点的位置，使用se(3)因为BA通常处理的是刚体变换，不考虑尺度，将回环约束均匀传播到整个关键帧图)。关于优化部分的内容，将会在专门的文章中加以介绍。关于回环矫正的更详细的思路，可以参考本文后面地图合并的部分内容。 问题2：首先将当前关键帧及其共视关键帧应用一致的矫正变换然后再进行全局优化。这个步骤不添加是否会有影响？ 回答：一般的情况是回环匹配帧由于在地图的时间点前面，其误差和尺度漂移的程度要小，当前关键帧和回环匹配帧之间也许走过不少的其他关键帧，积累的较多的误差，因此首先通过将当前关键帧的共视关键帧首先应用统一的矫正变换以实现初步的累积误差和尺度漂移矫正，这对于后面的全局优化的过程能够更加容易收敛。 关于地图合并的几个问题： 问题1：地图合并的场景有没有比较具体的应用场景的示例帮助理解？ 回答：ORB-SLAM3支持多地图(multi-map，由Atlas类来管理地图集)，在地图合并和回环状态的检测的过程中不同的地方在于匹配关键帧和当前关键帧在不在同一个地图里。比如家庭里的多个房间，工厂里的多个车间等。 问题2：地图合并时的实现逻辑是怎样的？ 回答: 地图合并时主要采取如下的步骤：（1）、首先根据当前帧来更新一定窗口大小的共视帧数量（如果当前关键帧的直接共视关键帧的数量不够，则继续在共视帧的共视帧中去扩展），同理也对合并匹配帧去获取其共视帧的集合。（2）、然后对当前关键帧窗口内的关键帧采用和回环纠正第一步的Sim(3)变换一样的操作实现初步的位姿矫正以便于后续的全局优化的迭代收敛。（3）、执行地图合并操作，将当前帧相关的关键帧（spLocalWindowKFs变量里的当前这的局部窗口内的帧的集合，包括前后一段时间内的连续关键帧和共视关键帧等）合并到融合的地图里pMergeMap->AddKeyFrame(pKFi)，并从当前的地图里移除对应的关键帧pCurrentMap->EraseKeyFrame(pKFi)。地图点的合并过程类似。然后Atlas地图集管理将融合地图设为当前地图（表示当前系统已经实现了地图跳转），并将当前地图标记为失效地图；（4）、将当前关键帧的父关键帧设为合并匹配关键帧，并逆序当前关键帧的父子关系链条；（5）、在合并后有些地图点存在着重复（比如合并匹配帧及附近的共视关键帧和当前关键帧及附近的共视关键帧有些地图点是重复的），通过sim(3)变换后去搜索这些重复的地图点并剔除冗余（调用SearchAndFuse(vCorrectedSim3, vpCheckFuseMapPoint)函数），针对每一个当前关键帧邻近窗口内的关键帧集合和融合关键帧相连接的关键帧集合的每一个关键帧调用UpdateConnections更新相关连接信息；（6）调用函数Optimizer::LocalBundleAdjustment(mpCurrentKF, vpLocalCurrentWindowKFs, vpMergeConnectedKFs,&bStop)进行局部优化，其中vpMergeConnectedKFs参数中的关键帧的位姿为fixed不参与优化（与优化的关键帧共享的地图点会参与优化，但也会受约束于固定的位姿，这给优化的过程提供了基准参考，防止优化出现漂移现象）。（7）对当前地图的没有融合到pMergeMap部分的关键帧同样采用Sim(3)矫正变换以保证整个地图的一致性，并再一次通过加锁机制实现地图的合并操作（相关关键帧和地图点在地图集里的关系更新，在代码的实现上和第三步的合并操作一样）（8）调用全局线束优化，对合并后的当前地图的所有关键帧位姿和地图点集合进行联合优化，以消除较长时间运行的累积误差，在优化前会让局部建图器停止工作。 问题3：地图合并的时候，将当前地图设置为失效，下次合并的时候也可能会再次变为当前地图对吗？ 回答：是的，在 ORB-SLAM3 中，地图合并时将当前地图设置为“失效”（即 SetMapBad()），目的是标记当前地图已经不再使用，并且可能在之后的合并过程中被再次激活作为当前地图。 问题4：由于在地图合并的时候存在着关键帧父子关系链的调整，会出现两个子问题：（1）、将当前关键帧的父关键这设为融合匹配关键帧，并调整当前关键帧的父子关键帧链条上的顺序结构，则之前的融合匹配关键帧的子关键帧链条如何维护？(2)、关键帧之间的父子关系链和关键帧之间的前后顺序双向链表定义是不同的数据结构意义，双向链表主要体现在时间上的先后顺序，父子关系主要体现在共视的重合程度吗？ 回答：（1）、在支持多地图的合并场景下，地图的合并将会形成在合并点出现树的分叉结构（每次融合可以理解将融合关键帧的现有子关键帧链保持，同时添加一个当前关键帧的子关键帧链条），这种分叉结构可以保证父子关键帧关系图不会出现闭环形结构。（2）、父子关键帧在一般情况下保持时序（一般在mbFirstConnection为true时建立父子关系），且主要体现了关键帧之间的共视重合度，但在上述的合并地图的场景下，为了优化地图结构，会调整关键帧之间的父子关系结构，此时部分父子关系链上的结构将不再保证时序关系。关键帧里的双向链表结构确实保持了关键帧的时序关系。可以方便获取当前帧在时间序列上的前一个和后一个关键帧。 问题5：地图合并时由于每个地图的坐标系独立，即一般在跟踪失效的时候会创建新地图，新地图的初始关键帧的坐标为标准的单位矩阵？在两个地图的融合时坐标如何对齐？和回环检测和纠正时的处理有什么具体的差异？ 回答：一般在回环检测和矫正以及地图合并检测和合并的处理过程中，会以先前的回环匹配帧或合并匹配帧作为基准（后面经过了一段时间的累积出现了累积的漂移误差），基于两个匹配帧的变换将当前帧及当前地图的所有帧都首先进行Sim(3)的变换进行初步纠正，这个变换逻辑是一样的。 References

这篇文章将向大家介绍回环和地图合并检测相关实现逻辑，和局部建图器类似，回环检测实现的类为LoopClosing（该类同时也实现了回环矫正和地图合并及矫正，其中矫正和合并的内容将于后续文章介绍），和局部建图器模块一样，该模块也在背景线程中运行，线程运行函数为LoopClosing::run()，为一个随着系统一直运行的while(1)循环。该模块的总体功能是检测地图回环和可能的地图合并，整体地图的维护和所有关键帧的位姿和地图点的全局优化，这篇文章将向大家介绍LoopClosing中的地图回环及合并检测。具体的回环和合并矫正的内容将在下一篇文章中较为详细的分析介绍。 回环检测首先从回环检测队列中取出一个关键帧进行处理，首先调用NewDetectCommonRegions函数来判断当前帧和历史帧是否存在着共同的区域，如果存在，有可能是检测到了回环或者是合并的场景，如果合并条件成立，则调用MergeLocal或MergeLocal2对地图进行合并；如果检测到回环，则进行回环校正。 NewDetectCommonRegions函数的大体逻辑是如果还没有检测到回环或合并发生的次数（mnLoopNumCoincidences，mnMergeNumCoincidences刚开始回环检测或者期间没有检测到匹配候选时重置为0）。回环的检测并不是在当前帧与回环候选关键帧之间的第一次匹配时就直接确认回环存在，而是需要连续几帧都与回环候选帧的匹配度和几何验证通过，才会最终确认回环。其中mnLoopNumCoincidences为检测到回环发生的次数记录，mnLoopNumCoincidences>3说明需要至少连续三帧的匹配才算有效的回环检测，以增加回环检测的鲁棒性，合并状态检测的情形类似。 NewDetectCommonRegions函数首先通过调用KeyFrameDatabase类的DetectNBestCandidates函数根据当前帧和关键帧数据库中关键帧的特征点对应的属性“单词”word形成的“文档”（BOW，bag of words）采用余弦相似度或者TF/IDF为度量标准进行相似性计算找出回环关键帧候选集合和合并关键帧候选集合（在同一个地图的为回环候选关键帧，在不同的地图中的为合并候选关键帧），其中BOW为一个“文档向量”，向量中的每一个元素可以表示特征对应的属性单词是否在该关键帧的在词典中出现，关于BOW的相关实现，可以参考文献[2]中的ORBVocabulary的介绍。 然后两次调用DetectCommonRegionsFromBoW函数分别从候选匹配帧集合中找出是否存在回环或需要合并的匹配关键帧，其基本思想是遍历上一个函数返回的候选的回环候选帧和合并候选帧的集合，将每一个候选和当前关键帧进行匹配相似度计算，并采用Sim3Solver进行几何验证，返回最佳的匹配候选帧，该函数分别调用两次，将最优的匹配结果分别更新到变量mpLoopMatchedKF和mpMergeMatchedKF中去。同时通过计算分别获得mg2oLoopSlw和mg2oMergeSlw变量，这两个变量的求解过程使用到了Sim3Solver算法[1]，该变量的含义为从回环匹配关键帧或合并匹配关键帧到当前关键帧进行Sim(3）变换进行纠正后的位姿。 如果检测到了回环的存在，则调用CorrectLoop函数进行回环矫正，如果检测到了合并状态的存在，则调用MergeLocal或MergeLocal2（有IMU的场景）进行地图的合并并进行位姿和地图点的矫正。具体的回环矫正和地图的合并的方法将在后续专门的博客文章里向大家介绍。 References

本篇文章将向大家介绍局部建图器的代码逻辑，局部建图器的类名为LocalMapping，在独立的背景线程中运行，和主线程的跟踪逻辑之间可以并发执行。局部建图器模块的主要作用是当前处理的关键帧及其邻近关键帧的位姿和地图点的联合优化。 LocalMapping的run函数为线程启动的函数，该函数在启动后一直循环运行，按顺序从现有的尚未处理的关键帧列表中依照时间先后顺序pop出关键帧进行处理(调用函数为ProcessNewKeyFrame)，ProcessNewKeyFrame的处理逻辑大体为：1、从关键帧列表中按时间顺序pop出一帧关键帧进行处理；2、在先前已经进行ORB特征点和特征描述符的基础上计算BOW以用于后面的回环检测，检查和更新地图点的观测关系；3、通过调用UpdateConnections函数更新关键帧的共视图相关属性，如连接的共视帧及其共享地图点等信息，具体解释可以参考文献[1]中的详细介绍。以及在地图中插入关键帧。 MapPointCulling函数对最近加入的地图点（如通过立体视觉加入，或者最近在跟踪的过程中发现的新的地图点）执行地图点清除工作，将发现率较低的点，较长时间未被观测到的等一些质量较低的地图点清除，以保证地图点的质量。 CreateNewMapPoints的功能是：1、通过关键帧找到最佳的邻近的共视关键帧集合；2、当前关键帧和每一个邻近的共视关键帧中的关键点之间进行特征点匹配，并将匹配的但尚未分配地图点的特征点进行三角化测量算法求出新的地图点，并更新地图点和关键帧之间的关联关系等，最后将地图点加入到地图点集合变量mlpRecentAddedMapPoints中。 调用Optimizer::LocalBundleAdjustment进行局部线束调整优化，在优化时，以当前的关键帧及其共视的关键帧作为优化的范围，更新关键帧集合和地图点集合，并采用g2o的基于李代数的Levenberg-Marquardt 算法来联合优化多帧相机位姿和地图点坐标。 通过调用KeyFrameCulling进行冗余关键帧剔除操作，其粗略主线逻辑为：首先根据当前关键帧获取最佳共视邻近关键帧集合；然后对于集合里的每一个关键帧，统计其地图点的观测属性，如果发现超过一定量的地图点都被较多的重复观测到，则定义该关键帧为冗余关键帧，标记该关键帧为无效（SetBadFlag），减少地图中的关键帧数量，简化后续的优化过程。 接下来的步骤中需要判断是否有IMU支持，如果有IMU的场景下，则在系统启动时间（minit的变量）一定范围内对IMU启动两轮惯性标定优化，然后根据设定的时间窗口综合IMU的测量和关键帧数据（关键帧和地图点数据）对地图的尺度进行调整，确保地图的精度。 最后通过调用mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame)语句，将当前关键帧加入到闭环检测模块中。关于回环和合并检测，可以参考后续相关连载主题详细说明。 References