CN116664893A - 一种初始化姿态确定方法、装置、计算机设备及存储介质 - Google Patents
一种初始化姿态确定方法、装置、计算机设备及存储介质 Download PDFInfo
- Publication number
- CN116664893A CN116664893A CN202310709099.1A CN202310709099A CN116664893A CN 116664893 A CN116664893 A CN 116664893A CN 202310709099 A CN202310709099 A CN 202310709099A CN 116664893 A CN116664893 A CN 116664893A
- Authority
- CN
- China
- Prior art keywords
- image
- matching
- matching point
- moment
- determining
- 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
- 238000000034 method Methods 0.000 title claims abstract description 61
- 239000013598 vector Substances 0.000 claims abstract description 107
- 230000008859 change Effects 0.000 claims abstract description 38
- 230000036544 posture Effects 0.000 claims description 52
- 238000010606 normalization Methods 0.000 claims description 30
- 239000011159 matrix material Substances 0.000 claims description 14
- 206010034719 Personality change Diseases 0.000 claims description 13
- 230000000007 visual effect Effects 0.000 claims description 10
- 238000010276 construction Methods 0.000 claims description 9
- 238000004590 computer program Methods 0.000 claims description 8
- 238000004891 communication Methods 0.000 claims description 5
- 238000013507 mapping Methods 0.000 claims description 3
- 230000004807 localization Effects 0.000 claims description 2
- 230000003287 optical effect Effects 0.000 description 10
- 230000008569 process Effects 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 5
- 230000006870 function Effects 0.000 description 5
- 238000013475 authorization Methods 0.000 description 3
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000011084 recovery Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000001413 cellular effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
Classifications
-
- 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/74—Image or video pattern matching; Proximity measures in feature spaces
- G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
- G06V10/751—Comparing pixel values or logical combinations thereof, or feature values having positional relevance, e.g. template matching
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Evolutionary Computation (AREA)
- Computing Systems (AREA)
- Databases & Information Systems (AREA)
- Artificial Intelligence (AREA)
- General Health & Medical Sciences (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Health & Medical Sciences (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Multimedia (AREA)
- User Interface Of Digital Computer (AREA)
Abstract
本公开提供了一种初始化姿态确定方法、装置、计算机设备及存储介质,包括:获取自动驾驶设备在初始化阶段的目标时刻采集的第一图像,以及在所述目标时刻的前一时刻采集的第二图像;分别提取所述第一图像和所述第二图像的特征点,并将提取的特征点进行匹配,确定所述第一图像和所述第二图像对应同一物体的匹配点;基于所述匹配点在图像上的第一位置信息,构建各匹配点分别对应的方向向量;基于各匹配点分别对应的方向向量的叉乘结果,确定所述目标时刻相较于所述前一时刻的第一姿态变化量,并基于所述前一时刻的初始姿态和所述第一姿态变化量,确定所述自动驾驶设备在所述目标时刻的目标姿态。
Description
技术领域
本公开涉及自动驾驶技术领域,具体而言,涉及一种初始化姿态确定方法、装置、计算机设备及存储介质。
背景技术
随着无人驾驶技术的发展,实时定位与地图构建(Simultaneous Localizationand Mapping,slam)技术和视觉惯性里程计(Visual-Inertial Odometry,vio)的应用也越来越广泛。
在视觉slam或者vio在初始化时,一般需要先进行从运动恢复结构(structurefrom motion,SFM),恢复空间三维点和相机位姿,然后视觉SFM在没有姿态先验的情况下,误差较大,因此如何获得准确的初始化位姿成为亟待解决的技术问题。
发明内容
本公开实施例至少提供一种初始化姿态确定方法、装置、计算机设备及存储介质。
第一方面,本公开实施例提供了一种初始化姿态确定方法,包括:
获取自动驾驶设备在初始化阶段的目标时刻采集的第一图像,以及在所述目标时刻的前一时刻采集的第二图像;
分别提取所述第一图像和所述第二图像的特征点,并将提取的特征点进行匹配,确定所述第一图像和所述第二图像对应同一物体的匹配点;
基于所述匹配点在图像上的第一位置信息,构建各匹配点分别对应的方向向量;
基于各匹配点分别对应的方向向量的叉乘结果,确定所述目标时刻相较于所述前一时刻的第一姿态变化量,并基于所述前一时刻的初始姿态和所述第一姿态变化量,确定所述自动驾驶设备在所述目标时刻的目标姿态。
一种可能的实施方式中,所述基于所述匹配点在图像上的第一位置信息,构建各匹配点分别对应的方向向量,包括:
基于所述匹配点在图像上的第一位置信息,将所述匹配点投影至归一化平面;
基于所述匹配点在所述归一化平面的第二位置信息,构建各匹配点分别对应的方向向量。
一种可能的实施方式中,所述归一化平面为距离相机为预设距离的垂直平面;所述匹配点包括位于第一图像的第一匹配点,和与第一匹配点对应的位于所述第二图像的第二匹配点;
所述基于所述匹配点在图像上的第一位置信息,将所述匹配点投影至归一化平面,包括:
基于所述相机的内参、畸变参数和所述第一匹配点的第一位置信息,将所述第一匹配点投影至第一归一化平面;以及,基于所述相机的内参、畸变参数和所述第二匹配点的第一位置信息,将所述第二匹配点投影至第二归一化平面。
一种可能的实施方式中,所述匹配点包括位于第一图像的第一匹配点,和与第一匹配点对应的位于所述第二图像的第二匹配点;
所述基于各匹配点分别对应的方向向量的叉乘结果,确定所述目标时刻相较于所述前一时刻的第一姿态变化量,包括:
针对任一第一匹配点,将该第一匹配点对应的方向向量与该第一匹配点对应的第二匹配点对应的方向向量进行叉乘运算,得到与该第一匹配点对应的叉乘结果向量;
基于各第一匹配点对应的叉乘结果向量,构建叉乘向量矩阵;
基于所述叉乘向量矩阵,确定所述目标时刻相比较于所述前一时刻的第一姿态变化量。
一种可能的实施方式中,所述初始化阶段为实时定位与地图构建的初始化阶段。
一种可能的实施方式中,在所述初始化阶段为视觉惯性里程计的初始化阶段的情况下,所述方法还包括:
针对任一时刻,基于该时刻采集的角速度信息,确定该时刻相较于前一时刻的第二姿态变化量;
在确定N个时刻的姿态之后,基于所述N个时刻的第一姿态变化量和第二姿态变化量,确定陀螺零偏,其中,N为预设值。
第二方面,本公开实施例还提供一种车载装置,所述车载装置用于执行第一方面,或第一方面任一种可能的实施方式所述的初始化姿态确定方法。
第三方面,本公开实施例还提供一种初始化姿态确定装置,包括:
获取模块,用于获取自动驾驶设备在初始化阶段的目标时刻采集的第一图像,以及在所述目标时刻的前一时刻采集的第二图像;
匹配模块,用于分别提取所述第一图像和所述第二图像的特征点,并将提取的特征点进行匹配,确定所述第一图像和所述第二图像对应同一物体的匹配点;
构建模块,用于基于所述匹配点在图像上的第一位置信息,构建各匹配点分别对应的方向向量;
确定模块,用于基于各匹配点分别对应的方向向量的叉乘结果,确定所述目标时刻相较于所述前一时刻的第一姿态变化量,并基于所述前一时刻的初始姿态和所述第一姿态变化量,确定所述自动驾驶设备在所述目标时刻的目标姿态。
第四方面,本公开实施例还提供一种计算机设备,包括:处理器、存储器和总线,所述存储器存储有所述处理器可执行的机器可读指令,当计算机设备运行时,所述处理器与所述存储器之间通过总线通信,所述机器可读指令被所述处理器执行时执行上述第一方面,或第一方面中任一种可能的实施方式中的步骤。
第五方面,本公开实施例还提供一种计算机可读存储介质,该计算机可读存储介质上存储有计算机程序,该计算机程序被处理器运行时执行上述第一方面,或第一方面中任一种可能的实施方式中的步骤。
本公开提供的初始化姿态确定方法,可以在确定目标时刻采集的第一图像与前一时刻采集的第二图像上对应同一物体的匹配点之后,基于对极约束将各匹配分别对应的方向向量进行叉乘,并基于叉乘结果确定第一姿态变化量,然后结合前一时刻的初始姿态和第一姿态变化量,确定所述自动驾驶设备在所述目标时刻的目标姿态。通过这种方式,无需进行SFM即可确定出初始化阶段各个时刻的姿态,提升了初始化阶段姿态的精度。
为使本公开的上述目的、特征和优点能更明显易懂,下文特举较佳实施例,并配合所附附图,作详细说明如下。
附图说明
为了更清楚地说明本公开实施例的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,此处的附图被并入说明书中并构成本说明书中的一部分,这些附图示出了符合本公开的实施例,并与说明书一起用于说明本公开的技术方案。应当理解,以下附图仅示出了本公开的某些实施例,因此不应被看作是对范围的限定,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他相关的附图。
图1示出了本公开实施例所提供的一种初始化姿态确定方法的流程图;
图2示出了本公开实施例所提供的初始化姿态确定方法中,归一化平面投影方法的示意图;
图3示出了本公开实施例所提供的一种初始化姿态确定装置的架构示意图;
图4示出了本公开实施例所提供的一种计算机设备的结构示意图。
具体实施方式
为使本公开实施例的目的、技术方案和优点更加清楚,下面将结合本公开实施例中附图,对本公开实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本公开一部分实施例,而不是全部的实施例。通常在此处附图中描述和示出的本公开实施例的组件可以以各种不同的配置来布置和设计。因此,以下对在附图中提供的本公开的实施例的详细描述并非旨在限制要求保护的本公开的范围,而是仅仅表示本公开的选定实施例。基于本公开的实施例,本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例,都属于本公开保护的范围。
相关技术中,在视觉slam或者vio在初始化时,一般需要先进行从运动恢复结构(structure from motion,SFM),恢复空间三维点和相机位姿,然后视觉SFM在没有姿态先验的情况下,误差较大。
进一步的,在vio中,需要利用陀螺积分,提供各关键帧的姿态约束,优化得到最优姿态和陀螺零偏,然而在这个过程中,在恢复空间三维点后进行姿态优化所带来的算力开销比较大,姿态和陀螺零偏的估计精度也会比较低。
基于上述研究,本公开提供了一种初始化姿态确定方法、装置、计算机设备及存储介质,可以在确定目标时刻采集的第一图像与前一时刻采集的第二图像上对应同一物体的匹配点之后,基于对极约束将各匹配分别对应的方向向量进行叉乘,并基于叉乘结果确定第一姿态变化量,然后结合前一时刻的初始姿态和第一姿态变化量,确定所述自动驾驶设备在所述目标时刻的目标姿态。通过这种方式,无需进行SFM即可确定出初始化阶段各个时刻的姿态,提升了初始化阶段姿态的精度。
应注意到:相似的标号和字母在下面的附图中表示类似项,因此,一旦某一项在一个附图中被定义,则在随后的附图中不需要对其进行进一步定义和解释。
本文中术语“和/或”,仅仅是描述一种关联关系,表示可以存在三种关系,例如,A和/或B,可以表示:单独存在A,同时存在A和B,单独存在B这三种情况。另外,本文中术语“至少一种”表示多种中的任意一种或多种中的至少两种的任意组合,例如,包括A、B、C中的至少一种,可以表示包括从A、B和C构成的集合中选择的任意一个或多个元素。
可以理解的是,在使用本公开各实施例公开的技术方案之前,本公开中所涉及的个人信息(包括但不限于属性信息、人脸图像等等)是经用户授权获取的,具体可以是通过页面中弹窗、信息推送等方式,向用户发送请求授权的提示信息,经用户同意授权后,再获取上述个人信息。
为便于对本实施例进行理解,首先对本公开实施例所公开的一种初始化姿态确定方法进行详细介绍,本公开实施例所提供的初始化姿态确定方法的执行主体一般为具有一定计算能力的计算机设备,该计算机设备例如包括:终端设备或服务器或其它处理设备,终端设备可以为用户设备(User Equipment,UE)、移动设备、用户终端、终端、蜂窝电话、无绳电话、个人数字助理(Personal Digital Assistant,PDA)、手持设备、计算设备、车载设备、可穿戴设备等。在一些可能的实现方式中,该初始化姿态确定方法可以通过处理器调用存储器中存储的计算机可读指令的方式来实现。
下面以执行主体为终端设备为例对本公开实施例提供的初始化姿态确定方法加以说明。
参见图1所示,为本公开实施例提供的一种初始化姿态确定方法的流程图,所述方法包括步骤101~步骤104,其中:
步骤101、获取自动驾驶设备在初始化阶段的目标时刻采集的第一图像,以及在所述目标时刻的前一时刻采集的第二图像。
步骤102、分别提取所述第一图像和所述第二图像的特征点,并将提取的特征点进行匹配,确定所述第一图像和所述第二图像对应同一物体的匹配点。
步骤103、基于所述匹配点在图像上的第一位置信息,构建各匹配点分别对应的方向向量。
步骤104、基于各匹配点分别对应的方向向量的叉乘结果,确定所述目标时刻相较于所述前一时刻的第一姿态变化量,并基于所述前一时刻的初始姿态和所述第一姿态变化量,确定所述自动驾驶设备在所述目标时刻的目标姿态。
以下是对上述步骤的详细说明。需要说明的是,本公开所提供的方法仅用于初始化阶段的姿态确定,在完成初始化之后,后续再进行位姿确定时可以仍旧采用本公开所提供的方法,也可以采用除本公开所提供的方法外的其他方法本公开并不限定。
针对步骤101、
实际应用场景中,所述自动驾驶设备可以在待建图区域内不断移动,在移动过程中可以采集所述待建图区域的图像,不同时刻采集图像可以是所述自动驾驶设备在不同位姿下采集的图像。
一种可能的实施方式中,所述自动驾驶设备在移动过程中采集的可以是所述待建图区域的视频,然后可以按照预设时间间隔对所述视频进行抽帧采样,得到不同时刻采集的图像。
这里,所述第一图像和所述第二图像可以是在采集时刻上相邻的图像,并且由于后续需要对所述第一图像和所述第二图像进行特征点匹配,因此,所述第一图像和所述第二图像可以是包含部分相同内容的图像。
针对步骤102、
所述提取所述第一图像和所述第二图像的特征点,可以是指确定所述第一图像和所述第二图像中具有明显标识作用的像素点;示例性的,可以分别提取所述第一图像和所述第二图像中的fast角点。
具体的,可以在所述第一图像中提取第一特征点,在所述第二图像中提取第二特征点,在确定所述第一图像和所述第二图像对应同一物体的匹配点时,可以是计算所述第一特征点的描述子与所述第二特征点的描述子之间的特征距离。
示例性的,所述第一特征点和所述第二特征点的描述子可以为brief描述子,所述计算所述第一特征点的描述子与所述第二特征点的描述子之间的特征距离可以是指计算所述第一特征点的描述子与所述第二特征点的描述子之间的汉明距离。
可选地,可以将对应的特征距离满足预设条件的第一特征点和第二特征点作为匹配点。需要说明的是,所述匹配点是成对存在的,所述匹配点包括位于第一图像的第一匹配点和,与所述第一匹配点对应的位于所述第二图像的第二匹配点。
例如,在时刻A拍摄的图像中包括物体1,在下一时刻B拍摄的图像中也包括物体1,两个拍摄时刻是在不同角度拍摄了物体1,则在时刻A拍摄的图像中物体1对应的第一特征点与,时刻B拍摄的图像中物体1对应的第二特征点构成一组匹配点。
针对步骤103、
所述匹配点对应的方向向量可以理解为所述匹配点对应的成像位置与相机光心构成的向量,其用于表述在拍摄图像时,所述匹配点对应的实体物体与所述相机之间的相对位置关系。
所述匹配点在图像上的第一位置信息可以是指,所述匹配点在所述图像对应的图像坐标系下的二维坐标,所述图像坐标系例如可以是以图像的左下角为坐标原点建立的二维坐标系。
一种可能的实施方式中,在基于所述匹配点在图像上的第一位置信息,构建各匹配点分别对应的方向向量时,可以先基于所述匹配点在图像上的第一位置信息,将所述匹配点投影至归一化平面;然后再基于所述匹配点在所述归一化平面的第二位置信息,构建各匹配点分别对应的方向向量。
具体的,所述归一化平面可以为距离相机(或者距离相机光心)为预设距离的垂直平面。示例性的,如图2所示,点O1、O2可以为相机光心,平面I1和I2可以为在拍摄图像时的归一化平面。
需要说明的是,所述归一化平面可以是三维空间中的平面,所述归一化平面的位置可以与相机光心的位置相关,不同图像采集时刻对应的归一化平面可以是不同的平面。
例如,在将所述匹配点投影至归一化平面时,可以先基于所述相机的内参、畸变参数以及位于第一图像的第一匹配点的第一位置信息,将第一匹配点投影至第一归一化平面;以及,基于所述相机的内参、畸变参数和所述第二平面的第二匹配点的第一位置信息,将所述第二匹配点投影至第二归一化平面。
其中,所述相机的内参和畸变参数可以是预先标定好的。
如图2所示,点P1、P2可以为投影至归一化平面之后的点,由图2可知,由于两个时刻在采集图像时相机光心在不同的位置,因此两个点投影至不同的归一化平面。
所述匹配点在所述归一化平面的第二位置信息可以是指,所述匹配点在所述归一化平面的三维坐标,由于所述归一化平面为三维空间上的平面,因此所述第二位置信息可以是指三维坐标。
在基于所述匹配点在所述归一化平面的第二位置信息,构建各匹配点分别对应的方向向量时,例如可以以所述相机光心位置为向量起点,以所述匹配点在所述归一化平面的第二位置信息为向量终点,构建所述匹配点对应的方向向量。
如图2所示,对应同一物体的多个特征点的方向向量相交的位置为所述物体的真实位置,因此,通过所述方向向量能够反映出在拍摄图像时,所述物体相对所述相机光心的相对方向。
针对步骤104、
在向量叉乘运算中,若向量X=(a1,b1,c1),向量Y=(a2,b2,c2),则向量Z=X×Y=(b1c2-b2c1,a2c1-a1c2,a1b2-a2b1),向量Z同时垂直于向量X和向量Y,即垂直于向量X和向量Y构成的极平面。
一种可能的实施方式中,在基于各匹配点分别对应的方向向量的叉乘结果,确定所述目标时刻相较于所述前一时刻的第一姿态变化量时,可以通过如下步骤:
步骤a1、针对任一第一匹配点,将该第一匹配点对应的方向向量与该第一匹配点对应的第二匹配点对应的方向向量进行叉乘运算,得到与该第一匹配点对应的叉乘结果向量。
步骤a2、基于各第一匹配点对应的叉乘结果向量,构建叉乘向量矩阵。
步骤a3、基于所述叉乘向量矩阵,确定所述目标时刻相比较于所述前一时刻的第一姿态变化量。
具体的,针对任一第一匹配点,在将该第一匹配点对应的方向向量与该第一匹配点对应的第二匹配点对应的方向向量进行叉乘运算时,示例性的可以通过如下公式:
其中,表示第i时刻和第i+1时刻拍摄的图像上的第j个特征点的方向向量的叉乘结果,/>表示第i时刻拍摄的图像上的第j个特征点对应的方向向量,/>表示第i+1时刻拍摄的图像上第j个特征点对应的方向向量,δQi+1表示第i时刻和第i+1时刻之间的第二位姿增量。
这里在进行叉乘运算时,需要先将第i+1时刻拍摄的图像上第j个特征点对应的方向向量与位姿增量相乘的原因在于,是在第i时刻的坐标系下的向量,该时刻的坐标系可以是以第i时刻相机光心的位置为坐标原点建立的坐标系,/>是在第i+1时刻的坐标系下的向量,该时刻的坐标系可以是以第i+1时刻相机光心的位置为坐标原点建立的坐标系,两个坐标系的坐标原点并不统一,因此需要乘以δQi+1将两个向量转换到同一坐标系下。
需要说明的是,δQi+1可以为第二姿态变化量,所述第二姿态变化量可以通过陀螺积分的方式来确定,例如可以通过如下公式来确定:
其中,ri表示第i时刻的角速度信息,ti-ti-1表示第i时刻与第i-1时刻之间的时间差值,Q为四元数。
步骤a2中,在基于各第一匹配点对应的叉乘结果向量,构建叉乘向量矩阵时,示例性的可以通过如下公式:
其中,j表示特征点个数,由于方向向量是三维向量,因此叉乘结果向量也为三维向量,所以叉乘向量矩阵为3×j维的矩阵。表示第i时刻和第i+1时刻拍摄的图像上的第j个特征点的方向向量的叉乘结果。
所述第i+1时刻可以对应于本公开中的目标时刻,所述第i时刻可以对应于本公开中的目标时刻的前一时刻。
这里,由于叉乘得到的叉乘结果向量垂直方向向量,因此叉乘结果向量垂直基于两个方向向量构成极平面,也就垂直于极平面上的极线,即叉乘结果向量垂直极线。
任意不同的两个方向向量叉乘,求得的叉乘结果向量不同,但是叉乘结果向量都垂直与同一极线,因此,所有特征点对应的叉乘结果向量共面,因此,M3×j 必然有理论值为0的特征值,具体可以通过如下公式表示:
由于上述叉乘过程中加入了δQi+1,因此矩阵M为包含δQi+1的矩阵,通过求解上述公式,可得所述第一姿态变化量。例如可以通过如下公式进行计算:
示例性的,可以通过梯度下降法对上述函数进行求解,得到第i+1时刻的第一姿态变化量,也即目标时刻的第一姿态变化量。
在基于前一时刻的初始姿态和所述第一姿态变化量,确定所述目标时刻的目标姿态时,示例性的可以将所述前一时刻的初始姿态与所述第一姿态变化量的乘积作为所述目标时刻的目标姿态。
示例性的,可以通过如下公式进行计算:
其中,Qi为前一时刻的初始姿态,Qi+1为目标时刻的目标姿态。
其中,第0时刻的初始姿态可以为预设值,例如可以为0。第0时刻之后的任意时刻可以通过上述公式来进行目标姿态的计算。
一种可能的应用场景中,上述初始化阶段为slam的初始化阶段,在这个阶段中,可以仅确定各个初始化阶段的目标姿态。
在另外一种可能的应用场景中,上述初始化阶段还可以是视觉惯性里程计的初始化阶段,在这个阶段中,除了初始化目标姿态,还可以对陀螺零偏进行初始化。
具体的,针对任一时刻,可以基于该时刻采集的角速度信息,确定该时刻相较于前一时刻的第二姿态变化量;然后在确定N个时刻的姿态之后,可以基于所述N个时刻的第一姿态变化量和第二姿态变化量,确定陀螺零偏。
所述第二姿态变化量例如可以通过公式来计算。关于此公式的详细介绍参照上述实施例,在此不再赘述。
这里,在累计确定N个时刻的姿态之后再去确定陀螺零偏的目的在于,使陀螺零偏和旋转量之间有实际可观的数值区分。
示例性的,在基于N个时刻的第一姿态变化量和第二姿态变化量,确定陀螺零偏时,示例性的可以通过如下公式进行计算:
其中,ω表示陀螺零偏。
通过求解上述公式,求解得到陀螺零偏ω,在后续陀螺积分过程汇总可以更准确的利用陀螺零偏进行姿态变化量的推算。
本公开提供的上述初始化姿态确定方法,可以在确定目标时刻采集的第一图像与前一时刻采集的第二图像上对应同一物体的匹配点之后,基于对极约束将各匹配分别对应的方向向量进行叉乘,并基于叉乘结果确定第一姿态变化量,然后结合前一时刻的初始姿态和第一姿态变化量,确定所述自动驾驶设备在所述目标时刻的目标姿态。通过这种方式,无需进行SFM即可确定出初始化阶段各个时刻的姿态,提升了初始化阶段姿态的精度。
本领域技术人员可以理解,在具体实施方式的上述方法中,各步骤的撰写顺序并不意味着严格的执行顺序而对实施过程构成任何限定,各步骤的具体执行顺序应当以其功能和可能的内在逻辑确定。
本公开还提供一种车载装置,所述车载装置用于执行上述实施例所述的初始化姿态确定方法。
基于同一发明构思,本公开实施例中还提供了与初始化姿态确定方法对应的初始化姿态确定装置,由于本公开实施例中的装置解决问题的原理与本公开实施例上述初始化姿态确定方法相似,因此装置的实施可以参见方法的实施,重复之处不再赘述。
参照图3所示,为本公开实施例提供的一种初始化姿态确定装置的架构示意图,所述装置包括:获取模块301、匹配模块302、构建模块303以及确定模块304;其中,
获取模块301,用于获取自动驾驶设备在初始化阶段的目标时刻采集的第一图像,以及在所述目标时刻的前一时刻采集的第二图像;
匹配模块302,用于分别提取所述第一图像和所述第二图像的特征点,并将提取的特征点进行匹配,确定所述第一图像和所述第二图像对应同一物体的匹配点;
构建模块303,用于基于所述匹配点在图像上的第一位置信息,构建各匹配点分别对应的方向向量;
确定模块304,用于基于各匹配点分别对应的方向向量的叉乘结果,确定所述目标时刻相较于所述前一时刻的第一姿态变化量,并基于所述前一时刻的初始姿态和所述第一姿态变化量,确定所述自动驾驶设备在所述目标时刻的目标姿态。
一种可能的实施方式中,所述构建模块303,在基于所述匹配点在图像上的第一位置信息,构建各匹配点分别对应的方向向量时,用于:
基于所述匹配点在图像上的第一位置信息,将所述匹配点投影至归一化平面;
基于所述匹配点在所述归一化平面的第二位置信息,构建各匹配点分别对应的方向向量。
一种可能的实施方式中,所述归一化平面为距离相机为预设距离的垂直平面;所述匹配点包括位于第一图像的第一匹配点,和与第一匹配点对应的位于所述第二图像的第二匹配点;
所述构建模块303,在基于所述匹配点在图像上的第一位置信息,将所述匹配点投影至归一化平面时,用于:
基于所述相机的内参、畸变参数和所述第一匹配点的第一位置信息,将所述第一匹配点投影至第一归一化平面;以及,基于所述相机的内参、畸变参数和所述第二匹配点的第一位置信息,将所述第二匹配点投影至第二归一化平面。
一种可能的实施方式中,所述匹配点包括位于第一图像的第一匹配点,和与第一匹配点对应的位于所述第二图像的第二匹配点;
所述确定模块304,在基于各匹配点分别对应的方向向量的叉乘结果,确定所述目标时刻相较于所述前一时刻的第一姿态变化量时,用于:
针对任一第一匹配点,将该第一匹配点对应的方向向量与该第一匹配点对应的第二匹配点对应的方向向量进行叉乘运算,得到与该第一匹配点对应的叉乘结果向量;
基于各第一匹配点对应的叉乘结果向量,构建叉乘向量矩阵;
基于所述叉乘向量矩阵,确定所述目标时刻相比较于所述前一时刻的第一姿态变化量。
一种可能的实施方式中,所述初始化阶段为实时定位与地图构建的初始化阶段。
一种可能的实施方式中,在所述初始化阶段为视觉惯性里程计的初始化阶段的情况下,所述确定模块304,还用于:
针对任一时刻,基于该时刻采集的角速度信息,确定该时刻相较于前一时刻的第二姿态变化量;
在确定N个时刻的姿态之后,基于所述N个时刻的第一姿态变化量和第二姿态变化量,确定陀螺零偏,其中,N为预设值。
关于装置中的各模块的处理流程、以及各模块之间的交互流程的描述可以参照上述方法实施例中的相关说明,这里不再详述。
基于同一技术构思,本公开实施例还提供了一种计算机设备。参照图4所示,为本公开实施例提供的计算机设备400的结构示意图,包括处理器401、存储器402、和总线403。其中,存储器402用于存储执行指令,包括内存4021和外部存储器4022;这里的内存4021也称内存储器,用于暂时存放处理器401中的运算数据,以及与硬盘等外部存储器4022交换的数据,处理器401通过内存4021与外部存储器4022进行数据交换,当计算机设备400运行时,处理器401与存储器402之间通过总线403通信,使得处理器401在执行以下指令:
获取自动驾驶设备在初始化阶段的目标时刻采集的第一图像,以及在所述目标时刻的前一时刻采集的第二图像;
分别提取所述第一图像和所述第二图像的特征点,并将提取的特征点进行匹配,确定所述第一图像和所述第二图像对应同一物体的匹配点;
基于所述匹配点在图像上的第一位置信息,构建各匹配点分别对应的方向向量;
基于各匹配点分别对应的方向向量的叉乘结果,确定所述目标时刻相较于所述前一时刻的第一姿态变化量,并基于所述前一时刻的初始姿态和所述第一姿态变化量,确定所述自动驾驶设备在所述目标时刻的目标姿态。
一种可能的实施方式中,处理器401执行的指令中,所述基于所述匹配点在图像上的第一位置信息,构建各匹配点分别对应的方向向量,包括:
基于所述匹配点在图像上的第一位置信息,将所述匹配点投影至归一化平面;
基于所述匹配点在所述归一化平面的第二位置信息,构建各匹配点分别对应的方向向量。
一种可能的实施方式中,处理器401执行的指令中,所述归一化平面为距离相机为预设距离的垂直平面;所述匹配点包括位于第一图像的第一匹配点,和与第一匹配点对应的位于所述第二图像的第二匹配点;
所述基于所述匹配点在图像上的第一位置信息,将所述匹配点投影至归一化平面,包括:
基于所述相机的内参、畸变参数和所述第一匹配点的第一位置信息,将所述第一匹配点投影至第一归一化平面;以及,基于所述相机的内参、畸变参数和所述第二匹配点的第一位置信息,将所述第二匹配点投影至第二归一化平面。
一种可能的实施方式中,处理器401执行的指令中,所述匹配点包括位于第一图像的第一匹配点,和与第一匹配点对应的位于所述第二图像的第二匹配点;
所述基于各匹配点分别对应的方向向量的叉乘结果,确定所述目标时刻相较于所述前一时刻的第一姿态变化量,包括:
针对任一第一匹配点,将该第一匹配点对应的方向向量与该第一匹配点对应的第二匹配点对应的方向向量进行叉乘运算,得到与该第一匹配点对应的叉乘结果向量;
基于各第一匹配点对应的叉乘结果向量,构建叉乘向量矩阵;
基于所述叉乘向量矩阵,确定所述目标时刻相比较于所述前一时刻的第一姿态变化量。
一种可能的实施方式中,处理器401执行的指令中,所述初始化阶段为实时定位与地图构建的初始化阶段。
一种可能的实施方式中,处理器401执行的指令中,在所述初始化阶段为视觉惯性里程计的初始化阶段的情况下,所述方法还包括:
针对任一时刻,基于该时刻采集的角速度信息,确定该时刻相较于前一时刻的第二姿态变化量;
在确定N个时刻的姿态之后,基于所述N个时刻的第一姿态变化量和第二姿态变化量,确定陀螺零偏,其中,N为预设值。
本公开实施例还提供一种计算机可读存储介质,该计算机可读存储介质上存储有计算机程序,该计算机程序被处理器运行时执行上述方法实施例中所述的初始化姿态确定方法的步骤。其中,该存储介质可以是易失性或非易失的计算机可读取存储介质。
本公开实施例还提供一种计算机程序产品,该计算机产品承载有程序代码,所述程序代码包括的指令可用于执行上述方法实施例中所述的初始化姿态确定方法的步骤,具体可参见上述方法实施例,在此不再赘述。
其中,上述计算机程序产品可以具体通过硬件、软件或其结合的方式实现。在一个可选实施例中,所述计算机程序产品具体体现为计算机存储介质,在另一个可选实施例中,计算机程序产品具体体现为软件产品,例如软件开发包(Software Development Kit,SDK)等等。
所属领域的技术人员可以清楚地了解到,为描述的方便和简洁,上述描述的系统和装置的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘述。在本公开所提供的几个实施例中,应该理解到,所揭露的系统、装置和方法,可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的,例如,所述单元的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,又例如,多个单元或组件可以结合或者可以集成到另一个系统,或一些特征可以忽略,或不执行。另一点,所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些通信接口,装置或单元的间接耦合或通信连接,可以是电性,机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外,在本公开各个实施例中的各功能单元可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个单元中。
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个处理器可执行的非易失的计算机可读取存储介质中。基于这样的理解,本公开的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)执行本公开各个实施例所述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(Read-OnlyMemory,ROM)、随机存取存储器(Random Access Memory,RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
最后应说明的是:以上所述实施例,仅为本公开的具体实施方式,用以说明本公开的技术方案,而非对其限制,本公开的保护范围并不局限于此,尽管参照前述实施例对本公开进行了详细的说明,本领域的普通技术人员应当理解:任何熟悉本技术领域的技术人员在本公开揭露的技术范围内,其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化,或者对其中部分技术特征进行等同替换;而这些修改、变化或者替换,并不使相应技术方案的本质脱离本公开实施例技术方案的精神和范围,都应涵盖在本公开的保护范围之内。因此,本公开的保护范围应所述以权利要求的保护范围为准。
Claims (10)
1.一种初始化姿态确定方法,其特征在于,包括:
获取自动驾驶设备在初始化阶段的目标时刻采集的第一图像,以及在所述目标时刻的前一时刻采集的第二图像;
分别提取所述第一图像和所述第二图像的特征点,并将提取的特征点进行匹配,确定所述第一图像和所述第二图像对应同一物体的匹配点;
基于所述匹配点在图像上的第一位置信息,构建各匹配点分别对应的方向向量;
基于各匹配点分别对应的方向向量的叉乘结果,确定所述目标时刻相较于所述前一时刻的第一姿态变化量,并基于所述前一时刻的初始姿态和所述第一姿态变化量,确定所述自动驾驶设备在所述目标时刻的目标姿态。
2.根据权利要求1所述的方法,其特征在于,所述基于所述匹配点在图像上的第一位置信息,构建各匹配点分别对应的方向向量,包括:
基于所述匹配点在图像上的第一位置信息,将所述匹配点投影至归一化平面;
基于所述匹配点在所述归一化平面的第二位置信息,构建各匹配点分别对应的方向向量。
3.根据权利要求2所述的方法,其特征在于,所述归一化平面为距离相机为预设距离的垂直平面;所述匹配点包括位于第一图像的第一匹配点,和与第一匹配点对应的位于所述第二图像的第二匹配点;
所述基于所述匹配点在图像上的第一位置信息,将所述匹配点投影至归一化平面,包括:
基于所述相机的内参、畸变参数和所述第一匹配点的第一位置信息,将所述第一匹配点投影至第一归一化平面;以及,基于所述相机的内参、畸变参数和所述第二匹配点的第一位置信息,将所述第二匹配点投影至第二归一化平面。
4.根据权利要求1所述的方法,其特征在于,所述匹配点包括位于第一图像的第一匹配点,和与第一匹配点对应的位于所述第二图像的第二匹配点;
所述基于各匹配点分别对应的方向向量的叉乘结果,确定所述目标时刻相较于所述前一时刻的第一姿态变化量,包括:
针对任一第一匹配点,将该第一匹配点对应的方向向量与该第一匹配点对应的第二匹配点对应的方向向量进行叉乘运算,得到与该第一匹配点对应的叉乘结果向量;
基于各第一匹配点对应的叉乘结果向量,构建叉乘向量矩阵;
基于所述叉乘向量矩阵,确定所述目标时刻相比较于所述前一时刻的第一姿态变化量。
5.根据权利要求1所述的方法,其特征在于,所述初始化阶段为实时定位与地图构建的初始化阶段。
6.根据权利要求1所述的方法,其特征在于,在所述初始化阶段为视觉惯性里程计的初始化阶段的情况下,所述方法还包括:
针对任一时刻,基于该时刻采集的角速度信息,确定该时刻相较于前一时刻的第二姿态变化量;
在确定N个时刻的姿态之后,基于所述N个时刻的第一姿态变化量和第二姿态变化量,确定陀螺零偏,其中,N为预设值。
7.一种车载装置,其特征在于,所述车载装置用于执行如权利要求1~6任一所述的初始化姿态确定方法。
8.一种初始化姿态确定装置,其特征在于,包括:
获取模块,用于获取自动驾驶设备在初始化阶段的目标时刻采集的第一图像,以及在所述目标时刻的前一时刻采集的第二图像;
匹配模块,用于分别提取所述第一图像和所述第二图像的特征点,并将提取的特征点进行匹配,确定所述第一图像和所述第二图像对应同一物体的匹配点;
构建模块,用于基于所述匹配点在图像上的第一位置信息,构建各匹配点分别对应的方向向量;
确定模块,用于基于各匹配点分别对应的方向向量的叉乘结果,确定所述目标时刻相较于所述前一时刻的第一姿态变化量,并基于所述前一时刻的初始姿态和所述第一姿态变化量,确定所述自动驾驶设备在所述目标时刻的目标姿态。
9.一种计算机设备,其特征在于,包括:处理器、存储器和总线,所述存储器存储有所述处理器可执行的机器可读指令,当计算机设备运行时,所述处理器与所述存储器之间通过总线通信,所述机器可读指令被所述处理器执行时执行如权利要求1至6任一项所述的初始化姿态确定方法的步骤。
10.一种计算机可读存储介质,其特征在于,该计算机可读存储介质上存储有计算机程序,该计算机程序被处理器运行时执行如权利要求1至6任一项所述的初始化姿态确定方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310709099.1A CN116664893A (zh) | 2023-06-14 | 2023-06-14 | 一种初始化姿态确定方法、装置、计算机设备及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310709099.1A CN116664893A (zh) | 2023-06-14 | 2023-06-14 | 一种初始化姿态确定方法、装置、计算机设备及存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116664893A true CN116664893A (zh) | 2023-08-29 |
Family
ID=87727797
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310709099.1A Pending CN116664893A (zh) | 2023-06-14 | 2023-06-14 | 一种初始化姿态确定方法、装置、计算机设备及存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116664893A (zh) |
-
2023
- 2023-06-14 CN CN202310709099.1A patent/CN116664893A/zh active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110246147B (zh) | 视觉惯性里程计方法、视觉惯性里程计装置及移动设备 | |
CN107990899B (zh) | 一种基于slam的定位方法和系统 | |
EP3028252B1 (en) | Rolling sequential bundle adjustment | |
EP2715667B1 (en) | Planar mapping and tracking for mobile devices | |
WO2021043213A1 (zh) | 标定方法、装置、航拍设备和存储介质 | |
US20210183100A1 (en) | Data processing method and apparatus | |
CN111127524A (zh) | 一种轨迹跟踪与三维重建方法、系统及装置 | |
Saurer et al. | Homography based visual odometry with known vertical direction and weak manhattan world assumption | |
US11922658B2 (en) | Pose tracking method, pose tracking device and electronic device | |
WO2022183657A1 (zh) | 点云模型构建方法、装置、电子设备、存储介质和程序 | |
WO2022083038A1 (zh) | 视觉定位方法及相关装置、设备和计算机可读存储介质 | |
CN111609868A (zh) | 一种基于改进光流法的视觉惯性里程计方法 | |
WO2023005457A1 (zh) | 位姿计算方法和装置、电子设备、可读存储介质 | |
AliAkbarpour et al. | Parallax-tolerant aerial image georegistration and efficient camera pose refinement—without piecewise homographies | |
US8509522B2 (en) | Camera translation using rotation from device | |
CN113034347A (zh) | 倾斜摄影图像处理方法、装置、处理设备及存储介质 | |
CN112750164B (zh) | 轻量化定位模型的构建方法、定位方法、电子设备 | |
CN116664893A (zh) | 一种初始化姿态确定方法、装置、计算机设备及存储介质 | |
Ornhag et al. | Efficient real-time radial distortion correction for uavs | |
US11158119B2 (en) | Systems and methods for reconstructing a three-dimensional object | |
JP3452188B2 (ja) | 2次元動画像中の特徴点の追跡方法 | |
US11222430B2 (en) | Methods, devices and computer program products using feature points for generating 3D images | |
CN110660134B (zh) | 三维地图构建方法、三维地图构建装置及终端设备 | |
CN117115333B (zh) | 一种结合imu数据的三维重建方法 | |
CN115342829A (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 |