CN116804553A - 基于事件相机/imu/自然路标的里程计***及方法 - Google Patents

Abstract

本发明提供了基于事件相机/IMU/自然路标的里程计***及方法，其中***包括：事件相机跟踪模块，传统相机跟踪模块，联合优化模块。本发明利用事件相机和传统相机性能上互补的优势，采用两者结合IMU的方案来构建视觉里程计，提高了***在高速高动态场景下的鲁棒性，同时通过在场景中预设已知位置信息的自然地标的方案，计算出当前时刻的位姿并进行更新，实现了类似回环检测的功能。本发明在传统相机跟踪模块中，还加入了动态点剔除的步骤，进一步提高了***在动态场景中的鲁棒性。

Description

基于事件相机/IMU/自然路标的里程计***及方法

技术领域

本发明属于计算机软件技术领域，涉及同时定位与地图构建技术，具体涉及基于事件相机/IMU/自然路标的里程计***及方法。

背景技术

同时定位与建图(SLAM)技术在自主机器人控制或增强/虚拟现实等各个领域都有重要应用。由于相机和惯性传感器良好的互补性能，近年来视觉惯性里程计已经取得了巨大进展。然而，由于传统相机的一些众所周知的限制(运动模糊和低动态范围)，这些视觉惯性里程计方法仍然存在一些难以应对的情况，例如高速运动或高动态范围场景。事件相机的出现为克服这些问题提供了巨大的潜力，与以固定帧速率输出像素强度的传统相机不同，事件相机(如动态视觉传感器DVS)只输出像素强度的变化。与传统相机相比，事件相机在一些方面具有明显的优势：微秒级的延迟和非常高的动态范围(一般是传统相机的2倍以上)。尤其是由于所有像素都是独立地采集光信息，事件相机不会受到运动模糊的影响。利用事件相机进行状态估计的研究已经有了一定的发展，提出了不少纯事件相机以及结合事件相机和IMU元件进行位姿估计的方案，但是这些方案大都缺少了回环检测模块，***长期运行结果难以保证。

发明内容

为克服现有技术的不足，考虑到事件相机和传统相机在性能上存在一定的互补性以及视觉惯性里程计在鲁棒性和精度上的优异表现，本发明在已有的基础上提出了新的基于事件相机/IMU/自然路标结合的里程计***及方法，并通过布设已知位置信息的预设自然地标，利用目标检测算法对地标进行识别后计算并更新位姿来实现类似回环检测的功能，以提升***的性能。

为了达到上述目的，本发明提供如下技术方案：

基于事件相机/IMU/自然路标的里程计实现方法，包括如下步骤：

步骤一，事件相机跟踪

根据事件相机输出的事件流，基于SAE(活动事件表面，Surface ofActiveEvents)直接在原始事件上提取特征点，使用LK光流法对提取的特征点进行跟踪并计算其逆深度后生成地图点，后续对地图点进行进一步地跟踪；

步骤二，传统相机跟踪

在标准图像帧上提取特征点并进行跟踪的同时加入目标检测模型YOLOV5用来识别预设自然地标，当检测到预设自然地标时使用PnP算法计算出位姿并更新当前帧的位姿；

步骤三，联合优化

通过非线性优化实现视觉惯性的融合，将视觉惯性的融合问题转化成对目标函数的最小化问题，该目标函数包括四个部分，边缘化后的先验误差，事件相机和传统相机的重投影误差，以及惯性测量的误差项，利用G2O优化库进行求解后得到精确的位姿估计。

进一步的，所述事件使用带极性的TS(时间表面，Time Surface)来表示：其中T_p为事件的强度，x,y为像素坐标，t为事件发生的时间，p代表了像素强度变化的极性，S为该像素上一次触发事件的时间戳，τ代表了一个恒定的衰减率。

进一步的，所述步骤一具体包括如下子步骤：

步骤1，根据事件相机提供的事件流来生成SAE，SAE记录了事件流中的每个像素上一次触发事件的时间戳，它是一个二维数组，数组中每个元素对应一个像素，其值对应上一次触发事件的时间戳；

步骤2，当接收到新的事件流时，使用Arc*算法在生成的SAE上提取角点，Arc*算法维护两个环形的事件集合，并检测连续的弧或其补弧是否在一定的范围内来提取角点；

步骤3，使用LK光流法对已经提取的特征点进行跟踪，跟踪成功的话则三角化生成地图点并进行进一步的跟踪，跟踪失败则丢弃该特征点，当特征点数量低于设定的阈值时提取新的特征点。

进一步的，所述步骤2中Arc*算法提取特征点的具体步骤为：

(1)设定两个环形的事件集合，每个集合包含了最近的N个事件；

(2)当一个新事件到达时，将其加入到两个环形事件集合中，计算该事件与环形中其他事件之间的弧度，记录下连续的弧或其补弧的范围；

(3)对每个新加入的事件，检查其对应的连续弧或补弧是否在一定的范围内，如果是则将该事件标记为角点；

(4)从两个环形的事件集合中将最旧的事件删掉，以保证集合中只包含最近的N个事件。

进一步地，所述步骤3中跟踪过程中进行关键帧的提取，将提取的关键帧送到联合优化模块进行优化计算；关键帧的选择基于当前被跟踪的特征的数量以及与两个连续时间戳之间被跟踪的特征的平均视差，分别设置一个阈值，当被跟踪的特征的数量低于设置的阈值时或者两个连续时间戳之间被跟踪的特征点的平均视差大于设置的阈值时选取新的关键帧。

进一步的，所述步骤二具体包括如下子步骤：

步骤1，预先采集图像数据对YOLOV5模型进行训练，

步骤2，对传统相机的标准图像帧进行目标检测，识别图像中是否存在地标，

步骤3，在标准图像帧中提取特征点，根据目标检测的结果识别潜在的动态物体，利用静态一致性将潜在的动态物体区域中的特征点分为静态特征点和动态特征点，将动态的特征点剔除；

步骤4，使用光流法对提取到的静态特征点进行跟踪，

步骤5，如果步骤2中识别到了地标，利用PnP算法计算出位姿并更新此时的相机位姿。

进一步的，所述步骤3中将动态的特征点剔除的具体步骤为：

(1)利用最新的YOLOV5算法对每一帧图像进行处理，识别其中的所有物体；

(2)将识别结果中潜在的动态物体所在的矩形区域保存下来，保存的格式为(x₁,y₁,h,w)，x₁为区域左上角的横坐标，y₁为区域左上角的纵坐标，h矩形区域的高度，w为矩形区域的宽度；

(3)通过静态一致性将潜在的运动物体区域中的特征点分为静态特征点和动态特征点，最后剔除动态的特征点；具体通过以下过程区分静态特征点和动态特征点：

针对t时刻图像帧中的特征点i,j及其对应t+1时刻图像帧中的特征点i,j，用表示，定义同一时刻图像帧中的两特征点的距离为：

其中，u,v为特征点在像素坐标系中的横坐标和纵坐标；当不同时刻图像帧中对应的特征点之间的距离是相同的或是距离差值在一定阈值范围内，则两个特征点为静态特征点，阈值由非潜在运动物体区域内的特征点来确定，其计算公式为：

其中，S为非潜在运动物体区域内的特征点的集合，d_t(i,j)为t时刻S集合中任意两特征点之间的距离，N为S集合中的特征点的数目。

进一步的，步骤2和步骤5中通过识别人工地标来更新位姿估计的具体步骤为：

(1)从不同的角度采集预先在场景中布置好的预设自然地标的图像数据；

(2)在采集好的图像数据上进行目标标注，将预设自然地标的部分标注出来；

(3)使用标注好的数据集对模型进行训练；

(4)利用训练好的模型识别图像中的地标，识别成功后得到地标在世界坐标系中的3D坐标和在图像中的2D坐标，使用PnP算法求解得到位姿估计后实时更新位姿。

进一步的，所述步骤三中的目标函数为：

其中，等式左边的χ为待估计的变量，等式右边第一项为边缘化后剩余的先验残差，第二项为IMU预积分的残差，第三项为事件相机的测量残差，第四项为传统相机的测量残差；W_(·)为每一项残差对应的权重。

本发明还提供了基于事件相机/传统相机/IMU/自然路标的视觉里程计***，用于实现上述基于事件相机/传统相机/IMU/自然路标的视觉里程计实现方法，包括事件相机跟踪模块，传统相机跟踪模块，联合优化模块；其中，

事件相机跟踪模块用于根据事件相机输出的事件流，基于SAE(Surface ofActiveEvents)直接在原始事件上提取特征点，使用LK光流法对提取的特征点进行跟踪并计算其逆深度后生成地图点，后续对地图点进行进一步地跟踪；

传统相机跟踪模块用于在标准图像帧上提取特征点并进行跟踪的同时加入目标检测模型YOLOV5用来识别预设自然地标，当检测到预设自然地标时使用PnP算法计算出位姿并更新当前帧的位姿；

联合优化模块用于通过非线性优化实现视觉惯性的融合，将视觉惯性的融合问题转化成对目标函数的最小化问题，该目标函数包括四个部分，边缘化后的先验误差，事件相机和传统相机的重投影误差，以及惯性测量的误差项，利用G2O优化库进行求解后得到精确的位姿估计。

与现有技术相比，本发明具有如下优点和有益效果：

1.本发明利用事件相机和传统相机性能上互补的优势，采用事件相机/传统相机/IMU/自然路标结合的方案来构建视觉里程计，能克服传统视觉惯性里程计方法在高速高动态场景下无法工作的问题，同时相比单独事件相机或者事件相机结合IMU的方案提高了鲁棒性和精度。

2.通过在场景中布设的已知位置的地标，利用目标检测算法来进行识别，计算出当前时刻的相机位姿并进行位姿更新，在不需要轨迹中出现回环的情况下实现了类似回环检测的功能。

3.通过目标检测算法识别场景中的潜在运动物体，利用静态一致性找出动态特征点并剔除，可以减少特征匹配中出现的误匹配，提高***的鲁棒性。

附图说明

图1为本发明***结构图。

图2为识别地标更新位姿的步骤。

图3为剔除动态点的步骤。

具体实施方式

以下将结合具体实施例对本发明提供的技术方案进行详细说明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

本发明实施例提供一种基于事件相机/IMU/自然路标的里程计***，在移动小车平台上搭载传统相机，事件相机和IMU元件等传感器，传感器之间为刚性连接，预先标定好各传感器的内参数以及相互之间的外参数。在任务场景中布设位置已知的地标，同时预先采集好目标检测算法训练模型所需要的图像数据集。

令移动小车的起点为原点，车头的朝向为x轴正向，车头朝向的左手为y轴正向，车身正上方为z轴正向建立世界坐标系。在移动小车的运行过程中，实时地获取事件相机提供的事件流、传统相机提供的图像流以及IMU元件提供的加速度和角速度测量值。

本发明***包括事件相机跟踪模块，传统相机跟踪模块，联合优化模块。事件相机跟踪模块用于根据事件相机输出的事件流，基于SAE(Surface ofActive Events)直接在原始事件上提取特征点，使用LK光流法对提取的特征点进行跟踪并计算其逆深度后生成地图点，后续对地图点进行进一步地跟踪。传统相机跟踪模块用于在标准图像帧上提取特征点并进行跟踪的同时加入目标检测模型YOLOV5用来识别预设自然地标，当检测到预设自然地标时使用PnP算法计算出位姿并更新当前帧的位姿。联合优化模块用于通过非线性优化实现视觉惯性的融合，将视觉惯性的融合问题转化成对目标函数的最小化问题，该目标函数包括四个部分，边缘化后的先验误差，事件相机和传统相机的重投影误差，以及惯性测量的误差项，利用G2O优化库进行求解后得到精确的位姿估计。

本发明定义事件的表示方式，使用带极性的TS(Time Surface)来表示：其中T_p为事件的强度，x,y为像素坐标，t为事件发生的时间，p代表了像素强度变化的极性，S为该像素上一次触发事件的时间戳，τ代表了一个恒定的衰减率。

采用事件相机跟踪模块进行如下处理：

步骤1，根据事件相机提供的事件流来生成SAE，SAE记录了事件流中的每个像素上一次触发事件的时间戳，它是一个二维数组，数组中每个元素对应一个像素，其值对应上一次触发事件的时间戳。

步骤2，当接收到新的事件流时，使用Arc*算法在生成的SAE上提取角点，Arc*算法维护两个环形的事件集合，并检测连续的弧或其补弧是否在一定的范围内来提取角点，Arc*算法的具体步骤为：

(1)设定两个环形的事件集合，每个集合包含了最近的N个事件；

(2)当一个新事件到达时，将其加入到两个环形事件集合中，计算该事件与环形中其他事件之间的弧度，记录下连续的弧或其补弧的范围；

(3)对每个新加入的事件，检查其对应的连续弧或补弧是否在一定的范围内，如果是则将该事件标记为角点；

(4)从两个环形的事件集合中将最旧的事件删掉，以保证集合中只包含最近的N个事件。

步骤3，使用LK光流法对已经提取的特征点进行跟踪，跟踪成功的话则三角化生成地图点并进行进一步的跟踪，跟踪失败则丢弃该特征点，当特征点数量低于设定的阈值时提取新的特征点。

在跟踪过程中进行关键帧的提取，将提取的关键帧送到联合优化模块进行优化计算。关键帧的选择基于当前被跟踪的特征的数量以及与两个连续时间戳之间被跟踪的特征的平均视差，分别设置一个阈值，当被跟踪的特征的数量低于设置的阈值时或者两个连续时间戳之间被跟踪的特征点的平均视差大于设置的阈值时选取新的关键帧。

对传统相机获取的图像数据进行处理，先进行去畸变矫正等预处理工作。采用传统相机跟踪模块对图片进行处理：

对预处理之后的图片进行地标识别，依靠YOLOV5目标检测算法来实现(YOLOV5模型需要预先进行训练)，其具体步骤为：

(1)控制搭载传统相机的小车在布设有地标的场景中采集图像数据集；

(2)从采集的数据集中抽取部分图像进行目标标注，将地标标注出来；

(3)将标注好的图像划分为训练集和验证集，并在预训练网络的基础上进行训练；

(4)模型训练完成后就可以利用其进行地标的目标检测。

在标准图像帧上同样提取FAST角点，提取的特征点中可能存在动态物体的特征点导致后续跟踪过程中出现错误，需要剔除其中动态的特征点，剔除动态点的具体步骤为：

(1)利用最新的YOLOV5算法对每一帧图像进行处理，识别其中的所有物体；

(2)将识别结果中潜在的动态物体所在的矩形区域保存下来，保存的格式为(x₁,y₁,h,w)，x₁为区域左上角的横坐标，y₁为区域左上角的纵坐标，h为矩形区域的高度，w为矩形区域的宽度；

(3)通过静态一致性将潜在的运动物体区域中的特征点分为静态特征点和动态特征点，最后剔除动态的特征点。针对t时刻图像帧中的特征点i,j及其对应t+1时刻图像帧中的特征点i,j，用表示，定义同一时刻图像帧中的两特征点的距离为其中，u,v为特征点在像素坐标系中的横坐标和纵坐标。根据静态物体的一致性，如果两个特征点为静态特征点，则不同时刻图像帧中对应的特征点之间的距离是相同的。考虑到误差的存在，不同时刻图像帧中对应的特征点之间的距离差值在一定阈值范围内即可。阈值的确定由非潜在运动物体区域内的特征点来确定，其计算公式为/>S为非潜在运动物体区域内的特征点的集合，d_t(i,j)为t时刻S集合中任意两特征点之间的距离，N为S集合中的特征点的数目。

使用LK光流法对得到的静态特征点进行跟踪，在跟踪中的过程选择关键帧，关键帧的选择基于被跟踪的特征点数量以及与上一个关键帧的距离，分别设置一个阈值，当被跟踪的关键帧数量低于设定的阈值或与上一个关键帧的距离大于设定的阈值时，选取新的关键帧。

如果在利用目标检测算法识别地标的步骤中识别到了地标，由于地标本身的3D位置是已知的，完成地标识别后可以得到地标在图像中的2D坐标，可以得到一组对应的2D-3D点对，使用PNP算法求解可以得到当前时刻相对于地标的位姿估计，再将其转化为相对于世界坐标系的位姿估计：T_WC＝T_WL·T_LC，利用得到计算结果更新当前时刻图像的位姿。

利用事件相机跟踪模块和传统相机跟踪模块分别提供的信息，构建一个最小二乘问题，对目标函数进行最小化，使用G2O优化库求解该问题，输出联合优化后的位姿估计。整个优化过程都是基于关键帧进行，同时使用滑动窗口的策略，只对包含最后K个关键帧的滑动进行优化，在帧与帧之间，使用IMU测量来传播对传感器状态的预测。

目标函数为：

其中，等式左边的χ为待估计的变量，等式右边第一项为边缘化后剩余的先验残差，第二项为IMU预积分的残差，第三项为事件相机的测量残差，第四项为传统相机的测量残差。W_(·)为每一项残差对应的权重。

本发明使用事件相机/传统相机/IMU/自然路标结合的方案，能够克服传统视觉惯性里程计在高速高动态场景下无法工作的问题，同时利用在场景中预先布设已知位置信息的地标的方法在***中实现了类似回环检测的功能。此外还在传统相机跟踪模块中加入了剔除环境中动态点的步骤，提高了***的鲁棒性。

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (10)

1.基于事件相机/IMU/自然路标的里程计实现方法，其特征在于，包括如下步骤：

步骤一，事件相机跟踪

根据事件相机输出的事件流，基于SAE(Surface ofActive Events)直接在原始事件上提取特征点，使用LK光流法对提取的特征点进行跟踪并计算其逆深度后生成地图点，后续对地图点进行进一步地跟踪；

步骤二，传统相机跟踪

在标准图像帧上提取特征点并进行跟踪的同时加入目标检测模型YOLOV5用来识别预设自然地标，当检测到预设自然地标时使用PnP算法计算出位姿并更新当前帧的位姿；

步骤三，联合优化

通过非线性优化实现视觉惯性的融合，将视觉惯性的融合问题转化成对目标函数的最小化问题，该目标函数包括四个部分，边缘化后的先验误差，事件相机和传统相机的重投影误差，以及惯性测量的误差项，利用G2O优化库进行求解后得到精确的位姿估计。

2.根据权利要求1所述的基于事件相机/IMU/自然路标的里程计实现方法，其特征在于，所述事件使用带极性的TS来表示：其中T_p为事件的强度，x,y为像素坐标，t为事件发生的时间，p代表了像素强度变化的极性，S为该像素上一次触发事件的时间戳，τ代表了一个恒定的衰减率。

3.根据权利要求1所述的基于事件相机/IMU/自然路标的里程计实现方法，其特征在于，所述步骤一具体包括如下子步骤：

步骤1，根据事件相机提供的事件流来生成SAE，SAE记录了事件流中的每个像素上一次触发事件的时间戳，它是一个二维数组，数组中每个元素对应一个像素，其值对应上一次触发事件的时间戳；

步骤2，当接收到新的事件流时，使用Arc*算法在生成的SAE上提取角点，Arc*算法维护两个环形的事件集合，并检测连续的弧或其补弧是否在一定的范围内来提取角点；

步骤3，使用LK光流法对已经提取的特征点进行跟踪，跟踪成功的话则三角化生成地图点并进行进一步的跟踪，跟踪失败则丢弃该特征点，当特征点数量低于设定的阈值时提取新的特征点。

4.根据权利要求3所述的基于事件相机/IMU/自然路标的里程计实现方法，其特征在于，所述步骤2中Arc*算法提取特征点的具体步骤为：

(1)设定两个环形的事件集合，每个集合包含了最近的N个事件；

(2)当一个新事件到达时，将其加入到两个环形事件集合中，计算该事件与环形中其他事件之间的弧度，记录下连续的弧或其补弧的范围；

(3)对每个新加入的事件，检查其对应的连续弧或补弧是否在一定的范围内，如果是则将该事件标记为角点；

(4)从两个环形的事件集合中将最旧的事件删掉，以保证集合中只包含最近的N个事件。

5.根据权利要求3所述的基于事件相机/IMU/自然路标的里程计实现方法，其特征在于，所述步骤3中跟踪过程中进行关键帧的提取，将提取的关键帧送到联合优化模块进行优化计算；关键帧的选择基于当前被跟踪的特征的数量以及与两个连续时间戳之间被跟踪的特征的平均视差，分别设置一个阈值，当被跟踪的特征的数量低于设置的阈值时或者两个连续时间戳之间被跟踪的特征点的平均视差大于设置的阈值时选取新的关键帧。

6.根据权利要求1所述的基于事件相机/IMU/自然路标的里程计实现方法，其特征在于，所述步骤二具体包括如下子步骤：

步骤1，预先采集图像数据对YOLOV5模型进行训练，

步骤2，对传统相机的标准图像帧进行目标检测，识别图像中是否存在地标，

步骤3，在标准图像帧中提取特征点，根据目标检测的结果识别潜在的动态物体，利用静态一致性将潜在的动态物体区域中的特征点分为静态特征点和动态特征点，将动态的特征点剔除；

步骤4，使用光流法对提取到的静态特征点进行跟踪，

步骤5，如果步骤2中识别到了地标，利用PnP算法计算出位姿并更新此时的相机位姿。

7.根据权利要求6所述的基于事件相机/IMU/自然路标的里程计实现方法，其特征在于，所述步骤3中将动态的特征点剔除的具体步骤为：

(1)利用最新的YOLOV5算法对每一帧图像进行处理，识别其中的所有物体；

(2)将识别结果中潜在的动态物体所在的矩形区域保存下来，保存的格式为(x₁,y₁,h,w)，x₁为区域左上角的横坐标，y₁为区域左上角的纵坐标，h矩形区域的高度，w为矩形区域的宽度；

(3)通过静态一致性将潜在的运动物体区域中的特征点分为静态特征点和动态特征点，最后剔除动态的特征点；具体通过以下过程区分静态特征点和动态特征点：

针对t时刻图像帧中的特征点i,j及其对应t+1时刻图像帧中的特征点i,j，用表示，定义同一时刻图像帧中的两特征点的距离为：

其中，u,v为特征点在像素坐标系中的横坐标和纵坐标；当不同时刻图像帧中对应的特征点之间的距离是相同的或是距离差值在一定阈值范围内，则两个特征点为静态特征点，阈值由非潜在运动物体区域内的特征点来确定，其计算公式为：

其中，S为非潜在运动物体区域内的特征点的集合，d_t(i,j)为t时刻S集合中任意两特征点之间的距离，N为S集合中的特征点的数目。

8.根据权利要求6所述的基于事件相机/IMU/自然路标的里程计实现方法，其特征在于，步骤2和步骤5中通过识别人工地标来更新位姿估计的具体步骤为：

(1)从不同的角度采集预先在场景中布置好的预设自然地标的图像数据；

(2)在采集好的图像数据上进行目标标注，将预设自然地标的部分标注出来；

(3)使用标注好的数据集对模型进行训练；

(4)利用训练好的模型识别图像中的地标，识别成功后得到地标在世界坐标系中的3D坐标和在图像中的2D坐标，使用PnP算法求解得到位姿估计后实时更新位姿。

9.根据权利要求1所述的基于事件相机/IMU/自然路标的里程计实现方法，其特征在于，所述步骤三中的目标函数为：

其中，等式左边的χ为待估计的变量，等式右边第一项为边缘化后剩余的先验残差，第二项为IMU预积分的残差，第三项为事件相机的测量残差，第四项为传统相机的测量残差；W_(·)为每一项残差对应的权重。

10.基于事件相机/IMU/自然路标的里程计***，其特征在于，用于实现权利要求1-9中任意一项所述的基于事件相机/传统相机/IMU/自然路标的视觉里程计实现方法，包括事件相机跟踪模块，传统相机跟踪模块，联合优化模块；其中，

事件相机跟踪模块用于根据事件相机输出的事件流，基于SAE(Surface of ActiveEvents)直接在原始事件上提取特征点，使用LK光流法对提取的特征点进行跟踪并计算其逆深度后生成地图点，后续对地图点进行进一步地跟踪；

传统相机跟踪模块用于在标准图像帧上提取特征点并进行跟踪的同时加入目标检测模型YOLOV5用来识别预设自然地标，当检测到预设自然地标时使用PnP算法计算出位姿并更新当前帧的位姿；

联合优化模块用于通过非线性优化实现视觉惯性的融合，将视觉惯性的融合问题转化成对目标函数的最小化问题，该目标函数包括四个部分，边缘化后的先验误差，事件相机和传统相机的重投影误差，以及惯性测量的误差项，利用G2O优化库进行求解后得到精确的位姿估计。