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

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

【技术实现步骤摘要】
一种纯旋转运动状态判定方法及装置
本申请涉及传感器运动状态判定
，尤其涉及一种纯旋转运动状态判定方法及装置。
技术介绍
目前，SLAM(SimultaneousLocalizationAndMapping，即时定位与建图)技术越来越多的应用在增强现实、自动驾驶、移动机器人等领域。另外，基于视觉SLAM算法越来越多的受到科研人员的关注和应用，然而在视觉SLAM算法中经常会遇到目标设备仅作纯旋转的运动情况，这种退化的运动状态给算法实现带来诸多挑战和问题，例如纯旋转运动情况下多视图极限约束不成立、三角测量在纯旋转情况下会失效或者误差很大等。这些问题需要设备在特定时刻避免纯旋转运动情况，那么在算法实现的过程中就需要能够准确判定纯旋转的运动状态。当前虽然存在纯旋转运动状态的判定的方法，如构造矩阵或者构造向量来实现状态判定，但是在构造矩阵或者构造向量的步骤中缺乏足够的理论支撑，另外，判定阈值是根据经验获取的，使得判定结果不够准确和鲁棒较差。
技术实现思路
本申请提供了一种纯旋转运动状态判定方法及装置，其专利技术目的在于，如何准确和鲁棒的实现纯旋转运动状态判定。为了实现上述目的，本申请提供了以下技术方案：一种纯旋转运动状态判定方法，应用于纯旋转运动状态的判定，该方法包括：从前后两帧RGBD图像上提取视觉特征点，并获取前后两帧所述RGBD图像之间的IMU数据，所述视觉特征点包含特征点及其描述子，所述描述子表征特征点的局部特征信息，用于前后两帧RGBD图像之间特征点的匹配；r>利用特征匹配算法得到前后两帧所述RGBD图像上匹配的特征点对，并筛选出具有有效深度的特征点对集合；对前后两帧所述RGBD图像之间的IMU数据求积分，得到前后两帧所述RGBD图像之间的旋转矩阵；根据预设算法计算前后两帧所述RGBD图像之间所有匹配特征点的重投影误差，并取所有匹配特征的重投影误差的平均值，得到误差平均值；将所述误差平均值与预设阈值进行比较，当所述误差平均值小于所述预设阈值时，则确定当前处于纯旋转运动。进一步的，所述利用特征匹配算法得到前后两帧所述RGBD图像上匹配的特征点对，并筛选出具有有效深度的特征点对集合，包括：通过特征匹配算法获得前后两帧所述RGBD图像匹配的第一特征点对集合；利用特征点深度信息筛选出特征点的深度在有效深度范围内的特征点对，作为具有有效深度的特征点对集合。进一步的，所述对前后两帧所述RGBD图像之间的IMU数据求积分，得到前后两帧所述RGBD图像之间的旋转矩阵，包括：假设前后两帧所述RGBD图像的IMU数据之间的时间间隔为dt，陀螺仪传感器测得的三轴角速度wg，则前后两帧所述RGBD图像IMU数据之间的三轴旋转角为φ＝wgΔt，由罗德里格斯公式确定前后两帧陀螺仪之间的3*3旋转矩阵ΔRg，其中，||φ||为旋转角的模值，φΛ为旋转角的反对称矩阵，I为3*3的单位矩阵，设φ＝[x,y,z]T，则对G帧IMU数据进行积分，得到前后两帧所述RGBD图像之间的旋转矩阵ΔRb，设陀螺仪到相机的旋转外参为则图像帧Iq到图像帧Ip之间的旋转矩阵进一步的，所述根据预设算法计算前后两帧所述RGBD图像之间所有匹配特征点的重投影误差，并取所有匹配特征的重投影误差的平均值，得到误差平均值，包括：由多视图几何相关知识，假设前后图像帧之间的旋转为R，平移为T，相机的内参矩阵为K；由小孔相机模型可以得到后一帧图像上的一个特征点qj投影到前一帧图像上坐标p的计算公式为p＝K·(R·dj·K-1qj+T)，其中，dj为qj的深度值；若qj对应前一帧图像上匹配的特征点为pi，则对于第s对特征点来说，重投影误差为：根据预设公式计算所有匹配特征的重投影误差的平均值，得到所述误差平均值，所述预设公式为其中，s＝1,2,...,S。一种纯旋转运动状态判定装置，应用于纯旋转运动状态的判定，该装置包括：第一处理单元，用于从前后两帧所述RGBD图像上提取视觉特征点，并获取前后两帧所述RGBD图像之间的IMU数据，所述视觉特征点包含特征点及其描述子，所述描述子表征特征点的局部特征信息，用于前后两帧RGBD图像之间特征点的匹配；第二处理单元，用于利用特征匹配算法得到前后两帧所述RGBD图像上匹配的特征点对，并筛选出具有有效深度的特征点对集合；第三处理单元，用于对前后两帧所述RGBD图像之间的IMU数据求积分，得到前后两帧所述RGBD图像之间的旋转矩阵；第四处理单元，用于根据预设算法计算前后两帧所述RGBD图像之间所有匹配特征点的重投影误差，并取所有匹配特征的重投影误差的平均值，得到误差平均值；第五处理单元，用于将所述误差平均值与预设阈值进行比较，当所述误差平均值小于所述预设阈值时，则确定当前处于纯旋转运动。进一步的，所述第二处理单元用于：通过特征匹配算法获得前后两帧所述RGBD图像匹配的第一特征点对集合；利用特征点深度信息筛选出特征点的深度在有效深度范围内的特征点对，作为具有有效深度的特征点对集合。进一步的，所述第三处理单元用于：假设前后两帧所述RGBD图像的陀螺仪数据之间的时间间隔为dt，陀螺仪传感器测得的三轴角速度wg，则前后两帧所述RGBD图像的IMU数据之间的三轴旋转角为φ＝wgΔt，由罗德里格斯公式确定前后两帧陀螺仪之间的3*3旋转矩阵ΔRg，其中，||φ||为旋转角的模值，φΛ为旋转角的反对称矩阵，I为3*3的单位矩阵，设φ＝[x,y,z]T，则对G帧IMU数据进行积分，得到前后两帧所述RGBD图像之间的旋转矩阵ΔRb，设陀螺仪到相机的旋转外参为则图像帧Iq到图像帧Ip之间的旋转矩阵进一步的，所述第四处理单元用于：由多视图几何相关知识，假设前后图像帧之间的旋转为R，平移为T，相机的内参矩阵为K；由小孔相机模型可以得到后一帧图像上的一个特征点qj投影到前一帧图像上坐标p的计算公式为p＝K·(R·dj·K-1qj+T)，其中，dj为qj的深度值；若qj对应前一帧图像上匹配的特征点为pi，则对于第s对特征点来说，重投影误差为：根据预设公式计算所有匹配特征的重投影误差的平均值，得到所述误差平均值，所述预设公式为其中，s＝1,2,...,S。一种存储介质，所述存储介质包括存储的程序，其中，在所述程序运行时控制所述存储介质所在的设备执行如上述所述的纯旋转运动状态判定方法。一种电子设备，所述电子设备包括至少一个处理器、以及与所述处理器连接的至少一个存储器、总线；其中，所述处理器、所述存储器通过所述总线完成相互间的通信；所述处理器用于调用所述存储器中的程序指令，以执行如上述所述的纯旋转运动状态判定方法。本申请所述的纯旋转运动状态判定方法及装置，应用于纯旋转运动状态的判定，首先获取前后两帧RGBD图像；从前后两帧所述RGBD图像上提取视觉特征点，并获取前后两帧所述RGBD图像之间的IMU数据；本文档来自技高网...

【技术保护点】
1.一种纯旋转运动状态判定方法，其特征在于，应用于纯旋转运动状态的判定，该方法包括：/n从前后两帧RGBD图像上提取视觉特征点，并获取前后两帧所述RGBD图像之间的IMU数据，所述视觉特征点包含特征点及其描述子，所述描述子表征特征点的局部特征信息，用于前后两帧RGBD图像之间特征点的匹配；/n利用特征匹配算法得到前后两帧所述RGBD图像上匹配的特征点对，并筛选出具有有效深度的特征点对集合；/n对前后两帧所述RGBD图像之间的IMU数据求积分，得到前后两帧所述RGBD图像之间的旋转矩阵；/n根据预设算法计算前后两帧所述RGBD图像之间所有匹配特征点的重投影误差，并取所有匹配特征的重投影误差的平均值，得到误差平均值；/n将所述误差平均值与预设阈值进行比较，当所述误差平均值小于所述预设阈值时，则确定当前处于纯旋转运动。/n

【技术特征摘要】
1.一种纯旋转运动状态判定方法，其特征在于，应用于纯旋转运动状态的判定，该方法包括：
从前后两帧RGBD图像上提取视觉特征点，并获取前后两帧所述RGBD图像之间的IMU数据，所述视觉特征点包含特征点及其描述子，所述描述子表征特征点的局部特征信息，用于前后两帧RGBD图像之间特征点的匹配；
利用特征匹配算法得到前后两帧所述RGBD图像上匹配的特征点对，并筛选出具有有效深度的特征点对集合；
对前后两帧所述RGBD图像之间的IMU数据求积分，得到前后两帧所述RGBD图像之间的旋转矩阵；
根据预设算法计算前后两帧所述RGBD图像之间所有匹配特征点的重投影误差，并取所有匹配特征的重投影误差的平均值，得到误差平均值；
将所述误差平均值与预设阈值进行比较，当所述误差平均值小于所述预设阈值时，则确定当前处于纯旋转运动。


2.根据权利要求1所述的方法，其特征在于，所述利用特征匹配算法得到前后两帧所述RGBD图像上匹配的特征点对，并筛选出具有有效深度的特征点对集合，包括：
通过特征匹配算法获得前后两帧所述RGBD图像匹配的第一特征点对集合；
利用特征点深度信息筛选出特征点的深度在有效深度范围内的特征点对，作为具有有效深度的特征点对集合。


3.根据权利要求1所述的方法，其特征在于，所述对前后两帧所述RGBD图像之间的IMU数据求积分，得到前后两帧所述RGBD图像之间的旋转矩阵，包括：
假设前后两帧所述RGBD图像的IMU数据之间的时间间隔为dt，陀螺仪传感器测得的三轴角速度wg，则前后两帧所述RGBD图像IMU数据之间的三轴旋转角为φ＝wgΔt，由罗德里格斯公式确定前后两帧陀螺仪之间的3*3旋转矩阵ΔRg，其中，||φ||为旋转角的模值，φΛ为旋转角的反对称矩阵，I为3*3的单位矩阵，设φ＝[x,y,z]T，则
对G帧IMU数据进行积分，得到前后两帧所述RGBD图像之间的旋转矩阵ΔRb，设陀螺仪到相机的旋转外参为则图像帧Iq到图像帧Ip之间的旋转矩阵


4.根据权利要求1所述的方法，其特征在于，所述根据预设算法计算前后两帧所述RGBD图像之间所有匹配特征点的重投影误差，并取所有匹配特征的重投影误差的平均值，得到误差平均值，包括：
由多视图几何相关知识，假设前后图像帧之间的旋转为R，平移为T，相机的内参矩阵为K；
由小孔相机模型可以得到后一帧图像上的一个特征点qj投影到前一帧图像上坐标p的计算公式为p＝K·(R·dj·K-1qj+T)，其中，dj为qj的深度值；
若qj对应前一帧图像上匹配的特征点为pi，则对于第s对特征点来说，重投影误差为：
根据预设公式计算所有匹配特征的重投影误差的平均值，得到所述误差平均值，所述预设公式为其中，s＝1,2,...,S。


5.一种纯旋转运动状态判定装置，其特征在于，应用于纯旋转运动状态的判定，该装置包括：
第一处理单元，用于从前后两帧所述RGBD图...

【专利技术属性】
技术研发人员：孙赛，李骊，
申请(专利权)人：北京华捷艾米科技有限公司，
类型：发明
国别省市：北京;11