CN109785428A - 一种基于多态约束卡尔曼滤波的手持式三维重建方法 - Google Patents

一种基于多态约束卡尔曼滤波的手持式三维重建方法 Download PDF

Info

Publication number
CN109785428A
CN109785428A CN201910053569.7A CN201910053569A CN109785428A CN 109785428 A CN109785428 A CN 109785428A CN 201910053569 A CN201910053569 A CN 201910053569A CN 109785428 A CN109785428 A CN 109785428A
Authority
CN
China
Prior art keywords
polymorphic
state
kalman filter
constrained kalman
constrained
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.)
Granted
Application number
CN201910053569.7A
Other languages
English (en)
Other versions
CN109785428B (zh
Inventor
余雷
李长頔
徐浩楠
付晓凡
方一凡
李光强
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Suzhou University
Original Assignee
Suzhou University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Suzhou University filed Critical Suzhou University
Priority to CN201910053569.7A priority Critical patent/CN109785428B/zh
Publication of CN109785428A publication Critical patent/CN109785428A/zh
Application granted granted Critical
Publication of CN109785428B publication Critical patent/CN109785428B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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

  • Image Analysis (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种一种基于多态约束卡尔曼滤波的手持式三维重建方法，引入基于多态约束卡尔曼滤波算法的紧耦合视觉惯性里程计，并在多态约束卡尔曼滤波算法中观察多帧图像和延迟线性化；所述多态约束卡尔曼滤波算法包括状态发布和状态更新，所述状态发布包括从IMU向量中计算并发布出状态和协方差矩阵，所述状态更新包括状态增强、图像处理、更新和状态管理。本发明能够实现大场景高精度的三维重建。

Description

一种基于多态约束卡尔曼滤波的手持式三维重建方法
技术领域
本发明属于三维采集、重建技术领域，具体涉及一种基于多态约束卡尔曼滤波的手持式三维重建方法，可应用于project Tango设备。
背景技术
随着人工智能时代的到来，在机器人导航、无人驾驶和测绘等场合对于三维重建的应用日渐增多。基于视觉惯性里程计(Visual–Inertial Odometry，VIO)的三维重建***是目前研究的关键问题，而如何将三维重建***应用到运算资源受限的手机设备上也亟待解决。VIO将融合视觉和惯性测量单元(Inertial Measurement Unit，IMU)的信息，利用滤波的方法进行多传感器的数据融合是一种常见的方法。传统的纯视觉的三维重建***存在以下问题：(1)相机抖动或迅速移动情况下，导致帧与帧之间重叠较少难以特征匹配，视觉信息在此过程中失效，无法准确对模型进行三维重建；(2)基于多幅特征单帧图像获取位姿，极易受到目标状态变化干扰，对姿态解算产生误差，建立错位三维重建模型；(3)传统三维重建***算法实时线性化方法导致算法复杂，实时性差，对三维重建设备性能要求较高。
近年来，研究者对以上问题进行许多探索性研究，根据Kaiser的分类方法，VIO分成基于滤波(filter-based)和基于优化(optimization-based)两个大类。基于滤波的视觉惯性里程计，利用最大后验概率估计(Maximum a posteriori estimation,简称MAP)的方法，姿态的先验分布和似然分布分别由内部传感器和外部传感器测量建立。基于优化的视觉惯性里程计，利用最大似然估计(Maximum likelihood estimation,简称MLE)的方法，迭代地找到测量的总概率最高的状态，把批处理过程转化成求解线性代数方程组。根据特征向量中是否包含视觉特征信息，VIO又可以分为松耦合(loosely-coupled)和紧耦合(tightly-coupled)。其中，基于优化的松耦合代表是Falquez提出的惯性辅助方法，该方法将视觉里程计得到的位姿变换融合到惯性测量的优化框架中，但是该方法进行信息融合时，分别处理相机和IMU测量结果不可能对偏差进行最佳估计，导致误差较大。基于优化的紧耦合代表是OKVIS，该方法将惯性测量紧密集成到基于关键帧的视觉***中，获得重力方向的全局一致性和使用IMU运动学运动模型的鲁棒离群值排除，对比纯视觉和松耦合方法，OKVIS表现出更高的精度和鲁棒性。但是OKVIS需要建立以一个统一的损失函数进行联合优化，导致该算法运算复杂，难以在移动设备上进行使用。基于滤波的松耦合方案较为简单，不把图像的特征信息加入状态向量，而是先对视觉里程计处理后再和惯性数据进行融合，其中较为优秀的是Stephen Weiss等人提出的MSF(Multi-Sensor Fusion，多传感器融合)方法。但是，该方法的缺陷是需要进行失败检测，导致算法占用较多的运算资源，不易应用在移动设备上。
发明内容
本发明的发明目的是提供一种基于多态约束卡尔曼滤波的手持式三维重建方法，能够实现大场景高精度的三维重建。
为达到上述发明目的，本发明采用的技术方案是：一种基于多态约束卡尔曼滤波的手持式三维重建方法，引入基于多态约束卡尔曼滤波算法的紧耦合视觉惯性里程计，并在多态约束卡尔曼滤波算法中观察多帧图像和延迟线性化；
所述多态约束卡尔曼滤波算法包括状态发布和状态更新，
所述状态发布包括从IMU向量中计算并发布出状态和协方差矩阵，所述状态更新包括状态增强、图像处理、更新和状态管理。
上述技术方案中，所述状态增强，用当前IMU位置和方向增加状态向量以及对应的协方差矩阵；
所述图像处理，提取角点特征并执行特征匹配；
所述更新，对于轨迹已完成的每个特征点，计算残差向量，并执行马氏距离测试，使用所有通过测试的特征点进行更新；
所述状态管理，从状态向量中删除所有特征点计算已经完成的IMU状态。
上述技术方案中，所述多态约束卡尔曼滤波算法中利用滑动窗口方法进行观察多帧图像和延迟线性化。
由于上述技术方案运用，本发明与现有技术相比具有下列优点：
1.本发明通过引入多态约束卡尔曼滤波的紧耦合视觉惯性里程计，克服了相机抖动、建模场景特征点少和状态变化等带来的问题，并在多态约束卡尔曼滤波算法中，创新性的观察多帧图像和延迟线性化，提高三维重建精准度和效率；
2.本发明在KITTI和Starry Night数据集上进行了实验，有效验证本发明三维运动跟踪的精准度，且在project Tango手机上进行不同场景的三维重建实验，通过与其他***进行比较和分析，验证了本发明可以实现大场景高精度的三维重建，优于目前已有***。
附图说明
图1是本发明实施例一的多态约束卡尔曼滤波算法的结构示意图。
图2是本发明实施例一的Starry Night数据集测试结果示意图。
图3是本发明实施例一的Starry Night数据集测试分析示意图。
图4～12是本发明实施例一的三维重建效果对比图。
具体实施方式
下面结合附图及实施例对本发明作进一步描述：
实施例一：
参见图1所示，一种基于多态约束卡尔曼滤波的手持式三维重建方法，引入基于多态约束卡尔曼滤波算法的紧耦合视觉惯性里程计，并在多态约束卡尔曼滤波算法中观察多帧图像和延迟线性化；
所述多态约束卡尔曼滤波算法包括状态发布和状态更新，
所述状态发布包括从IMU向量中计算并发布出状态和协方差矩阵，所述状态更新包括状态增强、图像处理、更新和状态管理。
本实施例中，所述状态增强，用当前IMU位置和方向增加状态向量以及对应的协方差矩阵；
所述图像处理，提取角点特征并执行特征匹配；
所述更新，对于轨迹已完成的每个特征点，计算残差向量，并执行马氏距离测试，使用所有通过测试的特征点进行更新；
所述状态管理，从状态向量中删除所有特征点计算已经完成的IMU状态。
本实施例中，所述多态约束卡尔曼滤波算法中利用滑动窗口方法进行观察多帧图像和延迟线性化。
具体地，本发明构建了一个联合VO和IMU的VIO***，将一个坐标系{I}附加到IMU，并跟踪这个帧相对于全局帧的运动。
遵循标准惯例，时间l处的IMU状态向量被定义为16×1向量，其中是单位四元数，表示在时间l处从全局帧到IMU帧的旋转，Gpl和Gvl分别是全局帧中的IMU位置和速度，而bgl和bal分别是陀螺仪和加速度计偏差。
如公式(1)所示：
使用位置、速度和偏差的标准增量误差定义IMU偏差状态。采用四元数来描述真实和估计方向之间的差异，定义方位误差在如公式(2)所示：
其中是四元数乘法。直观地说，是使估计的全局帧与真实帧匹配所需的旋转。为了获得这种旋转的最小表示，我们注意到可以写成：
其中是描述关于三个轴的方向误差的3×1向量。通过上述误差定义，IMU错误状态被定义为15×1向量：
本发明的MSCKF是一种基于EKF的改进算法，其状态向量中维持了基于滑动窗口的姿态，并使用特征观察在这些姿态之间施加概率约束。时间l处的MSCKF的状态向量被定义为：
其中，是对于时间i＝l-N,L,l-1处，记录的最后N个图像时的IMU姿态。
在MSCKF算法过程中，IMU状态测量被用于IMU状态估计和滤波协方差矩阵计算。当摄像机记录新图像时，MSCKF状态和协方差就会增加当前IMU姿态的副本，并对图像进行处理以提取和匹配特征。跟踪每个特征直到其所有测量值变得可用，此时使用所有测量同时进行更新。
在更新方程中，时间l处的更新使用MSCKF状态向量中记录的N个图像观察到的特征fi的情况。该过程的第一步是获取特征位置的估计值，因此使用所有特征的测量值通过Gauss-Newton最小化来估计其位置。最后，在公式(6)中计算j＝l-N,...,l-1时的残差：
其中和分别是第j个姿态的当前估计误差和特征位置误差；矩阵和是相应的雅可比矩阵，其由和评估得到。因为由j＝l-N,...,l-1的和zij算出，在公式(7)中的残差的特征位置误差与和nij都相关。因此，在MSCKF中，继续从残差方程中去除首先，建立包含来自所有特征测量的N个残差的矢量：
其中，ri和ni分别是元素rij和nij的块向量，和是块行和的矩阵。随后，定义残差向量其中Vi是一个矩阵，其列构成的左侧零空间的基础。由公式(9)可得：
其中，且无在公式(10)中残差向量ri o与特征坐标中的误差无关。为了提高算法效率，ri o和在没有明确形成Vi的情况下即进行计算。一旦计算出ri o和就对残差ri o进行Mahalanobis门控测试。具体来说，计算
并将其与由具有2N-3自由度的χ2分布的第95百分位给出的阈值进行比较(2N-3是残差向量ri o中的元素的数量)。
如果该功能通过测试，即继续使用ri o及通过门控测试的所有其他功能的残差进行更新。此更新发生后，即从滑动窗口中删除那些观察到的已用于完成更新的姿态。
本发明利用滑动窗口进行约束和延迟线性化的方法，可以使基于MSCKF的三维重建***数据融合更加精准；状态管理及时删去冗余信息，大量节约运存和内存，使基于MSCKF的三维重建***更加适用于计算资源有限的移动设备。
为了验证本发明算法的有效性，本发明分别设置了在KITTI和Starry Night数据集上对本发明算法进行仿真测试和在project Tango平台上进行三维重建实验评估。在KITTI数据集与Starry Night数据集实验中，实验硬件配置为内存8G的Intel Core i7-4790 CPU的台式电脑，使用的软件环境是ubuntu 14.04下的MATLAB R2017a进行实验。在三维重建实验中，源码开发环境是在ubuntu 16.04的Eclipse 3.8.1中进行，之后打包成48.6M的APK文件安装运行于project Tango设备上。
首先搭建基于project Tango设备的实验平台，本发明采用搭载project Tango平台的联想PHB2手机作为实验平台。联想PHB2手机采用8核1.8GHz高通骁龙652处理器，4G运存，64G内存，安卓版本为6.0.1，Tango版本为1.2。该手机有包含宽视场鱼眼相机、深度相机和彩色相机。鱼眼相机视角接近180度，采用全局快门工作方式。彩色摄像头在本发明***中并不用于运动追踪，而用于给三维重建模型渲染色彩。深度摄像头基于红外结构光，采集到的深度图像分辨率是QVGA(Quarter Video Graphics Array)，稠密程度和质量都类似于Kinect。除此之外，该手机还具有低功耗的的惯性测量单元，由3轴加速计和3轴陀螺仪组成。该***基于哈希结构(hash)的密集排列方式，利用改进的TSDF算法，可以在有限的计算资源下完成实时三维重建而不用GPU加速。其中，***的核心在于基于MSCKF的视觉惯性里程计。
为了有效验证基于MSCKF算法的***的有效性，选择了Starry Night数据集和KITTI数据集对本发明基于MSCKF算法的***进行测试，两个数据集都包含了双目立体相机和IMU信息，由于本发明***被设计在project Tango手机上使用，在VIO中仅使用鱼眼相机和IMU，所以使用数据采集的左侧相机图像和IMU信息。
参见图2所示，是在Starry Night数据集下不同特征点数量的基于MSCKF算法的本发明***和未加入MSCKF融合视觉信息的***对比示意图。在特征点数量为40、60、80和100的场景下，分析了本发明***和未加入MSCKF的***的旋转均方根误差和位移均方根误差。可以看出在不同特征点情况下，本发明***的旋转均方根误差和位移均方根误差都远小于未加入MSCKF算法的***。
参见图3所示，计算出在1415到1715的时间段内两***的平均旋转均方根误差和平均位移均方根误差。可以看出本发明***在特征点更加密集的情形下，***的平均旋转平方根误差和平均位移平方根误差更小。对比未加入MSCKF的***，本发明***的平均旋转平方根误差和平均位移平方根误差更小，显示出***计算位姿更加精准，对比现有的***，本发明***在各种特征点密度下都有较小的误差，具有较强的鲁棒性和稳定性。在进行三维重建时，可以有效避免漂移错位等现象，模型的准确度也会更加高。
为了有效验证本发明***在移动设备上的稳定性和优势，选取Open Constructor和RTAB-Map两款可应用于Project Tango设备上的***作为对比。图4、图7和图10为本发明***的三维重建效果图，图5、图8和图11为RTAB-Map***的三维重建效果图，图6、图9和图12为Open Constructor***的三维重建效果图。从图中可以看出，RTAB-Map***在面对玻璃、白墙等视觉难以匹配的帧时，极易出现闭环失败的情况，从而导致位姿漂移，模型错位；Open Constructor***中，在相机抖动情况下，无法较好处理位姿变化，导致建立出的模型极易扭曲变形，且局部描绘严重失真。相比以上两种***，可以看出在本发明***中，基于MSCKF的VIO对位姿求解精准，可以较好的应对特征点稀疏的情况。在局部抖动，迅速旋转相机和上下楼情况下，本发明***对位姿求解精准，仍然可以建立效果较好的三维模型。
在表1中，从以上各三维重建模型中对应选取20个特征线段进行误差分析，将特征线段的真实值与测量值进行比较，计算误差率。其中，图5实验误差过大，没有建立出相应的三维模型。本发明***误差远小于RTAB-Map和Open Constructor，在建模及优化时间方面，在总体建模优化时间较短的情况下，达到了较低的误差率，提高三维模型的准确性和效率。
表1实验结果对比
通过以上实验可以证明，本发明提出的基于MSCKF的三维***轨迹跟踪能力较强，三维位姿计算精准，稳定性强。在project Tango手机上的各种场景下的三维重建实验以及对比同类***，可以看出本发明***三维重建效果优于同类***，且适合大场景高精度三维重建。

Claims (3)

1.一种基于多态约束卡尔曼滤波的手持式三维重建方法，其特征在于：引入基于多态约束卡尔曼滤波算法的紧耦合视觉惯性里程计，并在多态约束卡尔曼滤波算法中观察多帧图像和延迟线性化；
所述多态约束卡尔曼滤波算法包括状态发布和状态更新，
所述状态发布包括从IMU向量中计算并发布出状态和协方差矩阵，所述状态更新包括状态增强、图像处理、更新和状态管理。
2.根据权利要求1所述的基于多态约束卡尔曼滤波的手持式三维重建方法，其特征在于：所述状态增强，用当前IMU位置和方向增加状态向量以及对应的协方差矩阵；
所述图像处理，提取角点特征并执行特征匹配；
所述更新，对于轨迹已完成的每个特征点，计算残差向量，并执行马氏距离测试，使用所有通过测试的特征点进行更新；
所述状态管理，从状态向量中删除所有特征点计算已经完成的IMU状态。
3.根据权利要求1所述的基于多态约束卡尔曼滤波的手持式三维重建方法，其特征在于：所述多态约束卡尔曼滤波算法中利用滑动窗口方法进行观察多帧图像和延迟线性化。
CN201910053569.7A 2019-01-21 2019-01-21 一种基于多态约束卡尔曼滤波的手持式三维重建方法 Active CN109785428B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910053569.7A CN109785428B (zh) 2019-01-21 2019-01-21 一种基于多态约束卡尔曼滤波的手持式三维重建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910053569.7A CN109785428B (zh) 2019-01-21 2019-01-21 一种基于多态约束卡尔曼滤波的手持式三维重建方法

Publications (2)

Publication Number Publication Date
CN109785428A true CN109785428A (zh) 2019-05-21
CN109785428B CN109785428B (zh) 2023-06-23

Family

ID=66500961

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910053569.7A Active CN109785428B (zh) 2019-01-21 2019-01-21 一种基于多态约束卡尔曼滤波的手持式三维重建方法

Country Status (1)

Country Link
CN (1) CN109785428B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110262521A (zh) * 2019-07-24 2019-09-20 北京智行者科技有限公司 一种自动驾驶控制方法
CN110340887A (zh) * 2019-06-12 2019-10-18 西安交通大学 一种基于图像的加油机器人视觉引导的方法
CN113674412A (zh) * 2021-08-12 2021-11-19 浙江工商大学 基于位姿融合优化的室内地图构建方法、***及存储介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107656545A (zh) * 2017-09-12 2018-02-02 武汉大学 一种面向无人机野外搜救的自主避障与导航方法
CN108629793A (zh) * 2018-03-22 2018-10-09 中国科学院自动化研究所 使用在线时间标定的视觉惯性测程法与设备
CN109029448A (zh) * 2018-06-28 2018-12-18 东南大学 单目视觉惯性定位的imu辅助跟踪模型

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107656545A (zh) * 2017-09-12 2018-02-02 武汉大学 一种面向无人机野外搜救的自主避障与导航方法
CN108629793A (zh) * 2018-03-22 2018-10-09 中国科学院自动化研究所 使用在线时间标定的视觉惯性测程法与设备
CN109029448A (zh) * 2018-06-28 2018-12-18 东南大学 单目视觉惯性定位的imu辅助跟踪模型

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
ANASTASIOS I. MOURIKIS等: "A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation", 《2007 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION》 *
TONG QIN等: "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator", 《IEEE TRANSACTIONS ON ROBOTICS》 *
万众: "微小型飞行器立体视觉避障与自主导航关键技术研究", 《万方》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110340887A (zh) * 2019-06-12 2019-10-18 西安交通大学 一种基于图像的加油机器人视觉引导的方法
CN110262521A (zh) * 2019-07-24 2019-09-20 北京智行者科技有限公司 一种自动驾驶控制方法
CN113674412A (zh) * 2021-08-12 2021-11-19 浙江工商大学 基于位姿融合优化的室内地图构建方法、***及存储介质
CN113674412B (zh) * 2021-08-12 2023-08-29 浙江工商大学 基于位姿融合优化的室内地图构建方法、***及存储介质

Also Published As

Publication number Publication date
CN109785428B (zh) 2023-06-23

Similar Documents

Publication Publication Date Title
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
Qin et al. Vins-mono: A robust and versatile monocular visual-inertial state estimator
CN109993113B (zh) 一种基于rgb-d和imu信息融合的位姿估计方法
CN111156984B (zh) 一种面向动态场景的单目视觉惯性slam方法
CN108062776B (zh) 相机姿态跟踪方法和装置
Newcombe et al. Kinectfusion: Real-time dense surface mapping and tracking
CN104658012B (zh) 一种基于惯性与光学测量融合的运动捕捉方法
CN109166149A (zh) 一种融合双目相机与imu的定位与三维线框结构重建方法与***
US20220051031A1 (en) Moving object tracking method and apparatus
CN113052908B (zh) 一种基于多传感器数据融合的移动机器人位姿估计算法
CN109540126A (zh) 一种基于光流法的惯性视觉组合导航方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN111862316B (zh) 一种基于优化的imu紧耦合稠密直接rgbd的三维重建方法
JP6483832B2 (ja) Ｒｇｂ−ｄセンサを使用して物体を走査する方法とシステム
CN112556719B (zh) 一种基于cnn-ekf的视觉惯性里程计实现方法
Li et al. Large-scale, real-time 3D scene reconstruction using visual and IMU sensors
CN112815939A (zh) 移动机器人的位姿估计方法及计算机可读存储介质
CN109785428A (zh) 一种基于多态约束卡尔曼滤波的手持式三维重建方法
CN115371665B (zh) 一种基于深度相机和惯性融合的移动机器人定位方法
CN104848861A (zh) 一种基于图像消失点识别技术的移动设备姿态测量方法
CN114485640A (zh) 基于点线特征的单目视觉惯性同步定位与建图方法及***
CN111595332B (zh) 一种融合惯性技术与视觉建模的全环境定位方法
CN116619358A (zh) 一种矿山自主探测机器人自适应定位优化与建图方法
Huttunen et al. A monocular camera gyroscope
Ventura et al. P1ac: Revisiting absolute pose from a single affine correspondence

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant