CN113610918A - 位姿计算方法和装置、电子设备、可读存储介质 - Google Patents
位姿计算方法和装置、电子设备、可读存储介质 Download PDFInfo
- Publication number
- CN113610918A CN113610918A CN202110866966.3A CN202110866966A CN113610918A CN 113610918 A CN113610918 A CN 113610918A CN 202110866966 A CN202110866966 A CN 202110866966A CN 113610918 A CN113610918 A CN 113610918A
- Authority
- CN
- China
- Prior art keywords
- rgb image
- target
- pose
- image
- dimensional map
- 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.)
- Pending
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
-
- 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/10024—Color image
-
- 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/10028—Range image; Depth image; 3D point clouds
Landscapes
- Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Image Analysis (AREA)
- Image Processing (AREA)
Abstract
本申请涉及一种位姿计算方法和装置、电子设备、计算机可读存储介质,若在预设初始化滑窗内首次采集到当前环境的深度图像,则将采集深度图像时电子设备的位姿确定为初始位姿;其中,深度图像对应的目标RGB图像并非预设初始化滑窗内的最后一帧图像。根据初始位姿、目标RGB图像、深度图像及目标RGB图像的下一帧RGB图像,确定电子设备采集下一帧RGB图像时的位姿。因此,采用本申请中的位姿计算方法,不需要在预设初始化滑窗内采集满RGB图像,电子设备才能够输出采集预设初始化滑窗外的第一帧RGB图像时的位姿。而在预设初始化滑窗内就可以实时输出位姿,降低了实时输出位姿的等待时间。
Description
技术领域
本申请涉及计算机视觉技术领域,特别是涉及一种位姿计算方法和装置、电子设备、可读存储介质。
背景技术
电子设备在未知环境中的位置与姿态(简称位姿)问题,是增强现实、虚拟现实、移动机器人、无人驾驶等行业的关键技术之一。且随着这些行业的快速发展,对电子设备对周围环境中的物体进行定位的精确度也随之提出了越来越高的要求。而在采用VIO(Visual-Inertial Odometry,视觉-惯性里程计)系统对周围环境中的物体进行定位时,其核心的步骤之一为确定电子设备在拍摄每帧图像时的位姿。
传统方法,在确定电子设备在拍摄每帧图像时的位姿时,需要基于电子设备上的相机,对周围环境采集一定数目的RGB图或深度图像,才能够基于该一定数目的RGB图或深度图像进行初始化,进而实时输出电子设备在拍摄每帧图像时的位姿。显然,采集一定数目的RGB图和/或深度图像需要耗费较长的时间,进而导致实时输出位姿的等待时间也较长。
发明内容
本申请实施例提供了一种位姿计算方法和装置、电子设备、可读存储介质,可以降低实时输出位姿的等待时间。
一种位姿计算方法,所述方法包括:
若在预设初始化滑窗内首次采集到当前环境的深度图像,则将采集所述深度图像时电子设备的位姿确定为初始位姿;其中,所述深度图像对应的目标RGB图像并非所述预设初始化滑窗内的最后一帧图像;
根据所述初始位姿、所述目标RGB图像、所述深度图像及所述目标RGB图像的下一帧RGB图像,确定所述电子设备采集所述下一帧RGB图像时的位姿。
一种位姿计算装置,所述装置包括:
初始位姿确定模块,用于若在预设初始化滑窗内首次采集到当前环境的深度图像,则将采集所述深度图像时电子设备的位姿确定为初始位姿;其中,所述深度图像对应的目标RGB图像并非所述预设初始化滑窗内的最后一帧图像;
位姿确定模块,用于根据所述初始位姿、所述目标RGB图像、所述深度图像及所述目标RGB图像的下一帧RGB图像,确定所述电子设备采集所述下一帧RGB图像时的位姿。
一种电子设备,包括存储器及处理器,所述存储器中储存有计算机程序,所述计算机程序被所述处理器执行时,使得所述处理器执行如上所述的位姿计算方法的步骤。
一种计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时实现如上所述的位姿计算方法的步骤。
该位姿计算方法和装置、电子设备、计算机可读存储介质,若在预设初始化滑窗内首次采集到当前环境的深度图像,则将采集深度图像时电子设备的位姿确定为初始位姿;其中,深度图像对应的目标RGB图像并非预设初始化滑窗内的最后一帧图像。根据初始位姿、目标RGB图像、深度图像及目标RGB图像的下一帧RGB图像,确定电子设备采集下一帧RGB图像时的位姿。因此,采用本申请中的位姿计算方法,不需要在预设初始化滑窗内采集满RGB图像,电子设备才能够输出采集预设初始化滑窗外的第一帧RGB图像时的位姿。而是在预设初始化滑窗内首次采集到当前环境的深度图像,且深度图像对应的目标RGB图像并非预设初始化滑窗内的最后一帧图像时,就可以将采集深度图像时电子设备的位姿确定为初始位姿、目标RGB图像、深度图像及目标RGB图像的下一帧RGB图像,确定电子设备采集下一帧RGB图像时的位姿。显然,在预设初始化滑窗内就可以实时输出位姿,降低了实时输出位姿的等待时间。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为一个实施例中位姿计算方法的应用环境图;
图2为一个实施例中位姿计算方法的流程图;
图3为另一个实施例中位姿计算方法的流程图;
图4为一个实施例中若目标RGB图像不是预设初始化滑窗内的第一帧图像,构建当前环境的目标三维地图的示意图;
图5为一个实施例中采用预设透视投影PnP算法,计算下一帧RGB图像与目标RGB图像之间的第一位姿变换矩阵方法的流程图;
图6为一个实施例中采用预设透视投影PnP算法计算位姿变换矩阵的示意图;
图7为图5中计算下一帧RGB图像与目标RGB图像之间的第一平移变换矩阵方法的流程图;
图8为一个实施例中根据第一位姿变换矩阵及深度图像,构建当前环境的目标三维地图方法的流程图;
图9为图3中根据目标三维地图,计算电子设备在预设初始化滑窗内采集下一帧RGB图像之后的RGB图像时的位姿方法的流程图;
图10为一个实施例中计算采集下一帧RGB图像之后的RGB图像时的位姿及三维地图计算的示意图;
图11为再一个实施例中位姿计算方法的流程图;
图12为又一个实施例中位姿计算方法的示意图;
图13为一个具体的实施例中位姿计算方法的流程图;
图14为一个实施例中位姿计算装置的结构框图;
图15为一个实施例中位姿计算装置的结构框图;
图16为另一个实施例中位姿计算装置的结构框图;
图17为一个实施例中电子设备的内部结构示意图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本申请进行进一步详细说明。应当理解,此处所描述的具体实施例仅仅用以解释本申请,并不用于限定本申请。
电子设备在未知环境中的位置与姿态(简称位姿)问题,是增强现实、虚拟现实、移动机器人、无人驾驶等行业的关键技术之一。且随着这些行业的快速发展,对电子设备对周围环境中的物体进行定位的精确度也随之提出了越来越高的要求。而在采用VIO(Visual-Inertial Odometry,视觉-惯性里程计)系统对周围环境中的物体进行定位时,其核心的步骤之一为确定电子设备在拍摄每帧图像时的位姿。
其中一种传统方法,在确定电子设备在拍摄每帧图像时的位姿时,采用VINS(visual-inertial system,视觉惯性系统)-MONO算法。VINS-MONO算法的具体步骤包括:假设预设初始化滑窗内为10帧图像,当然,本申请中对预设初始化滑窗的大小不做具体限定。那么,第一步,通过电子设备上的相机采集RGB图像,在预设初始化滑窗内需累计满10帧RGB图像之后,从这10帧RGB图像中筛选出两帧视差满足条件的图像(例如L帧和R帧),再利用对极几何约束来计算这两帧之间的位姿。第二步,再利用该位姿使用三角化法恢复出两帧之间共视的地图点。第三步,将这些地图点投影至上述10帧RGB图像中除去L帧和R帧之外的其他帧中的任一帧上,通过最小化重投影误差来分别计算该任一帧的位姿。第四步,利用该任一帧与L帧、R帧之间采用三角化法恢复出该任一帧与L帧、R帧之间共视的地图点。重复执行第三步及第四步,便可计算出预设初始化滑窗内10帧RGB图像的位姿,以及这10帧RGB图像对应的地图点。
还有另一种传统方法,在确定电子设备在拍摄每帧图像时的位姿时,采用VINS-RGBD算法,VINS-RGBD算法的具体步骤包括:假设预设初始化滑窗内为10帧图像,那么,第一步,通过电子设备上的相机同时采集RGB图像及深度图像,且需保证每帧RGB图像均存在与其对应的深度图像。这里的对应指的是RGB图像在时间空间上均与深度图像对齐。第二步,在预设初始化滑窗内需累计满10帧RGB图像及对应的深度图像之后,从这10帧RGB图像中筛选出两帧视差满足条件的图像(例如L帧和R帧)。采用传统PnP算法同时计算L帧的位姿,再结合L帧对应的深度图像,采用重投影误差满足预设阈值来筛选有效地图点,进而恢复地图点。第三步,将这些地图点投影至上述10帧RGB图像中除去L帧和R帧之外的其他帧中的任一帧上,通过最小化重投影误差来分别计算该任一帧的位姿。第四步,利用该任一帧的深度图像与L帧之间恢复可以共视且重投影误差满足阈值要求的地图点。重复执行第三步及第四步,便可计算出预设初始化滑窗内10帧RGB图像的位姿,以及这10帧RGB图像对应的地图点。
以上两种传统方法,在实时输出位姿时,均需要基于电子设备上的相机,对周围环境采集一定数目的RGB图或深度图像,才能够基于该一定数目的RGB图或深度图像,实时输出电子设备在拍摄每帧图像时的位姿。显然,采集一定数目的RGB图和/或深度图像需要耗费较长的时间,进而导致实时输出位姿的等待时间也较长。
因此,为了解决实时输出位姿的等待时间较长的问题,本申请实施例中提出了一种位姿计算方法,不需要在预设初始化滑窗内采集满RGB图像,电子设备才能够输出采集预设初始化滑窗外的第一帧RGB图像时的位姿。而是在预设初始化滑窗内首次采集到当前环境的深度图像,且深度图像对应的目标RGB图像并非预设初始化滑窗内的最后一帧图像时,就可以将采集深度图像时电子设备的位姿确定为初始位姿、目标RGB图像、深度图像及目标RGB图像的下一帧RGB图像,确定电子设备采集下一帧RGB图像时的位姿。显然,在预设初始化滑窗内就可以实时输出位姿,降低了实时输出位姿的等待时间。
图1为一个实施例中位姿计算方法的应用场景图。如图1所示,该应用环境包括电子设备120,该电子设备120包括第一摄像头和第二摄像头。其中,第一摄像头为RGB摄像头,第二摄像头为采集深度图像的摄像头,例如,TOF(Time-offlight,飞行时间)摄像头或结构光摄像头等,本申请对此不做限定。电子设备120通过第一摄像头和第二摄像头分别采集当前环境的RGB图像及深度图像,若电子设备120在预设初始化滑窗内首次采集到当前环境的深度图像,则将采集深度图像时电子设备的位姿确定为初始位姿;其中,深度图像对应的目标RGB图像并非预设初始化滑窗内的最后一帧图像。根据初始位姿、目标RGB图像、深度图像及目标RGB图像的下一帧RGB图像,确定电子设备采集下一帧RGB图像时的位姿。电子设备120可以是手机、平板电脑、PDA(Personal Digital Assistant,个人数字助理)、穿戴式设备(智能手环、智能手表、智能眼镜、智能手套、智能袜子、智能腰带等)、VR(virtualreality,虚拟现实)设备、智能家居、无人驾驶汽车等任意终端设备。
图2为一个实施例中位姿计算方法的流程图。本实施例中的位姿计算方法,以运行于图1中的电子设备120上为例进行描述,该电子设备120包括第一摄像头和第二摄像头。其中,第一摄像头为RGB摄像头,第二摄像头为采集深度图像的摄像头。该位姿计算方法包括步骤220-步骤240,其中,
步骤220,若在预设初始化滑窗内首次采集到当前环境的深度图像,则将采集深度图像时电子设备的位姿确定为初始位姿;其中,深度图像对应的目标RGB图像并非预设初始化滑窗内的最后一帧图像。
传统方法中需要采集满一定数目的图像帧之后,才能够实时输出电子设备在拍摄每帧图像时的位姿。这些一定数目的图像帧就填满了预设初始化滑窗(以下简称滑窗),即预设初始化滑窗的大小与采集一定数目的图像帧的时长相等。例如,设置预设初始化滑窗内包括10帧RGB图像,则在采集了10帧RGB图像之后,这10帧RGB图像就填满了预设初始化滑窗,此时预设初始化滑窗的大小与采集10帧RGB图像的时长相等。
在本申请实施例中,不需要在预设初始化滑窗内采集满RGB图像,电子设备才能够输出采集预设初始化滑窗外的第一帧RGB图像时的位姿。而是通过电子设备中的第一摄像头采集RGB图像、同时通过第二摄像头采集深度图像。若在预设初始化滑窗内首次采集到当前环境的深度图像,且与该深度图像对应的目标RGB图像并非预设初始化滑窗内的最后一帧图像,则将采集深度图像时电子设备的位姿确定为初始位姿,并将目标RGB图像的图像坐标系作为世界坐标系,实现了对电子设备视觉上的初始化。且不需要从这10帧RGB图像中筛选出两帧视差满足条件的图像,因此,适应性更广。
其一,由于通过第二摄像头采集深度图像时的采集频率可能低于第一摄像头采集RGB图像时的采集频率,所以会导致不能采集到与每帧RGB图像对应的深度图像,即相对于RGB图像会缺失部分深度图像。其二,在电子设备中经常由于第二摄像头及相关的软硬件异常,也会导致相对于RGB图像缺失部分深度图像或所采集的RGB图像与深度图像对齐异常等情况。以上都会导致不能针对每帧RGB图像都能够采集到与之对应的深度图像。
步骤240,根据初始位姿、目标RGB图像、深度图像及目标RGB图像的下一帧RGB图像,确定电子设备采集下一帧RGB图像时的位姿。
将采集深度图像时电子设备的位姿确定为初始位姿,并将目标RGB图像的图像坐标系作为世界坐标系,实现了对电子设备视觉上的初始化之后,就可以实时输出采集下一帧RGB图像时的位姿。
具体的,在确定了初始位姿及世界坐标系之后,根据目标RGB图像、深度图像及目标RGB图像的下一帧RGB图像,确定电子设备采集下一帧RGB图像时的位姿。先可以采用透视投影PnP算法,基于目标RGB图像与该目标RGB图像的下一帧RGB图像之间相互匹配的2D-2D特征点对;其次,根据这些2D-2D特征点对结合深度图像上的3D点,得到匹配的3D-2D特征点对;再次,基于3D-2D特征点对,计算目标RGB图像的下一帧RGB图像与目标RGB图像之间的位姿变换矩阵。最后,基于该位姿变换矩阵及初始位姿,就可以生成该下一帧RGB图像的位姿。其中,位姿指的是位置和姿态,位姿是一个六维的矩阵,包含三个位置(X,Y,Z)和三个姿态角(航向,俯仰,横滚)。
其中,这里的透视投影PnP算法可以是传统的透视投影PnP算法,即基于3D-2D特征点对同时计算出两帧之间的旋转变换矩阵及平移变换矩阵,旋转变换矩阵及平移变换矩阵构成了位姿变换矩阵。当然,这里的透视投影PnP算法可以是新的透视投影PnP算法,即基于3D-2D特征点对分步计算出两帧之间的旋转变换矩阵及平移变换矩阵。
本申请实施例中,不需要在预设初始化滑窗内采集满RGB图像,电子设备才能够输出采集预设初始化滑窗外的第一帧RGB图像时的位姿。而是在预设初始化滑窗内首次采集到当前环境的深度图像,且深度图像对应的目标RGB图像并非预设初始化滑窗内的最后一帧图像时,就可以将采集深度图像时电子设备的位姿确定为初始位姿、目标RGB图像、深度图像及目标RGB图像的下一帧RGB图像,确定电子设备采集下一帧RGB图像时的位姿。将首次输出位姿的时间,从预设初始化滑窗外大大提前到滑窗内且提前到首次接收到深度图像的下一帧。从而,在预设初始化滑窗内就可以实时输出位姿,降低了实时输出位姿的等待时间。
同时,不需要在针对每帧RGB图像均对应采集到深度图像之后,才能够实时输出位姿。而是可以采用比采集RGB图像的频率较低的频率,来采集深度图,或可以采用可变频率来采集深度图,且只要在滑窗内采集到深度图,就可以开始初始化,并在初始化之后实时输出位姿。由于可以采用较低频率或可变频率采集深度图,就可以基于低频率的深度图去进行初始化及实时输出位姿,因此,避免了采集大量的数据,也避免了处理大量的数据,进而,降低了电子设备的功耗。
接上一个实施例,在计算出电子设备采集下一帧RGB图像时的位姿之后,依次计算电子设备在预设初始化滑窗内采集位于下一帧RGB图像之后的其他RGB图像时的位姿。那么,如图3所示,提供了一种位姿计算方法,还包括:
步骤260,根据电子设备采集下一帧RGB图像时的位姿以及深度图像,构建当前环境的目标三维地图。
在步骤240中计算出了电子设备采集下一帧RGB图像时的位姿,此时就可以基于目标RGB图像的深度图像以及该下一帧RGB图像时的位姿,构建当前环境的目标三维地图。其中,在构建当前环境的目标三维地图时,分为以下两种情况。
其中一种情况是,假设目标RGB图像为预设初始化滑窗内的第一帧图像,即针对预设初始化滑窗内的第一帧RGB图像,就采集到了与之对应的深度图像。那么,因为在该目标RGB图像之前不存在任何RGB图像,所以在构建当前环境的目标三维地图时,仅需要计算下一帧RGB图像与目标RGB图像之间的第一位姿变换矩阵,根据第一位姿变换矩阵及深度图像,就可以构建当前环境的目标三维地图。此处,如果在计算下一帧RGB图像的位姿的过程中,已经计算出了第一位姿变换矩阵,则就只需要直接获取即可。
另一种情况是,假设目标RGB图像不是预设初始化滑窗内的第一帧图像,即并不是在滑窗内的第一帧RGB图像就采集到了与之对应的深度图像。那么,在构建当前环境的目标三维地图时,因为该目标RGB图像之前还采集了RGB图像,则就不仅可以基于该目标RGB图像来恢复地图点,还可以基于该目标RGB图像之前采集的RGB图像来恢复地图点,以实现恢复更多的地图点来丰富当前环境的三维地图。
此时,同理计算下一帧RGB图像与目标RGB图像之间的第一位姿变换矩阵,根据第一位姿变换矩阵及深度图像,就可以构建当前环境的初始三维地图。此处,如果在计算下一帧RGB图像的位姿的过程中,已经计算出了第一位姿变换矩阵,则就只需要直接获取即可。然后,计算位于目标RGB图像之前的RGB图像与目标RGB图像之间的第二位姿变换矩阵,并根据第二位姿变换矩阵及深度图像对当前环境的初始三维地图进行更新,生成目标三维地图。
步骤280,根据目标三维地图,计算电子设备在预设初始化滑窗内采集位于下一帧RGB图像之后的其他RGB图像时的位姿。
在构建了目标三维地图之后,就可以根据目标三维地图,计算电子设备在滑窗内采集位于下一帧RGB图像之后的其他RGB图像时的位姿。由于目标三维地图是基于滑窗内第一帧RGB图像直到目标RGB图像的下一帧RGB图像,结合目标RGB图像的深度图像所构建的,即是基于这些RGB图像及该深度图像所共同恢复的地图点所构建的,显然目标三维地图上的地图点相对于该深度图像来说更加全面。
因此,计算电子设备在预设初始化滑窗内采集位于下一帧RGB图像之后的其他RGB图像时的位姿,就可以直接根据目标三维地图计算其他RGB图像的位姿。
本申请实施例中,根据电子设备采集下一帧RGB图像时的位姿以及深度图像,构建当前环境的目标三维地图。由于目标三维地图是基于滑窗内第一帧RGB图像直到目标RGB图像的下一帧RGB图像,结合目标RGB图像的深度图像所构建的,显然目标三维地图上的地图点相对于该深度图像来说更加全面。因此,根据目标三维地图,计算电子设备在预设初始化滑窗内采集位于下一帧RGB图像之后的其他RGB图像时的位姿。就大大提高了计算出的下一帧RGB图像之后的其他RGB图像时的位姿准确性。
在一个实施例中,步骤260,根据电子设备采集下一帧RGB图像时的位姿以及深度图像,构建当前环境的目标三维地图,包括:
计算下一帧RGB图像与目标RGB图像之间的第一位姿变换矩阵,根据第一位姿变换矩阵及深度图像,构建当前环境的目标三维地图。
在一个实施方式中,若目标RGB图像是预设初始化滑窗内的第一帧图像,则目标RGB图像之前不存在RGB图像,因此只需计算下一帧RGB图像与目标RGB图像之间的第一位姿变换矩阵。然后,根据第一位姿变换矩阵及深度图像,构建当前环境的目标三维地图。
在另一个实施方式中,若目标RGB图像不是预设初始化滑窗内的第一帧图像,则目标RGB图像之前存在RGB图像。因此,首先,计算下一帧RGB图像与目标RGB图像之间的第一位姿变换矩阵,根据第一位姿变换矩阵及深度图像,构建当前环境的初始三维地图。其次,计算位于目标RGB图像之前的RGB图像与目标RGB图像之间的第二位姿变换矩阵,并根据第二位姿变换矩阵及深度图像对当前环境的初始三维地图进行更新,生成目标三维地图。
本申请实施例中,若目标RGB图像是预设初始化滑窗内的第一帧图像,则只需计算下一帧RGB图像与目标RGB图像之间的第一位姿变换矩阵。然后,根据第一位姿变换矩阵及深度图像,直接构建出当前环境的目标三维地图。若目标RGB图像不是预设初始化滑窗内的第一帧图像,则先计算下一帧RGB图像与目标RGB图像之间的第一位姿变换矩阵。再根据第一位姿变换矩阵及深度图像,构建当前环境的初始三维地图。最后,计算位于目标RGB图像之前的RGB图像与目标RGB图像之间的第二位姿变换矩阵,并根据第二位姿变换矩阵及深度图像对当前环境的初始三维地图进行更新,生成目标三维地图。由于目标三维地图是基于滑窗内第一帧RGB图像直到目标RGB图像的下一帧RGB图像,结合目标RGB图像的深度图像所构建的,显然目标三维地图上的地图点相对于该深度图像来说更加全面。因此,根据目标三维地图,计算电子设备在预设初始化滑窗内采集位于下一帧RGB图像之后的其他RGB图像时的位姿。就大大提高了计算出的下一帧RGB图像之后的其他RGB图像时的位姿准确性。
在一个实施例中,根据电子设备采集下一帧RGB图像时的位姿以及深度图像,构建当前环境的目标三维地图,包括:
根据第一位姿变换矩阵及深度图像,构建当前环境的初始三维地图;
计算位于目标RGB图像之前的RGB图像与目标RGB图像之间的第二位姿变换矩阵,并根据第二位姿变换矩阵及深度图像对当前环境的初始三维地图进行更新,生成目标三维地图。
如图4所示,为一个实施例中若目标RGB图像不是预设初始化滑窗内的第一帧图像,构建当前环境的目标三维地图的示意图。例如,假设预设初始化滑窗内为10帧图像,在第4帧时采集到深度图像,即目标RGB图像为第4帧RGB图像。那么,此时构建当前环境的目标三维地图时,具体包括两个步骤:
第一步,构建当前环境的初始三维地图。具体的,计算下一帧RGB图像(第5帧)与目标RGB图像(第4帧)之间的第一位姿变换矩阵,根据第一位姿变换矩阵及深度图像,构建当前环境的初始三维地图。这里计算当前环境的初始三维地图的方式,与若目标RGB图像为预设初始化滑窗内的第一帧图像,则步骤260中根据电子设备采集下一帧RGB图像时的位姿以及深度图像,构建当前环境的目标三维地图的方式相同,再此不在赘述。
第二步,对初始三维地图进行更新,生成目标三维地图。具体的,计算位于目标RGB图像之前的RGB图像与目标RGB图像之间的第二位姿变换矩阵,并根据第二位姿变换矩阵及深度图像对当前环境的初始三维地图进行补充,生成目标三维地图。
针对目标RGB图像之前的RGB图像,即为针对第1、2、3帧,计算目标RGB图像之前的RGB图像与目标RGB图像之间的第二位姿变换矩阵,即为分别计算第4帧与第1、2、3帧之间的第二位姿变换矩阵。再根据第二位姿变换矩阵及深度图像对当前环境的初始三维地图进行更新,生成目标三维地图。
具体的,首先计算第4帧与第1帧之间的第二位姿变换矩阵,再根据该第二位姿变换矩阵及深度图像对当前环境的初始三维地图进行更新,生成更新后的三维地图。接着,计算第4帧与第2帧之间的第二位姿变换矩阵,再根据该第二位姿变换矩阵及深度图像对更新后的三维地图再次进行更新,生成再次更新后的三维地图。计算第4帧与第3帧之间的第二位姿变换矩阵,再根据该第二位姿变换矩阵及深度图像对当前环境的再次更新后的三维地图进行第三次更新,生成目标三维地图。
本申请实施例中,若目标RGB图像不是预设初始化滑窗内的第一帧图像,则在构建当前环境的目标三维地图时,包括:计算下一帧RGB图像与目标RGB图像之间的第一位姿变换矩阵,根据第一位姿变换矩阵及深度图像,构建当前环境的初始三维地图。再计算位于目标RGB图像之前的RGB图像与目标RGB图像之间的第二位姿变换矩阵,并根据第二位姿变换矩阵及深度图像对当前环境的初始三维地图进行更新,生成目标三维地图。使得最终所得的目标三维地图中,不仅恢复了目标RGB图像的下一帧RGB图像对应的地图点,还恢复了目标RGB图像之前的每一帧图像的地图点。因此,大大提高了此时所得到的目标三维地图的完整性、准确性。
在一个实施例中,提供了一种位姿计算方法,还包括:
采用预设透视投影PnP算法,计算第一位姿变换矩阵或第二位姿变换矩阵。其中,预设透视投影PnP算法用于分步计算目标RGB图像的相关RGB图像与目标RGB图像之间的旋转变换矩阵和平移变换矩阵,目标RGB图像的相关RGB图像为位于目标RGB图像之前的RGB图像或下一帧RGB图像。
其中,计算下一帧RGB图像与目标RGB图像之间的第一位姿变换矩阵,包括:
采用预设透视投影PnP算法,计算下一帧RGB图像与目标RGB图像之间的第一位姿变换矩阵(也可以简称为位姿);其中,预设透视投影PnP算法用于分步计算下一帧RGB图像与目标RGB图像之间的旋转变换矩阵和平移变换矩阵。
其中,预设透视投影PnP算法是相对于传统的透视投影PnP算法而言的,传统的透视投影PnP算法是基于3D-2D特征点对同时计算出两帧之间的旋转变换矩阵及平移变换矩阵。而预设透视投影PnP算法用于分步计算两帧之间的旋转变换矩阵和平移变换矩阵。
具体的,在采用预设透视投影PnP算法,计算下一帧RGB图像与目标RGB图像之间的第一位姿变换矩阵时,首先,计算下一帧RGB图像与目标RGB图像之间的第一旋转变换矩阵;其次,计算下一帧RGB图像与目标RGB图像之间的第一平移变换矩阵。且第一旋转变换矩阵与第一平移变换矩阵,构成了第一位姿变换矩阵。
同理,计算位于目标RGB图像之前的RGB图像与目标RGB图像之间的第一位姿变换矩阵,包括:
采用预设透视投影PnP算法,计算位于目标RGB图像之前的RGB图像与目标RGB图像之间的第二位姿变换矩阵(也可以简称为位姿);其中,预设透视投影PnP算法用于分步计算位于目标RGB图像之前的RGB图像与目标RGB图像之间的第二旋转变换矩阵和第二平移变换矩阵。该第二旋转变换矩阵与第二平移变换矩阵,构成了第二位姿变换矩阵。
本申请实施例中,在计算位于目标RGB图像之前的RGB图像或下一帧RGB图像与目标RGB图像之间的位姿变换矩阵时,将计算旋转变换矩阵与平移变换矩阵的过程进行解耦。就可以避免将计算旋转变换矩阵时所产生的误差,与计算平移变换矩阵时所产生的误差进行叠加。从而,提高了采用预设透视投影PnP算法,实现分步式计算的方式所最终得到的第一或第二位姿变换矩阵的准确性。
在一个实施例中,采用预设透视投影PnP算法,计算第一位姿变换矩阵或第二位姿变换矩阵,包括:
计算目标RGB图像的相关RGB图像与目标RGB图像之间的旋转变换矩阵;
根据深度图像、旋转变换矩阵,计算目标RGB图像的相关RGB图像与目标RGB图像之间的平移变换矩阵;
基于旋转变换矩阵及平移变换矩阵,生成目标RGB图像的相关RGB图像与目标RGB图像之间的第一位姿变换矩阵或第二位姿变换矩阵。
其中,若目标RGB图像的相关RGB图像为下一帧RGB图像,即目标RGB图像之前不存在RGB图像,则如图5所示,采用预设透视投影PnP算法,只需要计算第一位姿变换矩阵,包括:
步骤520,计算下一帧RGB图像与目标RGB图像之间的第一旋转变换矩阵。
由于所采集到的深度图像中会出现有些区域没有深度信息,或有些区域的深度信息不准确,因此,两帧RGB图像之间相互匹配的2D-2D特征点对,一般是比其中一帧的深度图像与另一帧RGB图像之间相互匹配的3D-2D特征点对要更多、更全面。
结合图6所示,为一个实施例中采用预设透视投影PnP算法计算位姿变换矩阵的示意图。图6中图像i为目标RGB图像,图像j为目标RGB图像的下一帧RGB图像,椭圆形框对应的是目标RGB图像对应的深度图像。图6中左侧的一对图像i及图像j所示,即为在图像i及图像j中确定相互匹配的2D-2D特征点对,及计算第一旋转变换矩阵Rij的示意图。
具体的,可以在下一帧RGB图像与目标RGB图像这两帧之间确定相互匹配的2D-2D特征点对,具体可以采用光流法或其他图像匹配方法来确定两帧之间的2D-2D特征点对。通过下一帧RGB图像与目标RGB图像这两帧之间相互匹配的2D-2D特征点对,结合对极几何约束计算下一帧RGB图像与目标RGB图像之间的第一旋转变换矩阵Rij。
步骤540,根据深度图像、第一旋转变换矩阵,计算下一帧RGB图像与目标RGB图像之间的第一平移变换矩阵。
具体的,首先,根据第一旋转变换矩阵对下一帧RGB图像与目标RGB图像上相互匹配的2D-2D特征点对进行剔除,得到剔除后的2D-2D特征点对。其次,根据深度图像将剔除后的2D-2D特征点对,转换为3D-2D特征点对。最后,根据3D-2D特征点对,计算下一帧RGB图像与目标RGB图像之间的第一平移变换矩阵tij。
步骤560,基于第一旋转变换矩阵及第一平移变换矩阵,生成下一帧RGB图像与目标RGB图像之间的第一位姿变换矩阵。
结合上面所计算出的第一旋转变换矩阵Rij及第一平移变换矩阵tij,生成下一帧RGB图像与目标RGB图像之间的第一位姿变换矩阵。
本申请实施例中,计算下一帧RGB图像与目标RGB图像之间的第一旋转变换矩阵。根据深度图像、第一旋转变换矩阵,计算下一帧RGB图像与目标RGB图像之间的第一平移变换矩阵。基于第一旋转变换矩阵及第一平移变换矩阵,生成下一帧RGB图像与目标RGB图像之间的第一位姿变换矩阵。采用预设透视投影PnP算法,实现分步式计算旋转变换矩阵及平移变换矩阵,避免了将两者计算过程中所产生的误差进行叠加,进而提高了最终得到的第一位姿变换矩阵的准确性。
接上一个实施例,如图7所示,步骤540,根据深度图像、第一旋转变换矩阵,计算下一帧RGB图像与目标RGB图像之间的第一平移变换矩阵,包括:
步骤542,根据第一旋转变换矩阵对下一帧RGB图像与目标RGB图像上相互匹配的2D-2D特征点对进行剔除,得到剔除后的2D-2D特征点对。
结合图6所示,图像i为目标RGB图像,图像j为目标RGB图像的下一帧RGB图像,椭圆形框对应的是目标RGB图像对应的深度图像。图6中右侧的一对图像i及图像j所示,即为在图像i及图像j中,根据第一旋转变换矩阵对图像i与图像j上相互匹配的2D-2D特征点对进行剔除,得到剔除后的2D-2D特征点对。
步骤544,根据深度图像将剔除后的2D-2D特征点对,转换为3D-2D特征点对。
结合图6中的椭圆形框所示的深度图像,根据深度图像将剔除后的2D-2D特征点对,转换为3D-2D特征点对。并采用Ransanc算法剔除3D-2D特征点对中的异常点对,生成剔除后的3D-2D特征点对。
步骤546,根据3D-2D特征点对,计算下一帧RGB图像与目标RGB图像之间的第一平移变换矩阵。
在计算出了目标RGB图像与下一帧RGB图像之间的3D-2D特征点对之后,就可以基于平移变换矩阵计算公式,计算下一帧RGB图像与目标RGB图像之间的第一平移变换矩阵。
平移变换矩阵计算公式具体如下所示:
针对多个3D-2D特征点对,采用上述公式(1-1)构建最小二乘法公式,就可以计算出最优的变量tij,作为下一帧RGB图像与目标RGB图像之间的第一平移变换矩阵。
本申请实施例中,在计算下一帧RGB图像与目标RGB图像之间的第一平移变换矩阵时,首先,在根据第一旋转变换矩阵对下一帧RGB图像与目标RGB图像上相互匹配的2D-2D特征点对进行剔除,得到剔除后的2D-2D特征点对。其次,根据深度图像将剔除后的2D-2D特征点对,转换为3D-2D特征点对。最后,根据3D-2D特征点对,计算下一帧RGB图像与目标RGB图像之间的第一平移变换矩阵。对特征点对进行了多次剔除异常点对,以及采用最小二乘法来计算第一平移变换矩阵,提高了所计算出的第一平移变换矩阵的准确性。
在一个实施例中,若目标RGB图像为预设初始化滑窗内的第一帧图像,则计算下一帧RGB图像与目标RGB图像之间的第一位姿变换矩阵,如图8所示,根据第一位姿变换矩阵及深度图像,构建当前环境的目标三维地图,包括:
步骤820,根据第一位姿变换矩阵,将深度图像上的3D特征点投影至下一帧RGB图像上,生成投影2D特征点。
其中,第一位姿变换矩阵是下一帧RGB图像与目标RGB图像之间的位姿变换矩阵。可以基于目标RGB图像对应的深度图像、目标RGB图像,确定3D-2D匹配点对,然后基于第一位姿变换矩阵就可以将深度图像上的3D特征点投影至下一帧RGB图像上,生成投影2D特征点。
步骤840,计算投影2D特征点与下一帧RGB图像上的2D特征点之间的重投影误差。
步骤860,若重投影误差小于预设误差阈值,则将深度图像上的3D特征点作为目标地图点。
具体的,下一帧RGB图像上原本就存在很多原始2D特征点,此时计算这些投影2D特征点与原始2D特征点位置之间的重投影误差。若重投影误差小于预设误差阈值,则认为重投影误差小于预设误差阈值对应的3D特征点为可信的地图点,将深度图像上的3D特征点作为目标地图点。
步骤880,根据目标地图点构建当前环境的目标三维地图。
再将深度图像上的3D特征点中满足条件的3D特征点,作为目标地图点之后,就可以基于这些目标地图点,构建当前环境的目标三维地图。
本申请实施例中,若目标RGB图像为预设初始化滑窗内的第一帧图像,则根据第一位姿变换矩阵,将深度图像上的3D特征点投影至下一帧RGB图像上,生成投影2D特征点。计算投影2D特征点与下一帧RGB图像上的2D特征点之间的重投影误差。若重投影误差小于预设误差阈值,则将深度图像上的3D特征点作为目标地图点,根据目标地图点构建当前环境的目标三维地图。实现了在目标RGB图像为预设初始化滑窗内的第一帧图像时,计算出当前环境的目标三维地图。然后,在计算电子设备在预设初始化滑窗内采集位于下一帧RGB图像之后的其他RGB图像时的位姿时,就可以直接采用目标三维地图进行计算。
在一个实施例中,采用预设透视投影PnP算法,计算第一位姿变换矩阵或第二位姿变换矩阵,包括:
计算目标RGB图像的相关RGB图像与目标RGB图像之间的旋转变换矩阵;
根据深度图像、旋转变换矩阵,计算目标RGB图像的相关RGB图像与目标RGB图像之间的平移变换矩阵;
基于旋转变换矩阵及平移变换矩阵,生成目标RGB图像的相关RGB图像与目标RGB图像之间的第一位姿变换矩阵或第二位姿变换矩阵。
其中,若目标RGB图像的相关RGB图像为位于目标RGB图像之前的RGB图像及下一帧RGB图像,即目标RGB图像之前存在RGB图像,则采用预设透视投影PnP算法,需要同时计算第一位姿变换矩阵及第二位姿变换矩阵。其中,计算第一位姿变换矩阵的过程在此不再赘述,计算位于目标RGB图像之前的RGB图像与目标RGB图像之间的第二位姿变换矩阵,包括:
采用预设透视投影PnP算法,计算位于目标RGB图像之前的RGB图像与目标RGB图像之间的第二位姿变换矩阵。
其中,预设透视投影PnP算法是相对于传统的透视投影PnP算法而言的,传统的透视投影PnP算法是基于3D-2D特征点对同时计算出两帧之间的旋转变换矩阵及平移变换矩阵。而预设透视投影PnP算法用于分步计算两帧之间的旋转变换矩阵和平移变换矩阵。
具体的,在采用预设透视投影PnP算法,计算目标RGB图像之前的RGB图像与目标RGB图像之间的第二位姿变换矩阵时,首先,计算目标RGB图像之前的RGB图像与目标RGB图像之间的第二旋转变换矩阵;其次,根据深度图像、第二旋转变换矩阵,计算目标RGB图像之前的RGB图像与目标RGB图像之间的第二平移变换矩阵。且第二旋转变换矩阵与第二平移变换矩阵,构成了第二位姿变换矩阵。
本申请实施例中,在计算目标RGB图像之前的RGB图像与目标RGB图像之间的第二位姿变换矩阵时,将计算第二旋转变换矩阵与第二平移变换矩阵的过程进行解耦。就可以避免将计算第二旋转变换矩阵时所产生的误差,与计算第二平移变换矩阵时所产生的误差进行叠加。从而,提高了采用预设透视投影PnP算法,实现了分步计算出第二位姿变换矩阵,进而提高了第二位姿变换矩阵的准确性。
在一个实施例中,根据深度图像、第二旋转变换矩阵,计算目标RGB图像之前的RGB图像与目标RGB图像之间的第二平移变换矩阵,包括:
步骤一,根据第二旋转变换矩阵对目标RGB图像之前的RGB图像与目标RGB图像上相互匹配的2D-2D特征点对进行剔除,得到剔除后的2D-2D特征点对。
结合图6所示,假设图像i为目标RGB图像,图像j为目标RGB图像的目标RGB图像之前的RGB图像,椭圆形框对应的是目标RGB图像对应的深度图像。图6中右侧的一对图像i及图像j所示,即为在图像i及图像j中,根据第二旋转变换矩阵对图像i与图像j上相互匹配的2D-2D特征点对进行剔除,得到剔除后的2D-2D特征点对。
步骤二,根据深度图像将剔除后的2D-2D特征点对,转换为3D-2D特征点对。
结合图6中的椭圆形框所示的深度图像,根据深度图像将剔除后的2D-2D特征点对,转换为3D-2D特征点对。并采用Ransanc算法剔除3D-2D特征点对中的异常点对,生成剔除后的3D-2D特征点对。
步骤三,根据3D-2D特征点对,计算目标RGB图像之前的RGB图像与目标RGB图像之间的第二平移变换矩阵。
在计算出了目标RGB图像与目标RGB图像之前的RGB图像之间的3D-2D特征点对之后,就可以基于平移变换矩阵计算公式,计算目标RGB图像之前的RGB图像与目标RGB图像之间的第二平移变换矩阵。平移变换矩阵计算公式具体如下所示:
针对多个3D-2D特征点对,采用上述公式(1-1)构建最小二乘法公式,就可以计算出最优的变量tij,作为目标RGB图像之前的RGB图像与目标RGB图像之间的第二平移变换矩阵。
本申请实施例中,在计算目标RGB图像之前的RGB图像与目标RGB图像之间的第二平移变换矩阵时,首先,在根据第二旋转变换矩阵对目标RGB图像之前的RGB图像与目标RGB图像上相互匹配的2D-2D特征点对进行剔除,得到剔除后的2D-2D特征点对。其次,根据深度图像将剔除后的2D-2D特征点对,转换为3D-2D特征点对。最后,根据3D-2D特征点对,计算目标RGB图像之前的RGB图像与目标RGB图像之间的第二平移变换矩阵。对特征点对进行了多次剔除异常点对,以及采用最小二乘法来计算第二平移变换矩阵,提高了所计算出的第二平移变换矩阵的准确性。
在一个实施例中,根据第二位姿变换矩阵及深度图像对当前环境的初始三维地图进行更新,生成目标三维地图,包括:
根据第二位姿变换矩阵,将深度图像上的3D特征点分别投影至位于目标RGB图像之前的RGB图像上,生成投影2D特征点;
计算投影2D特征点与位于目标RGB图像之前的RGB图像上的2D特征点之间的重投影误差;
若重投影误差小于预设误差阈值,则将深度图像上的3D特征点作为新的目标地图点;
将新的目标地图点添加至初始三维地图,生成目标三维地图。
具体的,其中,第二位姿变换矩阵是目标RGB图像之前的RGB图像与目标RGB图像之间的位姿变换矩阵。可以基于目标RGB图像对应的深度图像、目标RGB图像,确定3D-2D匹配点对,然后基于第二位姿变换矩阵就可以将深度图像上的3D特征点投影至目标RGB图像之前的RGB图像上,生成投影2D特征点。
而目标RGB图像之前的RGB图像上原本就存在很多原始2D特征点,此时计算这些投影2D特征点与原始2D特征点位置之间的重投影误差。若重投影误差小于预设误差阈值,则认为重投影误差小于预设误差阈值对应的3D特征点为可信的地图点,将3D特征点作为目标地图点。
再将深度图像上的3D特征点中满足条件的3D特征点,作为目标地图点之后,就可以基于这些目标地图点,构建当前环境的目标三维地图。
本申请实施例中,若目标RGB图像不是预设初始化滑窗内的第一帧图像,则根据第二位姿变换矩阵,将深度图像上的3D特征点投影至目标RGB图像之前的RGB图像上,生成投影2D特征点。计算投影2D特征点与目标RGB图像之前的RGB图像上的2D特征点之间的重投影误差。若重投影误差小于预设误差阈值,则将深度图像上的3D特征点作为目标地图点,根据目标地图点构建当前环境的目标三维地图。实现了在目标RGB图像不是预设初始化滑窗内的第一帧图像时,计算出当前环境的目标三维地图。然后,在计算电子设备在预设初始化滑窗内采集位于下一帧RGB图像之后的其他RGB图像时的位姿时,就可以直接采用目标三维地图进行计算。
在一个实施例中,如图9所示,步骤280,根据目标三维地图,计算电子设备在预设初始化滑窗内采集下一帧RGB图像之后的RGB图像时的位姿,包括:
步骤282,将下一帧RGB图像的下一帧作为当前帧,执行以下目标操作:
步骤284,根据目标三维地图、当前帧及位于当前帧之前的RGB图像,生成目标三维地图与当前帧之间相互匹配的3D-2D特征点对;
步骤286,基于3D-2D特征点对计算当前帧的位姿;
步骤288,根据当前帧对目标三维地图进行更新,并将更新后的三维地图作为新的目标三维地图。
将当前帧的下一帧作为新的当前帧,循环执行目标操作直到计算出预设初始化滑窗内最后一帧RGB图像的位姿为止。
结合图10所示,为一个实施例中计算采集下一帧RGB图像之后的RGB图像时的位姿及三维地图计算的示意图。例如,假设预设初始化滑窗内为10帧图像,在第4帧时采集到深度图像,即目标RGB图像为第4帧,目标RGB图像的下一帧RGB图像为第5帧。此时,计算采集下一帧RGB图像之后的RGB图像时的位姿,即为计算采集第6-10帧图像时的位姿。
从计算采集第6帧图像时的位姿开始,首先,将下一帧RGB图像(第5帧)的下一帧(第6帧)作为当前帧进行计算当前帧的位姿。
其次,根据目标三维地图、位于当前帧之前的RGB图像及当前帧(第6帧),生成目标三维地图与当前帧(第6帧)之间相互匹配的3D-2D特征点对。具体的,从位于当前帧之前的RGB图像及当前帧中,采用光流法或其他图像匹配方法来获取相互匹配的2D特征点对。从目标三维地图中的地图点中获取与2D特征点对匹配的3D特征点,根据匹配的3D特征点及2D特征点对,生成目标三维地图与当前帧之间相互匹配的3D-2D特征点对。
再次,基于3D-2D特征点对,采用传统的透视投影PnP算法计算当前帧的位姿。其中,传统的透视投影PnP算法是基于3D-2D特征点对同时计算出两帧之间的旋转变换矩阵及平移变换矩阵。因此,在采用传统的透视投影PnP算法计算当前帧的位姿时,基于3D-2D特征点对同时分别计算出位于当前帧之前的RGB图像与当前帧之间的旋转变换矩阵及平移变换矩阵。进而,基于旋转变换矩阵及平移变换矩阵,得到位姿变换矩阵,然后分别基于位姿变换矩阵及位于当前帧之前的RGB图像的位姿,得到当前帧的位姿。其中,具体可以是基于旋转变换矩阵及平移变换矩阵相乘得到位姿变换矩阵,本申请对此计算方式不做限定。
最后,根据当前帧对目标三维地图进行更新,并将更新后的三维地图作为新的目标三维地图。将当前帧的下一帧RGB图像(第7帧)作为新的当前帧,循环执行目标操作步骤284-286直到计算出预设初始化滑窗内最后一帧RGB图像的位姿为止。
本申请实施例中,在计算电子设备在预设初始化滑窗内采集下一帧RGB图像之后的RGB图像时的位姿的过程中,采用循环的方式,首先,基于目标三维地图及当前帧及位于当前帧之前的RGB图像,计算当前帧的位姿。其次,基于当前帧对目标三维地图进行更新,并将更新后的三维地图作为新的目标三维地图。最后,基于新的目标三维地图及下一组当前帧及位于当前帧之前的RGB图像,计算新的当前帧的位姿。其次,基于新的当前帧对新的目标三维地图再次进行更新,并将再次更新后的三维地图作为新的目标三维地图。如此循环,直到计算出预设初始化滑窗内最后一帧RGB图像的位姿为止。
由于计算后一帧的位姿时,均采用了基于前面所有帧所计算出的三维地图,因此,提高了所计算出的后一帧的位姿的准确性。
在一个实施例中,根据目标三维地图、当前帧及位于当前帧之前的RGB图像,生成目标三维地图与当前帧之间相互匹配的3D-2D特征点对,包括:
从当前帧及位于当前帧之前的RGB图像中,获取相互匹配的2D特征点对;
从目标三维地图中的地图点中获取与2D特征点对匹配的3D特征点,根据匹配的3D特征点及2D特征点对,生成目标三维地图与当前帧之间相互匹配的3D-2D特征点对。
本申请实施例中,从当前帧及位于当前帧之前的RGB图像中,采用光流法或其他图像匹配方法来获取相互匹配的2D特征点对。因为在计算目标三维地图时位于当前帧之前的RGB图像均提供了部分地图点,所以可以从目标三维地图中的地图点中获取与该2D特征点对匹配的3D特征点。然后,基于该3D特征点及2D特征点对中当前帧的2D特征点,就生成了目标三维地图与当前帧之间相互匹配的3D-2D特征点对。因此,基于3D-2D特征点对,就获取了目标三维地图上的3D特征点与当前帧上的2D特征点之间的匹配关系。
在一个实施例中,当前帧为滑窗内目标RGB图像的下一帧图像之后的图像帧,若当前帧存在对应的深度图像,则步骤288,根据当前帧对目标三维地图进行更新,生成更新后的三维地图,包括:
根据当前帧对应的深度图像、当前帧与预设滑窗内当前帧之前的RGB图像之间的位姿变换矩阵,对目标三维地图进行更新生成中间三维地图;
采用三角化法对中间三维地图进行更新,生成更新后的三维地图。
假设当前帧(第6帧)存在对应的深度图像,则在根据当前帧对目标三维地图进行更新,生成更新后的三维地图时,先根据当前帧对应的深度图像、当前帧与预设滑窗内当前帧之前的RGB图像之间的位姿变换矩阵,对目标三维地图进行更新生成中间三维地图。
具体的,首先,计算当前帧与预设滑窗内当前帧之前的RGB图像之间的位姿变换矩阵;其次,根据位姿变换矩阵,将当前帧的深度图像上的3D特征点投影至当前帧之前的RGB图像上,生成投影2D特征点。再次,计算投影2D特征点与当前帧之前的RGB图像上的2D特征点之间的重投影误差,若重投影误差小于预设误差阈值,则将当前帧的深度图像上的3D特征点作为目标地图点。最后,根据目标地图点对目标地图进行更新生成中间三维地图。
进而,再采用三角化法对中间三维地图进行更新,生成更新后的三维地图。其中,在多视几何中,相机在两个位置观测到同一个空间点,通过两次相机位姿和图像观测点坐标求得三维空间点坐标,这个过程即为三角化法的计算过程。采用三角化法可以恢复一些深度图像中所缺失的深度信息。
本申请实施例中,若当前帧存在对应的深度图像,根据当前帧对应的深度图像、当前帧与预设滑窗内当前帧之前的RGB图像之间的位姿变换矩阵,对目标三维地图进行更新生成中间三维地图。再采用三角化法对中间三维地图进行更新,生成更新后的三维地图。若当前帧存在对应的深度图像,那么结合之前的图像帧所构建的目标三维地图,再基于该当前帧的深度图像上的3D特征点去更新目标三维地图。最后,再采用三角化法可以恢复一些深度图像中所缺失的深度信息。从而,大大提高了此时所构建的三维地图的完整性及准确性。
接上一个实施例,若当前帧不存在对应的深度图像,则步骤288,根据当前帧对目标三维地图进行更新,生成更新后的三维地图,包括:
采用三角化法对目标三维地图进行更新,生成更新后的三维地图。
本申请实施例中,若当前帧不存在对应的深度图像,则采用三角化法对目标三维地图进行更新,生成更新后的三维地图。由于采用角化法可以恢复一些深度图像中所缺失的深度信息,所以也大大在一定程度上提高了此时所构建的三维地图的完整性及准确性。
在一个实施例中,如图11所示,在根据目标三维地图,计算电子设备在预设初始化滑窗内采集位于下一帧RGB图像之后的RGB图像时的位姿之后,还包括:
步骤1120,获取在预设滑窗内所采集的IMU数据。
采用VIO(Visual-Inertial Odometry,视觉-惯性里程计)系统可以将相机所采集的图像数据与IMU(Inertial measurement unit,惯性测量单元)数据进行融合实现更精确地定位。
由于电子设备中IMU采集IMU数据的采集频率大于采集RGB图像的频率,那么,在滑窗内包含10帧RGB图像时,一般情况下所采集到的IMU数据就多于10组。而在计算IMU的初始化信息时,是基于预设滑窗内所采集的所有IMU数据来进行计算的。因此,就需要获取在预设滑窗内所采集的所有IMU数据。
步骤1140,根据预设滑窗内各帧RGB图像的位姿、IMU数据,计算IMU的初始化信息;初始化信息包括初始速度、IMU的零偏及IMU的重力向量。
然后,就可以根据RGB图像的位姿、预设滑窗内所采集的所有IMU数据,利用RGB图像的位姿变换矩阵中的旋转变换矩阵进行旋转约束,利用RGB图像的平移变换矩阵进行平移约束,从而计算出IMU的初始化信息。IMU的初始化信息包括电子设备的初始速度、IMU的零偏(Bias)及IMU的重力向量。
步骤1160,根据初始位姿、目标三维地图及IMU的初始化信息,计算预设滑窗之后所采集的RGB图像的位姿。
在计算出了IMU的初始化信息之后,就可以根据初始位姿、目标三维地图及IMU的初始化信息,计算预设滑窗之后所采集的RGB图像的位姿。例如,计算第11帧图像的位姿,当然,本申请对此不做限定。其中,初始位姿就是首次采集到当前环境的深度图像对应的目标RGB图像的位姿。此时的目标三维地图就是基于滑窗内所有图像帧所构建的三维地图。
本申请实施例中,在计算预设滑窗之后所采集的RGB图像的位姿时,首先对IMU进行初始化,然后就可以不仅结合相机所采集的图像数据,还可以结合IMU数据计算预设滑窗之后所采集的RGB图像的位姿。从视觉和IMU两个维度上提高了位姿计算的适应性、鲁棒性。
在一个实施例中,提供了一种位姿计算方法,还包括:
若在预设初始化滑窗内未采集到当前环境的深度图像,则根据预设初始化滑窗内所采集的RGB图像,计算初始位姿及初始三维地图;
根据初始位姿及初始三维地图,计算预设初始化滑窗之后电子设备采集RGB图像时的位姿。
本申请实施例中,若在预设初始化滑窗内未采集到当前环境的深度图像,则采用传统的VINS-MONO算法,根据预设初始化滑窗内所采集的RGB图像,计算初始位姿及初始三维地图。再根据初始位姿及初始三维地图,计算预设初始化滑窗之后电子设备采集RGB图像时的位姿。保证了在预设初始化滑窗内未采集到当前环境的深度图像的情况下,仍然能够在滑窗内需累计满10帧RGB图像之后,实时输出电子设备采集RGB图像时的位姿。
在一个实施例中,提供了一种位姿计算方法,还包括:
若深度图像对应的目标RGB图像为预设初始化滑窗内的最后一帧图像,则根据预设初始化滑窗内所采集的RGB图像,计算初始位姿及初始三维地图;
根据初始位姿及初始三维地图,计算预设滑窗之后电子设备采集RGB图像时的位姿。
结合图12所示,为在预设初始化滑窗内未采集到当前环境的深度图像,或,深度图像对应的目标RGB图像为预设初始化滑窗内的最后一帧图像时计算位姿的示意图。
本申请实施例中,若深度图像对应的目标RGB图像为预设初始化滑窗内的最后一帧图像,那么若采用本申请中的将采集深度图像时电子设备的位姿确定为初始位姿。根据初始位姿、目标RGB图像、深度图像及目标RGB图像的下一帧RGB图像,确定电子设备采集下一帧RGB图像时的位姿。因此,采用本申请中的位姿计算方法也只能计算出滑窗之后电子设备采集RGB图像时的位姿,与采用传统的VINS-MONO算法计算预设初始化滑窗之后电子设备采集RGB图像时的位姿的时长基本相同。因此,若深度图像对应的目标RGB图像为预设初始化滑窗内的最后一帧图像,则可以采用传统的VINS-MONO算法、也可以采用本申请中的位姿计算方法来进行计算。因此,提供了多样化的位姿计算方式,更加灵活。
在一个具体的实施例中,如图13所示,提供了一种位姿计算方法,以首次采集到深度图像的RGB图像为预设初始化滑窗内的第二帧图像及第二帧图像之后的图像为例进行说明,该方法包括:
步骤1302,VIO系统启动初始化;
步骤1304,采集RGB图像、IMU数据及深度图像;
步骤1306,对所采集的RGB图像及深度图像,进行去畸变及对齐处理;
步骤1308,对所采集的RGB图像的帧号计数为frame_count;
步骤1310,判断当前所采集的RGB图像是否存在对应的深度图像(Depth图),且为首次采集到当前环境的Depth图;若是,则进入步骤1312;若否,则进入步骤1318;
步骤1312,将使用Depth图进行计算位姿的标志位start_depth_init=true;
步骤1314,将该RGB图像的图像坐标系设置为世界坐标系,将该RGB图像的位姿作为初始位姿,并将初始位姿设置为0;并将该RGB图像的frame_count记为first_depth;
步骤1316,采集到下一帧RGB图像,则将frame_count更新为frame_count+1;
步骤1318,判断first_depth是否小于滑窗大小windowsize(10帧);若是,则进入步骤1320;若否,则进入步骤1354;
步骤1320,判断当前帧的帧号frame_count+1是否等于first_depth+1;若是,则进入步骤1322;若否,则进入步骤1334;
步骤1322,采用预设透视投影PnP算法,计算first_depth帧与first_depth+1帧之间的第一位姿;
步骤1324,根据第一位姿,将Depth图上的3D特征点投影至first_depth+1帧上,生成投影2D特征点;计算投影2D特征点与first_depth+1帧上的2D特征点之间的重投影误差;
步骤1326,若重投影误差小于预设误差阈值,则将Depth图上的3D特征点作为目标地图点;根据目标地图点构建当前环境的初始三维地图。
步骤1328,计算位于first_depth帧之前的RGB图像与first_depth帧之间的第二位姿变换矩阵;
步骤1330,根据第二位姿变换矩阵,将Depth图上的3D特征点分别投影至位于first_depth帧之前的RGB图像上,生成投影2D特征点;计算投影2D特征点与位于first_depth帧之前的RGB图像上的2D特征点之间的重投影误差;
步骤1332,若重投影误差小于预设误差阈值,则将Depth图上的3D特征点作为新的目标地图点;将新的目标地图点添加至初始三维地图,生成目标三维地图;
步骤1334,根据目标三维地图、当前帧及位于当前帧之前的RGB图像,生成目标三维地图与当前帧之间相互匹配的3D-2D特征点对;
步骤1336,基于3D-2D特征点对,采用传统的PnP算法计算当前帧的位姿;并进入步骤1354,输出位姿;
步骤1338,判断当前帧是否存在对应的Depth图;若存在,则进入步骤1340,若不存在,则进入步骤1344;
步骤1340,根据当前帧对应的深度图像、当前帧与预设滑窗内当前帧之前的RGB图像之间的位姿变换矩阵,对目标三维地图进行更新生成中间三维地图;
步骤1342,采用三角化法对中间三维地图进行更新,生成更新后的三维地图;
步骤1344,采用三角化法对目标三维地图进行更新,生成更新后的三维地图;
步骤1346,判断frame_count是否等于windowsize(10帧);若是,则进入步骤1348;
步骤1348,对所计算出的10帧图像对应的位姿进行BA优化;
步骤1350,基于进行BA优化后的10帧图像对应的位姿,进行IMU初始化;
步骤1352,根据初始位姿、目标三维地图及IMU的初始化信息,计算预设滑窗之后所采集的RGB图像的位姿。
步骤1354,采用VINS-MONO算法计算位姿。
本申请实施例中,不需要在预设初始化滑窗内采集满RGB图像,电子设备才能够输出采集预设初始化滑窗外的第一帧RGB图像时的位姿。而是在预设初始化滑窗内首次采集到当前环境的深度图像,且深度图像对应的目标RGB图像并非预设初始化滑窗内的最后一帧图像时,就可以将采集深度图像时电子设备的位姿确定为初始位姿、目标RGB图像、深度图像及目标RGB图像的下一帧RGB图像,确定电子设备采集下一帧RGB图像时的位姿。将首次输出位姿的时间,从预设初始化滑窗外大大提前到滑窗内且提前到首次接收到深度图像的下一帧。从而,在预设初始化滑窗内就可以实时输出位姿,降低了实时输出位姿的等待时间。
若在预设初始化滑窗内未采集到当前环境的深度图像或深度图像对应的目标RGB图像为预设初始化滑窗内的最后一帧图像,则采用传统的VINS-MONO算法,根据预设初始化滑窗内所采集的RGB图像,计算预设初始化滑窗之后电子设备采集RGB图像时的位姿。保证了在预设初始化滑窗内未采集到当前环境的深度图像的情况下,仍然能够在滑窗内需累计满10帧RGB图像之后,实时输出电子设备采集RGB图像时的位姿。
同时,不需要在针对每帧RGB图像均对应采集到深度图像之后,才能够实时输出位姿。而是可以采用比采集RGB图像的频率较低的频率,来采集深度图,或可以采用可变频率来采集深度图,且只要在滑窗内采集到深度图,就可以开始初始化,并在初始化之后实时输出位姿。由于可以采用较低频率或可变频率采集深度图,就可以基于低频率的深度图去进行初始化及实时输出位姿,因此,避免了采集大量的数据,也避免了处理大量的数据,进而,降低了电子设备的功耗。
在一个实施例中,如图14所示,提供了一种位姿计算装置1400,装置包括:
初始位姿确定模块1420,用于若在预设初始化滑窗内首次采集到当前环境的深度图像,则将采集深度图像时电子设备的位姿确定为初始位姿;其中,深度图像对应的目标RGB图像并非预设初始化滑窗内的最后一帧图像;
下一帧RGB图像位姿确定模块1440,用于根据初始位姿、目标RGB图像、深度图像及目标RGB图像的下一帧RGB图像,确定电子设备采集下一帧RGB图像时的位姿。
在一个实施例中,如图15所示,提供了一种位姿计算装置1400,装置还包括:
目标三维地图构建模块1460,用于根据电子设备采集下一帧RGB图像时的位姿以及深度图像,构建当前环境的目标三维地图;
其他RGB图像位姿确定模块1480,用于根据目标三维地图,计算电子设备在预设初始化滑窗内采集位于下一帧RGB图像之后的其他RGB图像时的位姿。
在一个实施例中,目标三维地图构建模块1460,还包括:
初始三维地图构建单元,用于根据第一位姿变换矩阵及深度图像,构建当前环境的初始三维地图;
目标三维地图构建单元,用于计算位于目标RGB图像之前的RGB图像与目标RGB图像之间的第二位姿变换矩阵,并根据第二位姿变换矩阵及深度图像对当前环境的初始三维地图进行更新,构建当前环境的目标三维地图。
在一个实施例中,提供了一种位姿计算装置1400,装置还包括:
位姿变换矩阵计算单元,还用于采用预设透视投影PnP算法,计算第一位姿变换矩阵或第二位姿变换矩阵;
其中,预设透视投影PnP算法用于分步计算目标RGB图像的相关RGB图像与目标RGB图像之间的旋转变换矩阵和平移变换矩阵,目标RGB图像的相关RGB图像为位于目标RGB图像之前的RGB图像或下一帧RGB图像。
在一个实施例中,位姿变换矩阵计算单元,还用于,还包括:
旋转变换矩阵计算子单元,用于计算目标RGB图像的相关RGB图像与目标RGB图像之间的旋转变换矩阵;
平移变换矩阵计算子单元,用于根据深度图像、旋转变换矩阵,计算目标RGB图像的相关RGB图像与目标RGB图像之间的平移变换矩阵;
位姿变换矩阵计算子单元,用于基于旋转变换矩阵及平移变换矩阵,生成目标RGB图像的相关RGB图像与目标RGB图像之间的第一位姿变换矩阵或第二位姿变换矩阵。
在一个实施例中,平移变换矩阵计算子单元,还用于根据旋转变换矩阵对目标RGB图像的相关RGB图像与目标RGB图像上相互匹配的2D-2D特征点对进行剔除,得到剔除后的2D-2D特征点对;根据深度图像将剔除后的2D-2D特征点对,转换为3D-2D特征点对;根据3D-2D特征点对,计算目标RGB图像的相关RGB图像与目标RGB图像之间的平移变换矩阵。
在一个实施例中,初始三维地图构建单元,还用于根据第一位姿变换矩阵,将深度图像上的3D特征点投影至下一帧RGB图像上,生成投影2D特征点;计算投影2D特征点与下一帧RGB图像上的2D特征点之间的重投影误差;若重投影误差小于预设误差阈值,则将深度图像上的3D特征点作为目标地图点;根据目标地图点构建当前环境的初始三维地图。
在一个实施例中,目标三维地图构建单元,还用于根据第二位姿变换矩阵,将深度图像上的3D特征点分别投影至位于目标RGB图像之前的RGB图像上,生成投影2D特征点;计算投影2D特征点与位于目标RGB图像之前的RGB图像上的2D特征点之间的重投影误差;若重投影误差小于预设误差阈值,则将深度图像上的3D特征点作为新的目标地图点;将新的目标地图点添加至初始三维地图,构建当前环境的目标三维地图。
在一个实施例中,其他RGB图像位姿确定模块1480,包括:
当前帧定义单元,用于将下一帧RGB图像的下一帧作为当前帧,执行以下目标操作:
目标操作单元,用于根据目标三维地图、当前帧及位于当前帧之前的RGB图像,生成目标三维地图与当前帧之间相互匹配的3D-2D特征点对;基于3D-2D特征点对计算当前帧的位姿;根据当前帧对目标三维地图进行更新,并将更新后的三维地图作为新的目标三维地图;
循环单元,用于将当前帧的下一帧作为新的当前帧,循环执行目标操作直到计算出预设初始化滑窗内最后一帧RGB图像的位姿为止。
在一个实施例中,目标操作单元,还用于从当前帧及位于当前帧之前的RGB图像中,获取相互匹配的2D特征点对;从目标三维地图中的地图点中获取与2D特征点对匹配的3D特征点,根据匹配的3D特征点及2D特征点对,生成目标三维地图与当前帧之间相互匹配的3D-2D特征点对。
在一个实施例中,若当前帧存在对应的深度图像,则目标操作单元,还用于根据当前帧对应的深度图像、当前帧与预设滑窗内当前帧之前的RGB图像之间的位姿变换矩阵,对目标三维地图进行更新生成中间三维地图;采用三角化法对中间三维地图进行更新,生成更新后的三维地图。
在一个实施例中,若当前帧不存在对应的深度图像,则目标操作单元,还用于采用三角化法对目标三维地图进行更新,生成更新后的三维地图。
在一个实施例中,如图16所示,提供了一种位姿计算装置1600,装置还包括:
IMU数据获取模块1620,用于获取在预设滑窗内所采集的IMU数据;
IMU初始化模块1640,用于根据预设滑窗内各帧RGB图像的位姿、IMU数据,计算IMU的初始化信息;初始化信息包括初始速度、IMU的零偏及IMU的重力向量;
第一位姿计算模块1660,用于根据初始位姿、目标三维地图及IMU的初始化信息,计算预设滑窗之后所采集的RGB图像的位姿。
在一个实施例中,提供了一种位姿计算装置,该装置还包括:
第二位姿计算模块,用于若在预设初始化滑窗内未采集到当前环境的深度图像,则根据预设初始化滑窗内所采集的RGB图像,计算初始位姿及初始三维地图;
根据初始位姿及初始三维地图,计算预设初始化滑窗之后电子设备采集RGB图像时的位姿。
在一个实施例中,提供了一种位姿计算装置,该装置还包括:
第三位姿计算模块,用于若深度图像对应的目标RGB图像为预设初始化滑窗内的最后一帧图像,则根据预设初始化滑窗内所采集的RGB图像,计算初始位姿及初始三维地图;
根据初始位姿及初始三维地图,计算预设滑窗之后电子设备采集RGB图像时的位姿。
应该理解的是,虽然图中的流程图中的各个步骤按照箭头的指示依次显示,但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明,这些步骤的执行并没有严格的顺序限制,这些步骤可以以其它的顺序执行。而且,图中的至少一部分步骤可以包括多个子步骤或者多个阶段,这些子步骤或者阶段并不必然是在同一时刻执行完成,而是可以在不同的时刻执行,这些子步骤或者阶段的执行顺序也不必然是依次进行,而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
位姿计算装置中各个模块的划分仅用于举例说明,在其他实施例中,可将位姿计算装置按照需要划分为不同的模块,以完成位姿计算装置的全部或部分功能。
关于位姿计算装置的具体限定可以参见上文中对于位姿计算方法的限定,在此不再赘述。位姿计算装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中,也可以以软件形式存储于计算机设备中的存储器中,以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中,还提供了一种电子设备,包括存储器及处理器,存储器中储存有计算机程序,计算机程序被处理器执行时,使得处理器执行以上各个实施例所提供的一种位姿计算方法的步骤。
图17为一个实施例中电子设备的内部结构示意图。如图17所示,该电子设备包括通过系统总线连接的处理器和存储器。其中,该处理器用于提供计算得到和控制能力,支撑整个电子设备的运行。存储器可包括非易失性存储介质及内存储器。非易失性存储介质存储有操作系统和计算机程序。该计算机程序可被处理器所执行,以用于实现以上各个实施例所提供的一种位姿计算方法。内存储器为非易失性存储介质中的操作系统计算机程序提供高速缓存的运行环境。该电子设备可以是手机、平板电脑、PDA(Personal DigitalAssistant,个人数字助理)、POS(Point of Sales,销售终端)、车载电脑、穿戴式设备等任意终端设备。
本申请实施例中提供的位姿计算装置中的各个模块的实现可为计算机程序的形式。该计算机程序可在电子设备或电子设备上运行。该计算机程序构成的程序模块可存储在电子设备或电子设备的存储器上。该计算机程序被处理器执行时,实现本申请实施例中所描述方法的步骤。
本申请实施例还提供了一种计算机可读存储介质。一个或多个包含计算机可执行指令的非易失性计算机可读存储介质,当计算机可执行指令被一个或多个处理器执行时,使得处理器执行位姿计算方法的步骤。
一种包含指令的计算机程序产品,当其在计算机上运行时,使得计算机执行位姿计算方法。
本申请实施例所使用的对存储器、存储、数据库或其它介质的任何引用可包括非易失性和/或易失性存储器。合适的非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM),它用作外部高速缓冲存储器。作为说明而非局限,RAM以多种形式可得,诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDR SDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)。
以上初始化实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对本申请专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。
Claims (19)
1.一种位姿计算方法,其特征在于,所述方法包括:
若在预设初始化滑窗内首次采集到当前环境的深度图像,则将采集所述深度图像时电子设备的位姿确定为初始位姿;其中,所述深度图像对应的目标RGB图像并非所述预设初始化滑窗内的最后一帧图像;
根据所述初始位姿、所述目标RGB图像、所述深度图像及所述目标RGB图像的下一帧RGB图像,确定所述电子设备采集所述下一帧RGB图像时的位姿。
2.根据权利要求1所述的位姿计算方法,其特征在于,所述方法还包括:
根据所述电子设备采集所述下一帧RGB图像时的位姿以及所述深度图像,构建所述当前环境的目标三维地图;
根据所述目标三维地图,计算所述电子设备在所述预设初始化滑窗内采集位于所述下一帧RGB图像之后的其他RGB图像时的位姿。
3.根据权利要求2所述的位姿计算方法,其特征在于,所述根据所述电子设备采集所述下一帧RGB图像时的位姿以及所述深度图像,构建所述当前环境的目标三维地图,包括:
计算所述下一帧RGB图像与所述目标RGB图像之间的第一位姿变换矩阵,根据所述第一位姿变换矩阵及所述深度图像,构建所述当前环境的目标三维地图。
4.根据权利要求3所述的位姿计算方法,其特征在于,所述根据所述第一位姿变换矩阵及所述深度图像,构建所述当前环境的目标三维地图,包括:
根据所述第一位姿变换矩阵及所述深度图像,构建所述当前环境的初始三维地图;
计算位于所述目标RGB图像之前的RGB图像与所述目标RGB图像之间的第二位姿变换矩阵,并根据所述第二位姿变换矩阵及所述深度图像对所述当前环境的初始三维地图进行更新,构建所述当前环境的目标三维地图。
5.根据权利要求4所述的位姿计算方法,其特征在于,所述方法还包括:
采用预设透视投影PnP算法,计算所述第一位姿变换矩阵或所述第二位姿变换矩阵;
其中,所述预设透视投影PnP算法用于分步计算所述目标RGB图像的相关RGB图像与所述目标RGB图像之间的旋转变换矩阵和平移变换矩阵,所述目标RGB图像的相关RGB图像为所述位于所述目标RGB图像之前的RGB图像或所述下一帧RGB图像。
6.根据权利要求5所述的位姿计算方法,其特征在于,所述采用预设透视投影PnP算法,计算所述第一位姿变换矩阵或所述第二位姿变换矩阵,包括:
计算所述目标RGB图像的相关RGB图像与所述目标RGB图像之间的旋转变换矩阵;
根据所述深度图像、所述旋转变换矩阵,计算所述目标RGB图像的相关RGB图像与所述目标RGB图像之间的平移变换矩阵;
基于所述旋转变换矩阵及所述平移变换矩阵,生成所述目标RGB图像的相关RGB图像与所述目标RGB图像之间的所述第一位姿变换矩阵或所述第二位姿变换矩阵。
7.根据权利要求6所述的位姿计算方法,其特征在于,所述根据所述深度图像、所述旋转变换矩阵,计算所述目标RGB图像的相关RGB图像与所述目标RGB图像之间的平移变换矩阵,包括:
根据所述旋转变换矩阵对所述目标RGB图像的相关RGB图像与所述目标RGB图像上相互匹配的2D-2D特征点对进行剔除,得到剔除后的2D-2D特征点对;
根据所述深度图像将所述剔除后的2D-2D特征点对,转换为3D-2D特征点对;
根据所述3D-2D特征点对,计算所述目标RGB图像的相关RGB图像与所述目标RGB图像之间的所述平移变换矩阵。
8.根据权利要求4所述的位姿计算方法,其特征在于,所述根据第一位姿变换矩阵及所述深度图像,构建所述当前环境的初始三维地图,包括:
根据所述第一位姿变换矩阵,将所述深度图像上的3D特征点投影至所述下一帧RGB图像上,生成投影2D特征点;
计算所述投影2D特征点与所述下一帧RGB图像上的2D特征点之间的重投影误差;
若所述重投影误差小于预设误差阈值,则将所述深度图像上的3D特征点作为目标地图点;
根据所述目标地图点构建所述当前环境的初始三维地图。
9.根据权利要求4-8中任一项所述的位姿计算方法,其特征在于,所述根据所述第二位姿变换矩阵及所述深度图像对所述当前环境的初始三维地图进行更新,构建所述当前环境的目标三维地图,包括:
根据所述第二位姿变换矩阵,将所述深度图像上的3D特征点分别投影至位于所述目标RGB图像之前的RGB图像上,生成投影2D特征点;
计算所述投影2D特征点与位于所述目标RGB图像之前的RGB图像上的2D特征点之间的重投影误差;
若所述重投影误差小于预设误差阈值,则将所述深度图像上的3D特征点作为新的目标地图点;
将所述新的目标地图点添加至所述初始三维地图,构建所述当前环境的目标三维地图。
10.根据权利要求2所述的位姿计算方法,其特征在于,所述根据所述目标三维地图,计算所述电子设备在所述预设初始化滑窗内采集所述下一帧RGB图像之后的其他RGB图像时的位姿,包括:
将所述下一帧RGB图像的下一帧作为当前帧,执行以下目标操作:
根据所述目标三维地图、所述当前帧及位于所述当前帧之前的RGB图像,生成所述目标三维地图与所述当前帧之间相互匹配的3D-2D特征点对;
基于所述3D-2D特征点对计算所述当前帧的位姿;
根据所述当前帧对所述目标三维地图进行更新,并将更新后的三维地图作为新的目标三维地图;
将所述当前帧的下一帧作为新的当前帧,循环执行所述目标操作直到计算出所述预设初始化滑窗内最后一帧RGB图像的位姿为止。
11.根据权利要求10所述的位姿计算方法,其特征在于,所述根据所述目标三维地图、所述当前帧及位于所述当前帧之前的RGB图像,生成所述目标三维地图与所述当前帧之间相互匹配的3D-2D特征点对,包括:
从所述当前帧及位于所述当前帧之前的RGB图像中,获取相互匹配的2D特征点对;
从所述目标三维地图中的地图点中获取与2D特征点对匹配的3D特征点,根据所述匹配的3D特征点及所述2D特征点对,生成所述目标三维地图与所述当前帧之间相互匹配的3D-2D特征点对。
12.根据权利要求10所述的位姿计算方法,其特征在于,若所述当前帧存在对应的深度图像,则所述根据所述当前帧对所述目标三维地图进行更新,生成更新后的三维地图,包括:
根据所述当前帧对应的深度图像、所述当前帧与预设滑窗内所述当前帧之前的RGB图像之间的位姿变换矩阵,对所述目标三维地图进行更新生成中间三维地图;
采用三角化法对所述中间三维地图进行更新,生成所述更新后的三维地图。
13.根据权利要求10所述的位姿计算方法,其特征在于,若所述当前帧不存在对应的深度图像,则所述根据所述当前帧对所述目标三维地图进行更新,生成更新后的三维地图,包括:
采用三角化法对所述目标三维地图进行更新,生成更新后的三维地图。
14.根据权利要求2所述的位姿计算方法,其特征在于,在所述根据所述目标三维地图,计算所述电子设备在所述预设初始化滑窗内采集位于所述下一帧RGB图像之后的其他RGB图像时的位姿之后,还包括:
获取在所述预设滑窗内所采集的IMU数据;
根据所述预设滑窗内各帧RGB图像的位姿、所述IMU数据,计算所述IMU的初始化信息;所述初始化信息包括初始速度、所述IMU的零偏及所述IMU的重力向量;
根据所述初始位姿、所述目标三维地图及所述IMU的初始化信息,计算所述预设滑窗之后所采集的RGB图像的位姿。
15.根据权利要求1所述的位姿计算方法,其特征在于,所述方法还包括:
若在所述预设初始化滑窗内未采集到当前环境的深度图像,则根据所述预设初始化滑窗内所采集的RGB图像,计算所述初始位姿及初始三维地图;
根据所述初始位姿及所述初始三维地图,计算所述预设初始化滑窗之后所述电子设备采集RGB图像时的位姿。
16.根据权利要求1所述的位姿计算方法,其特征在于,所述方法还包括:
若所述深度图像对应的目标RGB图像为所述预设初始化滑窗内的最后一帧图像,则根据所述预设初始化滑窗内所采集的所述RGB图像,计算所述初始位姿及初始三维地图;
根据所述初始位姿及所述初始三维地图,计算所述预设滑窗之后所述电子设备采集RGB图像时的位姿。
17.一种位姿计算装置,其特征在于,所述装置包括:
初始位姿确定模块,用于若在预设初始化滑窗内首次采集到当前环境的深度图像,则将采集所述深度图像时电子设备的位姿确定为初始位姿;其中,所述深度图像对应的目标RGB图像并非所述预设初始化滑窗内的最后一帧图像;
下一帧RGB图像位姿确定模块,用于根据所述初始位姿、所述目标RGB图像、所述深度图像及所述目标RGB图像的下一帧RGB图像,确定所述电子设备采集所述下一帧RGB图像时的位姿。
18.一种电子设备,包括存储器及处理器,所述存储器中储存有计算机程序,其特征在于,所述计算机程序被所述处理器执行时,使得所述处理器执行如权利要求1至16中任一项所述的位姿计算方法的步骤。
19.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现如权利要求1至16中任一项所述的位姿计算方法的步骤。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110866966.3A CN113610918A (zh) | 2021-07-29 | 2021-07-29 | 位姿计算方法和装置、电子设备、可读存储介质 |
PCT/CN2022/098295 WO2023005457A1 (zh) | 2021-07-29 | 2022-06-13 | 位姿计算方法和装置、电子设备、可读存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110866966.3A CN113610918A (zh) | 2021-07-29 | 2021-07-29 | 位姿计算方法和装置、电子设备、可读存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN113610918A true CN113610918A (zh) | 2021-11-05 |
Family
ID=78306042
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110866966.3A Pending CN113610918A (zh) | 2021-07-29 | 2021-07-29 | 位姿计算方法和装置、电子设备、可读存储介质 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN113610918A (zh) |
WO (1) | WO2023005457A1 (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2023005457A1 (zh) * | 2021-07-29 | 2023-02-02 | Oppo广东移动通信有限公司 | 位姿计算方法和装置、电子设备、可读存储介质 |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117237553A (zh) * | 2023-09-14 | 2023-12-15 | 广东省核工业地质局测绘院 | 一种基于点云图像融合的三维地图测绘系统 |
CN117419690B (zh) * | 2023-12-13 | 2024-03-12 | 陕西欧卡电子智能科技有限公司 | 一种无人船的位姿估计方法、装置及介质 |
Family Cites Families (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10552981B2 (en) * | 2017-01-16 | 2020-02-04 | Shapetrace Inc. | Depth camera 3D pose estimation using 3D CAD models |
CN107610175A (zh) * | 2017-08-04 | 2018-01-19 | 华南理工大学 | 基于半直接法和滑动窗口优化的单目视觉slam算法 |
CN111160298B (zh) * | 2019-12-31 | 2023-12-01 | 深圳市优必选科技股份有限公司 | 一种机器人及其位姿估计方法和装置 |
CN112164117A (zh) * | 2020-09-30 | 2021-01-01 | 武汉科技大学 | 一种基于Kinect相机的V-SLAM位姿估算方法 |
CN112435206B (zh) * | 2020-11-24 | 2023-11-21 | 北京交通大学 | 利用深度相机对物体进行三维信息重建的方法 |
CN112907620A (zh) * | 2021-01-25 | 2021-06-04 | 北京地平线机器人技术研发有限公司 | 相机位姿的估计方法、装置、可读存储介质及电子设备 |
CN112819860B (zh) * | 2021-02-18 | 2023-12-22 | Oppo广东移动通信有限公司 | 视觉惯性系统初始化方法及装置、介质和电子设备 |
CN113610918A (zh) * | 2021-07-29 | 2021-11-05 | Oppo广东移动通信有限公司 | 位姿计算方法和装置、电子设备、可读存储介质 |
-
2021
- 2021-07-29 CN CN202110866966.3A patent/CN113610918A/zh active Pending
-
2022
- 2022-06-13 WO PCT/CN2022/098295 patent/WO2023005457A1/zh unknown
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2023005457A1 (zh) * | 2021-07-29 | 2023-02-02 | Oppo广东移动通信有限公司 | 位姿计算方法和装置、电子设备、可读存储介质 |
Also Published As
Publication number | Publication date |
---|---|
WO2023005457A1 (zh) | 2023-02-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110246147B (zh) | 视觉惯性里程计方法、视觉惯性里程计装置及移动设备 | |
CN109727288B (zh) | 用于单目同时定位与地图构建的系统和方法 | |
CN110135455B (zh) | 影像匹配方法、装置及计算机可读存储介质 | |
CN107292949B (zh) | 场景的三维重建方法、装置及终端设备 | |
CN113610918A (zh) | 位姿计算方法和装置、电子设备、可读存储介质 | |
EP3252715A1 (en) | Two-camera relative position calculation system, device and apparatus | |
US10621446B2 (en) | Handling perspective magnification in optical flow processing | |
CN111160298A (zh) | 一种机器人及其位姿估计方法和装置 | |
CN109859314B (zh) | 三维重建方法、装置、电子设备和存储介质 | |
CN112150518B (zh) | 一种基于注意力机制的图像立体匹配方法及双目设备 | |
CN115035235A (zh) | 三维重建方法及装置 | |
CN115205383A (zh) | 相机位姿的确定方法、装置、电子设备及存储介质 | |
CN111882655A (zh) | 三维重建的方法、装置、系统、计算机设备和存储介质 | |
CN113112542A (zh) | 一种视觉定位方法、装置、电子设备及存储介质 | |
CN112270748B (zh) | 基于图像的三维重建方法及装置 | |
CN112258647B (zh) | 地图重建方法及装置、计算机可读介质和电子设备 | |
KR20150097251A (ko) | 다중 영상간 대응점을 이용한 카메라 정렬 방법 | |
CN111860559A (zh) | 图像处理方法、装置、电子设备和存储介质 | |
CN112288817B (zh) | 基于图像的三维重建处理方法及装置 | |
CN115205807A (zh) | 车道线融合方法、装置、汽车、电子设备及计算机可读存储介质 | |
CN114757984A (zh) | 光场相机的场景深度估计方法及装置 | |
CN110119649B (zh) | 电子设备状态跟踪方法、装置、电子设备及控制系统 | |
CN114494612A (zh) | 构建点云地图的方法、装置和设备 | |
CN113763468A (zh) | 一种定位方法、装置、系统及存储介质 | |
CN111405266A (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 |