CN114812601A - 视觉惯性里程计的状态估计方法、装置、电子设备 - Google Patents
视觉惯性里程计的状态估计方法、装置、电子设备 Download PDFInfo
- Publication number
- CN114812601A CN114812601A CN202110127154.7A CN202110127154A CN114812601A CN 114812601 A CN114812601 A CN 114812601A CN 202110127154 A CN202110127154 A CN 202110127154A CN 114812601 A CN114812601 A CN 114812601A
- Authority
- CN
- China
- Prior art keywords
- visual
- inertial
- state
- image frame
- constraint condition
- 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
- 230000000007 visual effect Effects 0.000 title claims abstract description 298
- 238000000034 method Methods 0.000 title claims abstract description 194
- 230000008569 process Effects 0.000 claims abstract description 115
- 238000005457 optimization Methods 0.000 claims abstract description 74
- 238000005259 measurement Methods 0.000 claims abstract description 53
- 230000008859 change Effects 0.000 claims abstract description 44
- 230000000295 complement effect Effects 0.000 claims description 21
- 230000003190 augmentative effect Effects 0.000 claims description 4
- 230000006870 function Effects 0.000 description 26
- 238000010586 diagram Methods 0.000 description 10
- 238000012545 processing Methods 0.000 description 8
- 238000004891 communication Methods 0.000 description 6
- 238000004590 computer program Methods 0.000 description 6
- 238000010276 construction Methods 0.000 description 5
- 238000003672 processing method Methods 0.000 description 5
- 238000001914 filtration Methods 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 4
- 230000006399 behavior Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 230000008030 elimination Effects 0.000 description 2
- 238000003379 elimination reaction Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 239000011541 reaction mixture Substances 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Image Analysis (AREA)
Abstract
本公开实施例公开了一种视觉惯性里程计的状态估计方法、装置、电子设备，所述方法包括：获取当前图像帧以及惯性测量数据；在当前图像帧为关键帧时，将当前图像帧添加至基于滑动窗口确定的关键图像帧集合中；根据惯性测量数据确定第一惯性残差；基于第一惯性残差及第一视觉残差，更新第一惯性约束条件和第一视觉约束条件；针对滑动窗口内的关键图像帧，通过迭代优化的方式估计关键图像帧对应的视觉惯性里程计状态；在每一次迭代过程中，基于边缘化先验约束条件、第一视觉约束条件以及第一惯性约束条件，确定当前迭代过程相对于前一次迭代过程视觉惯性里程计状态的增量变化数据，并基于增量变化数据对视觉惯性里程计状态进行优化。
Description
技术领域
本公开涉及视觉定位技术领域，具体涉及一种视觉惯性里程计的状态估计方法、装置、电子设备。
背景技术
视觉惯性里程计(VIO)利用相机获得的视觉信息(图像)、IMU(惯性测量单元)获得的惯性测量数据以及连续图像帧之间的局部性，确定相机的位姿，并恢复场景的三维结构，满足了同步定位及三维重建对定位精度、鲁棒性以及低成本的需求。在实际应用场景中，视觉惯性里程计的状态估计一般采用图优化的方法进行状态估计。一些视觉惯性里程计中使用了开源的通用非线性最小二乘求解器，但是通用非线性最小二乘求解器为了适应不同类型的优化问题，其通常采用批量式最小二乘算法，牺牲了效率。虽然在一些平台上可以实时运行，但是实际应用场景范围较窄，例如无法在搭载了低成本计算单元的终端等设备上应用。因此，如何能够在不消耗过多算力资源的情况下，高效、准确地求解出视觉惯性里程计状态是当前需要解决的技术问题之一。
发明内容
本公开实施例提供一种视觉惯性里程计的状态估计方法、装置、电子设备。
第一方面，本公开实施例中提供了一种视觉惯性里程计的状态估计方法，其中，包括：
获取视觉惯性里程计集成的图像采集设备采集到的当前图像帧，以及视觉惯性里程计集成的惯性测量单元输出的惯性测量数据；
在所述当前图像帧为关键帧时，将所述当前图像帧添加至基于滑动窗口确定的关键图像帧集合中；
根据所述当前图像帧与上一相邻图像帧之间的所述惯性测量数据，确定所述当前图像帧以及所述上一相邻图像帧之间的第一惯性残差；
基于所述第一惯性残差及所述当前图像帧与所述关键图像帧集合内其他关键图像帧之间的第一视觉残差，更新第一惯性约束条件和第一视觉约束条件；
针对所述滑动窗口内的关键图像帧，通过迭代优化的方式估计所述关键图像帧对应的视觉惯性里程计状态；其中，在每一次迭代过程中，基于边缘化先验约束条件、所述第一视觉约束条件以及所述第一惯性约束条件，确定当前迭代过程相对于前一次迭代过程所述视觉惯性里程计状态的增量变化数据，并基于所述增量变化数据对所述视觉惯性里程计状态进行优化。
进一步地，基于所述第一惯性残差及所述当前图像帧与所述关键图像帧集合内其他关键图像帧之间的第一视觉残差，更新第一视觉约束条件和第一惯性约束条件，包括：
计算所述当前图像帧与所述滑动窗口内其他关键图像帧之间的所述第一视觉残差；
基于所述第一视觉残差以及所述第一惯性残差分别更新第一视觉约束条件以及第一惯性约束条件。
进一步地，在所述当前图像帧为关键帧时，将所述当前图像帧添加至基于滑动窗口确定的关键图像帧集合中，包括：
确定滑动窗口限定的关键图像帧集合中关键图像帧的数量；
在所述关键图像帧的数量大于或等于第一预设阈值时，基于所述关键图像帧集合内的待剔除关键帧与其他关键图像帧之间的惯性残差和视觉残差，更新所述边缘化先验约束条件，并将所述待剔除关键帧从所述滑动窗口限定的所述关键图像帧集合剔除；
将所述当前图像帧添加至所述滑动窗口限定的所述关键图像帧集合中。
进一步地，在每一次迭代过程中，基于边缘化先验约束条件、所述第一视觉约束条件以及所述第一惯性约束条件，确定当前迭代过程相对于前一次迭代过程所述视觉惯性里程计状态的增量变化数据，并基于所述增量变化数据对所述视觉惯性里程计状态进行优化，包括：
基于边缘化先验约束条件、所述第一视觉约束条件、所述第一惯性约束条件以及所述视觉惯性里程计状态的前一次迭代过程中构建的前次状态优化增量方程，构建所述视觉惯性里程计状态的当前状态优化增量方程；其中，所述当前状态优化增量方程中相对于所述前次状态优化增量方程的增量部分对应于目标状态的增量，所述目标状态包括前一次迭代过程中，对所述视觉惯性里程计状态进行优化前后变化值大于第二预设阈值的所述视觉惯性里程计状态；
对所述当前状态优化增量方程进行求解，获得当前迭代过程中所述视觉惯性里程计状态的优化值。
进一步地，对所述当前状态优化增量方程进行求解，获得当前迭代过程中所述视觉惯性里程计状态的优化值，包括：
基于所述当前状态优化增量方程构建消去三维空间点状态的舒尔补方程；
对所述舒尔补方程求解获得所述视觉惯性里程计状态中相机位姿状态的增量以及运动状态的增量；
基于所述相机位姿状态的增量以及运动状态的增量求解所述三维空间点的增量。
进一步地，在所述关键图像帧的数量大于或等于第一预设阈值时，基于所述关键图像帧集合内的待剔除关键帧与其他关键图像帧之间的惯性残差和视觉残差，更新所述边缘化先验约束条件，并将所述待剔除关键帧从所述滑动窗口剔除，包括：
基于第二惯性约束条件、第二视觉约束条件以及前次条件更新增量方程构建中间增量方程；其中，所述前次条件更新增量方程为前次对所述边缘化先验约束条件进行更新的过程中构建的增量方程；所述第二惯性约束条件和所述第二视觉约束条件分别为所述第一惯性约束条件和所述第一视觉约束条件中与所述预设关键帧相关联的约束条件；
通过舒尔补操作从所述中间增量方程去除所述预设关键帧的视觉惯性里程计状态，获得所述边缘化先验约束条件对应的当前条件更新增量方程；
利用所述当前条件更新增量方程更新所述边缘化先验约束条件。
进一步地，所述方法还包括：
在所述当前图像帧为非关键帧时，确定所述关键图像帧集合内与所述当前图像帧相匹配的至少一个匹配关键帧，以及所述当前图像帧与所述至少一个匹配关键帧之间的匹配特征点对应的三维空间点坐标；
计算所述当前图像帧与所述至少一个匹配关键帧之间的第二视觉残差，以及根据所述当前图像帧与其上一相邻图像帧之间的惯性测量数据计算第二惯性残差；
基于所述第二视觉残差构建非关键帧视觉约束条件，以及根据所述第二惯性残差构建非关键帧惯性约束条件；
基于所述非关键帧视觉约束条件、所述非关键帧惯性约束条件、所述至少一个匹配关键帧对应的所述视频惯性里程计状态的已知值以及所述三维空间点坐标获取所述当前图像帧对应的所述视频惯性里程计状态的目标值。
第二方面，本发明实施例中提供了一种视觉惯性里程计的状态估计装置，其中，包括：
第一获取模块，被配置为获取视觉惯性里程计集成的图像采集设备采集到的当前图像帧，以及视觉惯性里程计集成的惯性测量单元输出的惯性测量数据；
第一确定模块，被配置为根据所述当前图像帧与上一相邻图像帧之间的所述惯性测量数据，确定所述当前图像帧以及所述上一相邻图像帧之间的第一惯性残差；
添加模块，被配置为在所述当前图像帧为关键帧时，将所述当前图像帧添加至基于滑动窗口确定的关键图像帧集合中；
更新模块，被配置为基于所述第一惯性残差及所述当前图像帧与所述关键图像帧集合内其他关键图像帧之间的第一视觉残差，更新第一惯性约束条件和第一视觉约束条件；
估计模块，被配置为针对所述滑动窗口内的关键图像帧，通过迭代优化的方式估计所述关键图像帧对应的视觉惯性里程计状态；其中，在每一次迭代过程中，基于边缘化先验约束条件、所述第一视觉约束条件以及所述第一惯性约束条件，确定当前迭代过程相对于前一次迭代过程所述视觉惯性里程计状态的增量变化数据，并基于所述增量变化数据对所述视觉惯性里程计状态进行优化。
所述功能可以通过硬件实现，也可以通过硬件执行相应的软件实现。所述硬件或软件包括一个或多个与上述功能相对应的模块。
在一个可能的设计中，上述装置的结构中包括存储器和处理器，所述存储器用于存储一条或多条支持上述装置执行上述对应方法的计算机指令，所述处理器被配置为用于执行所述存储器中存储的计算机指令。上述装置还可以包括通信接口，用于上述装置与其他设备或通信网络通信。
第三方面，本公开实施例提供了一种电子设备，包括视觉惯性里程计、存储器和处理器；其中，所述存储器用于存储一条或多条计算机指令，其中，基于所述视觉惯性里程计输出的图像数据和惯性测量数据，所述一条或多条计算机指令被所述处理器执行以实现上述任一方面所述的方法。
第四方面，本公开实施例提供了一种在第三方面所述的电子设备上增强现实的实现方法，其中，基于第一方面所述的方法确定所述电子设备的位姿和电子设备周边的三维空间。
第五方面，本公开实施例提供了一种计算机可读存储介质，用于存储上述任一装置所用的计算机指令，其包含用于执行上述任一方面所述方法所涉及的计算机指令。
本公开实施例提供的技术方案可以包括以下有益效果：
本公开实施例提出了一种视觉惯性里程计的状态估计方法，该方法包括：获取视觉惯性里程计集成的图像采集设备采集到的当前图像帧，以及视觉惯性里程计集成的惯性测量单元输出的惯性测量数据；在所述当前图像帧为关键帧时，将所述当前图像帧添加至基于滑动窗口确定的关键图像帧集合中；根据所述当前图像帧与上一相邻图像帧之间的所述惯性测量数据，确定所述当前图像帧以及所述上一相邻图像帧之间的第一惯性残差；基于所述第一惯性残差及所述当前图像帧与所述关键图像帧集合内其他关键图像帧之间的第一视觉残差，更新第一视觉约束条件和第一惯性约束条件；针对所述滑动窗口内的关键图像帧，通过迭代优化的方式估计所述关键图像帧对应的视觉惯性里程计状态；其中，在每一次迭代过程中，基于边缘化先验约束条件、所述第一视觉约束条件以及所述第一惯性约束条件，确定当前迭代过程相对于前一次迭代过程所述视觉惯性里程计状态的增量变化数据，并基于所述增量变化数据对所述视觉惯性里程计状态进行优化。上述方法在迭代优化视觉惯性里程计状态时，每一次迭代过程中通过边缘化先验约束条件、基于当前图像帧更新过的第一视觉约束条件和第一惯性约束条件确定当前迭代过程相对于前次迭代过程，视觉惯性里程计状态的增量变化数据，进而再根据该增量变化数据得到当前迭代过程中视觉惯性里程计状态的优化值以及三维空间点坐标。本公开实施例通过增量式求解过程以及滑动窗口的边缘化操作相结合的方式，减少了迭代优化过程中视觉惯性里程计状态的更新规模，提高了视觉惯性里程计状态估计效率的同时，采用边缘化先验信息提高了视觉惯性里程计状态的估计精度。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本公开。
附图说明
结合附图，通过以下非限制性实施方式的详细描述，本公开的其它特征、目的和优点将变得更加明显。在附图中：
图1示出根据本公开一实施方式的视觉惯性里程计的状态估计方法的流程图；
图2示出根据本公开一实施方式在视觉惯性里程计的状态估计过程中对输入图像帧的处理流程示意图；
图3示出根据本公开一实施方式中手机定位和跟踪应用场景的框架示意图；
图4是适于用来实现根据本公开一实施方式的视觉惯性里程计的状态估计方法的电子设备的结构示意图。
具体实施方式
下文中，将参考附图详细描述本公开的示例性实施方式，以使本领域技术人员可容易地实现它们。此外，为了清楚起见，在附图中省略了与描述示例性实施方式无关的部分。
在本公开中，应理解，诸如“包括”或“具有”等的术语旨在指示本说明书中所公开的特征、数字、步骤、行为、部件、部分或其组合的存在，并且不欲排除一个或多个其他特征、数字、步骤、行为、部件、部分或其组合存在或被添加的可能性。
另外还需要说明的是，在不冲突的情况下，本公开中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本公开。
下面通过具体实施例详细介绍本公开实施例的细节。
图1示出根据本公开一实施方式的视觉惯性里程计的状态估计方法的流程图。如图1所示，该图像处理方法包括以下步骤：
在步骤S101中，获取视觉惯性里程计集成的图像采集设备采集到的当前图像帧，以及视觉惯性里程计集成的惯性测量单元输出的惯性测量数据；
在步骤S102中，在所述当前图像帧为关键帧时，将所述当前图像帧添加至基于滑动窗口确定的关键图像帧集合中；
在步骤S103中，根据所述当前图像帧与上一相邻图像帧之间的所述惯性测量数据，确定所述当前图像帧以及所述上一相邻图像帧之间的第一惯性残差；
在步骤S104中，基于所述第一惯性残差及所述当前图像帧与所述关键图像帧集合内其他关键图像帧之间的第一视觉残差，更新第一惯性约束条件和第一视觉约束条件；
在步骤S105中，针对所述滑动窗口内的关键图像帧，通过迭代优化的方式估计所述关键图像帧对应的视觉惯性里程计状态；其中，在每一次迭代过程中，基于边缘化先验约束条件、所述第一视觉约束条件以及所述第一惯性约束条件，确定当前迭代过程相对于前一次迭代过程所述视觉惯性里程计状态的增量变化数据，并基于所述增量变化数据对所述视觉惯性里程计状态进行优化。
本实施例中，视觉惯性里程计VIO(Visual Inertial Odometry)系统通过使用设置在同一设备上的图像采集设备和IMU(惯性测量单元)，在该设备移动的状态下采集一系列图像帧以及两图像帧之间的惯性测量数据，进而根据该一系列图像帧和该惯性测量数据估计出各图像帧及惯性测量数据对应的视觉惯性里程计状态以及依托于图像帧的三维空间点坐标，进而再基于视觉惯性里程计状态不断更新三维环境数据。在一些实施例中，视觉惯性里程计状态可以包括但不限于相机位置状态、相机姿态状态、相机运动状态(速度)、陀螺仪偏置状态、加速度计偏置状态、三维空间点状态中的一种或多种的组合。
目前常见的视觉惯性里程计状态估计方法可以分为基于滤波的方法，如扩展卡尔曼滤波(EKF，Extended Kalman Filter)、信息滤波(IF，Information Filter)等，以及基于图优化的方法，如高斯-牛顿法(GN,Gauss-Newton)，狗腿法(DL,Dog-Leg)等。相比于基于滤波的状态估计方法，基于图优化的方法使用了更多的历史信息，同时在迭代过程中对目标函数进行了重新线性化，使得其状态估计的精度明显提升。但是其带来的缺点也是相当明显的，随着使用历史信息的增多，其算力的消耗也随之增大。
为了解决上述图优化方法所带来的算力上消耗过大的问题，以及同时能够保证状态估计准确性的前提下，本公开实施提出了一种增量式的视觉惯性里程计状态的估计方案。相比于传统的图优化方法，本公开实施例引入了增量式的状态估计框架，减少了图优化方法中目标函数重新线性化的规模，从而减少图优化方法对算力的消耗。此外，本公开实施例还通过引入滑动窗口，限制了非线性优化中待优化状态的规模，使得时间复杂度从O(n)降到O(1)，进一步降低了算力的消耗。也即，本公开实施例针对所述滑动窗口内的关键图像帧，基于边缘化先验约束条件、第一视觉约束条件以及第一惯性约束条件，通过增量式求解方式对视觉惯性里程计状态进行迭代优化，其中，边缘化先验约束条件基于从滑动窗口剔除的历史关键帧获得。
本公开实施例中，滑动窗口的长度可以预先确定，第一预设阈值可以根据滑动窗口的长度确定，例如第一预设阈值可以设置为等于滑动窗口的长度或者小于滑动窗口的长度。滑动窗口内的关键图像帧的数量大于或等于第一预设阈值后，可以将滑动窗口内的待剔除关键帧从滑动窗口中剔除，并根据所剔除的关键帧生成边缘化先验约束条件，用于对滑动窗口内的剩余关键帧进行约束。
集成在视觉惯性里程计设备上的图像采集设备可以持续不断地采集周围环境中的图像帧，而集成在该视觉惯性里程计设备上的IMU也可以持续不断地采集视觉惯性里程计设备的惯性测量数据。从图像采集设备每获取到一帧图像，也即当前图像帧，以及上一帧图像至当前图像帧之间的惯性测量数据之后，则可以执行一次对视觉惯性里程计状态的优化过程。在对视觉惯性里程计状态的优化过程中，可以先确定当前图像帧是否为关键图像帧，并在该当前图像帧为关键图像帧时，基于本公开实施例提出的方案对视觉惯性里程计状态进行估计。
在一些实施例中，关键图像帧可以为与前面采集到的图像帧在图像特征上变化较大的图像帧，具体可以通过特征点匹配等方式确定，关键图像帧的判断方式可以采用已有的方式，在此不再赘述。
在当前图像帧为关键图像帧时，可以将关键图像帧加入滑动窗口限定的关键图像帧集合中，并可以基于滑动窗口内的关键图像帧集合对视觉惯性里程计状态进行一次估计。在视觉惯性里程计状态的一次估计过程中，可以根据IMU测量得到的上述惯性测量数据确定当前图像帧与上一相邻图像帧之间的第一惯性残差，之后再基于该当前图像帧与滑动窗口内关键图像帧之间的第一视觉残差以及第一惯性残差更新前一次估计过程中得到的第一视觉约束条件和第一惯性约束条件，最后可以针对滑动窗口内的关键图像帧，通过迭代优化的方式，逐渐逼近视觉惯性里程计状态在本次估计过程中的最终估计结果。需要说明的是，滑动窗口内的图像帧均为待观测的关键图像帧，非关键图像帧不放入滑动窗口内。还需要说明的是，在将当前图像帧加入滑动窗口之前，还需要保证滑动窗口内的关键图像帧的数量少于第一预设阈值，该第一预设阈值可以为等于或小于滑动窗口长度的数值。也即，滑动窗口内的关键图像帧的数量不能大于第一预设阈值。
在一些实施例中，第一视觉残差例如可以是视觉重投影误差，包括点投影误差和线投影误差等，可以基于视觉残差模型计算得到。第一惯性残差可以基于IMU残差模型计算得到。
在一些实施例中，在确定了当前图像帧与滑动窗口内所有关键图像帧之间的第一视觉残差、当前图像帧与上一相邻图像帧之间的第一惯性残差之后，可以利用该第一视觉残差以及第一惯性残差更新原来的第一视觉约束条件和第一惯性约束条件，也即上一次视觉惯性里程计状态的估计过程中得到的第一视觉约束条件和第一惯性约束条件。需要说明的是，更新前的第一视觉约束条件和第一惯性约束条件是在未考虑当前图像帧的情况下得到的约束条件。而更新后的第一视觉约束条件中则包括滑动窗口内所有关键图像帧(包括当前图像帧)两两之间的视觉残差约束，更新后的第一惯性约束条件中包括滑动窗口内所有关键图像帧(包括当前图像帧)前后相邻两帧之间的惯性残差约束。
在一些实施例中，在将当前图像帧加入滑动窗口限定的关键图像帧集合之后，可以针对滑动窗口内的所有关键图像帧，通过迭代优化的方式求解出滑动窗口内所有关键图像帧对应的视觉惯性里程计状态。需要说明的是，视觉惯性里程计状态中的三维空间点状态包括关键图像帧之间的匹配特征点在世界坐标系下的位置坐标。
在一些实施例中，在每次迭代过程中，可以基于上述边缘化先验约束条件、第一视觉约束条件以及第一惯性约束条件，确定当前迭代过程相对于前一次迭代过程视觉惯性里程计状态的增量变化数据，并基于该增量变化数据得到视觉惯性里程计状态在当前迭代过程中的优化值。在迭代优化完成之后，可以得到本次估计过程中滑动窗口内各关键图像帧对应的视觉惯性里程计状态的最终估计结果以及滑动窗口内关键图像帧对应的三维空间点坐标的最终估计结果。
需要说明的是，增量变化数据可以通过构建基于上述边缘化先验约束条件、第一视觉约束条件以及第一惯性约束条件求解视觉惯性里程计状态的正规方程对应的增量方程来确定。在传统的图优化方法中，基于边缘化先验约束条件、第一视觉约束条件以及第一惯性约束条件建立约束方程之后，由于直接对该约束方程求解比较困难，因此通常采用迭代的方法，给定待优化的视觉惯性里程计状态的初始值，在此基础上不断的迭代更新，使得目标函数的值不断下降来获取最终的状态估计结果，也即通过构建非线性化最小二乘法的正规方法进行求解。
以典型的高斯-牛顿法为例说明，每次迭代时，目标函数都会在当前状态估计结果的基础上进行重新线性化，因此每次迭代都会计算目标函数中每一个因子的雅可比及残差，并且基于解算出的雅可比及残差将每个因子进行累加的方式构建正规方程。但是，实际上这是没有必要的，因为对于位姿及三维空间点坐标没有发生变化或者变化较小的视觉惯性里程计状态所对应的目标函数，则并不需要重新线性化计算其雅可比矩阵及残差，因为这些状态可以近似认为没有发生变化。因此，本公开实施例通过将变化较大的视觉惯性里程计状态所关联的目标函数重新线性化并对所构建的正规方程进行更新即可。在实现过程中，本公开实施例通过基于边缘化先验约束条件、第一视觉约束条件以及第一惯性约束条件先确定视觉惯性里程计状态的增量变化数据，并将该增量变化数据叠加在上一次迭代过程中的正规方程上，即可求解出本次迭代过程中视觉惯性里程计状态的优化值。该增量变化数据可以理解为与上一次迭代结果相比，发生增量变化的数据。
本公开实施例在迭代优化视觉惯性里程计状态时，每一次迭代过程中通过边缘化先验约束条件、基于当前图像帧更新过的第一视觉约束条件和第一惯性约束条件确定当前迭代过程相对于前次迭代过程，视觉惯性里程计状态的增量变化数据，进而再根据该增量变化数据得到当前迭代过程中视觉惯性里程计状态的优化值以及三维空间点坐标。本公开实施例通过增量式求解过程以及滑动窗口的边缘化操作相结合的方式，减少了迭代优化过程中视觉惯性里程计状态的更新规模，提高了视觉惯性里程计状态估计效率的同时，采用边缘化先验信息提高了视觉惯性里程计状态的估计精度。
在本实施例的一个可选实现方式中，步骤S104，即基于所述第一惯性残差及所述当前图像帧与所述关键图像帧集合内其他关键图像帧之间的第一视觉残差，更新第一视觉约束条件和第一惯性约束条件的步骤，进一步包括以下步骤：
计算所述当前图像帧与所述滑动窗口内其他关键图像帧之间的所述第一视觉残差；
基于所述第一视觉残差以及所述第一惯性残差分别更新第一视觉约束条件以及第一惯性约束条件。
该可选的实现方式中，在获取到当前图像帧之后，如果当前图像帧为关键图像帧，则可以计算该当前图像帧与滑动窗口内各关键图像帧之间的第一视觉残差，并根据上一相邻图像帧至当前图像帧之间测得的惯性测量数据计算得到第一惯性残差。利用该第一视觉残差和第一惯性残差分别对第一视觉约束条件和第一惯性视觉约束条件进行更新，使得将当前图像帧加入滑动窗口之后，对滑动窗口内的各关键图像帧进行视觉惯性里程计状态估计的过程中，能够引入当前图像帧的视觉约束和惯性约束。
在本实施例的一个可选实现方式中，步骤S102，即在所述当前图像帧为关键帧时，将所述当前图像帧添加至基于滑动窗口确定的关键图像帧集合中的步骤，进一步包括以下步骤：
确定滑动窗口限定的关键图像帧集合中关键图像帧的数量；
在所述关键图像帧的数量大于或等于第一预设阈值时，基于所述关键图像帧集合内的待剔除关键帧与其他关键图像帧之间的惯性残差和视觉残差，更新所述边缘化先验约束条件，并将所述待剔除关键帧从所述滑动窗口限定的所述关键图像帧集合剔除；
将所述当前图像帧添加至所述滑动窗口限定的所述关键图像帧集合中。
该可选的实现方式中，该第一预设阈值可以根据需要设定，在加入滑动窗口的关键图像帧数量大于或等于该第一预设阈值后，可以将待剔除关键帧从滑动窗口内的关键图像帧集合中剔除。该待剔除关键帧可以是最先加入滑动窗口内的关键图像帧，也可以是与前后相邻关键帧区别最小的关键图像帧，具体可以根据实际需要设定，在此不做具体限制。在去除该待剔除关键帧之后，可以将该待剔除关键帧关联的视觉约束和惯性约束转化为对滑动窗口内其他关键图像帧的约束，也即通过该待剔除关键帧与滑动窗口内剩余关键图像帧之间的视觉残差和惯性残差对边缘化先验约束条件进行更新，以便后续视觉惯性里程计状态的估计过程中，边缘化先验约束中体现该被剔除的预设关键帧对滑动窗口内其他关键图像帧的视觉约束和惯性约束。
在本实施例的一个可选实现方式中，步骤S105中，在每一次迭代过程中，基于边缘化先验约束条件、所述第一视觉约束条件以及所述第一惯性约束条件，确定当前迭代过程相对于前一次迭代过程所述视觉惯性里程计状态的增量变化数据，并基于所述增量变化数据对所述视觉惯性里程计状态进行优化的步骤，进一步包括以下步骤：
基于边缘化先验约束条件、所述第一视觉约束条件、所述第一惯性约束条件以及所述视觉惯性里程计状态的前一次迭代过程中构建的前次状态优化增量方程，构建所述视觉惯性里程计状态的当前状态优化增量方程；其中，所述当前状态优化增量方程中相对于所述前次状态优化增量方程的增量部分对应于目标状态的增量，所述目标状态包括前一次迭代过程中，对所述视觉惯性里程计状态进行优化前后变化值大于第二预设阈值的所述视觉惯性里程计状态；
对所述当前状态优化增量方程进行求解，获得当前迭代过程中所述视觉惯性里程计状态的优化值。
该可选的实现方式中，由于是增量式求解视觉惯性里程计状态，因此在通过迭代优化方式对视觉惯性里程计状态进行估计的过中，当前迭代过程中，可以先确定与前次迭代过程迭代前后发生变化较大的视觉惯性里程计状态，将其确定为目标状态，例如迭代前后差值大于第二预设阈值的视觉惯性里程计状态可以被确定为目标状态，并基于前次迭代过程中所构建的正规方程也即前次状态优化增量方程以及目标状态的增量构建当前迭代过程中的正规方程，也即当前迭代过程构建的正规方程与前次迭代过程构建的正规方程之差为目标状态的增量部分相当于利用目标状态的增量部分对前次迭代过程构建的正规方程进行更新获得当前迭代过程的正规方程，因此该正规方程可以称之为增量方程，为了便于区分，将前次迭代过程中构建的该正规方程称之为前次状态优化增量方法，而当前迭代过程中构建的该正规方称之为当前状态优化增量方程。在当前迭代过程中，可以通过对当前状态优化增量方程求解得到当前迭代过程中目标状态的增量。
下面举例说明上述过程。
首先建立基于上述边缘化先验约束条件、第一视觉约束条件以及第一惯性约束条件的约束方程，如下式(1)所示：
约束方程如下所示
其中，χ＝[x0，x1，…，xn，λ0，λ1，…λm]为待估计的视觉惯性里程计状态，
为滑动窗口内第k个关键图像帧的位姿及运动状态，n为滑动窗口大小，
为位置状态，
为速度状态，
为姿态状态，ba，bg分别为陀螺仪偏置状态和加速度计偏置状态，λl，l∈[0，m]为第l个三维空间点的逆深度状态，共m个三维空间点。{rm，Hm}表示边缘化先验约束条件，
表示第一惯性约束条件，B为所有惯性测量数据的集合。
表示第一视觉约束条件，C为滑动窗口内至少两个关键图像帧上的特征点的集合。
由于直接求解上述公式(1)较难，通常采用迭代优化的方法，给定待优化的视觉惯性里程计状态的初始值，在此基础上不断的迭代优化，使得目标函数的值不断下降来获取最终的状态估计结果。在典型的高斯-牛顿法中，每次迭代目标函数都会在当前状态估计结果χ-的基础上进行重新线性化，可以如下表示重新线性化：
其中，rk为目标函数，χ-为当前状态估计结果为雅可比矩阵，ek＝rk(χ-)为残差项，
代表流形上的加法操作。增量δχ可以通过下述公式进行求解：
Aδχ＝b
[A|b]＝Σ[Ak|bk]
[Ak|bk]＝[Jk TJk|-Jkek] (4)
如果每次迭代都计算目标函数中每一个因子的雅可比及残差，并且将每个因子进行累加构建正规方程，则会产生大量的计算。而实际上这是没有必要的，因为对于位姿及三维空间点的坐标没有发生变化或者变化较小的视觉惯性里程计状态所对应的目标函数，则并不需要新线性化计算其雅可比矩阵及残差，因为这些状态可以近似认为没有发生变化。因此，本公开实施例中仅针对状态发生变化或者变化较大的状态所关联的目标函数进行重新线性化，并对正规方程进行更新，得到可以如下表示的增量方程：
其中，[A|b]+表示增量方程，也即当前迭代过程中的当前状态优化增量方程，[A|b]-表示前次迭代过程中的正规方程，也即前次状态优化增量方程，
表示增量部分，其对应于目标状态的增量变化，
表示需要进行重新线性化的目标函数的集合，也即涉及至少一个目标状态的目标函数集合。
通过对上述当前状态优化增量方程求解，即可得到当前迭代过程中视觉惯性里程计状态的优化值。通过上述方式，经过多次迭代优化之后，即可获得本次估计过程中视觉惯性里程计状态的最终估计结果。
在本实施例的一个可选实现方式中，对所述当前状态优化增量方程进行求解，获得当前迭代过程中所述视觉惯性里程计状态的优化值的步骤，进一步包括以下步骤：
基于所述当前状态优化增量方程构建消去三维空间点状态的舒尔补方程；
对所述舒尔补方程求解获得所述视觉惯性里程计状态中相机位姿状态的增量以及运动状态的增量；
基于所述相机位姿状态的增量以及运动状态的增量求解所述三维空间点的增量。
该可选的实现方式中，为了高效地求解出上述前次状态优化增量方程中视觉惯性里程计状态的增量，可以通过舒尔消元法先从前次状态优化增量方程中消去三维空间点状态，并求解出位姿状态(包括位置状态和姿态状态)及运动状态的增量，将位姿状态及运动状态的增量通过回代的方式求解出三维空间点状态的增量。
举例说明，视觉惯性里程计状态的增量可以如下表示：
δχ＝[δχc，δχp] (6)
其中，δχ为视觉惯性里程计状态的所有增量，δχc表示视觉惯性里程计状态中位姿状态及运动状态的增量，δχp表示视觉惯性里程计状态中三维空间点状态的增量。针对上述公式(6)可以得到：
消去上述公式(7)中的第二行可以得到仅与δχc有关的舒尔补方程，如下表示：
Sδχc＝s
[S|s]＝[U-WV-1WT|u-W/V-1v] (8)
其中，S表示舒尔补矩阵。从上述舒尔补方程的构建步骤中以看出，上述舒尔补方程的变化仅与视觉约束相关联。因此，当三维空间点状态所对应的目标函数需要重新线性化时，舒尔补方程才需要进行部分更新。
S中与(i1，i2)位姿状态、以及s中与第i个位姿状态相关联的参数块的更新可以如下表示：
其中，P代表需要重新线性化的目标函数涉及的三维空间点状态的集合，
代表被第i个关键图像帧追踪到的所有三维空间点的集合。
通过对上述公式(9)求解可以获得所述视觉惯性里程计状态中相机位姿状态的增量以及相机运动状态的增量，进而根据该相机位姿状态的增量以及相机运动状态的增量即可求解获得三维空间点状态的增量，三维空间点状态的增量的求解过程如下表示：
δχc＝S-1*s
δχp＝V-1*(v-WTδχc) (10)
在本实施例的一个可选实现方式中，在所述关键图像帧的数量大于或等于第一预设阈值时，基于所述关键图像帧集合内的待剔除关键帧与其他关键图像帧之间的惯性残差和视觉残差，更新所述边缘化先验约束条件，并将所述待剔除关键帧从所述滑动窗口剔除的步骤，进一步包括以下步骤：
基于第二惯性约束条件、第二视觉约束条件以及前次条件更新增量方程构建中间增量方程；其中，所述前次条件更新增量方程为前次对所述边缘化先验约束条件进行更新的过程中构建的增量方程；所述第二惯性约束条件和所述第二视觉约束条件分别为所述第一惯性约束条件和所述第一视觉约束条件中与所述预设关键帧相关联的约束条件；
通过舒尔补操作从所述中间增量方程去除所述预设关键帧的视觉惯性里程计状态，获得所述边缘化先验约束条件对应的当前条件更新增量方程；
利用所述当前条件更新增量方程更新所述边缘化先验约束条件。
该可选的实现方式中，由于被剔除的待剔除关键帧在下一优化过程中体现为边缘化先验约束条件，而第一视觉约束条件和第一惯性约束条件中仅体现的是滑动窗口内各关键图像帧之间的视觉约束和惯性约束，因此可以分别从第一视觉约束条件和第一惯性约束条件中将该待剔除关键帧关联的第二视觉约束条件和第二惯性约束条件去除，进而再利用该第二视觉约束条件和第二惯性约束条件更新边缘化先验约束条件。
利用该第二视觉约束条件和第二惯性约束条件更新边缘化先验约束条件可以理解为将第二视觉约束条件和第二惯性约束条件转化为对滑动窗口内其他关键图像帧的约束。
在一些实施例中，将第二视觉约束条件和第二惯性约束条件转化为对滑动窗口内其他关键图像帧的约束，可以看做是待剔除关键帧已知的情况下求解滑动窗口内其他关键图像帧的最大后验概率分布的问题，该最大后验概率分布如下表示：
由于本公开实施例采用的是增量更新方式，因此上述最大后验概率分布对应的增量方程可以如下表示：
其中，[Am|bm]即为基于第二惯性约束条件、第二视觉约束条件以及边缘化先验约束条件的前次更新过程中得到的前次条件更新增量方程构建的中间增量方程，[Alast-marg|blast-marg]表示更新前的增量方程，也即前次更新边缘化先验约束条件得到的前次条件更新增量方程，[AI|bI]表示根据待剔除关键帧与其相邻关键帧之间惯性约束所构建的增量方程，
表示根据待剔除关键帧与滑动窗口内其他关键图像帧之间的视觉约束所构建的增量方程，
为待剔除关键帧与滑动窗口内所有其他关键图像帧之间所有视觉约束的集合。
获得上述中间增量方程后，对待剔除关键帧的状态进行边缘化，也即从上述中间增量方程中去除待剔除关键帧所对应的相机位姿及运动状态、以及该待剔除关键帧对应的三维空间点状态，该待剔除关键帧所对应的相机位姿及运动状态、以及该待剔除关键帧对应的三维空间点状态的过程可以采用如下式所示的舒尔补操作：
其中，下标m表示待剔除关键帧的视觉惯性里程计状态，下标s表示滑动窗口内其他关键图像帧的视觉惯性里程计状态，[Sm|bm]为边缘化约束所对应的增量方程，也即更新后的边缘化先验约束条件对应的当前条件更新增量方程。本公开实施例通过边缘化操作，可以将滑窗中待剔除关键帧相关联的约束转变为对滑动窗口内各其他关键图像帧的约束，保证滑动窗口内各关键图像帧的可观测性。
在本实施例的一个可选实现方式中，该方法进一步还包括以下步骤：
在所述当前图像帧为非关键帧时，确定所述关键图像帧集合内与所述当前图像帧相匹配的至少一个匹配关键帧，以及所述当前图像帧与所述至少一个匹配关键帧之间的匹配特征点对应的三维空间点坐标；
计算所述当前图像帧与所述至少一个匹配关键帧之间的第二视觉残差，以及根据所述当前图像帧与其上一相邻图像帧之间的惯性测量数据计算第二惯性残差；
基于所述第二视觉残差构建非关键帧视觉约束条件，以及根据所述第二惯性残差构建非关键帧惯性约束条件；
基于所述非关键帧视觉约束条件、所述非关键帧惯性约束条件、所述至少一个匹配关键帧对应的所述视频惯性里程计状态的已知值以及所述三维空间点坐标获取所述当前图像帧对应的所述视频惯性里程计状态的目标值。
该可选的实现方式中，针对非关键图像帧采用与关键图像帧不同的处理方式。非关键图像帧不需要加入滑动窗口内，而只需基于滑动窗口内与该非关键图像帧相匹配的关键图像帧对非关键图像帧进行约束后，求解出非关键图像帧对应的视觉惯性里程计状态的目标值即可。
在一些实施例中，获得非关键图像帧之后，将非关键图像帧与滑动窗口内的关键图像帧进行匹配，获得一个或多个相匹配的匹配关键帧，以及与这些匹配关键帧之间的匹配特征点对应的三维空间点坐标。基于非关键图像帧与匹配关键帧之间的视觉残差(例如视觉重投影误差)构建非关键帧视觉约束条件，并基于与上一相邻图像帧之间测量得到的惯性测量数据确定惯性残差，基于该惯性残差构建非关键帧惯性约束条件。基于非关键帧视觉约束条件以及非关键帧惯性约束条件建立约束方程，并利用非线性最小二乘法构建约束方程的目标函数，通过图优化方法构建目标函数对应的正规方程。进而再根据已构建好的正规方程构建舒尔补方程，即在匹配关键帧以及三维空间点坐标已知的情况下，求解非关键帧也即当前图像帧对应的位姿状态以及运动状态的最大后验概率估计，如下表示：
其中，χnk表示非关键帧的位姿状态及运动状态。
图2示出根据本公开一实施方式在视觉惯性里程计的状态估计过程中对输入图像帧的处理流程示意图。如图2所示，输入图像帧包括关键图像帧和非关键图像帧，在接收到输入图像帧之后，在滑动窗口内寻找与输入图像帧相匹配的匹配关键帧以及输入图像帧与匹配图像帧之间的匹配特征点。
在输入图像为关键图像帧时，将输入图像帧加入滑动窗口内，并进行迭代优化过程。在该迭代优化过程中，每次迭代时通过增量式求解方式构建更新正规方程(也即上文中的当前状态优化增量方程)，该更新正规方包括原正规方程(也即上文中的前次状态优化增量方程，是前次迭代过程中构建的正规方程)以及当前迭代过程中的增量部分。利用舒尔补操作对更新正规方程进行求解，由于是增量式求解过程，因此该舒尔补操作中也通过对原舒尔补方程进行更新的方式构建更新舒尔补方程，更新舒尔补方程包括原舒尔补方程(也即上一次迭代中的舒尔补方程)以及增量部分。通过消元法从更新舒尔补方程中先消去三维空间点状态的方式求解出位姿状态及运动状态，之后再基于求解出的位姿状态和运动状态求解三维空间点状态。
在滑动窗口内的关键图像帧数量达到一定阈值时，将最先进入滑动窗口内的关键图像帧剔除，并且将该剔除的关键图像帧关联的视觉约束和惯性约束从上述更新正规方程和更新舒尔补方程中的视觉约束条件和惯性约束条件中剔除，所剔除的关键图像帧的视觉约束和惯性约束作为滑动窗口内其他关键图像帧的约束，也即在边缘化先验约束中加入所要剔除的关键图像帧的视觉约束和惯性约束。该加入过程可以参见上述对公式(11)-(13)的描述过程，在此不再赘述。
图3示出根据本公开一实施方式中手机定位和跟踪应用场景的框架示意图。如图3所示，手机上配备有相机和惯性测量单元(IMU)。该手机上也集成有执行本公开实施例提出的图像处理方法的视觉惯性里程计系统。视觉惯性里程计系统可以实时获取相机采集到的二维图像和IMU测量得到的惯性测量数据；相机按照固定帧率采集图像。手机在室内场景比如办公室、家庭、展馆等运动的状态下，手机上的相机拍摄周边环境，并将拍摄得到的图像提供给视觉惯性里程计系统。惯性测量单元持续测量手机相对本身坐标系的三轴旋转角速度和手机相对于本身坐标系的加速度计，并将该三轴旋转角速度和加速度计提供给视觉惯性里程计系统。
视觉惯性里程计系统在启动时，处于未初始化状态，在接收到相机输入的图像序列和IMU采集的数据(陀螺仪和加速度计的数据)后，进行初始化。在初始化过程中，首先初始周边环境的三维空间地图，并确定系统参数。
由于环境对定位系统是未知的，系统需要建立周边环境的三维地图，典型的三维地图由若干空间中位置确定的三维点组成；系统可以根据得到的图像序列，分别提取图像中的特征点进行两两图像之间的匹配，然后判断匹配特征点对在不同图像中的视差是否足够大；在同一个空间三维点在两幅图像中视差足够大的时候，根据两个时刻手机在相机坐标系下相对位姿，利用三角化方法可以恢复得到这个特征点的三维位置；多个已知三维位置的特征点即可以作为初始的空间三维地图；后续系统输出的手机的位置状态和姿态状态都是相对于该三维地图坐标系而言；手机的位置可以包括手机的空间位置坐标，而手机的姿态则可以包括手机的偏航角θ、俯仰角ψ和滚转角φ。
除了初始化得到三维空间地图之外，还需要计算系统的其他必要参数，比如相机坐标系和IMU坐标系的变换关系，从而完成相机坐标系和IMU坐标系的统一。
完成初始化之后，系统接收到相机采集的当前图像帧和IMU测量得到的当前图像帧与上一相邻图像帧之间的惯性测量数据之后，利用该当前图像帧、惯性测量数据、滑动窗口内关键图像帧以及前次迭代优化过程中产生的数据估计手机的位姿状态、运动状态以及三维空间地图中三维空间点状态。优化过程可以参见本公开实施例中上述图像处理方法中对视觉惯性里程计状态的优化流程，在此不再赘述。
随着用户手持手机不断移动的过程中，周围的环境随之发生变化，相机也不断看到新的物体，为了保证一直能够建立相机图像上特征点和地图点之间的对应关系，初始化过程中建立的三维地图需要随着手机的移动进行扩展。因此，系统可以利用优化过程中优化得到的视觉惯性里程计状态对三维空间地图进行更新；通过建立当前帧与之前图像帧上特征点之间的匹配关系，对于没有对应三维点的匹配点对，可以通过三角化的方式，计算出其在空间中的三维位置，进而添加到地图中。
下述为本公开装置实施例，可以用于执行本公开方法实施例。
根据本公开一实施方式的视觉惯性里程计的状态估计装置，该装置可以通过软件、硬件或者两者的结合实现成为电子设备的部分或者全部。该视觉惯性里程计的状态估计装置包括：
第一获取模块，被配置为获取视觉惯性里程计集成的图像采集设备采集到的当前图像帧，以及视觉惯性里程计集成的惯性测量单元输出的惯性测量数据；
第一确定模块，被配置为根据所述当前图像帧与上一相邻图像帧之间的所述惯性测量数据，确定所述当前图像帧以及所述上一相邻图像帧之间的第一惯性残差；
添加模块，被配置为在所述当前图像帧为关键帧时，将所述当前图像帧添加至基于滑动窗口确定的关键图像帧集合中；
更新模块，被配置为基于所述第一惯性残差及所述当前图像帧与所述关键图像帧集合内其他关键图像帧之间的第一视觉残差，更新第一惯性约束条件和第一视觉约束条件；
估计模块，被配置为针对所述滑动窗口内的关键图像帧，通过迭代优化的方式估计所述关键图像帧对应的视觉惯性里程计状态；其中，在每一次迭代过程中，基于边缘化先验约束条件、所述第一视觉约束条件以及所述第一惯性约束条件，确定当前迭代过程相对于前一次迭代过程所述视觉惯性里程计状态的增量变化数据，并基于所述增量变化数据对所述视觉惯性里程计状态进行优化。
在本实施例的一个可选实现方式中，所述更新模块，包括：
第一计算子模块，被配置为计算所述当前图像帧与所述滑动窗口内其他关键图像帧之间的所述第一视觉残差；
第一更新子模块，被配置为基于所述第一视觉残差以及所述第一惯性残差分别更新第一视觉约束条件以及第一惯性约束条件。
在本实施例的一个可选实现方式中，所述第一确定模块，包括：
第一确定子模块，被配置为确定滑动窗口限定的关键图像帧集合中关键图像帧的数量；
第二更新子模块，被配置为在所述关键图像帧的数量大于或等于第一预设阈值时，基于所述关键图像帧集合内的待剔除关键帧与其他关键图像帧之间的惯性残差和视觉残差，更新所述边缘化先验约束条件，并将所述待剔除关键帧从所述滑动窗口限定的所述关键图像帧集合剔除；
添加子模块，被配置为将所述当前图像帧添加至所述滑动窗口限定的所述关键图像帧集合中。
在本实施例的一个可选实现方式中，所述估计模块，包括：
第一构建子模块，被配置为基于边缘化先验约束条件、所述第一视觉约束条件、所述第一惯性约束条件以及所述视觉惯性里程计状态的前一次迭代过程中构建的前次状态优化增量方程，构建所述视觉惯性里程计状态的当前状态优化增量方程；其中，所述当前状态优化增量方程中相对于所述前次状态优化增量方程的增量部分对应于目标状态的增量，所述目标状态包括前一次迭代过程中，对所述视觉惯性里程计状态进行优化前后变化值大于第二预设阈值的所述视觉惯性里程计状态；
第一求解子模块，被配置为对所述当前状态优化增量方程进行求解，获得当前迭代过程中所述视觉惯性里程计状态的优化值。
在本实施例的一个可选实现方式中，所述第一求解子模块，包括：
第二构建子模块，被配置为基于所述当前状态优化增量方程构建消去三维空间点状态的舒尔补方程；
第二求解子模块，被配置为对所述舒尔补方程求解获得所述视觉惯性里程计状态中相机位姿状态的增量以及运动状态的增量；
第三求解子模块，被配置为基于所述相机位姿状态的增量以及运动状态的增量求解所述三维空间点的增量。
在本实施例的一个可选实现方式中，所述第二更新子模块，包括：
第三构建子模块，被配置为基于第二惯性约束条件、第二视觉约束条件以及前次条件更新增量方程构建中间增量方程；其中，所述前次条件更新增量方程为前次对所述边缘化先验约束条件进行更新的过程中构建的增量方程；所述第二惯性约束条件和所述第二视觉约束条件分别为所述第一惯性约束条件和所述第一视觉约束条件中与所述预设关键帧相关联的约束条件；
获取子模块，被配置为通过舒尔补操作从所述中间增量方程去除所述预设关键帧的视觉惯性里程计状态，获得所述边缘化先验约束条件对应的当前条件更新增量方程；
第三更新子模块，被配置为利用所述当前条件更新增量方程更新所述边缘化先验约束条件。
在本实施例的一个可选实现方式中，所述装置还包括：
第二确定模块，被配置为在所述当前图像帧为非关键帧时，确定所述关键图像帧集合内与所述当前图像帧相匹配的至少一个匹配关键帧，以及所述当前图像帧与所述至少一个匹配关键帧之间的匹配特征点对应的三维空间点坐标；
计算模块，被配置为计算所述当前图像帧与所述至少一个匹配关键帧之间的第二视觉残差，以及根据所述当前图像帧与其上一相邻图像帧之间的惯性测量数据计算第二惯性残差；
构建模块，被配置为基于所述第二视觉残差构建非关键帧视觉约束条件，以及根据所述第二惯性残差构建非关键帧惯性约束条件；
第二获取模块，被配置为基于所述非关键帧视觉约束条件、所述非关键帧惯性约束条件、所述至少一个匹配关键帧对应的所述视频惯性里程计状态的已知值以及所述三维空间点坐标获取所述当前图像帧对应的所述视频惯性里程计状态的目标值。
本实施例中的图像处理装置与上述图像处理方法对应一致，具体细节可以参见上述对图像处理方法的描述，在此不再赘述。
图4是适于用来实现根据本公开实施方式的视觉惯性里程计状态的估计方法的电子设备的结构示意图。
如图4所示，电子设备400包括处理单元401，其可实现为CPU、GPU、FPGA、NPU等处理单元。处理单元401可以根据存储在只读存储器(ROM)402中的程序或者从存储部分408加载到随机访问存储器(RAM)403中的程序而执行本公开上述任一方法的实施方式中的各种处理。在RAM403中，还存储有电子设备400操作所需的各种程序和数据。处理单元401、ROM402以及RAM403通过总线404彼此相连。输入/输出(I/O)接口405也连接至总线404。
以下部件连接至I/O接口405：包括键盘、鼠标等的输入部分406；包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分407；包括硬盘等的存储部分408；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分409。通信部分409经由诸如因特网的网络执行通信处理。驱动器410也根据需要连接至I/O接口405。可拆卸介质411，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器410上，以便于从其上读出的计算机程序根据需要被安装入存储部分408。
特别地，根据本公开的实施方式，上文参考本公开实施方式中的任一方法可以被实现为计算机软件程序。例如，本公开的实施方式包括一种计算机程序产品，其包括有形地包含在及其可读介质上的计算机程序，所述计算机程序包含用于执行本公开实施方式中任一方法的程序代码。在这样的实施方式中，该计算机程序可以通过通信部分409从网络上被下载和安装，和/或从可拆卸介质411被安装。
在一些实施例中，该电子设备400还包括视觉惯性里程计(图中未示出)，该视觉惯性里程计集成有图像采集设备和惯性测量单元。在视觉惯性里程计状态的估计过程中，图像采集设备可以连续采集环境中的图像数据，惯性测量单元可以持续不断地测量得到惯性测量数据。上述图像数据和惯性测量数据可以输出至处理器，并由处理器基于该图像数据和惯性测量数据执行本公开实施例提出的上述视觉惯性里程计的状态估计方法。
附图中的流程图和框图，图示了按照本公开各种实施方式的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，路程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本公开实施方式中所涉及到的单元或模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的单元或模块也可以设置在处理器中，这些单元或模块的名称在某种情况下并不构成对该单元或模块本身的限定。
作为另一方面，本公开还提出了一种增强现实的实现方法，该方法在上述电子设备上实现，并且基于本公开实施例提出的上述视觉惯性里程计的状态估计方法确定该电子设备的位姿和该电子设备周边的三维空间。该电子设备可以是机器人、无人机、自动驾驶车辆等，在利用本公开实施例提出的上述视觉惯性里程计的状态估计方法得到视觉惯性里程计状态的最终估计结果之后，可以基于该视觉惯性里程计状态的最终估计结果确定该电子设备的位姿和周边的三维空间数据。视觉惯性里程计状态包括视觉惯性里程计上集成的图像采集设备的位姿数据和三维空间点状态，根据该图像采集设备的位姿数据以及图像采集设备与电子设备之间相对位姿变化即可确定电子设备的位姿，而基于上述三维空间点的状态即可确定电子设备周边的三维空间。该电子设备的位姿以及周边的三维空间数据课用于实现增强现实方法。
作为另一方面，本公开还提供了一种计算机可读存储介质，该计算机可读存储介质可以是上述实施方式中所述装置中所包含的计算机可读存储介质；也可以是单独存在，未装配入设备中的计算机可读存储介质。计算机可读存储介质存储有一个或者一个以上程序，所述程序被一个或者一个以上的处理器用来执行描述于本公开的方法。
以上描述仅为本公开的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本公开中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离所述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本公开中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。
Claims (10)
1.一种视觉惯性里程计的状态估计方法，其中，包括：
获取视觉惯性里程计集成的图像采集设备采集到的当前图像帧，以及视觉惯性里程计集成的惯性测量单元输出的惯性测量数据；
在所述当前图像帧为关键帧时，将所述当前图像帧添加至基于滑动窗口确定的关键图像帧集合中；
根据所述当前图像帧与上一相邻图像帧之间的所述惯性测量数据，确定所述当前图像帧以及所述上一相邻图像帧之间的第一惯性残差；
基于所述第一惯性残差及所述当前图像帧与所述关键图像帧集合内其他关键图像帧之间的第一视觉残差，更新第一惯性约束条件和第一视觉约束条件；
针对所述滑动窗口内的关键图像帧，通过迭代优化的方式估计所述关键图像帧对应的视觉惯性里程计状态；其中，在每一次迭代过程中，基于边缘化先验约束条件、所述第一视觉约束条件以及所述第一惯性约束条件，确定当前迭代过程相对于前一次迭代过程所述视觉惯性里程计状态的增量变化数据，并基于所述增量变化数据对所述视觉惯性里程计状态进行优化。
2.根据权利要求1所述的方法，其中，基于所述第一惯性残差及所述当前图像帧与所述关键图像帧集合内其他关键图像帧之间的第一视觉残差，更新第一视觉约束条件和第一惯性约束条件，包括：
计算所述当前图像帧与所述滑动窗口内其他关键图像帧之间的所述第一视觉残差；
基于所述第一视觉残差以及所述第一惯性残差分别更新第一视觉约束条件以及第一惯性约束条件。
3.根据权利要求1或2所述的方法，其中，在所述当前图像帧为关键帧时，将所述当前图像帧添加至基于滑动窗口确定的关键图像帧集合中，包括：
确定滑动窗口限定的关键图像帧集合中关键图像帧的数量；
在所述关键图像帧的数量大于或等于第一预设阈值时，基于所述关键图像帧集合内的待剔除关键帧与其他关键图像帧之间的惯性残差和视觉残差，更新所述边缘化先验约束条件，并将所述待剔除关键帧从所述滑动窗口限定的所述关键图像帧集合剔除；
将所述当前图像帧添加至所述滑动窗口限定的所述关键图像帧集合中。
4.根据权利要求1或2所述的方法，其中，在每一次迭代过程中，基于边缘化先验约束条件、所述第一视觉约束条件以及所述第一惯性约束条件，确定当前迭代过程相对于前一次迭代过程所述视觉惯性里程计状态的增量变化数据，并基于所述增量变化数据对所述视觉惯性里程计状态进行优化，包括：
基于边缘化先验约束条件、所述第一视觉约束条件、所述第一惯性约束条件以及所述视觉惯性里程计状态的前一次迭代过程中构建的前次状态优化增量方程，构建所述视觉惯性里程计状态的当前状态优化增量方程；其中，所述当前状态优化增量方程中相对于所述前次状态优化增量方程的增量部分对应于目标状态的增量，所述目标状态包括前一次迭代过程中，对所述视觉惯性里程计状态进行优化前后变化值大于第二预设阈值的所述视觉惯性里程计状态；
对所述当前状态优化增量方程进行求解，获得当前迭代过程中所述视觉惯性里程计状态的优化值。
5.根据权利要求4所述的方法，其中，对所述当前状态优化增量方程进行求解，获得当前迭代过程中所述视觉惯性里程计状态的优化值，包括：
基于所述当前状态优化增量方程构建消去三维空间点状态的舒尔补方程；
对所述舒尔补方程求解获得所述视觉惯性里程计状态中相机位姿状态的增量以及运动状态的增量；
基于所述相机位姿状态的增量以及运动状态的增量求解所述三维空间点的增量。
6.根据权利要求3所述的方法，其中，在所述关键图像帧的数量大于或等于第一预设阈值时，基于所述关键图像帧集合内的待剔除关键帧与其他关键图像帧之间的惯性残差和视觉残差，更新所述边缘化先验约束条件，并将所述待剔除关键帧从所述滑动窗口剔除，包括：
基于第二惯性约束条件、第二视觉约束条件以及前次条件更新增量方程构建中间增量方程；其中，所述前次条件更新增量方程为前次对所述边缘化先验约束条件进行更新的过程中构建的增量方程；所述第二惯性约束条件和所述第二视觉约束条件分别为所述第一惯性约束条件和所述第一视觉约束条件中与所述预设关键帧相关联的约束条件；
通过舒尔补操作从所述中间增量方程去除所述预设关键帧的视觉惯性里程计状态，获得所述边缘化先验约束条件对应的当前条件更新增量方程；
利用所述当前条件更新增量方程更新所述边缘化先验约束条件。
7.根据权利要求1-2、5-6任一项所述的方法，其中，所述方法还包括：
在所述当前图像帧为非关键帧时，确定所述关键图像帧集合内与所述当前图像帧相匹配的至少一个匹配关键帧，以及所述当前图像帧与所述至少一个匹配关键帧之间的匹配特征点对应的三维空间点坐标；
计算所述当前图像帧与所述至少一个匹配关键帧之间的第二视觉残差，以及根据所述当前图像帧与其上一相邻图像帧之间的惯性测量数据计算第二惯性残差；
基于所述第二视觉残差构建非关键帧视觉约束条件，以及根据所述第二惯性残差构建非关键帧惯性约束条件；
基于所述非关键帧视觉约束条件、所述非关键帧惯性约束条件、所述至少一个匹配关键帧对应的所述视频惯性里程计状态的已知值以及所述三维空间点坐标获取所述当前图像帧对应的所述视频惯性里程计状态的目标值。
8.一种视觉惯性里程计的状态估计装置，其中，包括：
第一获取模块，被配置为获取视觉惯性里程计集成的图像采集设备采集到的当前图像帧，以及视觉惯性里程计集成的惯性测量单元输出的惯性测量数据；
第一确定模块，被配置为根据所述当前图像帧与上一相邻图像帧之间的所述惯性测量数据，确定所述当前图像帧以及所述上一相邻图像帧之间的第一惯性残差；
添加模块，被配置为在所述当前图像帧为关键帧时，将所述当前图像帧添加至基于滑动窗口确定的关键图像帧集合中；
更新模块，被配置为基于所述第一惯性残差及所述当前图像帧与所述关键图像帧集合内其他关键图像帧之间的第一视觉残差，更新第一惯性约束条件和第一视觉约束条件；
估计模块，被配置为针对所述滑动窗口内的关键图像帧，通过迭代优化的方式估计所述关键图像帧对应的视觉惯性里程计状态；其中，在每一次迭代过程中，基于边缘化先验约束条件、所述第一视觉约束条件以及所述第一惯性约束条件，确定当前迭代过程相对于前一次迭代过程所述视觉惯性里程计状态的增量变化数据，并基于所述增量变化数据对所述视觉惯性里程计状态进行优化。
9.一种电子设备，其中，包括视觉惯性里程计、存储器和处理器；其中，
所述存储器用于存储一条或多条计算机指令，其中，基于所述视觉惯性里程计输出的图像数据和惯性测量数据，所述一条或多条计算机指令被所述处理器执行以实现权利要求1-7任一项所述的方法。
10.一种在权利要求9所述电子设备上增强现实的实现方法，其中，基于权利要求1-7任一项所述的方法确定所述电子设备的位姿和电子设备周边的三维空间。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110127154.7A CN114812601A (zh) | 2021-01-29 | 2021-01-29 | 视觉惯性里程计的状态估计方法、装置、电子设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110127154.7A CN114812601A (zh) | 2021-01-29 | 2021-01-29 | 视觉惯性里程计的状态估计方法、装置、电子设备 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114812601A true CN114812601A (zh) | 2022-07-29 |
Family
ID=82526567
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110127154.7A Pending CN114812601A (zh) | 2021-01-29 | 2021-01-29 | 视觉惯性里程计的状态估计方法、装置、电子设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114812601A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116772828A (zh) * | 2023-08-24 | 2023-09-19 | 长春工业大学 | 一种基于图优化的多传感器融合定位与建图方法 |
| CN117705107A (zh) * | 2024-02-06 | 2024-03-15 | 电子科技大学 | 基于两阶段稀疏舒尔补的面向视觉惯性定位方法 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
| WO2019157925A1 (zh) * | 2018-02-13 | 2019-08-22 | 视辰信息科技(上海)有限公司 | 视觉惯性里程计的实现方法及系统 |
| CN110763251A (zh) * | 2019-10-18 | 2020-02-07 | 华东交通大学 | 视觉惯性里程计优化的方法及系统 |
| CN111780764A (zh) * | 2020-06-30 | 2020-10-16 | 杭州海康机器人技术有限公司 | 一种基于视觉地图的视觉定位方法、装置 |
| CN111780754A (zh) * | 2020-06-23 | 2020-10-16 | 南京航空航天大学 | 基于稀疏直接法的视觉惯性里程计位姿估计方法 |
| CN111780781A (zh) * | 2020-06-23 | 2020-10-16 | 南京航空航天大学 | 基于滑动窗口优化的模板匹配视觉和惯性组合里程计 |
-
2021
- 2021-01-29 CN CN202110127154.7A patent/CN114812601A/zh active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2019157925A1 (zh) * | 2018-02-13 | 2019-08-22 | 视辰信息科技(上海)有限公司 | 视觉惯性里程计的实现方法及系统 |
| CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
| CN110763251A (zh) * | 2019-10-18 | 2020-02-07 | 华东交通大学 | 视觉惯性里程计优化的方法及系统 |
| CN111780754A (zh) * | 2020-06-23 | 2020-10-16 | 南京航空航天大学 | 基于稀疏直接法的视觉惯性里程计位姿估计方法 |
| CN111780781A (zh) * | 2020-06-23 | 2020-10-16 | 南京航空航天大学 | 基于滑动窗口优化的模板匹配视觉和惯性组合里程计 |
| CN111780764A (zh) * | 2020-06-30 | 2020-10-16 | 杭州海康机器人技术有限公司 | 一种基于视觉地图的视觉定位方法、装置 |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116772828A (zh) * | 2023-08-24 | 2023-09-19 | 长春工业大学 | 一种基于图优化的多传感器融合定位与建图方法 |
| CN116772828B (zh) * | 2023-08-24 | 2023-12-19 | 长春工业大学 | 一种基于图优化的多传感器融合定位与建图方法 |
| CN117705107A (zh) * | 2024-02-06 | 2024-03-15 | 电子科技大学 | 基于两阶段稀疏舒尔补的面向视觉惯性定位方法 |
| CN117705107B (zh) * | 2024-02-06 | 2024-04-16 | 电子科技大学 | 基于两阶段稀疏舒尔补的面向视觉惯性定位方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111811506B (zh) | 视觉/惯性里程计组合导航方法、电子设备及存储介质 | |
| US10247556B2 (en) | Method for processing feature measurements in vision-aided inertial navigation | |
| CN105809687B (zh) | 一种基于图像中边沿点信息的单目视觉测程方法 | |
| CN107341814B (zh) | 基于稀疏直接法的四旋翼无人机单目视觉测程方法 | |
| Panahandeh et al. | Vision-aided inertial navigation based on ground plane feature detection | |
| CN112304307A (zh) | 一种基于多传感器融合的定位方法、装置和存储介质 | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| US20210183100A1 (en) | Data processing method and apparatus | |
| US20130162785A1 (en) | Method and system for fusing data arising from image sensors and from motion or position sensors | |
| CN114013449B (zh) | 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆 | |
| CN112113582A (zh) | 时间同步处理方法、电子设备及存储介质 | |
| CN114693754B (zh) | 一种基于单目视觉惯导融合的无人机自主定位方法与系统 | |
| CN111623773B (zh) | 一种基于鱼眼视觉和惯性测量的目标定位方法及装置 | |
| CN114812601A (zh) | 视觉惯性里程计的状态估计方法、装置、电子设备 | |
| CN114001733A (zh) | 一种基于地图的一致性高效视觉惯性定位算法 | |
| CN114494150A (zh) | 一种基于半直接法的单目视觉里程计的设计方法 | |
| CN112233149A (zh) | 场景流的确定方法及装置、存储介质、电子装置 | |
| CN113327270A (zh) | 视觉惯导方法、装置、设备及计算机可读存储介质 | |
| CN108827287B (zh) | 一种复杂环境下的鲁棒视觉slam系统 | |
| CN113155152B (zh) | 基于李群滤波的相机与惯性传感器空间关系自标定方法 | |
| CN115578417A (zh) | 一种基于特征点深度的单目视觉惯性里程计方法 | |
| CN114705223A (zh) | 多移动智能体在目标跟踪中的惯导误差补偿方法及系统 | |
| CN112037261A (zh) | 一种图像动态特征去除方法及装置 | |
| CN110864685A (zh) | 一种基于松耦合的车辆单目视觉轮式里程计定位方法 | |
| CN112097758A (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 |