一种纯旋转运动状态判定方法及装置

技术领域

本申请涉及传感器运动状态判定技术领域，尤其涉及一种纯旋转运动状态判定方法及装置。

背景技术

目前，SLAM(Simultaneous Localization And Mapping，即时定位与建图)技术越来越多的应用在增强现实、自动驾驶、移动机器人等领域。另外，基于视觉SLAM算法越来越多的受到科研人员的关注和应用，然而在视觉SLAM算法中经常会遇到目标设备仅作纯旋转的运动情况，这种退化的运动状态给算法实现带来诸多挑战和问题，例如纯旋转运动情况下多视图极限约束不成立、三角测量在纯旋转情况下会失效或者误差很大等。这些问题需要设备在特定时刻避免纯旋转运动情况，那么在算法实现的过程中就需要能够准确判定纯旋转的运动状态。

当前虽然存在纯旋转运动状态的判定的方法，如构造矩阵或者构造向量来实现状态判定，但是在构造矩阵或者构造向量的步骤中缺乏足够的理论支撑，另外，判定阈值是根据经验获取的，使得判定结果不够准确和鲁棒较差。

发明内容

本申请提供了一种纯旋转运动状态判定方法及装置，其发明目的在于，如何准确和鲁棒的实现纯旋转运动状态判定。

为了实现上述目的，本申请提供了以下技术方案：

一种纯旋转运动状态判定方法，应用于纯旋转运动状态的判定，该方法包括：

从前后两帧RGBD图像上提取视觉特征点，并获取前后两帧所述RGBD图像之间的IMU数据，所述视觉特征点包含特征点及其描述子，所述描述子表征特征点的局部特征信息，用于前后两帧RGBD图像之间特征点的匹配；

利用特征匹配算法得到前后两帧所述RGBD图像上匹配的特征点对，并筛选出具有有效深度的特征点对集合；

对前后两帧所述RGBD图像之间的IMU数据求积分，得到前后两帧所述RGBD图像之间的旋转矩阵；

根据预设算法计算前后两帧所述RGBD图像之间所有匹配特征点的重投影误差，并取所有匹配特征的重投影误差的平均值，得到误差平均值；

将所述误差平均值与预设阈值进行比较，当所述误差平均值小于所述预设阈值时，则确定当前处于纯旋转运动。

进一步的，所述利用特征匹配算法得到前后两帧所述RGBD图像上匹配的特征点对，并筛选出具有有效深度的特征点对集合，包括：

通过特征匹配算法获得前后两帧所述RGBD图像匹配的第一特征点对集合；

利用特征点深度信息筛选出特征点的深度在有效深度范围内的特征点对，作为具有有效深度的特征点对集合。

进一步的，所述对前后两帧所述RGBD图像之间的IMU数据求积分，得到前后两帧所述RGBD图像之间的旋转矩阵，包括：

假设前后两帧所述RGBD图像的IMU数据之间的时间间隔为dt，陀螺仪传感器测得的三轴角速度w

对G帧IMU数据进行积分，得到前后两帧所述RGBD图像之间的旋转矩阵ΔR

进一步的，所述根据预设算法计算前后两帧所述RGBD图像之间所有匹配特征点的重投影误差，并取所有匹配特征的重投影误差的平均值，得到误差平均值，包括：

由多视图几何相关知识，假设前后图像帧之间的旋转为R，平移为T，相机的内参矩阵为K；

由小孔相机模型可以得到后一帧图像上的一个特征点q

若q

根据预设公式计算所有匹配特征的重投影误差的平均值，得到所述误差平均值，所述预设公式为

一种纯旋转运动状态判定装置，应用于纯旋转运动状态的判定，该装置包括：

第一处理单元，用于从前后两帧所述RGBD图像上提取视觉特征点，并获取前后两帧所述RGBD图像之间的IMU数据，所述视觉特征点包含特征点及其描述子，所述描述子表征特征点的局部特征信息，用于前后两帧RGBD图像之间特征点的匹配；

第二处理单元，用于利用特征匹配算法得到前后两帧所述RGBD图像上匹配的特征点对，并筛选出具有有效深度的特征点对集合；

第三处理单元，用于对前后两帧所述RGBD图像之间的IMU数据求积分，得到前后两帧所述RGBD图像之间的旋转矩阵；

第四处理单元，用于根据预设算法计算前后两帧所述RGBD图像之间所有匹配特征点的重投影误差，并取所有匹配特征的重投影误差的平均值，得到误差平均值；

第五处理单元，用于将所述误差平均值与预设阈值进行比较，当所述误差平均值小于所述预设阈值时，则确定当前处于纯旋转运动。

进一步的，所述第二处理单元用于：

通过特征匹配算法获得前后两帧所述RGBD图像匹配的第一特征点对集合；

利用特征点深度信息筛选出特征点的深度在有效深度范围内的特征点对，作为具有有效深度的特征点对集合。

进一步的，所述第三处理单元用于：

假设前后两帧所述RGBD图像的陀螺仪数据之间的时间间隔为dt，陀螺仪传感器测得的三轴角速度w

对G帧IMU数据进行积分，得到前后两帧所述RGBD图像之间的旋转矩阵ΔR

进一步的，所述第四处理单元用于：

由多视图几何相关知识，假设前后图像帧之间的旋转为R，平移为T，相机的内参矩阵为K；

由小孔相机模型可以得到后一帧图像上的一个特征点q

若q

根据预设公式计算所有匹配特征的重投影误差的平均值，得到所述误差平均值，所述预设公式为

一种存储介质，所述存储介质包括存储的程序，其中，在所述程序运行时控制所述存储介质所在的设备执行如上述所述的纯旋转运动状态判定方法。

一种电子设备，所述电子设备包括至少一个处理器、以及与所述处理器连接的至少一个存储器、总线；其中，所述处理器、所述存储器通过所述总线完成相互间的通信；所述处理器用于调用所述存储器中的程序指令，以执行如上述所述的纯旋转运动状态判定方法。

本申请所述的纯旋转运动状态判定方法及装置，应用于纯旋转运动状态的判定，首先获取前后两帧RGBD图像；从前后两帧所述RGBD图像上提取视觉特征点，并获取前后两帧所述RGBD图像之间的IMU数据；再利用特征匹配算法得到前后两帧所述RGBD图像上匹配的特征点对，并筛选出具有有效深度的特征点对集合；然后对前后两帧所述RGBD图像之间的IMU数据求积分，得到前后两帧所述RGBD图像之间的旋转矩阵；根据预设算法计算前后两帧所述RGBD图像之间所有匹配特征点的重投影误差，并取所有匹配特征的重投影误差的平均值；最后将所述误差平均值与预设阈值进行比较，当所述误差平均值小于所述预设阈值时，则确定当前处于纯旋转运动。本申请利用RGBD相机和陀螺仪传感器，通过提取前后两帧所述RGBD图像上的视觉特征点和陀螺仪数据，来准确和鲁棒的实现纯旋转运动状态判定。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本申请实施例公开的一种纯旋转运动状态判定方法的流程示意图；

图2为本申请实施例公开的一种纯旋转运动状态判定装置的结构示意图；

图3为本申请实施例公开的一种电子设备的结构示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

请参见附图1，为本申请实施例提供的一种纯旋转运动状态判定方法流程示意图。如图1所示，本申请实施例提供了一种纯旋转运动状态判定方法，应用于纯旋转运动状态的判定，该方法包括：

S101：从前后两帧RGBD图像上提取视觉特征点，并获取前后两帧所述RGBD图像之间的陀螺仪数据。

在本步骤中，在进行视觉特征点提取和获取陀螺仪数据之前，需要先获取前后两帧RGBD图像，RGBD图像包含彩色(RGB)图像和深度(Depth)图像，并且RGB图像和Depth图像是经过配准的，也就是说彩色图像和深度图像中同一个坐标处的像素在场景中是对应相同的点。

需要说明的是，视觉特征点包含特征点及其描述子，本申请实施例并不限定特征提取方法，主流的特征提取算法有ORB(Oriented FAST and Rotated BRIEF)、SIFT(ScaleInvariant Feature Transform)、SURF(Speeded Up Robust Features)、BRISK(BinaryRobust Invariant Scalable Keypoints)等，这些算法都适用于本申请，描述子表征特征点的局部特征信息，主要用于前后帧之间特征点的匹配。

进一步的，本申请实施例中，陀螺仪传感器和RGBD相机固定在同一个运动平台上，相机坐标系和陀螺仪坐标系之间存在刚体变换，称之为为外参，包含旋转和平移，外参可以通过离线标定获取到。

本申请实施例中，在采集RGBD图像时，需要同步记录陀螺仪数据，然而由于陀螺仪数据的频率通常比图像数据的频率要高，两帧RGBD图像之间会包含多帧的陀螺仪数据。假设前后两帧RGBD图像分别用I

S102：利用特征匹配算法得到前后两帧所述RGBD图像上匹配的特征点对，并筛选出具有有效深度的特征点对集合。

需要说明的是，基于特征描述子的特征匹配的方法有很多，常用的特征匹配方法有KNN匹配、暴力匹配等，本申请不限定特征匹配方法。

在本步骤中，需要先用特征匹配算法初步获得两帧图像匹配的特征点对集合，因为深度相机可以获得图像中每个像素的深度值，即本申请实施例可以根据特征点坐标获取到每个特征点的深度，但是深度相机的测量距离是有限的，然后利用特征点深度信息筛选出那些特征点的深度在有效深度范围内的特征点对。

具体的，上述所述利用特征匹配算法得到前后两帧所述RGBD图像上匹配的特征点对，并筛选出具有有效深度的特征点对集合，包括如下步骤：

通过特征匹配算法获得前后两帧所述RGBD图像匹配的第一特征点对集合；

利用特征点深度信息筛选出特征点的深度在有效深度范围内的特征点对，作为具有有效深度的特征点对集合。

需要说明的是，假设具有有效深度的特征点对集合为{(p

S103：对前后两帧所述RGBD图像之间的陀螺仪数据求积分，得到前后两帧所述RGBD图像之间的旋转矩阵。

本申请实施例中，上述所述对前后两帧所述RGBD图像之间的陀螺仪数据求积分，得到前后两帧所述RGBD图像之间的旋转矩阵，包括如下步骤：

假设前后两帧所述RGBD图像的陀螺仪数据之间的时间间隔为dt，陀螺仪测得的三轴角速度w

对G帧陀螺仪数据进行积分，得到前后两帧所述RGBD图像之间的旋转矩阵ΔR

S104：根据预设算法计算前后两帧所述RGBD图像之间所有匹配特征点的重投影误差，并取所有匹配特征的重投影误差的平均值，得到误差平均值。

本申请实施例中，上述所述根据预设算法计算前后两帧所述RGBD图像之间所有匹配特征点的重投影误差，并取所有匹配特征的重投影误差的平均值，得到误差平均值，包括如下步骤：

由多视图几何相关知识，假设前后图像帧之间的旋转为R，平移为T，相机的内参矩阵为K。

需要说明的是，相机的内参矩阵为K也可以通过标定相机内参得到。

由小孔相机模型可以得到后一帧图像上的一个特征点q

需要说明的是，如果是纯旋转运动的话，T＝0，

若q

根据预设公式计算所有匹配特征的重投影误差的平均值，得到所述误差平均值，所述预设公式为

S105：将所述误差平均值与预设阈值进行比较。

本申请实施例中，上述所述的预设阈值需要根据经验和实践，并根据实际应用场景来设定，本申请不限定预设阈值的具体值。

S106：当所述误差平均值小于所述预设阈值时，则确定当前处于纯旋转运动。

S107：当所述误差平均值大于所述预设阈值时，则确定当前处于非纯旋转运动。

本申请实施例提供的一种纯旋转运动状态判定方法，应用于纯旋转运动状态的判定，首先获取前后两帧RGBD图像；从前后两帧所述RGBD图像上提取视觉特征点，并获取前后两帧所述RGBD图像之间的IMU数据；再利用特征匹配算法得到前后两帧所述RGBD图像上匹配的特征点对，并筛选出具有有效深度的特征点对集合；然后对前后两帧所述RGBD图像之间的IMU数据求积分，得到前后两帧所述RGBD图像之间的旋转矩阵；根据预设算法计算前后两帧所述RGBD图像之间所有匹配特征点的重投影误差，并取所有匹配特征的重投影误差的平均值；最后将所述误差平均值与预设阈值进行比较，当所述误差平均值小于所述预设阈值时，则确定当前处于纯旋转运动。本申请利用RGBD相机和陀螺仪传感器，通过提取前后两帧所述RGBD图像上的视觉特征点和IMU数据，来准确和鲁棒的实现纯旋转运动状态判定。

请参阅图2，基于上述实施例公开的一种纯旋转运动状态判定方法，本实施例对应公开了一种纯旋转运动状态判定装置，应用于纯旋转运动状态的判定，该装置包括：

第一处理单元201，用于从前后两帧所述RGBD图像上提取视觉特征点，并获取前后两帧所述RGBD图像之间的IMU数据，所述视觉特征点包含特征点及其描述子，所述描述子表征特征点的局部特征信息，用于前后两帧RGBD图像之间特征点的匹配；

第二处理单元202，用于利用特征匹配算法得到前后两帧所述RGBD图像上匹配的特征点对，并筛选出具有有效深度的特征点对集合；

第三处理单元203，用于对前后两帧所述RGBD图像之间的IMU数据求积分，得到前后两帧所述RGBD图像之间的旋转矩阵；

第四处理单元204，用于根据预设算法计算前后两帧所述RGBD图像之间所有匹配特征点的重投影误差，并取所有匹配特征的重投影误差的平均值，得到误差平均值；

第五处理单元205，用于将所述误差平均值与预设阈值进行比较，当所述误差平均值小于所述预设阈值时，则确定当前处于纯旋转运动。

进一步的，所述第二处理单元202用于：

通过特征匹配算法获得前后两帧所述RGBD图像匹配的第一特征点对集合；

利用特征点深度信息筛选出特征点的深度在有效深度范围内的特征点对，作为具有有效深度的特征点对集合。

进一步的，所述第三处理单元203用于：

假设前后两帧所述RGBD图像的陀螺仪数据之间的时间间隔为dt，陀螺仪传感器测得的三轴角速度w

对G帧IMU数据进行积分，得到前后两帧所述RGBD图像之间的旋转矩阵ΔR

进一步的，所述第四处理单元204用于：

由多视图几何相关知识，假设前后图像帧之间的旋转为R，平移为T，相机的内参矩阵为K；

由小孔相机模型可以得到后一帧图像上的一个特征点q

若q

根据预设公式计算所有匹配特征的重投影误差的平均值，得到所述误差平均值，所述预设公式为

所述纯旋转运动状态判定装置包括处理器和存储器，上述第一处理单元、第二处理单元、第三处理单元、第四处理单元和第五处理单元等均作为程序单元存储在存储器中，由处理器执行存储在存储器中的上述程序单元来实现相应的功能。

处理器中包含内核，由内核去存储器中调取相应的程序单元。内核可以设置一个或以上，通过调整内核参数来达到准确和鲁棒的实现纯旋转运动状态判定。

本申请实施例提供了一种存储介质，其上存储有程序，该程序被处理器执行时实现所述纯旋转运动状态判定方法。

本申请实施例提供了一种处理器，所述处理器用于运行程序，其中，所述程序运行时执行所述纯旋转运动状态判定方法。

本申请实施例提供了一种电子设备，如图3所示，该电子设备30包括至少一个处理器301、以及与所述处理器连接的至少一个存储器302、总线303；其中，所述处理器301、所述存储器302通过所述总线303完成相互间的通信；处理器301用于调用所述存储器302中的程序指令，以执行上述的所述纯旋转运动状态判定方法。

本文中的电子设备可以是服务器、PC、PAD、手机等。

本申请还提供了一种计算机程序产品，当在数据处理设备上执行时，适于执行初始化有如下方法步骤的程序：

从前后两帧RGBD图像上提取视觉特征点，并获取前后两帧所述RGBD图像之间的IMU数据，所述视觉特征点包含特征点及其描述子，所述描述子表征特征点的局部特征信息，用于前后两帧RGBD图像之间特征点的匹配；

利用特征匹配算法得到前后两帧所述RGBD图像上匹配的特征点对，并筛选出具有有效深度的特征点对集合；

对前后两帧所述RGBD图像之间的IMU数据求积分，得到前后两帧所述RGBD图像之间的旋转矩阵；

根据预设算法计算前后两帧所述RGBD图像之间所有匹配特征点的重投影误差，并取所有匹配特征的重投影误差的平均值，得到误差平均值；

将所述误差平均值与预设阈值进行比较，当所述误差平均值小于所述预设阈值时，则确定当前处于纯旋转运动。

进一步的，所述利用特征匹配算法得到前后两帧所述RGBD图像上匹配的特征点对，并筛选出具有有效深度的特征点对集合，包括：

通过特征匹配算法获得前后两帧所述RGBD图像匹配的第一特征点对集合；

利用特征点深度信息筛选出特征点的深度在有效深度范围内的特征点对，作为具有有效深度的特征点对集合。

进一步的，所述对前后两帧所述RGBD图像之间的IMU数据求积分，得到前后两帧所述RGBD图像之间的旋转矩阵，包括：

假设前后两帧所述RGBD图像的IMU数据之间的时间间隔为dt，陀螺仪传感器测得的三轴角速度w

对G帧IMU数据进行积分，得到前后两帧所述RGBD图像之间的旋转矩阵ΔR

进一步的，所述根据预设算法计算前后两帧所述RGBD图像之间所有匹配特征点的重投影误差，并取所有匹配特征的重投影误差的平均值，得到误差平均值，包括：

由多视图几何相关知识，假设前后图像帧之间的旋转为R，平移为T，相机的内参矩阵为K；

由小孔相机模型可以得到后一帧图像上的一个特征点q

若q

根据预设公式计算所有匹配特征的重投影误差的平均值，得到所述误差平均值，所述预设公式为

本申请是根据本申请实施例的方法、设备(系统)、计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

在一个典型的配置中，设备包括一个或多个处理器(CPU)、存储器和总线。设备还可以包括输入/输出接口、网络接口等。

存储器可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)，存储器包括至少一个存储芯片。存储器是计算机可读介质的示例。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括要素的过程、方法、商品或者设备中还存在另外的相同要素。

本领域技术人员应明白，本申请的实施例可提供为方法、系统或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

以上仅为本申请的实施例而已，并不用于限制本申请。对于本领域技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本申请的权利要求范围之内。