CN109917644B - 一种提高视觉惯导***鲁棒性的方法、装置和机器人设备 - Google Patents
一种提高视觉惯导***鲁棒性的方法、装置和机器人设备 Download PDFInfo
- Publication number
- CN109917644B CN109917644B CN201811603305.6A CN201811603305A CN109917644B CN 109917644 B CN109917644 B CN 109917644B CN 201811603305 A CN201811603305 A CN 201811603305A CN 109917644 B CN109917644 B CN 109917644B
- Authority
- CN
- China
- Prior art keywords
- frame
- oldest
- reference frame
- frames
- next new
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
- G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/22—Matching criteria, e.g. proximity measures
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/25—Fusion techniques
- G06F18/254—Fusion techniques of classification results, e.g. of results related to same input data
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
- G06T7/248—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments involving reference images or patches
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/269—Analysis of motion using gradient-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/77—Processing image or video features in feature spaces; using data integration or data reduction, e.g. principal component analysis [PCA] or independent component analysis [ICA] or self-organising maps [SOM]; Blind source separation
- G06V10/80—Fusion, i.e. combining data from various sources at the sensor level, preprocessing level, feature extraction level or classification level
- G06V10/809—Fusion, i.e. combining data from various sources at the sensor level, preprocessing level, feature extraction level or classification level of classification results, e.g. where the classifiers operate on the same input data
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/10—Terrestrial scenes
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/20—Scenes; Scene-specific elements in augmented reality scenes
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10016—Video; Image sequence
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30244—Camera pose
Landscapes
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Evolutionary Computation (AREA)
- Artificial Intelligence (AREA)
- Data Mining & Analysis (AREA)
- Software Systems (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Medical Informatics (AREA)
- Databases & Information Systems (AREA)
- Computing Systems (AREA)
- Life Sciences & Earth Sciences (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Evolutionary Biology (AREA)
- General Engineering & Computer Science (AREA)
- Image Analysis (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及视觉和惯导融合的同时定位与地图构建领域,特别是涉及一种提高视觉惯导***鲁棒性的方法、装置和机器人设备,其中所述提供视觉惯导***鲁棒性的方法,通过确定参考帧;对所述参考帧进行修正;根据最老帧和修正后的参考帧,对除了所述参考帧和最老帧之外的帧进行非线性优化,完成对图形在静止或匀速状态下,将新的视觉约束引入到***中去,优化了现有的视觉约束和惯性约束机制,很好的解决了轨迹飘走的问题,显著提高了***的鲁棒性。
Description
技术领域
本发明实施例涉及视觉和惯导融合的同时定位与地图构建领域,特别是涉及一种提高视觉惯导***鲁棒性的方法、装置和机器人设备。
背景技术
同时定位与地图构建***(SLAM,Simultaneous localization and mapping,以下简称SLAM)是指搭载特定传感器的主体,如机器人、无人机、AR设备等,在没有环境先验信息的情况下,于运动过程中建立环境的模型,同时估计自己运动的过程。SLAM中的传感器可以为视觉相机、IMU、GPS、激光雷达等,本文中讨论的传感器主要为视觉相机和IMU,通常称这种基于视觉和观星测量单元IMU(Inertial Measurement Unit)融合的SLAM***为VINS(visual-inertial system),或VIO(visual-inertial odometry),即视觉惯导***。
VIO按照视觉和IMU的状态向量是否放在一起优化,可分为松耦合和紧耦合,在紧耦合VIO***中,通过将视觉提供的重投影误差的视觉约束,和IMU提供的帧间约束,放到一个整体的目标函数中进行优化。但是,当出现快速旋转、快速移动、相机遮挡或无纹理等情况,会导致视觉约束丢失,这种情况在AR、机器人等中是经常会出现的,以手机等AR设备为例,当快速运动时,因为运动模糊等原因,会导致图像质量发生模糊,此时想在这种图像上进行视觉的特征点检测和跟踪将变得不现实。
当视觉约束丢失时,VIO***的整体目标函数中仅有IMU约束起作用,但是又因为IMU的累积误差,将导致计算的状态向量出现错误,当视觉丢失后静止不动时,相机轨迹仍会沿某个方向加速飘走。
发明内容
鉴于上述问题,提出了本发明实施例以便提供一种克服上述问题的方法、装置、机器人设备及计算机存储介质。
为解决上述技术问题,本发明实施例采用的一个技术方案是:提供一种提高视觉惯导***鲁棒性的方法,包括
确定参考帧;
对所述参考帧进行修正;
根据最老帧和修正后的参考帧,对除了所述参考帧和最老帧之外的帧进行非线性优化。
所述参考帧为相同帧的次新帧。
所述确定参考帧,进一步包括确定需要边缘化的帧,包括:
根据当前帧,判断最老帧是否是需要边缘化的帧;
如果是,则将最老帧确定为需要边缘化的帧;
如果不是,则进一步判断当前帧和次新帧是否相同,如果相同,则将次新帧确定为参考帧,并将最老帧确定为需要边缘化的帧。
如果当前帧和次新帧不相同,则将次新帧确定为需要边缘化的帧,并在后续不对参考帧进行修正。
所述判断当前帧和次新帧是否相同,具体为:
判断当前帧和次新帧跟踪成功的点数是否小于预先设定的阈值,如果不小于,则认为当前帧和次新帧相同;如果小于,则认为当前帧和次新帧不同。
所述修正参考帧,具体为对参考帧的速度和旋转进行修正。
所述修正参考帧,具体为:
所述将除了最老帧和参考帧之外的其他帧进行非线性优化,进一步包括:
判断需要边缘化的帧是否是最老帧,如果是,则在将除了最老帧和参考帧之外的其他帧进行线性化后,边缘化最老帧,将参考帧向最老帧方向平移一帧。
所述将除了最老帧和参考帧之外的其他帧进行非线性优化,进一步包括:
判断需要边缘化的帧是否是最老帧,如果不是,则在将除了最老帧和参考帧之外的其他帧进行非线性化后,边缘化次新帧。
所述对除了参考帧和最老帧之外的帧进行非线性优化之前,进一步包括:
判断所述参考帧是否在滑动窗口内,如果不在,则对所有帧进行非线性优化。
本发明实施例还提出了一种提高视觉惯导***鲁棒性的装置,包括:
参考帧确定模块:用于确定当前滑动窗口内的参考帧;
参考帧修正模块:用于对参考帧进行修正;
非线性优化模块:用于根据最老帧和修正后的参考帧,对除了所述参考帧和最老帧之外的帧进行非线性优化。
本发明实施例还提供一种机器人,包括处理器、存储器、通信接口和通信总线,所述处理器、所述存储器和所述通信接口通过所述通信总线完成相互间的通信;
所述存储器用于存放至少一可执行指令,所述可执行指令使所述处理器执行如上述实施例中所述提高视觉惯导***鲁棒性的方法对应的操作。
本发明实施例还提出了一种计算机存储介质,所述存储介质中存储有至少一可执行指令,所述可执行指令使处理器执行上述方法实施例中所述的提高视觉惯导***鲁棒性的方法对应的操作。
由上可知,本发明实施例通过对参考帧进行修正,然后固定住参考帧和最老帧,将新的视觉约束引入到***中去,优化了现有的视觉约束和惯性约束机制,很好的解决了轨迹飘走的问题,显著提高了***的鲁棒性。
上述说明仅是本发明技术方案的概述,为了能够更清楚了解本发明的技术手段,而可依照说明书的内容予以实施,并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂,以下特举本发明的具体实施例。
附图说明
通过阅读下文优选实施例的详细描述,各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施例的目的,而并不认为是对本发明的限制。而且在整个附图中,用相同的参考符号表示相同的部件。在附图中:
图1是本发明实施例提供的滑动窗口内各帧状态一;
图2是本发明实施例提供的滑动窗口内各帧状态二;
图3是本发明实施例提供的滑动窗口内各帧状态三;
图4是本发明实施例提供的滑动窗口内各帧状态四;
图5是本发明实施例提供的提高视觉惯导******鲁棒性的流程图一;
图6是本发明实施例提供的提高视觉惯导******鲁棒性的流程图二;
图7-1是本发明实施例提供的滑动窗口内各帧状态五;
图7-2是本发明实施例提供的滑动窗口内各帧状态六;
图7-3是本发明实施例提供的滑动窗口内各帧状态七;
图7-4是本发明实施例提供的滑动窗口内各帧状态八;
图8-1是本发明实施例提供的滑动窗口内各帧状态九;
图8-2是本发明实施例提供的滑动窗口内各帧状态十;
图8-3是本发明实施例提供的滑动窗口内各帧状态十一;
图9-1是本发明实施例提供的滑动窗口内各帧状态十二;
图9-2是本发明实施例提供的滑动窗口内各帧状态十三;
图9-3是本发明实施例提供的滑动窗口内各帧状态十四;
图10是本发明实施例提供的提高视觉惯导***鲁棒性的装置;
图11是本发明实施例提供机器人结构图。
具体实施方式
下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例,然而应当理解,可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反,提供这些实施例是为了能够更透彻地理解本公开,并且能够将本公开的范围完整的传达给本领域的技术人员。
本发明实施例提供的技术方案,是当图像采集设备发生视觉丢失后静止不动时,通过添加新的视觉约束,修正静止图像帧的旋转矩阵,来阻止轨迹继续飘走。我们假设图像采集设备快速移动后会静止,而且该假设是合理的,比如在AR中,人们经常会拿着手机突然转向另外一个方向,并停止不动进行观察,我们的操作就发生在停止不动的过程中。
下面将针对本发明实施例的工作背景进行说明。在VIO***中,我们采用在一个滑动窗口内进行优化的思路,如图1所示,这里假设滑动窗口内能容纳5帧图像,圆型框表示相应第i帧的位置和旋转:其中,表示从IMU的局部坐标系bi到世界坐标系w的旋转,表示从IMU的局部坐标系bi到世界坐标系w的平移。方型框表示第i帧的速度和bias:其中,表示从IMU的局部坐标系bi到世界坐标系w的速度,ba和bg分别表示acc(加速度计)和gyr(陀螺仪)的bias偏差。黑色菱形小方块表示视觉约束,即重投影误差,黑色正方形小方块表示帧间的IMU约束。最上方的Map Points是表示滑窗内所有帧可以共同观测到的路标点。
当采集点在快速旋转过程中时,会发生视觉约束丢失,如图2所示,P5为快速旋转过程中采集的图像,因为运动模糊等问题,视觉跟踪失败,导致视觉约束丢失(图中虚框处)。这时所有状态向量只靠IMU约束进行优化,为了保持活动窗口内帧数固定,我们将边缘化掉最老帧P0。
当采集点在快速旋转后突然静止时,虽然图像质量清晰,但因为静止后的第一帧P6仍无法成功跟踪到旋转过程中的模糊图像P5,因此仍无法引入视觉约束,如图3所示。这时,仍会将最老帧P1边缘化掉。此时在旋转过程中计算的错误状态仍无法得到修正,导致P6仍然有问题。
当静止后的第二帧P7进到滑动窗口时,如图4所示,因与P6的图像很相似,视差量过小,无法进行三角化,导致仍无法引入视觉约束。此时,P6与P7的图像很相似,因此会选择边缘化掉P6。这时若一直静止下去,将导致轨迹发生直线漂移。
本发明实施例考虑到静止后的图像清晰,且根据光流跟踪结果可明显知道两帧之间(P6与P7)跟踪匹配关系,本发明实施例提出将这种约束加入到整体的目标函数中,来修正旋转过程中基于纯IMU计算错误的过程量,解决位姿飘走问题。
如图5所示,本发明实施例提出了一种提高视觉惯导***鲁棒性的方法,包括:
501:确定参考帧。
本发明实施例仍然采用在滑动窗口内进行优化的方式进行,当有新的帧进入滑动窗口内时,***将对比当前帧和次新帧,确定参考帧和需要边缘化的帧。
如图4所示,第0帧为P2,第4帧P5为模糊的动态帧,第5帧P6为第一静止帧。当P7进入滑动窗口时,P7也是静止帧。
通常情况下,当P7进入滑动窗口时,***会根据当前帧的跟踪点数和相邻帧的视差量,来决定需要边缘化的帧,比如:当P7进入滑动窗口时,如果P6和P7视差量较小,则需要边缘化掉次新帧P6,确定次新帧为需要边缘化的帧;当P6和P7视差量较大时,则认为P7具有较多的代表信息,则选择最老帧进行边缘化,确定最老帧为需要边缘化的帧。
我们假设需要边缘化的帧为次新帧,则***进一步判断P7和P6是否相同,即是否像P6一样为静止帧,通常我们会根据当前帧和次新帧跟踪成功的点数是否大于某个阈值,来进行判断。当大与预先设定的阈值时,则认为当前帧和次新帧相同,即相机处于静止状态。
当当前帧和次新帧相同时,***将最老帧确定为需要边缘化的帧;当当前帧和次新帧不相同时,***将次新帧确定为需要边缘化的帧。
默认的,***将不会设置初始参考帧,即当***中不存在静止帧时,***将不存在参考帧,只有在有两个及以上个为静止帧时,***才会设置参考帧。
502:对确定好的参考帧进行修正。
当判断当前帧和次新帧都处于静止状态时,考虑到静止后的图像清晰,且根据光流跟踪结果可明显知道两帧之间(P6与P7)跟踪匹配关系,但是目前的VIO框架并未考虑这种约束,我们把这种约束加入到整体的目标函数中,来修正旋转过程中基于纯IMU计算错误的过程量,我们可以对参考帧的速度和旋转进行修正,以解决位姿飘走问题。
503:根据最老帧和修正后的参考帧,将除了最老帧和参考帧之外的其他帧进行非线性化。
***固定住修改后的参考帧和最老帧,并在非线性优化求解过程中,不对这两帧进行优化,基于修正后的参考帧和最老帧,只对除了这两帧以外的其他帧进行非线性优化求解。
当确定完参考帧后,***会判断参考帧在滑动窗口内,如果不在,则以常规的方式进行非线性优化求解,对每一帧都进行非线性优化。当参考帧在滑动窗口内时,***将固定住最老帧P2和参考帧P6,不对其进行非线性优化,只对除了最老帧P2和参考帧P6之外的其他帧进行非线性优化求解。
进一步的,判断需要边缘化的帧是否是最老帧,如果是,则在将除了最老帧和参考帧之外的其他帧进行线性化后,将参考帧向最老帧方向平移一帧,保持当前的参考帧在下一图像到来时,仍旧是参考帧。如果需要边缘化的帧不是最老帧,是次新帧,则直接进行非线性优化,即当前静止帧,在下一图像到来时,作为参考帧。
本发明实施例通过上述方法,通过对参考帧进行修正,然后固定住参考帧和最老帧,将新的视觉约束引入到***中去,优化了现有的视觉约束和惯性约束机制,很好的解决了轨迹飘走的问题,显著提高了***的鲁棒性。
本发明另一实施例给出了详细的流程图,如图6所示。假设基于图7-1的帧模式,进行详细的描述,图3中P5帧为动态帧,P6为第一帧静止帧。我们假设滑动窗口长度为N=5,***中的最老帧为第0帧,设置MargFlag为需要边缘化帧标示位,OLD代表最老帧,SECOND_NEW代表次新帧,NumNeedFix为参考帧标示位,NumNeedFix初始值设为-1。
601:对进入滑动窗口的当前帧进行判断,是否需要边缘化最老帧。
***通常根据当前帧跟踪点数和相邻帧的视差量判断需要边缘化的帧,这里最老帧为第0帧,滑动窗口总帧数为N+1,则当前帧为第N帧,次新帧为N-1帧。
如果当前帧和次新帧视差量比较大,则认为当前帧为比较重要的帧,则边缘化最老的帧;
如果当前帧和次新帧视差量比较小,则认为当前帧和次新帧类似,则边缘化掉次新帧。
在图3中,P5动态帧,P6为第一帧静止帧,因此P5和P6的市差量较大,则需要边缘化的帧为最老帧P1。
根据图6,则转步骤605.
605:将需要边缘化的帧设置为最老帧。
设置MargFlag=OLD,转步骤607.
607:判断NumNeedFix是否大于0.
根据***的预设NumNeedFix初始值为-1,因此NumNeedFix<0,转步骤611.
611:非线性优化求解。
基于修正后的参考帧和最老帧,对滑动窗口内的所有帧进行非线性化优化。
612:判断MargFlag的值。
如果MargFlag==OLD,边缘化掉最老帧P1;
如果MargFlag==SECOND_NEW,则边缘化掉次新帧;
由图3可知,***将边缘化掉最老帧P1。然后***持续监测下一帧的到来。
如图7-1所示,假设帧P7进入了滑动窗口,我们假设P7为静止帧,并且P6和P7为相同的图像。***将进行如下流程:
601:判断是否边缘化最老帧。
由于P6和P7同样为静止帧,两者的视差量很小,因此,***需要边缘化掉次新帧,保留最老帧,如图7-1所示,并转步骤602.
602:判断当前帧和次新帧跟踪成功的点数是否大于某个阈值。
根据当前帧和次新帧跟踪成功的点数,比较P6和P7帧的差异,通常我们设置97%为阈值,如果两帧的跟踪成功的点数比例大于97%,则认为两副图像是相同的图像,则转步骤604。如果小于97%,我们认为两幅图像不完全相同,则转步骤603.
604:设置MargFlag=OLD。
当P7和P6为相同的两幅图像时,将需要边缘化的帧设置为最老帧,设置MargFlag=OLD,如图7-2所示。并转步骤606.
606:对次新帧的速度和旋转进行修正,并将NumNeedFix设置为N-1。
当判断当前帧和次新帧都处于静止状态时,考虑到静止后的图像清晰,且根据光流跟踪结果可明显知道两帧之间(P6与P7)跟踪匹配关系,但是目前的VIO框架并未考虑这种约束,若可以把这种约束加入到整体的目标函数中,来修正旋转过程中基于纯IMU计算错误的过程量,从而解决位姿飘走问题。
当图像静止时,即相邻两帧的跟踪点数大于某个阈值后,这两帧的速度差为零。
另外,根据IMU预积分的速度测量误差方程,可得:
我们将公式(1)整理为如下形式:
因为图像静止,所以第k帧和第k+1帧的速度为0,即:
可将公式(1)化简为:
由此得出:
通过分析公式(4)可以发现,当计算错误时,会导致IMU预积分出来的速度变化量,无法很好的抵消掉因重力加速度产生的速度变化,从而产生一个多余的不希望出现的速度分量。这时,我们可以根据公式(5)重新计算出一个正确的来作为次新帧即第k帧的旋转状态。
将NumNeedFix从-1,修改为:N-1。
607:判断参考帧指示位NumNeedFix是否大于0,如果是则转步骤608,如果不是,则转611。
由上一步607可以,NumNeedFix=N-1,N为5,则NumNeedFix=4,所以NumNeedFix>0,转步骤608。
608:固定住最老帧和第NumNeedFix帧。
即固定住最老帧P2和参考帧P6,所述的固定指在非线性优化求解过程中,不对最老帧和参考帧进行非线性优化求解,只对其他帧进行非线性优化求解。
609:判断需要边缘化的帧是否是最老帧。
即判断MargFlag是否等于OLD,如果等于,则转步骤610,如果不等于,则转步骤611。
由步骤604可知,MargFlag==OLD。
610:将参考帧向最老帧方向平移一帧。
即将参考帧指示位NumNeedFix修改为NumNeedFix-1,即NumNeedFix=3。
611:非线性优化求解。
由于在步骤608中对最老帧P2和参考帧P6进行了固定,即不参与非线性优化求解,因此***基于修正后的参考帧和最老帧,只对除了最老帧P2和参考帧P6之外的所有帧进行非线性优化求解。非线性优化求解的方式与现有的非线性优化求解的方式相同,在这里不再赘述。
612:判断需要边缘化的帧,如果是最老帧,则转步骤614,如果是次新帧,则转613。
***判断MargFlag的值,根据需要边缘化的帧指示位边缘化掉相应的帧。
613:如果MargFlag==SECOND_NEW,则边缘化掉次新帧。
614:如果MargFlag==OLD,则边缘化掉最老帧P2。
由步骤604可知,***将MargFlag由SECOND_NEW更改为了OLD,所以,MargFlag=OLD,边缘化掉最老帧P2。最终优化后的帧结构如图7-3所示,被边缘化掉的帧为P2,最老帧为P3,参考帧变为P6,并且P6为修正后的帧,在P6和P7之间成功引入了新的约束。
***继续监控新的进入滑动窗口内的帧。假设,后续进入滑动窗口的帧都是跟P7一样的相同静止帧的情况下,***将不停地将最老帧边缘化掉,并持续将次新帧作为参考帧,进行修正,并固定住最老帧和次新帧,进行非线性优化求解,最终所有滑动窗口内的帧都变成经过修正的静止帧,直到图像采集设备重新运动起来或图像发生变化。
假设图像采集设备发生从快到慢的移动,导致图像不清楚时,虽然在***看来仍然需要边缘化掉次新帧,但是,其实两帧图像还是有不同,比如新的帧P11进入滑动窗口,P11图像发生微小变化,则***将执行如下操作:
601:判断是否边缘化最老帧。
由于P11和P10两帧图像发生微小变化,所以,两者的视差量很小,因此,***会需要边缘化掉次新帧,保留最老帧,如图8-1所示,并转步骤602.
602:判断当前帧和次新帧跟踪成功的点数是否大于某个阈值。
比较P11和P10的差异,同样的,阈值为97%,由于两帧又产生微小变化,因此,两帧跟踪成功的点数小于97%,所以,***认为P10和P11不同,因此,不认为相机处理完全静止的状态,转步骤603.
603:将需要边缘化的帧设为次新帧。
将MargFlag设置为SECOND_NEW,如图8-2所示,并转步骤607.
607:判断参考帧指示位NumNeedFix是否大于0,如果是则转步骤608,如果不是,则转611。
由上可以,由于前述各帧都是静止帧,因此参考帧一直是次新帧,即NumNeedFix=4,所以,大于0,转步骤608.
608:固定住最老帧0和第NumNeedFix帧。
即固定住最老帧P6和参考帧P9,所述的固定指在非线性优化求解过程中,不对最老帧和参考帧进行非线性优化求解,只对其他帧进行非线性优化求解。
609:判断需要边缘化的帧是否是最老帧。
由步骤603可知,MargFlag=SECOND_NEW,不是最老帧,转步骤611.
611:非线性化求解。
由于在步骤608中对最老帧P6和参考帧P9进行了固定,即不参与非线性优化求解,因此***只对除了最老帧P6和参考帧P9之外的所有帧进行非线性优化求解。非线性优化求解的方式与现有的非线性优化求解的方式相同,在这里不再赘述。
612:判断需要边缘化的帧,如果是最老帧,则转步骤614,如果是次新帧,则转613。
由上可以知道MargFlag=SECOND_NEW,因此将转步骤613。
613:如果MargFlag==SECOND_NEW,则边缘化掉次新帧。
因此,当新进帧和次新帧有细小的差别时,***将继续保持最老帧P6和参考帧P9不变,而是把次新帧P10边缘化掉,如8-3所示。***将持续监控进入滑动窗口内的其他帧,如果持续有细小差别的帧进入时,***将继续保持最老帧P6和参考帧P9不变,保持以最准确的帧为基础进行优化。
我们假设图像采集设备运动起来后,又采集到新的帧P12进入滑动窗口,则***将进行如下操作,如图9-1所示,P12为正常运动状态的运动帧,具体执行过程如下:
601:判断是否边缘化最老帧。
由于P12帧是图像采集设备移动状态下采集到的帧,其和P11的视差量及当前帧跟踪到的点数差别很大,***将判断需要边缘化最老帧P6。转步骤605.
605:设置MargFlag=OLD。
将需要边缘化的帧设置为最老帧,即设置MargFlag=OLD。转步骤607.
607:判断参考帧指示位NumNeedFix是否大于0,如果是则转步骤608,如果不是,则转611.
由图8-3可以,参考帧为P9,即NumNeedFix=4,所以,NumNeedFix>0,转608.
608:固定住最老帧和第NumNeedFix帧。
即固定住最老帧P6和P9,在后续,只对其他帧进行非线性优化求解。
609:判断MargFlag是否等于OLD。
由图9-1可知,需要边缘化的帧为最老帧,所以MargFlag==OLD,转步骤610.
610:将参考帧向最老帧方向平移一帧。
即将参考帧指示位NumNeedFix修改为NumNeedFix-1,即NumNeedFix=3。
611:非线性优化求解。
对除了P6和P9之外的其他帧进行非线性优化求解。
612:判断需要边缘化的帧,如果是最老帧,则转步骤614,如果是次新帧,则转613。
右上可知,MargFlag=OLD,所以,转步骤614。
614:如果MargFlag==OLD,则边缘化掉最老帧P6。
如图9-2所示,当新的运动帧P12进入滑动窗口时,由于和次新帧差异较大,***将固定住最老帧和参考帧,而且参考帧为经过修正的参考帧,只对其他帧进行非线性优化求解,同时,将最老帧边缘化。即P6帧被边缘化掉,最老帧为P7,参考帧依然为P9,P11和P12之间也成功引入了新的约束。
***将继续监控新的进入滑动窗口内的帧,假设后续进入滑动窗口的帧都跟P12一样属于不同的运动帧,***将始终将参考帧固定在P9,因为P9是经过修正后的帧,而且将参考帧向最老帧方向移动,直到参考帧移出滑动窗口,***将恢复正常运动状态下的视觉约束。
需要指出的时,当图像采集设备处于匀速运动过程中时,该方法同样适用。当处于匀速运动状态时,步骤602将通过判断图像采集设备的运动速度、陀螺仪的旋转等,来判断当前帧和次新帧是否相同,进而根据情况执行相应的操作。步骤606,当需要对参考帧的速度和旋转进行修正时,由于处于匀速运动状态,所以,当前帧和次新帧的运动速度且C为测量出来的固定值,由上可知,可以将公式(1)化简为:由此同样得出修正后的旋转为:因此,在匀速状态下,同样可以通过本发明提供的实施例实现本发明的发明目的。
通过上述实施例,通过利用静止帧之间的跟踪匹配关系,引用新的约束,对参考帧进行修正,然后基于修正后的参考帧,来修正旋转过程中基于纯IMU计算错误的过程量,从而解决位姿飘走问题。
本发明实施例还提供一种提高视觉惯导***鲁棒性的装置10,用于执行上述实施例中提供的提高视觉惯导***鲁棒性的方法,具体如图10所示,包括:
参考帧确定模块1002:用于确定当前滑动窗口内的参考帧;
参考帧修正模块1003:对参考帧进行修正;
非线性优化模块1004:根据最老帧和修正后的参考帧,对除了参考帧和最老帧之外的帧进行非线性优化。
所述参考帧确定模块进一步用于将相同帧的次新帧确定为参考帧。
进一步的,该装置还包括需要边缘化帧确定模块1001,用于根据当前帧,判断最老帧是否是需要边缘化的帧;如果是,则将最老帧确定为需要边缘化的帧;如果不是,则进一步判断当前帧和次新帧是否相同,如果相同,则将最老帧确定为需要边缘化的帧;如果不同,则通知参考帧确定模块将所述次新帧确定为参考帧。所述需要边缘化帧确定模块判断当前帧和次新帧是否相同,进一步为:判断当前帧和次新帧跟踪成功的点数是否小于预先设定的阈值,如果不小于,则认为当前帧和次新帧相同;如果小于,则认为当前帧和次新帧不同。具体执行过程和上述方法实施例提供的方法,在这里不再赘述。
该装置还包括边缘化模块:用于将需要边缘化的帧边缘化掉。
所述需要边缘化帧确定模块进一步的用于判断需要边缘化的帧是否是最老帧,如果是,则在将除了最老帧和参考帧之外的其他帧进行线性化后,边缘化最老帧,将参考帧向最老帧方向平移一帧。
所述非线性优化模块,进一步用于判断参考帧是否在滑动窗口内,如果在的话,则将除了最老帧和参考帧之外的其他镇进行非线性优化,同时通知边缘化模块,边缘化次新帧;如果不在的话,则对所有帧进行非线性优化。
所述边缘化模块根据MargFlag的指示位标示,来将相应的帧进行边缘化处理。
通过本发明实施例提供的提高视觉惯导***鲁棒性的装置,通过利用静止帧之间的跟踪匹配关系,引用新的约束,对参考帧进行修正,然后基于修正后的参考帧,来修正旋转过程中基于纯IMU计算错误的过程量,从而解决位姿飘走问题。
本申请实施例提供了一种非易失性计算机存储介质,所述计算机存储介质存储有至少一可执行指令,该计算机可执行指令可执行上述任意方法实施例中的提高视觉惯导***鲁棒性的方法。
图11为本发明实施例提供的一种机器人的结构示意图,本发明具体实施例并不对计算设备的具体实现做限定。
如图11所示,该机器人设备可以包括:处理器(processor)501、通信接口(Communications Interface)502、存储器(memory)504、以及通信总线503。
其中:
处理器、通信接口、以及存储器通过通信总线完成相互间的通信。
通信接口,用于与其它设备比如客户端或其它服务器等的网元通信。
处理器,用于执行程序505,具体可以执行上述提高视觉惯导***鲁棒性的方法实施例中的相关步骤。
具体地,程序可以包括程序代码,该程序代码包括计算机操作指令。
处理器可能是中央处理器CPU,或者是特定集成电路ASIC(Application SpecificIntegrated Circuit),或者是被配置成实施本发明实施例的一个或多个集成电路。计算设备包括的一个或多个处理器,可以是同一类型的处理器,如一个或多个CPU;也可以是不同类型的处理器,如一个或多个CPU以及一个或多个ASIC。
存储器,用于存放程序。存储器可能包含高速RAM存储器,也可能还包括非易失性存储器(non-volatile memory),例如至少一个磁盘存储器。
程序具体可以用于使得处理器执行以下操作:
确定参考帧;
对所述参考帧进行修正;
对除了参考帧和最老帧之外的帧进行非线性优化。
进一步的,所述程序还可以控制处理器执行如下操作:
将相同帧的次新帧确定为参考帧。
进一步确定需要边缘化的帧,包括:
根据当前帧,判断最老帧是否是需要边缘化的帧;
如果是,则将最老帧确定为需要边缘化的帧;
如果不是,则进一步判断当前帧和次新帧是否相同,如果相同,则将次新帧确定为参考帧,并将最老帧确定为需要边缘化的帧。如果当前帧和次新帧不相同,则将次新帧确定为需要边缘化的帧,并在后续不对参考帧进行修正。
上述判断当前帧和次新帧是否相同,具体为:
判断当前帧和次新帧跟踪成功的点数是否小于预先设定的阈值,如果不小于,则认为当前帧和次新帧相同;如果小于,则认为当前帧和次新帧不同。
上述修正参考帧,具体为对参考帧的速度和旋转进行修正,具体为:
上将除了最老帧和参考帧之外的其他帧进行非线性优化,进一步包括:
判断需要边缘化的帧是否是最老帧,如果是,则在将除了最老帧和参考帧之外的其他帧进行线性化后,边缘化最老帧,将参考帧向最老帧方向平移一帧。
上述将除了最老帧和参考帧之外的其他帧进行非线性优化,进一步包括:
判断需要边缘化的帧是否是最老帧,如果不是,则在将除了最老帧和参考帧之外的其他帧进行非线性化后,边缘化次新帧。
上述对除了参考帧和最老帧之外的帧进行非线性优化之前,进一步包括:
判断所述参考帧是否在滑动窗口内,如果不在,则对所有帧进行非线性优化。
在此提供的算法和显示不与任何特定机器人、虚拟***或者其它设备固有相关。各种通用***也可以与基于在此的示教一起使用。根据上面的描述,构造这类***所要求的结构是显而易见的。此外,本发明也不针对任何特定编程语言。应当明白,可以利用各种编程语言实现在此描述的本发明的内容,并且上面对特定语言所做的描述是为了披露本发明的最佳实施例。
在此处所提供的说明书中,说明了大量具体细节。然而,能够理解,本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中,并未详细示出公知的方法、结构和技术,以便不模糊对本说明书的理解。
类似地,应当理解,为了精简本公开并帮助理解各个发明方面中的一个或多个,在上面对本发明的示例性实施例的描述中,本发明的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而,并不应将该公开的方法解释成反映如下意图:即所要求保护的本发明要求比在每个权利要求中所明确记载的特征更多的特征。更确切地说,如下面的权利要求书所反映的那样,发明方面在于少于前面公开的单个实施例的所有特征。因此,遵循具体实施例的权利要求书由此明确地并入该具体实施例,其中每个权利要求本身都作为本发明的单独实施例。
本领域那些技术人员可以理解,可以对实施例中的设备中的模块进行自适应性地改变并且把它们设置在与该实施例不同的一个或多个设备中。可以把实施例中的模块或单元或组件组合成一个模块或单元或组件,以及此外可以把它们分成多个子模块或子单元或子组件。除了这样的特征和/或过程或者单元中的至少一些是相互排斥之外,可以采用任何组合对本说明书(包括伴随的权利要求、摘要和附图)中公开的所有特征以及如此公开的任何方法或者设备的所有过程或单元进行组合。除非另外明确陈述,本说明书(包括伴随的权利要求、摘要和附图)中公开的每个特征可以由提供相同、等同或相似目的的替代特征来代替。
此外,本领域的技术人员能够理解,尽管在此所述的一些实施例包括其它实施例中所包括的某些特征而不是其它特征,但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如,在下面的权利要求书中,所要求保护的实施例的任意之一都可以以任意的组合方式来使用。
本发明的各个部件实施例可以以硬件实现,或者以在一个或者多个处理器上运行的软件模块实现,或者以它们的组合实现。本领域的技术人员应当理解,可以在实践中使用微处理器或者数字信号处理器(DSP)来实现根据本发明实施例的消息弹窗的展示装置中的一些或者全部部件的一些或者全部功能。本发明还可以实现为用于执行这里所描述的方法的一部分或者全部的设备或者装置程序(例如,计算机程序和计算机程序产品)。这样的实现本发明的程序可以存储在计算机可读介质上,或者可以具有一个或者多个信号的形式。这样的信号可以从因特网网站上下载得到,或者在载体信号上提供,或者以任何其他形式提供。
应该注意的是上述实施例对本发明进行说明而不是对本发明进行限制,并且本领域技术人员在不脱离所附权利要求的范围的情况下可设计出替换实施例。在权利要求中,不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的元件或步骤。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。本发明可以借助于包括有若干不同元件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中,这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。
Claims (12)
1.一种提高视觉惯导***鲁棒性的方法,其特征在于,包括:
确定参考帧,所述参考帧为相同帧的次新帧;
对所述参考帧的速度和旋转进行修正;
根据最老帧和修正后的参考帧,对除了所述参考帧和最老帧之外的帧进行非线性优化。
2.根据权利要求1所述的方法,其特征在于,所述确定参考帧,进一步包括确定需要边缘化的帧,包括:
根据当前帧,判断最老帧是否是需要边缘化的帧;
如果是,则将最老帧确定为需要边缘化的帧;
如果不是,则进一步判断当前帧和次新帧是否相同,如果相同,则将次新帧确定为参考帧,并将最老帧确定为需要边缘化的帧。
3.如权利要求2所述的方法,其特征在于,如果当前帧和次新帧不相同,则将次新帧确定为需要边缘化的帧,并在后续不对参考帧进行修正。
4.根据权利要求2或3所述的方法,其特征在于,所述判断当前帧和次新帧是否相同,具体为:
判断当前帧和次新帧跟踪成功的点数是否小于预先设定的阈值,如果不小于,则认为当前帧和次新帧相同;如果小于,则认为当前帧和次新帧不同。
7.根据权利要求1所述的方法,其特征在于,所述对除了所述参考帧和最老帧之外的帧进行非线性优化,进一步包括:
判断需要边缘化的帧是否是最老帧,如果是,则在对除了所述参考帧和最老帧之外的帧进行非线性优化后,边缘化最老帧,将参考帧向最老帧方向平移一帧。
8.根据权利要求1所述的方法,其特征在于,所述对除了所述参考帧和最老帧之外的帧进行非线性优化,进一步包括:
判断需要边缘化的帧是否是最老帧,如果不是,则在对除了所述参考帧和最老帧之外的帧进行非线性优化后,边缘化次新帧。
9.如权利要求1所述的方法,其特征在于,所述除了所述参考帧和最老帧之外的帧进行非线性优化之前,进一步包括:
判断所述参考帧是否在滑动窗口内,如果不在,则对所有帧进行非线性优化。
10.一种提高视觉惯导***鲁棒性的装置,其特征在于,包括:
参考帧确定模块:用于确定当前滑动窗口内的参考帧,所述参考帧为相同帧的次新帧;
参考帧修正模块:用于对参考帧的速度和旋转进行修正;
非线性优化模块:用于根据最老帧和修正后的参考帧,对除了参考帧和最老帧之外的帧进行非线性优化。
11.一种机器人,其特征在于,包括处理器、存储器、通信接口和通信总线,所述处理器、所述存储器和所述通信接口通过所述通信总线完成相互间的通信;
所述存储器用于存放至少一可执行指令,所述可执行指令使所述处理器执行如权利要求1-9中任一项所述提高视觉惯导***鲁棒性的方法对应的操作。
12.一种计算机存储介质,其特征在于,所述存储介质中存储有至少一可执行指令,所述可执行指令使处理器执行如权利要求1-9中任一项所述的提高视觉惯导***鲁棒性的方法对应的操作。
Priority Applications (3)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811603305.6A CN109917644B (zh) | 2018-12-26 | 2018-12-26 | 一种提高视觉惯导***鲁棒性的方法、装置和机器人设备 |
JP2019229698A JP7210429B2 (ja) | 2018-12-26 | 2019-12-19 | 視覚的慣性オドメトリシステムのロバストネスを向上させる方法、装置及びロボット機器、コンピュータ記憶媒体 |
US16/725,755 US11188754B2 (en) | 2018-12-26 | 2019-12-23 | Method for improving robustness of visual-inertial navigation system, and robot thereof |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811603305.6A CN109917644B (zh) | 2018-12-26 | 2018-12-26 | 一种提高视觉惯导***鲁棒性的方法、装置和机器人设备 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109917644A CN109917644A (zh) | 2019-06-21 |
CN109917644B true CN109917644B (zh) | 2022-06-14 |
Family
ID=66960013
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811603305.6A Active CN109917644B (zh) | 2018-12-26 | 2018-12-26 | 一种提高视觉惯导***鲁棒性的方法、装置和机器人设备 |
Country Status (3)
Country | Link |
---|---|
US (1) | US11188754B2 (zh) |
JP (1) | JP7210429B2 (zh) |
CN (1) | CN109917644B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112633122B (zh) * | 2020-12-17 | 2024-01-23 | 厦门大学 | 一种单目vio***的前端里程计算法及*** |
EP4024005A1 (en) * | 2021-01-04 | 2022-07-06 | Aptiv Technologies Limited | Method, device, and computer program for determining the change in position and/or orientation of the mobile apparatus |
Family Cites Families (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9576183B2 (en) * | 2012-11-02 | 2017-02-21 | Qualcomm Incorporated | Fast initialization for monocular visual SLAM |
US20140192210A1 (en) * | 2013-01-04 | 2014-07-10 | Qualcomm Incorporated | Mobile device based text detection and tracking |
US9658070B2 (en) * | 2014-07-11 | 2017-05-23 | Regents Of The University Of Minnesota | Inverse sliding-window filters for vision-aided inertial navigation systems |
US10365363B2 (en) * | 2015-05-08 | 2019-07-30 | Humatics Corporation | Mobile localization using sparse time-of-flight ranges and dead reckoning |
US10132933B2 (en) * | 2016-02-02 | 2018-11-20 | Qualcomm Incorporated | Alignment of visual inertial odometry and satellite positioning system reference frames |
US10162362B2 (en) | 2016-08-29 | 2018-12-25 | PerceptIn, Inc. | Fault tolerance to provide robust tracking for autonomous positional awareness |
US10317214B2 (en) * | 2016-10-25 | 2019-06-11 | Massachusetts Institute Of Technology | Inertial odometry with retroactive sensor calibration |
CN107869989B (zh) * | 2017-11-06 | 2020-02-07 | 东北大学 | 一种基于视觉惯导信息融合的定位方法及*** |
CN108592919B (zh) * | 2018-04-27 | 2019-09-17 | 百度在线网络技术(北京)有限公司 | 制图与定位方法、装置、存储介质和终端设备 |
CN108827315B (zh) * | 2018-08-17 | 2021-03-30 | 华南理工大学 | 基于流形预积分的视觉惯性里程计位姿估计方法及装置 |
JP2020057264A (ja) | 2018-10-03 | 2020-04-09 | 株式会社日立製作所 | 計算機システム及びデータ分類の分析方法 |
-
2018
- 2018-12-26 CN CN201811603305.6A patent/CN109917644B/zh active Active
-
2019
- 2019-12-19 JP JP2019229698A patent/JP7210429B2/ja active Active
- 2019-12-23 US US16/725,755 patent/US11188754B2/en active Active
Also Published As
Publication number | Publication date |
---|---|
US20200210701A1 (en) | 2020-07-02 |
CN109917644A (zh) | 2019-06-21 |
JP2020107336A (ja) | 2020-07-09 |
JP7210429B2 (ja) | 2023-01-23 |
US11188754B2 (en) | 2021-11-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110084832B (zh) | 相机位姿的纠正方法、装置、***、设备和存储介质 | |
CN107888828B (zh) | 空间定位方法及装置、电子设备、以及存储介质 | |
CN112734852B (zh) | 一种机器人建图方法、装置及计算设备 | |
CN110595466B (zh) | 轻量级的基于深度学习的惯性辅助视觉里程计实现方法 | |
CN108827341B (zh) | 用于确定图像采集装置的惯性测量单元中的偏差的方法 | |
CN106814753B (zh) | 一种目标位置矫正方法、装置及*** | |
CN110660098B (zh) | 基于单目视觉的定位方法和装置 | |
CN111829532B (zh) | 一种飞行器重定位***和重定位方法 | |
CN110207693B (zh) | 一种鲁棒立体视觉惯性预积分slam方法 | |
CN114136315B (zh) | 一种基于单目视觉辅助惯性组合导航方法及*** | |
CN111026081B (zh) | 一种误差计算方法、装置、设备及存储介质 | |
CN109917644B (zh) | 一种提高视觉惯导***鲁棒性的方法、装置和机器人设备 | |
JP2023021994A (ja) | 自動運転車両に対するデータ処理方法及び装置、電子機器、記憶媒体、コンピュータプログラム、ならびに自動運転車両 | |
CN111721305B (zh) | 定位方法和装置、自动驾驶车辆、电子设备和存储介质 | |
CN107942090B (zh) | 一种基于模糊星图提取光流信息的航天器角速度估计方法 | |
WO2023142353A1 (zh) | 一种位姿预测方法及装置 | |
CN116958452A (zh) | 三维重建方法和*** | |
CN112556699A (zh) | 导航定位方法、装置、电子设备及可读存储介质 | |
EP3718302B1 (en) | Method and system for handling 360 degree image content | |
CN112461258A (zh) | 一种参数修正的方法及装置 | |
CN114440877B (zh) | 一种异步多相机视觉惯性里程计定位方法 | |
CN113126117B (zh) | 一种确定sfm地图绝对尺度的方法及电子设备 | |
CN113034538B (zh) | 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备 | |
CN111829552B (zh) | 一种视觉惯性***的误差修正方法和装置 | |
CN115128655B (zh) | 自动驾驶车辆的定位方法和装置、电子设备和存储介质 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |