建图方法及装置、计算机可读存储介质、电子设备
技术领域
本发明实施例涉及计算机
技术领域
,具体而言,涉及一种建图方法、建图装置、计算机可读存储介质以及电子设备。背景技术
在机器人导盲、无人驾驶、增强现实(Augmented Reality,AR)等很多领域,都需要用到环境地图,在相关技术中,该环境地图多是使用双目视觉建图方法生成的。
当前双目视觉建图方法中,主要采用双目相机,利用前后帧特征点匹配对帧间位姿进行估计,进而计算出当前帧位姿,判断当前帧是否为关键帧,若为关键帧则将当前帧的特征点与前几帧生成的地图点构成的局部地图进行匹配,对当前帧位姿进行进一步优化,然后利用局部地图关联的所有图像帧以及地图点之间的约束关系构建图优化方程,使用捆集优化方法(Bundle Adjustment)求解,使局部地图中地图点在所有图像帧的重投影误差之和最小,进而得到局部最优的当前帧位姿,最后利用相机左右目匹配特征点的视差生成新的地图点。
但是,上述双目视觉建图方法存在如下缺陷:由于需要根据前后帧匹配后的结果来判断当前帧是否为关键帧,因此需要对每一帧都进行特征提取和描述子的计算,而特征点的提取和描述子的计算都是比较耗时的,进而使得地图点的生成效率较低。
因此,需要提供一种新的建图方法及装置。
需要说明的是,在上述
背景技术
部分发明的信息仅用于加强对本发明的背景的理解,因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。
发明内容
本发明的目的在于提供一种建图方法、建图装置、计算机可读存储介质以及电子设备,进而至少在一定程度上克服由于相关技术的限制和缺陷而导致的地图点的生成效率较低的问题。
根据本公开的一个方面,提供一种建图方法,包括:
计算当前图像帧所在时刻的里程计的位姿增量,并根据所述里程计的位姿增量判断所述当前图像帧是否为关键帧;
在确定所述当前图像帧为所述关键帧时,提取所述当前图像帧的特征点,并根据所述当前图像帧的特征点与上一帧关键帧的特征点得到目标特征点匹配对;
根据所述目标特征点匹配对以及与所述上一帧关键帧的特征点对应的历史地图点,计算所述当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量;
根据所述姿态旋转矩阵以及位置向量生成新的地图点,并根据所述新的地图点对当前地图进行更新。
在本公开的一种示例性实施例中,计算当前图像帧所在时刻的里程计的位姿增量包括:
根据所述当前图像帧的时间戳以及所述里程计的时间戳之间的时间差,计算所述当前图像帧所在时刻的里程计的位姿增量。
在本公开的一种示例性实施例中,根据所述当前图像帧的时间戳以及所述里程计的时间戳之间的时间差,计算所述当前图像帧所在时刻的里程计的位姿增量包括:
获取码盘计中的与所述当前图像帧所在时刻对应的上一时刻的里程计的位姿信息以及下一时刻的里程计的位姿信息;
根据所述当前图像帧的时间戳以及所述里程计的时间戳之间的时间差、所述上一时刻的里程计的位姿信息以及所述下一时刻的里程计的位姿信息计算所述当前图像帧所在时刻的里程计的位姿信息;
根据所述当前图像帧所在时刻的里程计的位姿信息以及下一图像帧所在时刻的里程计的位姿信息,计算所述里程计的位姿增量。
在本公开的一种示例性实施例中,所述里程计的位姿增量包括里程计的位移增量以及里程计的姿态角增量;
其中,根据所述里程计的位姿增量判断所述当前图像帧是否为关键帧包括:
根据所述里程计的位移增量是否大于第一预设阈值,和/或所述里程计的姿态角增量是否大于第二预设阈值判断所述当前图像帧是否为关键帧。
在本公开的一种示例性实施例中,如果所述位里程计的移增量大于第一预设阈值,和/或所述里程计的姿态角增量大于第二预设阈值,则确定所述当前图像帧为关键帧。
在本公开的一种示例性实施例中,根据所述当前图像帧的特征点与上一帧关键帧的特征点得到目标特征点匹配对包括:
根据所述当前图像帧的特征点,以及与所述当前图像帧的各特征点对应的所述上一帧关键帧中的特征点,得到多个当前特征点匹配对;
计算所述当前图像帧的特征点的当前描述子,并计算所述当前描述子与所述上一帧关键帧的特征点的历史描述子之间的距离;
根据所述当前描述子以及所述历史描述子之间的距离,计算各所述当前特征点匹配对之间的匹配得分;
将匹配得分大于预设分数的当前特征点匹配对进行剔除,并将剩余的当前特征点匹配对作为所述目标特征点匹配对;其中,所述预设分数为所述匹配得分的中值。
在本公开的一种示例性实施例中,所述建图方法还包括:
根据所述里程计的位姿增量预测所述当前图像帧的当前帧位姿。
在本公开的一种示例性实施例中,根据所述目标特征点匹配对以及与所述上一帧关键帧的特征点对应的历史地图点,计算所述当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量包括:
根据所述当前图像帧的二维像素点、所述历史地图点、双目相机的内参矩阵、所述当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量构建投影方程;
根据所述目标特征点匹配对以及所述投影方程构建最小二乘优化方案;
以所述当前帧位姿为初始值,根据高斯牛顿迭代法对所述最小二乘优化方案进行求解得到所述姿态旋转矩阵以及位置向量。
在本公开的一种示例性实施例中,根据所述姿态旋转矩阵以及位置向量生成新的地图点包括:
根据所述内参矩阵、所述姿态旋转矩阵、所述位置向量、所述双目相机的左右目特征点的深度以及左右目匹配点构建约束方程;
根据所述约束方程计算所述左右目特征点的深度,并根据所述左右目特征点的深度以及所述左右目匹配点,计算所述双目相机坐标下的三维点;
根据所述当前帧位姿以及所述三维点得到所述新的地图点。
根据本公开的一个方面,提供一种建图装置,包括:
位姿增量计算模块,用于计算当前图像帧所在时刻的里程计的位姿增量,并根据所述里程计的位姿增量判断所述当前图像帧是否为关键帧;
特征点提取模块,用于在确定所述当前图像帧为所述关键帧时,提取所述当前图像帧的特征点,并根据所述当前图像帧的特征点与上一帧关键帧的特征点得到目标特征点匹配对;
计算模块,用于根据所述目标特征点匹配对以及与所述上一帧关键帧的特征点对应的历史地图点,计算所述当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量;
地图点生成模块,用于根据所述姿态旋转矩阵以及位置向量生成新的地图点,并根据所述新的地图点对当前地图进行更新。
根据本公开的一个方面,提供一种计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时实现上述任意一项所述的建图方法。
根据本公开的一个方面,提供一种电子设备,包括:
处理器;以及
存储器,用于存储所述处理器的可执行指令;
其中,所述处理器配置为经由执行所述可执行指令来执行上述任意一项所述的建图方法。
本发明实施例提供的一种建图方法,一方面,通过计算当前图像帧所在时刻的里程计的位姿增量,并在确定当前图像帧为关键帧时,提取当前图像帧的特征点;然后根据目标特征点匹配对以及与上一帧关键帧的特征点对应的历史地图点,计算当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量;最后根据姿态旋转矩阵以及位置向量生成新的地图点,并根据新的地图点对当前地图进行更新;解决了现有技术中由于需要根据前后帧匹配后的结果来判断当前帧是否为关键帧,因此需要对每一帧都进行特征提取和描述子的计算,而特征点的提取和描述子的计算都是比较耗时的,进而使得地图点的生成效率较低的问题,提高了地图点的生成效率;另一方面,由于是解决了现有技术中当建图过程中存在长时间的白墙或者光线昏暗等纹理特征少的场景时,容易导致建图失败的问题;再一方面,提高了生成的新的地图点的准确率,进而提高了更新后的地图的准确率。
应当理解的是,以上的一般描述和后文的细节描述仅是示例性和解释性的,并不能限制本发明。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分,示出了符合本发明的实施例,并与说明书一起用于解释本发明的原理。显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1示意性示出根据本发明示例实施例的一种建图方法的流程图。
图2示意性示出根据本发明示例实施例的一种根据所述当前图像帧的时间戳以及所述里程计的时间戳之间的时间差,计算所述当前图像帧所在时刻的里程计的位姿增量的方法流程图。
图3示意性示出根据本发明示例实施例的一种当前图像帧的时间戳以及所述里程计的时间戳之间的时间差的示例图。
图4示意性示出根据本发明示例实施例的一种根据所述当前图像帧的特征点与上一帧关键帧的特征点得到目标特征点匹配对的方法流程图。
图5示意性示出根据本发明示例实施例的一种根据所述目标特征点匹配对以及与所述上一帧关键帧的特征点对应的历史地图点,计算所述当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量的方法流程图。
图6示意性示出根据本发明示例实施例的一种根据所述姿态旋转矩阵以及位置向量生成新的地图点的方法流程图。
图7示意性示出根据本发明示例实施例的另一种建图方法的流程图。
图8示意性示出根据本发明示例实施例的一种建图装置的框图。
图9示意性示出根据本发明示例实施例的一种用于实现上述建图方法的电子设备。
具体实施方式
现在将参考附图更全面地描述示例实施方式。然而,示例实施方式能够以多种形式实施,且不应被理解为限于在此阐述的范例;相反,提供这些实施方式使得本发明将更加全面和完整,并将示例实施方式的构思全面地传达给本领域的技术人员。所描述的特征、结构或特性可以以任何合适的方式结合在一个或更多实施方式中。在下面的描述中,提供许多具体细节从而给出对本发明的实施方式的充分理解。然而,本领域技术人员将意识到,可以实践本发明的技术方案而省略所述特定细节中的一个或更多,或者可以采用其它的方法、组元、装置、步骤等。在其它情况下,不详细示出或描述公知技术方案以避免喧宾夺主而使得本发明的各方面变得模糊。
此外,附图仅为本发明的示意性图解,并非一定是按比例绘制。图中相同的附图标记表示相同或类似的部分,因而将省略对它们的重复描述。附图中所示的一些方框图是功能实体,不一定必须与物理或逻辑上独立的实体相对应。可以采用软件形式来实现这些功能实体,或在一个或多个硬件模块或集成电路中实现这些功能实体,或在不同网络和/或处理器装置和/或微控制器装置中实现这些功能实体。
本示例实施方式中首先提供了一种建图方法,该方法可以运行于服务器、服务器集群或云服务器等;当然,本领域技术人员也可以根据需求在其他平台运行本发明的方法,本示例性实施例中对此不做特殊限定。参考图1所示,该建图方法可以包括以下步骤:
步骤S110.计算当前图像帧所在时刻的里程计的位姿增量,并根据所述里程计的位姿增量判断所述当前图像帧是否为关键帧。
步骤S120.在确定所述当前图像帧为所述关键帧时,提取所述当前图像帧的特征点,并根据所述当前图像帧的特征点与上一帧关键帧的特征点得到目标特征点匹配对。
步骤S130.根据所述目标特征点匹配对以及与所述上一帧关键帧的特征点对应的历史地图点,计算所述当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量。
步骤S140.根据所述姿态旋转矩阵以及位置向量生成新的地图点,并根据所述新的地图点对当前地图进行更新。
上述建图方法中,一方面,通过计算当前图像帧所在时刻的里程计的位姿增量,并在确定当前图像帧为关键帧时,提取当前图像帧的特征点;然后根据目标特征点匹配对以及与上一帧关键帧的特征点对应的历史地图点,计算当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量;最后根据姿态旋转矩阵以及位置向量生成新的地图点,并根据新的地图点对当前地图进行更新;解决了现有技术中由于需要根据前后帧匹配后的结果来判断当前帧是否为关键帧,因此需要对每一帧都进行特征提取和描述子的计算,而特征点的提取和描述子的计算都是比较耗时的,进而使得地图点的生成效率较低的问题,提高了地图点的生成效率;另一方面,由于是解决了现有技术中当建图过程中存在长时间的白墙或者光线昏暗等纹理特征少的场景时,容易导致建图失败的问题;再一方面,提高了生成的新的地图点的准确率,进而提高了更新后的地图的准确率。
以下,将结合附图对本发明示例实施例建图方法中涉及的各步骤进行详细的解释以及说明。
首先,对本发明示例实施例的发明目的进行解释以及说明。具体的,本发明示例实施例提出一种融合码盘计的双目相机建图方法,首先利用码盘计提供图像帧之间的位姿增量判断当前帧是否为关键帧,若为关进帧则使用该增量对当前帧位姿进行预测,然后对当前帧进行特征点提取与描述子计算并将当前帧与前一关键帧进行特征点匹配对当前帧位姿进行求解优化,大大降低计算开销。同时,当面对纹理特征少的场景,码盘计依然可以提供两个图像关键帧之间的位姿约束关系,提高建图算法鲁棒性。
在步骤S110中,计算当前图像帧所在时刻的里程计的位姿增量,并根据所述里程计的位姿增量判断所述当前图像帧是否为关键帧。
在本示例实施例中,首先,计算当前图像帧所在时刻的里程计的位姿增量,具体的可以包括:根据所述当前图像帧的时间戳以及所述里程计的时间戳之间的时间差,计算所述当前图像帧所在时刻的里程计的位姿增量。进一步的,参考图2所示,根据所述当前图像帧的时间戳以及所述里程计的时间戳之间的时间差,计算所述当前图像帧所在时刻的里程计的位姿增量可以包括步骤S210-步骤S230。其中:
在步骤S210中,获取码盘计中的与所述当前图像帧所在时刻对应的上一时刻的里程计的位姿信息以及下一时刻的里程计的位姿信息。
在步骤S220中,根据所述当前图像帧的时间戳以及所述里程计的时间戳之间的时间差、所述上一时刻的里程计的位姿信息以及所述下一时刻的里程计的位姿信息计算所述当前图像帧所在时刻的里程计的位姿信息。
在步骤S230中,根据所述当前图像帧所在时刻的里程计的位姿信息以及下一图像帧所在时刻的里程计的位姿信息,计算所述里程计的位姿增量。
以下,将对步骤S210-步骤S230进行解释以及说明。首先,从码盘计中获取与所述当前图像帧所在时刻对应的上一时刻的里程计的位姿信息以及下一时刻的里程计的位姿信息;其次,由于码盘计帧率和图像帧率是不同步的,因此可以利用时间戳插值方法计算得到当前图像帧所在时刻的里程计的位姿信息。具体的,参考图3所示,to可以表示里程计的时间戳,tc表示图像帧的时间戳;里程计的位姿信息可以包括位移信息以及姿态角信息,分别使用p和q表示,则利用时间戳插值方法计算图像帧所在时刻里程计的位姿信息的可以如下所示:
其中,pc1为当前图像帧所在时刻的里程计的位移信息,qc1为当前图像帧所在时刻的里程计的姿态角信息;po2为与当前图像帧所在时刻对应的下一时刻的里程计的位移信息,po1为与当前图像帧所在时刻对应的上一时刻的里程计的位移信息;tc1为当前图像帧的时间戳,to1以及to2分别为里程计的时间戳;qo2与当前图像帧所在时刻对应的下一时刻的里程计的姿态角信息,qo1与当前图像帧所在时刻对应的上一时刻的里程计的姿态角信息。
利用上述公式分别计算出tc1、tc2时刻的里程计位姿,从而计算出从tc1时刻到tc2时刻的里程计的位姿增量。通过该方法,解决了现有技术中由于需要根据前后帧匹配后的结果来判断当前帧是否为关键帧,因此需要对每一帧都进行特征提取和描述子的计算,而特征点的提取和描述子的计算都是比较耗时的,进而使得地图点的生成效率较低的问题。
其次,在本示例实施例中,当得到上述里程计的位姿增量以后,可以根据所述里程计的位姿增量判断所述当前图像帧是否为关键帧;其中,该里程计的位姿增量包括里程计的位移增量以及里程计的姿态角增量。具体的,根据所述里程计的位姿增量判断所述当前图像帧是否为关键帧具体的可以包括:根据所述里程计的位移增量是否大于第一预设阈值,和/或所述里程计的姿态角增量是否大于第二预设阈值判断所述当前图像帧是否为关键帧。
譬如,如果所述里程计的位移增量大于第一预设阈值,和/或所述里程计的姿态角增量大于第二预设阈值,则确定所述当前图像帧为关键帧。需要补充说明的是,该第一预设阈值以及第二预设阈值可以根据实际情况进行决定,本示例对此不做特殊限制。
在步骤S120中,在确定所述当前图像帧为所述关键帧时,提取所述当前图像帧的特征点,并根据所述当前图像帧的特征点与上一帧关键帧的特征点得到目标特征点匹配对。
在本示例实施例中,在确定当前图像帧为关键帧时,可以基于常用的特征提取方法提取该当前图像帧的特征点;其中,特征提取方法可以包括SIFT(Scale-InvariantFeature Transform,尺度不变特征变换)、SURF(Speeded Up Robust Features,加速版SIFT算法)以及ORB(Oriented FAST and Rotated BRIEF,快速特征点提取和描述的算法)等等。当当前图像帧的特征点提取完成后,再根据该当前图像帧的特征点与上一帧关键帧的特征点得到目标特征点匹配对。具体的,参考图4所示,根据该当前图像帧的特征点与上一帧关键帧的特征点得到目标特征点匹配对可以包括步骤S410-步骤S440。其中:
在步骤S410中,根据所述当前图像帧的特征点,以及与所述当前图像帧的各特征点对应的所述上一帧关键帧中的特征点,得到多个当前特征点匹配对。
在步骤S420中,计算所述当前图像帧的特征点的当前描述子,并计算所述当前描述子与所述上一帧关键帧的特征点的历史描述子之间的距离。
在步骤S430中,根据所述当前描述子以及所述历史描述子之间的距离,计算各所述当前特征点匹配对之间的匹配得分。
在步骤S440中,将匹配得分大于预设分数的当前特征点匹配对进行剔除,并将剩余的当前特征点匹配对作为所述目标特征点匹配对;其中,所述预设分数为所述匹配得分的中值。
以下,将对步骤S410-步骤S440进行解释以及说明。首先,基于当前图像帧的特征点以及各当前图像帧的各特征点对应的上一帧关键帧中的特征点得到多个当前特征点匹配对;然后,基于特征提取方法计算当前图像帧的特征点的当前描述子,并计算当前描述子与上一帧关键帧的特征点的历史描述子之间的距离;其中,描述子距离,以ORB特征为例,ORB特征描述子为128位的二进制数,则将两个二进制数的不同位数作为两个描述子之间的距离。将该距离作为特征点匹配对的匹配得分,然后将各匹配得分按照从高到低进行排序,并将匹配的分大于预设分数之前的特征点匹配对进行剔除,以避免错误匹配,提高特征点匹配对的准确率;其中,该预设分数可以是匹配得分的中值;最后,将剩下的匹配得分小于中值的特征点匹配对作为目标特征点匹配对。
在步骤S130中,根据所述目标特征点匹配对以及与所述上一帧关键帧的特征点对应的历史地图点,计算所述当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量。
在本示例实施例中,为了得到上述当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量,首先,需要预测当前图像帧的当前帧位姿。具体的,可以根据所述里程计的位姿增量预测所述当前图像帧的当前帧位姿。
具体的:
Pc2=pc1+pc1-2; (公式3)
Qc2=qc1-2*qc1; (公式4)
其中,Pc2以及Qc2分别为当前帧位姿的位移信息以及姿态角信息;pc1-2以及qc1-2分别为里程计的位移增量以及姿态角增量;pc1为当前图像帧所在时刻的里程计的位移信息,qc1为当前图像帧所在时刻的里程计的姿态角信息。
其次,当得到当前图像帧的当前帧位姿以后,可以根据目标特征点匹配对以及与上一帧关键帧的特征点对应的历史地图点,计算当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量。具体的,参考图5所示,根据所述目标特征点匹配对以及与所述上一帧关键帧的特征点对应的历史地图点,计算所述当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量可以包括步骤S510-步骤S530。其中:
在步骤S510中,根据所述当前图像帧的二维像素点、所述历史地图点、双目相机的内参矩阵、所述当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量构建投影方程。
在步骤S520中,根据所述目标特征点匹配对以及所述投影方程构建最小二乘优化方案。
在步骤S530中,以所述当前帧位姿为初始值,根据高斯牛顿迭代法对所述最小二乘优化方案进行求解得到所述姿态旋转矩阵以及位置向量。
以下,将对步骤S510-步骤S530进行解释以及说明。具体的,当得到目标特征点匹配对后,进而可以获取到上一帧关键帧的特征点对应的历史地图点,从而构造3d-2d的PnP(Perspective-n-Point)问题。
进一步的,在理想条件下,历史三维地图点与当前图像帧的二维像素点满足如下投影方程:
p=K(RP+t); (公式5)
其中,P为历史(三维)地图点,p为当前图像帧的二维像素点,K为双目相机的相机内参矩阵,R为当前图像帧在世界坐标系下的姿态旋转矩阵,t为当前图像帧在世界坐标系下的位置向量。
然后,对于N个3d-2d匹配对则可以构造如下的最小二乘优化问题:
最后,再根据上述当前图像帧的当前帧位姿作为初值,使用高斯牛顿法迭代求解得到R和t。通过该方法,解决了现有技术中由于需要对当前帧位姿进行三次求解优化,进一步增加了计算开销,增加了系统的负担的问题,降低了系统的负担,同时提高了地图点的生成效率。
在步骤S140中,根据所述姿态旋转矩阵以及位置向量生成新的地图点,并根据所述新的地图点对当前地图进行更新。
在本示例实施例中,参考图6所示,根据所述姿态旋转矩阵以及位置向量生成新的地图点可以包括步骤S610-步骤S630。其中:
在步骤S610中,根据所述内参矩阵、所述姿态旋转矩阵、所述位置向量、所述双目相机的左右目特征点的深度以及左右目匹配点构建约束方程。
在步骤S620中,根据所述约束方程计算所述左右目特征点的深度,并根据所述左右目特征点的深度以及所述左右目匹配点,计算所述双目相机坐标下的三维点。
在步骤S630中,根据所述当前帧位姿以及所述三维点得到所述新的地图点。
以下,将对步骤S610-步骤S630进行解释以及说明。首先,假设双目相机的左右目匹配点为x1、x2,则存在如下约束方程:
s1x1=K(Rs2x2+t);(公式7)
其中,R、t分别为上述姿态旋转矩阵以及位置向量;K为双目相机的相机内参矩阵;s1为左目特征点的深度,s2为右目特征点的深度,方程分别乘以则可得到如下方程组:
将待求的s1、s2提出来并将方程组改写成矩阵形式得到:
进一步的,假设有:
则有:
当得到得s1后,则s1x1为双目相机坐标系下的三维点,最后再根据当前帧位姿将该三维点转换到世界坐标系下的新的地图点。
进一步的,当得到新的地图点以后,可以基于该新的地图点对当前地图进行更新,进而得到更新后的地图。通过该方法,可以提高更新后的地图的准确率。
以下,结合图7对本发明示例实施例的建图方法进行进一步的解释以及说明。具体的,参考图7所示,该建图方法可以包括以下步骤:
步骤S710,获取码盘计中的与当前图像帧所在时刻对应的上一时刻的里程计的位姿信息以及下一时刻的里程计的位姿信息;
步骤S720,根据当前图像帧所在时刻的里程计的位姿信息以及下一图像帧所在时刻的里程计的位姿信息,计算里程计的位姿增量;
步骤S730,根据里程计的位姿增量判断当前图像帧是否为关键帧;如果是,则跳转至步骤S740;如果否,则跳转至步骤S710;
步骤S740,根据当前图像帧所在时刻的里程计的位姿信息以及下一图像帧所在时刻的里程计的位姿信息以及里程计的位姿增量计算当前图像帧的当前帧位姿;
步骤S750,提取当前图像帧的特征点,并根据当前图像帧的特征点与上一帧关键帧的特征点得到目标特征点匹配对;
步骤S760,根据目标特征点匹配对构建最小二乘优化问题,并根据当前图像帧的当前帧位姿对该最小二乘优化问题进行求解得到前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量;
步骤S770,根据姿态旋转矩阵以及位置向量生成新的地图点,并根据所述新的地图点对当前地图进行更新。
本发明示例实施例提供的建图方法中,通过使用码盘计提供的里程计信息简化了双目相机建图时的位姿优化过程,并能够在特征点稀疏情况下提高算法鲁棒性。同时,为了解决时间不同步引入的误差,本发明使用了时间戳插值方法,提高了位姿预测的准确性。
本发明示例实施例还提供了一种建图装置。参考图8所示,该建图装置可以包括位姿增量计算模块810、特征点提取模块820、计算模块830以及地图点生成模块840。其中:
位姿增量计算模块810可以用于计算当前图像帧所在时刻的里程计的位姿增量,并根据所述里程计的位姿增量判断所述当前图像帧是否为关键帧。
特征点提取模块820可以用于在确定所述当前图像帧为所述关键帧时,提取所述当前图像帧的特征点,并根据所述当前图像帧的特征点与上一帧关键帧的特征点得到目标特征点匹配对。
计算模块830可以用于根据所述目标特征点匹配对以及与所述上一帧关键帧的特征点对应的历史地图点,计算所述当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量。
地图点生成模840可以,用于根据所述姿态旋转矩阵以及位置向量生成新的地图点,并根据所述新的地图点对当前地图进行更新。
在本公开的一种示例性实施例中,计算当前图像帧所在时刻的里程计的位姿增量包括:
根据所述当前图像帧的时间戳以及所述里程计的时间戳之间的时间差,计算所述当前图像帧所在时刻的里程计的位姿增量。
在本公开的一种示例性实施例中,根据所述当前图像帧的时间戳以及所述里程计的时间戳之间的时间差,计算所述当前图像帧所在时刻的里程计的位姿增量包括:
获取码盘计中的与所述当前图像帧所在时刻对应的上一时刻的里程计的位姿信息以及下一时刻的里程计的位姿信息。
根据所述当前图像帧的时间戳以及所述里程计的时间戳之间的时间差、所述上一时刻的里程计的位姿信息以及所述下一时刻的里程计的位姿信息计算所述当前图像帧所在时刻的里程计的位姿信息。
根据所述当前图像帧所在时刻的里程计的位姿信息以及下一图像帧所在时刻的里程计的位姿信息,计算所述里程计的位姿增量。
在本公开的一种示例性实施例中,所述里程计的位姿增量包括里程计的位移增量以及里程计的姿态角增量;
其中,根据所述里程计的位姿增量判断所述当前图像帧是否为关键帧包括:
根据所述里程计的位移增量是否大于第一预设阈值,和/或所述里程计的姿态角增量是否大于第二预设阈值判断所述当前图像帧是否为关键帧。
在本公开的一种示例性实施例中,如果所述里程计的位移增量大于第一预设阈值,和/或所述里程计的姿态角增量大于第二预设阈值,则确定所述当前图像帧为关键帧。
在本公开的一种示例性实施例中,根据所述当前图像帧的特征点与上一帧关键帧的特征点得到目标特征点匹配对包括:
根据所述当前图像帧的特征点,以及与所述当前图像帧的各特征点对应的所述上一帧关键帧中的特征点,得到多个当前特征点匹配对;
计算所述当前图像帧的特征点的当前描述子,并计算所述当前描述子与所述上一帧关键帧的特征点的历史描述子之间的距离;
根据所述当前描述子以及所述历史描述子之间的距离,计算各所述当前特征点匹配对之间的匹配得分;
将匹配得分大于预设分数的当前特征点匹配对进行剔除,并将剩余的当前特征点匹配对作为所述目标特征点匹配对;其中,所述预设分数为所述匹配得分的中值。
在本公开的一种示例性实施例中,所述建图装置还包括:
预测模块,可以用于根据所述里程计的位姿增量预测所述当前图像帧的当前帧位姿。
在本公开的一种示例性实施例中,根据所述目标特征点匹配对以及与所述上一帧关键帧的特征点对应的历史地图点,计算所述当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量包括:
根据所述当前图像帧的二维像素点、所述历史地图点、双目相机的内参矩阵、所述当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量构建投影方程;
根据所述目标特征点匹配对以及所述投影方程构建最小二乘优化方案;
以所述当前帧位姿为初始值,根据高斯牛顿迭代法对所述最小二乘优化方案进行求解得到所述姿态旋转矩阵以及位置向量。
在本公开的一种示例性实施例中,根据所述姿态旋转矩阵以及位置向量生成新的地图点包括:
根据所述内参矩阵、所述姿态旋转矩阵、所述位置向量、所述双目相机的左右目特征点的深度以及左右目匹配点构建约束方程;
根据所述约束方程计算所述左右目特征点的深度,并根据所述左右目特征点的深度以及所述左右目匹配点,计算所述双目相机坐标下的三维点;
根据所述当前帧位姿以及所述三维点得到所述新的地图点。
上述建图装置中各模块的具体细节已经在对应的建图方法中进行了详细的描述,因此此处不再赘述。
应当注意,尽管在上文详细描述中提及了用于动作执行的设备的若干模块或者单元,但是这种划分并非强制性的。实际上,根据本发明的实施方式,上文描述的两个或更多模块或者单元的特征和功能可以在一个模块或者单元中具体化。反之,上文描述的一个模块或者单元的特征和功能可以进一步划分为由多个模块或者单元来具体化。
此外,尽管在附图中以特定顺序描述了本发明中方法的各个步骤,但是,这并非要求或者暗示必须按照该特定顺序来执行这些步骤,或是必须执行全部所示的步骤才能实现期望的结果。附加的或备选的,可以省略某些步骤,将多个步骤合并为一个步骤执行,以及/或者将一个步骤分解为多个步骤执行等。
在本发明的示例性实施例中,还提供了一种能够实现上述方法的电子设备。
所属技术领域的技术人员能够理解,本发明的各个方面可以实现为系统、方法或程序产品。因此,本发明的各个方面可以具体实现为以下形式,即:完全的硬件实施方式、完全的软件实施方式(包括固件、微代码等),或硬件和软件方面结合的实施方式,这里可以统称为“电路”、“模块”或“系统”。
下面参照图9来描述根据本发明的这种实施方式的电子设备900。图9显示的电子设备900仅仅是一个示例,不应对本发明实施例的功能和使用范围带来任何限制。
如图9所示,电子设备900以通用计算设备的形式表现。电子设备900的组件可以包括但不限于:上述至少一个处理单元910、上述至少一个存储单元920、连接不同系统组件(包括存储单元920和处理单元910)的总线930以及显示单元940。
其中,所述存储单元存储有程序代码,所述程序代码可以被所述处理单元910执行,使得所述处理单元910执行本说明书上述“示例性方法”部分中描述的根据本发明各种示例性实施方式的步骤。例如,所述处理单元910可以执行如图1中所示的步骤S110:计算当前图像帧所在时刻的里程计的位姿增量,并根据所述里程计的位姿增量判断所述当前图像帧是否为关键帧;步骤S120:在确定所述当前图像帧为所述关键帧时,提取所述当前图像帧的特征点,并根据所述当前图像帧的特征点与上一帧关键帧的特征点得到目标特征点匹配对;步骤S130:根据所述目标特征点匹配对以及与所述上一帧关键帧的特征点对应的历史地图点,计算所述当前图像帧在世界坐标系下的姿态旋转矩阵以及位置向量;步骤S140:根据所述姿态旋转矩阵以及位置向量生成新的地图点,并根据所述新的地图点对当前地图进行更新。
存储单元920可以包括易失性存储单元形式的可读介质,例如随机存取存储单元(RAM)9201和/或高速缓存存储单元9202,还可以进一步包括只读存储单元(ROM)9203。
存储单元920还可以包括具有一组(至少一个)程序模块9205的程序/实用工具9204,这样的程序模块9205包括但不限于:操作系统、一个或者多个应用程序、其它程序模块以及程序数据,这些示例中的每一个或某种组合中可能包括网络环境的实现。
总线930可以为表示几类总线结构中的一种或多种,包括存储单元总线或者存储单元控制器、外围总线、图形加速端口、处理单元或者使用多种总线结构中的任意总线结构的局域总线。
电子设备900也可以与一个或多个外部设备1000(例如键盘、指向设备、蓝牙设备等)通信,还可与一个或者多个使得用户能与该电子设备900交互的设备通信,和/或与使得该电子设备900能与一个或多个其它计算设备进行通信的任何设备(例如路由器、调制解调器等等)通信。这种通信可以通过输入/输出(I/O)接口950进行。并且,电子设备900还可以通过网络适配器960与一个或者多个网络(例如局域网(LAN),广域网(WAN)和/或公共网络,例如因特网)通信。如图所示,网络适配器960通过总线930与电子设备900的其它模块通信。应当明白,尽管图中未示出,可以结合电子设备900使用其它硬件和/或软件模块,包括但不限于:微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、RAID系统、磁带驱动器以及数据备份存储系统等。
通过以上的实施方式的描述,本领域的技术人员易于理解,这里描述的示例实施方式可以通过软件实现,也可以通过软件结合必要的硬件的方式来实现。因此,根据本发明实施方式的技术方案可以以软件产品的形式体现出来,该软件产品可以存储在一个非易失性存储介质(可以是CD-ROM,U盘,移动硬盘等)中或网络上,包括若干指令以使得一台计算设备(可以是个人计算机、服务器、终端装置、或者网络设备等)执行根据本发明实施方式的方法。
在本发明的示例性实施例中,还提供了一种计算机可读存储介质,其上存储有能够实现本说明书上述方法的程序产品。在一些可能的实施方式中,本发明的各个方面还可以实现为一种程序产品的形式,其包括程序代码,当所述程序产品在终端设备上运行时,所述程序代码用于使所述终端设备执行本说明书上述“示例性方法”部分中描述的根据本发明各种示例性实施方式的步骤。
根据本发明的实施方式的用于实现上述方法的程序产品,其可以采用便携式紧凑盘只读存储器(CD-ROM)并包括程序代码,并可以在终端设备,例如个人电脑上运行。然而,本发明的程序产品不限于此,在本文件中,可读存储介质可以是任何包含或存储程序的有形介质,该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。
所述程序产品可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以为但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件,或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括:具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号,其中承载了可读程序代码。这种传播的数据信号可以采用多种形式,包括但不限于电磁信号、光信号或上述的任意合适的组合。可读信号介质还可以是可读存储介质以外的任何可读介质,该可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。
可读介质上包含的程序代码可以用任何适当的介质传输,包括但不限于无线、有线、光缆、RF等等,或者上述的任意合适的组合。
可以以一种或多种程序设计语言的任意组合来编写用于执行本发明操作的程序代码,所述程序设计语言包括面向对象的程序设计语言—诸如Java、C++等,还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。在涉及远程计算设备的情形中,远程计算设备可以通过任意种类的网络,包括局域网(LAN)或广域网(WAN),连接到用户计算设备,或者,可以连接到外部计算设备(例如利用因特网服务提供商来通过因特网连接)。
此外,上述附图仅是根据本发明示例性实施例的方法所包括的处理的示意性说明,而不是限制目的。易于理解,上述附图所示的处理并不表明或限制这些处理的时间顺序。另外,也易于理解,这些处理可以是例如在多个模块中同步或异步执行的。
本领域技术人员在考虑说明书及实践这里发明的发明后,将容易想到本发明的其他实施例。本申请旨在涵盖本发明的任何变型、用途或者适应性变化,这些变型、用途或者适应性变化遵循本发明的一般性原理并包括本发明未发明的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的,本发明的真正范围和精神由权利要求指出。
- 上一篇:石墨接头机器人自动装卡簧、装栓机
- 下一篇:基于改进RIFT的多模遥感图像配准方法