CN108629793A - 使用在线时间标定的视觉惯性测程法与设备 - Google Patents
使用在线时间标定的视觉惯性测程法与设备 Download PDFInfo
- Publication number
- CN108629793A CN108629793A CN201810239278.2A CN201810239278A CN108629793A CN 108629793 A CN108629793 A CN 108629793A CN 201810239278 A CN201810239278 A CN 201810239278A CN 108629793 A CN108629793 A CN 108629793A
- Authority
- CN
- China
- Prior art keywords
- image
- imu
- camera
- time
- frame
- 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
Links
- 238000000034 method Methods 0.000 claims abstract description 73
- 238000005457 optimization Methods 0.000 claims abstract description 38
- 230000010354 integration Effects 0.000 claims abstract description 19
- 238000005259 measurement Methods 0.000 claims description 85
- 230000000007 visual effect Effects 0.000 claims description 46
- 230000006870 function Effects 0.000 claims description 43
- 230000036544 posture Effects 0.000 claims description 27
- 239000013598 vector Substances 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 11
- 230000001934 delay Effects 0.000 claims description 9
- 238000007429 general method Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 4
- 238000012935 Averaging Methods 0.000 claims description 3
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 238000012216 screening Methods 0.000 claims description 3
- 241000470001 Delaya Species 0.000 claims description 2
- 238000012937 correction Methods 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 4
- 230000000052 comparative effect Effects 0.000 description 2
- 238000009616 inductively coupled plasma Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000003190 augmentative effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000003111 delayed effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
- 230000036962 time dependent Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
-
- 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
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
-
- 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/10004—Still image; Photographic image
Landscapes
- Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Multimedia (AREA)
- Studio Devices (AREA)
Abstract
本发明涉及多传感器时间标定算法领域，具体涉及一种使用在线时间标定的视觉惯性测程法与设备，目的在于提高相机位姿计算的精度。本发明的视觉惯性测程法包括：将相机图片的时间戳与IMU的时间系统粗略对齐，并把相机和IMU的数据作为输入；把由相机数据和IMU数据分别计算出的相机姿态最接近的时间延迟作为相机时间延迟的初步估计值；采用带有时间延迟参数的IMU误差项表示方法，在IMU的预积分结果中添加与时间延迟有关的修正项，将其作为非线性优化的参数，实现时间延迟的精确标定。本方法对时间延迟的标定比现有的方法更准确，对于未经硬件同步的设备，在设备快速转弯和突然停止的时候有更好的性能。
Description
技术领域
本发明涉及多传感器时间标定算法领域，具体涉及一种使用在线时间标定的视觉惯性测程法与设备。
背景技术
使用相机和IMU(惯性测量单元)的视觉惯性测程法近年来取得了很好的效果，因为视觉和IMU的信息有互补的特点。早期有MSCKF算法使用扩展卡尔曼滤波(EKF)实现相机跟踪，随后出现了使用非线性优化的OKVIS和VINS，以及ORB-SLAM算法的使用视觉和IMU信息的版本。这些算法被广泛地用于微型飞行器定位和增强现实。这些算法都假设相机和IMU的时间戳是同步的，但是在安卓手机上相机和IMU并没有做到硬件同步，相机的时间戳相对于它的曝光时间有时间延迟。因为这个时间延迟在每次程序运行的时候都不一样，所以无法使用离线的时间标定算法如Kalibr工具包进行时间标定。
现有的相机和IMU之间的可以实现时间标定的方法主要有以下四种：
(1)首先使用相机和IMU的数据分别计算相机和IMU的姿态，然后通过计算相关系数或相位一致性，或使用ICP算法对齐；
(2)使用非线性优化的离线标定；
(3)使用EKF算法的在线标定；
(4)使用非线性优化的在线标定。
其中，第(1)种方法因为分成两步计算，所以在对齐的时候无法利用相机和IMU姿态的精度信息，因此，一个被错误估计的姿态会对时间标定结果产生较大影响；第(2)种方法无法实现在线计算；第(3)种方法是基于EKF算法的，无法用于OKVIS、VINS、ORB-SLAM等使用非线性优化的VIO(Visual–Inertial Odometry，视觉惯性里程计)算法。
VINS最新更新的开源代码是在此之前唯一公开的使用非线性优化的在线时间标定方法。这种方法用连续两帧图像上特征点坐标的差除以两帧之间的时间间隔计算出特征点在图像上移动的速度，再用它乘以时间延迟作为特征点坐标的改正量加到特征点坐标的观测值上。使用这种方法，时间延迟就成了非线性优化的参数之一。然而，图像采集的频率显著低于IMU的量测值的频率，因此，使用上述方法计算出的特征点在图像上移动的速度也有较大的误差。
发明内容
为了解决现有技术中的上述问题，本发明提出了一种使用在线时间标定的视觉惯性测程法与设备，提高了相机位姿计算的精度。
本发明的一方面，提出一种使用在线时间标定的视觉惯性测程法，包括以下步骤：
步骤S1000，实时获取相机图像和IMU量测值，并根据IMU时间基准，调整图像的曝光时刻时间戳；将调整时间戳后的所述相机图像和IMU量测值加入滑动窗口；
步骤S3000，当新的图像进入滑动窗口时，根据所述新的图像和上一帧图像的曝光时刻时间戳以及上一帧图像对应的时间延迟，调整对IMU量测值进行预积分的时间区间，进而对IMU量测值进行预积分；计算包含重投影误差项、IMU误差项和边缘化误差项的代价函数，将所述时间延迟与相机位置、速度和姿态一起进行迭代优化，从而得到所述新的图像对应的所述时间延迟，以及相机位置、速度和姿态；
其中，
所述IMU误差项的残差向量中包含与所述时间延迟有关的变量。
优选地，步骤S1000具体包括：
步骤S1001，使用camera2API实时获取相机图像，并获取相机时间基准下所述相机图像的曝光时刻时间戳；根据IMU时间基准，调整所述相机图像的曝光时刻时间戳；实时获取IMU量测值；
步骤S1002，将调整时间戳后的所述相机图像，以及获取的IMU量测值加入滑动窗口；
其中，
所述IMU量测值的获取频率高于所述相机图像的获取频率；
所述相机图像的曝光时刻时间戳根据下式调整：
表示调整后的第i帧所述相机图像的曝光时刻时间戳；表示在IMU时间基准下第1帧图像对应的回调函数执行时刻的时间戳；和分别表示在相机时间基准下第i帧和第1帧图像的曝光时刻时间戳。
优选地，所述获取相机时间基准下所述相机图像的曝光时刻时间戳的方法为：
通过onImageAvailable()函数获取Image类型的对象，调用该对象的成员函数getTimestamp()获取相机时间基准下所述相机图像的曝光时刻时间戳；
所述回调函数执行时刻的时间戳的获取方法为：
使用SystemClock类的elapsedRealtime()函数获取IMU时间基准下所述回调函数执行时刻时间戳。
优选地，步骤S3000具体包括：
步骤S3001，若有一帧新的图像进入滑动窗口，则判断该图像加入窗口前窗口内最后一帧图像是否为关键帧；若其不是关键帧则将其丢弃，否则最前面一帧旧的图像滑出窗口，最新进入滑动窗口的图像的帧序号为K；如果最前面一帧旧的图像滑出窗口，那么其余各帧序号均减1，否则其余各帧序号不变；对时间戳位于和之间的IMU量测值进行预积分，得到和
其中，TK、TK-1分别为滑动窗口中最新图像和倒数第二帧图像的曝光时刻时间戳，均为经过步骤S1000调整后的值；为所述倒数第二帧图像的时间延迟，初值为0；
步骤S3002，按照基于非线性优化的VIO算法的通用做法在局部窗口中构造代价函数，使用Dogleg算法进行非线性优化求解，得到时间延迟和相机位置、速度、姿态的估计值；
其中，
所述代价函数中包括重投影误差项、IMU误差项和边缘化误差项；
所述重投影误差项和所述边缘化误差项使用基于非线性优化的VIO算法的通用做法进行构造；
所述IMU误差项使用下述方法进行构造：
其中，CIMU(x)表示IMU误差项；x由x1,x2,…,xk组成；k表示滑动窗口内图像的帧序号；xk是第k帧的状态向量，表示协方差矩阵，通过IMU预积分计算出；表示残差向量，与第k帧和第k+1帧的状态向量以及它们之间的IMU预积分有关：
为待估计的所述时间延迟，和分别为世界坐标系下第k帧图像曝光时刻相机的位置和速度；和分别为世界坐标系下第k+1帧曝光时刻相机的位置和速度；和分别为在第k帧IMU本体坐标系下加速度计和陀螺仪的偏移；和分别为在第k+1帧IMU本体坐标系下加速度计和陀螺仪的偏移；为用四元数表示的在第k帧曝光时刻从世界坐标系到IMU本体坐标系的旋转；为的旋转矩阵形式；为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；gw为世界坐标系中的重力加速度，是一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值，Δtk＝Tk+1-Tk；和的计算方法为：
表示在对当前滑动窗口内第k帧和第k+1帧之间的IMU量测值预积分的时候对时间偏移的估计值；ab,k和ae,k的计算方法分别为其中和分别为和时刻IMU量测值中的加速度；和分别为IMU第k帧和第k+1帧图像之间IMU预积分的结果；和分别表示为：
和分别为和时刻IMU量测值中的角速度；和是四元数和对应的旋转矩阵；
和均为待优化的参数；
步骤S3003，将非线性优化求解得到的所述时间延迟作为当前滑动窗口中最新图像对应的时间延迟的估计值：
所述时间延迟的估计值在下一帧图像进入滑动窗口后，将对应于步骤S3001中的值。
优选地，所述视觉惯性测程法还包括一次性执行：
步骤S2000，分别根据所述相机图像和所述IMU量测值计算相机的姿态；计算出使得上述两组姿态偏差最小的时间延迟，作为时间延迟的初步估计值；
相应地，将步骤S2000计算得到的所述时间延迟的初步估计值作为步骤S3001中所述倒数第二帧图像的时间延迟的初值。
优选地，步骤S2000具体包括：
步骤S2001，检测是否有新的IMU量测值加入滑动窗口，若是，则转至步骤S2003；否则，转至步骤S2002；
步骤S2002，检测是否有新的一帧图像加入滑动窗口，若是，则转至步骤S2004；否则，转至步骤S2001；
步骤S2003，计算转角变化量Δq(t)并存入队列，更新非线性优化的滑动窗口中的所述累计转角q；转至步骤S2005；
其中，
q为用四元数表示的所述累计转角，是队列中全部所述转角变化量的累计值；为IMU量测值的角速度；Δt为IMU相邻两次观测的时间差；表示四元数的乘法；t为当前最新IMU量测值的时间戳；
步骤S2004，若有新的一帧图像加入滑动窗口，则判断该图像加入窗口前窗口内最后一帧图像是否为关键帧；若其不是关键帧则将其丢弃，否则窗口内最前面一帧旧的图像滑出窗口；计算滑动窗口中每一帧图像对应的第一个和最后一个IMU量测值中角速度差的模长的平均值用该平均值来衡量角速度变化的情况；
若滑出窗口的是最前面的一帧图像，则对当前队列中每个所述转角变化量分别判断该转角变化量对应的时间戳是否小于该滑出图像的时间戳；若是，则将该转角变化量出队列，并更新所述累计转角：
其中，Δq(t′)为根据时间戳为t′的IMU量测值计算出的所述转角变化量；t′小于该滑出图像的时间戳；
步骤S2005，计算所述累计转角q对应的沿最近转轴转动的角度θ(q)：
其中，qx，qy，qz和qw表示四元数q的四个分量；
步骤S2006，若θ1<θ(qa)<θ2且或则转至步骤S2007；否则，转至步骤S2001；
其中，阈值θ1和θ2用来筛选出具有较好几何结构的场景；阈值ω1和ω2用来平衡时间延迟估计的及时性和可靠性，若要提高及时性则减小ω1和ω2，若要提高可靠性则增大ω1和ω2；
步骤S2007，在应用本方法的VIO算法运行的过程中，滑动窗口中的图像特征点和IMU信息会共同参与优化得到滑动窗口中的相机位姿，将这个位姿作为本步骤中仅使用图像特征点的捆绑调整的初值。只使用图像上的标准化的特征点坐标作为观测值，对窗口内相机的K个位置和姿态进行捆绑调整；
步骤S2008，若捆绑调整失败，则继续转至步骤S2001；若成功，则转至步骤S2009；
步骤S2009，利用捆绑调整后的相机旋转计算两帧之间相机的相对旋转：
其中，和分别为通过捆绑调整得到的第k帧和第k+1帧相机坐标系到世界坐标系的旋转四元数，为从第k+1帧相机坐标系到第k帧相机坐标系的旋转四元数；
步骤S2010，将时间延迟在它的取值区间内进行离散采样；对于每一个采样得到的td，对于滑动窗口中每两帧相邻的图像，通过时间戳介于Tk-td和Tk+1-td之间的IMU量测值中的角速度推算相邻两帧之间IMU本体坐标系间的旋转四元数：
其中，Tk和Tk+1分别为相机第k帧和第k+1帧图像的时间戳；t为IMU量测值的时间戳；表示时间戳为t的IMU量测值中的角速度；П表示四元数的连乘；
步骤S2011，结合已知的相机和IMU坐标系间的旋转四元数计算相邻两帧之间相机坐标系间的旋转四元数：
步骤S2012，对于不同的时间延迟td，计算由相机信息和IMU信息获得的旋转四元数的差，并转换成旋转角度，在窗口内所有的相邻两帧之间取平均值，找到旋转角度平均值最小的td：
若
则记录一个时间延迟的估计值
其中，K为滑动窗口中包含的帧的数目；
步骤S2013，判断是否记录了预设个数的所述时间延迟的估计值若是，则对所述预设个数的所述时间延迟的估计值求中值，作为最终得到的时间延迟的初步估计值；否则，转至步骤S2001。
优选地，步骤S2007中“只使用图像上的标准化的特征点坐标作为观测值，对窗口内相机的K个位置和姿态进行捆绑调整”，具体为：
使用Ceres Solver库进行优化，使用huber范数作为损失函数，各个特征点的误差相互独立，每个特征点观测值的协方差矩阵均为：
其中，fx和fy分别为以x方向和y方向像素个数表示的相机焦距；
若捆绑调整完成后代价函数值小于预设的代价函数阈值，则认为捆绑调整成功，否则认为捆绑调整失败。
本发明的第二方面，提出一种存储设备，其存储有程序，所述程序适于由处理器加载并执行，以实现上面所述的使用在线时间标定的视觉惯性测程法。
本发明的第三方面，提出一种处理设备，包括：处理器和存储器；
所述处理器，适于执行程序；
所述存储器，适于存储该程序；
所述程序适于由所述处理器加载并执行，以实现上面所述的使用在线时间标定的视觉惯性测程法。
本发明的第四方面，提出一种使用在线时间标定的视觉惯性测程设备，包括：图像与IMU量测值获取模块、时间延迟与相机姿态计算模块；
所述图像与IMU量测值获取模块，配置为：实时获取相机图像和IMU量测值，并根据IMU时间基准，调整图像的曝光时刻时间戳；将调整时间戳后的所述相机图像和IMU量测值加入滑动窗口；
所述时间延迟与相机姿态计算模块，配置为：当新的图像进入滑动窗口时，根据所述新的图像和上一帧图像的曝光时刻时间戳以及上一帧图像对应的时间延迟，调整对IMU量测值进行预积分的时间区间，进而对IMU量测值进行预积分；计算包含重投影误差项、IMU误差项和边缘化误差项的代价函数，将所述时间延迟与相机位置、速度和姿态一起进行迭代优化，从而得到所述新的图像对应的所述时间延迟，以及相机位置、速度和姿态；
其中，所述IMU误差项的残差向量中包含与所述时间延迟有关的变量。
优选地，所述视觉惯性测程设备还包括：时间延迟初值估计模块；
所述时间延迟初值估计模块，包括：分别根据所述相机图像和所述IMU量测值计算相机的姿态；计算出使得上述两组姿态偏差最小的时间延迟，作为所述时间延迟的初值。
本发明的有益效果：
本发明所提出的使用在线时间标定的视觉惯性测程法，能够在对时间延迟没有任何先验知识的情况下，在基于非线性优化的视觉惯性测程法运行的同时估计出时间延迟，同时调整IMU的预积分时间范围，使IMU的预积分结果与图像数据在时间上保持一致。在EuRoc MAV数据集上的测试结果显示，与已有的方法相比，本方法对时间延迟的估计更准确，而且当时间延迟存在时，使用本方法的VIO算法得到的轨迹更准确。当不存在时间延迟时，本方法也不会对VIO算法得到的轨迹产生明显的负面影响。在安卓手机采集的数据上，本方法相比已有的方法能更快地得到时间延迟的较准确的估计值，使得相机图像和IMU数据的对应更准确，因此在设备快速转弯或突然停止的时候使用本方法的VIO算法有更好的表现。
附图说明
图1是本发明的使用在线时间标定的视觉惯性测程法实施例一的流程示意图；
图2是本发明的使用在线时间标定的视觉惯性测程法实施例二的流程示意图；
图3(a)-(b)是视觉惯性测程法实施例一和视觉惯性测程法实施例二中时间延迟标定值随时间变化的比较图；
其中，图3(a)表示在EuRoc MAV数据集的MH_01_easy数据上人工添加30ms的时间延迟的情况下对时间延迟的标定，图3(b)表示在安卓手机采集的数据上对时间延迟的标定；
图4(a)-(d)是本发明视觉惯性测程法实施例一和现有的时间标定方法在EuRocMAV数据集上时间标定误差随时间变化的比较图；
其中图4(a)、(b)、(c)、(d)分别表示在数据集上将图像的时间戳人工添加60ms、30ms、0ms、-30ms的时间延迟后时间标定的误差；
图5(a)-(b)是使用本发明视觉惯性测程法实施例一的VIO算法、使用现有的时间标定方法的VIO算法以及不进行时间标定的VIO算法在安卓手机采集的含有快速转弯和突然停止的数据上的轨迹比较图；
其中，图5(a)为含有相机突然停止的数据时对应的轨迹曲线；和快速转弯的数据；图5(b)为含有相机快速转弯的数据时对应的轨迹曲线；
图6是本发明的使用在线时间标定的视觉惯性测程设备实施例一的构成示意图；
图7是本发明的使用在线时间标定的视觉惯性测程设备实施例二的构成示意图。
具体实施方式
下面参照附图来描述本发明的优选实施方式。本领域技术人员应当理解的是，这些实施方式仅用于解释本发明的技术原理，并非旨在限制本发明的保护范围。
本发明提出了一种使用在线时间标定的视觉惯性测程法，通过对相机和IMU系统的时间延迟的在线精确标定，有效提高了相机跟踪的准确度。本发明用于在基于非线性优化的VIO算法例如VINS中实现时间延迟的在线标定；这些算法首先使用IMU量测值进行预积分，然后在局部滑动窗口内，通过对代价函数进行非线性优化得到相机的实时位姿；代价函数中包含重投影误差项，IMU误差项和边缘化误差项，其中IMU误差项由IMU预积分的结果和待优化参数构成。
本专利为了使时间标定的结果更精确，在IMU预积分结果中添加与时间延迟有关的项，而不在特征点坐标上添加。在测试数据集上，本算法有更高的时间标定精度。
图1是本发明的使用在线时间标定的视觉惯性测程法实施例一的流程示意图。如图1所示，本实施例的视觉惯性测程法包括以下步骤：
在步骤S1000中，进行数据采集与时间粗略对齐：实时获取相机图像和IMU量测值，并根据IMU时间基准，调整图像的曝光时刻时间戳，使其与IMU量测值数据大致位于同一时间基准下；将调整时间戳后的所述相机图像和IMU量测值作为输入。该步骤可以具体包括步骤S1001-S1002：
在步骤S1001中，在安卓系统上使用camera2API实时获取相机图像，并获取相机时间基准下所述相机图像的曝光时刻时间戳；然后用回调函数第一次的调用时刻代替相机第一次曝光时刻，根据IMU时间基准，调整所述相机图像的曝光时刻时间戳；另外，还要实时获取IMU量测值。
其中，所述IMU量测值的获取频率高于所述相机图像的获取频率。调整相机图像的曝光时刻时间戳的方法如公式(1)所示：
表示在IMU时间基准下第1帧图像对应的回调函数执行时刻的时间戳；和分别表示在相机时间基准下第i帧和第1帧图像的曝光时刻时间戳；上述两种时间戳都可以直接获取。表示粗略对齐后的IMU时间基准下第i帧相机图像的曝光时刻时间戳，它与实际值的偏差为IMU时间基准下第1帧图像对应的回调函数执行时刻和第1帧图像曝光时刻的时间差，一般为数十毫秒，且为正值。
本实施例中，通过onImageAvailable()函数获取Image类型的对象，调用该对象的成员函数getTimestamp()获取相机时间基准下所述相机图像的曝光时刻时间戳；使用SystemClock类的elapsedRealtime()函数获取IMU时间基准下所述回调函数执行时刻时间戳。
在步骤S1002中，将调整时间戳后的所述相机图像，以及获取的IMU量测值加入滑动窗口。
在步骤S3000中，进行时间延迟的精确标定：当新的图像进入滑动窗口时，根据所述新的图像和上一帧图像的曝光时刻时间戳以及上一帧图像对应的时间延迟，调整对IMU量测值进行预积分的时间区间，进而对IMU量测值进行预积分；计算包含重投影误差项、IMU误差项和边缘化误差项的代价函数，将所述时间延迟与相机位姿一起进行迭代优化，从而得到所述新的图像对应的所述时间延迟，以及相机位姿；其中，所述IMU误差项的残差向量中包含与所述时间延迟有关的变量。这种方法对IMU误差项的表示比现有方法更加准确，而且利用到了采集的所有数据，因此对时间延迟的标定结果更加准确。
该步骤可以具体包括步骤S3001-S3003：
在步骤S3001中，若有一帧新的图像进入滑动窗口，则判断该图像加入窗口前窗口内最后一帧图像是否为关键帧；若其不是关键帧则将其丢弃，否则最前面一帧旧的图像滑出窗口，最新进入滑动窗口的图像的帧序号为K；如果最前面一帧旧的图像滑出窗口，那么其余各帧序号均减1，否则其余各帧序号不变；对时间戳位于和之间的IMU量测值进行预积分，得到和
这里所说的“最后一帧”和“最前面一帧”，分别指滑动窗口中时间戳最新的和最旧的一帧图像。
其中，TK、TK-1分别为滑动窗口中最新图像和倒数第二帧图像的曝光时刻时间戳，均为经过步骤S1000调整后的值；为所述倒数第二帧图像的时间延迟，初值为0；当滑动窗口中图像数量未达到预设的滑动窗口中最大图像数量时，暂时不执行步骤S3002至步骤S3003；
在步骤S3002中，按照基于非线性优化的VIO算法的通用做法在局部窗口中构造代价函数，使用Dogleg算法进行非线性优化求解，得到时间延迟和相机位置、速度、姿态的估计值。
其中，所述代价函数中包括重投影误差项、IMU误差项和边缘化误差项；所述重投影误差项和所述边缘化误差项使用基于非线性优化的VIO算法的通用做法进行构造；所述IMU误差项使用如公式(2)所示的方法进行构造：
其中，CIMU(x)表示IMU误差项；x由x1,x2,…,xk组成；k表示滑动窗口内图像的帧序号；xk是第k帧的状态向量，表示协方差矩阵，通过IMU预积分计算出；表示残差向量，与第k帧和第k+1帧的状态向量以及它们之间的IMU预积分有关，如公式(3)所示：
为待估计的所述时间延迟，和分别为世界坐标系下第k帧图像曝光时刻相机的位置和速度；和分别为世界坐标系下第k+1帧曝光时刻相机的位置和速度；和分别为在第k帧IMU本体坐标系下加速度计和陀螺仪的偏移；和分别为在第k+1帧IMU本体坐标系下加速度计和陀螺仪的偏移；为用四元数表示的在第k帧曝光时刻从世界坐标系到IMU本体坐标系的旋转；为的旋转矩阵形式；为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；gw为世界坐标系中的重力加速度，是一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值，Δtk＝Tk+1-Tk；和可以由和其他IMU量测值表示，如公式(4)-(6)所示：
表示在对当前滑动窗口内第k帧和第k+1帧之间的IMU量测值预积分的时候对时间偏移的估计值；ab,k和ae,k的计算方法分别为其中和分别为和时刻IMU量测值中的加速度；和分别为IMU第k帧和第k+1帧图像之间IMU预积分的结果；和分别如公式(7)、(8)所示：
和分别为和时刻IMU量测值中的角速度；和是四元数和对应的旋转矩阵。
和均为待优化的参数。利用构造IMU误差项CIMU(x)，与重投影误差项、边缘化误差项一起构成非线性优化的代价函数，使用Dogleg算法非线性优化求解，得到所有待优化参数的估计值。
在步骤S3003中，将非线性优化求解得到的所述时间延迟作为当前滑动窗口中最新图像对应的时间延迟的估计值，如公式(9)所示：
所述时间延迟的估计值在下一帧图像进入滑动窗口后，将对应于步骤S3001中的值。
随着滑动窗口不断向前滑动，时间延迟的估计量越来越接近接近真值当新的一帧加入窗口时，当前对时间偏移的估计值也越来越接近真值这时候IMU预积分的起始时刻和结束时刻就越来越接近相机曝光时间tK-1和tK，也就实现了相机和IMU系统的精确时间同步。
图2是本发明的使用在线时间标定的视觉惯性测程法实施例二的流程示意图。如图2所示，本实施例的视觉惯性测程法与实施例一相比，还包括一次性执行步骤S2000：
在步骤S2000中，进行时间延时的初步估计：分别根据所述相机图像和所述IMU量测值计算相机的姿态；再计算出使得上述两组姿态偏差最小的时间延迟，作为相机时间延迟的初步估计值。本步骤只在最初执行一次，算出一个初步估计值，作为步骤S3001中所述倒数第二帧图像的时间延迟的初值。
当实际的时间延迟较大时，采用实施例二的方法将这里算出的初步估计之作为步骤S3000中精确标定所需要的初值，相比实施例一中以0为初值可以增加算法的稳定性。
步骤S2000可以具体包括：
在步骤S2001中，检测是否有新的IMU量测值加入滑动窗口，若是，则转至步骤S2003；否则，转至步骤S2002。
在步骤S2002中，检测是否有新的一帧图像加入滑动窗口，若是，则转至步骤S2004；否则，转至步骤S2001。
在步骤S2003中，计算转角变化量Δq(t)并存入队列，更新非线性优化的滑动窗口中的所述累计转角q；转至步骤S2005。
其中，Δq(t)与q分别如公式(10)、(11)所示：
q为用四元数表示的所述累计转角，是队列中全部所述转角变化量的累计值；为IMU量测值的角速度；Δt为IMU相邻两次观测的时间差；表示四元数的乘法；t为当前最新IMU量测值的时间戳；
在步骤S2004中，若有新的一帧图像加入滑动窗口，则判断该图像加入窗口前窗口内最后一帧图像是否为关键帧；若其不是关键帧则将其丢弃，否则窗口内最前面一帧旧的图像滑出窗口；计算滑动窗口中每一帧图像对应的第一个和最后一个IMU量测值中角速度差的模长的平均值用该平均值来衡量角速度变化的情况；这里的“最后一帧”和“最前面一帧”，分别指滑动窗口中时间戳最新的和最旧的图像。
若滑出窗口的是最前面的一帧图像，则对当前队列中每个所述转角变化量分别判断该转角变化量对应的时间戳是否小于该滑出图像的时间戳；若是，则将该转角变化量出队列，并更新所述累计转角，如公式(12)所示：
其中，Δq(t′)为根据时间戳为t′的IMU量测值计算出的所述转角变化量；t′小于该滑出图像的时间戳。
在步骤S2005中，计算所述累计转角q对应的沿最近转轴转动的角度θ(q)，如公式(13)所示：
其中，qx，qy，qz和qw表示四元数q的四个分量；
在步骤S2006中，若θ1<θ(qa)<θ2且或则转至步骤S2007-S2012进行时间戳延迟的初步估计；否则，转至步骤S2001。
其中，阈值θ1和θ2用来筛选出具有较好几何结构的场景，θ1取10°，θ2取50°；阈值ω1和ω2用来平衡时间延迟估计的及时性和可靠性，若要提高及时性则减小ω1和ω2，若要提高可靠性则增大ω1和ω2。ω1取0.25-0.35弧度/秒，ω2取0.5-0.7弧度/秒。
在步骤S2007中，在应用本方法的VIO算法运行的过程中，滑动窗口中的图像特征点和IMU信息会共同参与优化得到滑动窗口中的相机位姿，将这个位姿作为本步骤中仅使用图像特征点的捆绑调整的初值。只使用图像上的标准化的特征点坐标作为观测值，对窗口内相机的K个位置和姿态进行捆绑调整。
只使用图像上的标准化的特征点坐标作为观测值，对窗口内相机的K个位置和姿态进行捆绑调整的时候，使用Ceres Solver库进行优化，使用huber范数作为损失函数，各个特征点的误差相互独立，每个特征点观测值的协方差矩阵如公式(14)所示：
其中，fx和fy分别为以x方向和y方向像素个数表示的相机焦距。若捆绑调整完成后代价函数值小于预设的代价函数阈值0.07，则认为捆绑调整成功，否则认为捆绑调整失败。
在步骤S2008中，若捆绑调整失败，则继续转至步骤S2001；若成功，则转至步骤S2009。
在步骤S2009中，利用捆绑调整后的相机旋转计算两帧之间相机的相对旋转，如公式(15)所示：
其中，和分别为通过捆绑调整得到的第k帧和第k+1帧相机坐标系到世界坐标系的旋转四元数，为从第k+1帧相机坐标系到第k帧相机坐标系的旋转四元数。
在步骤S2010中，将时间延迟在它的取值区间内进行离散采样；对于每一个采样得到的td，对于滑动窗口中每两帧相邻的图像，通过时间戳介于Tk-td和Tk+1-td之间的IMU量测值中的角速度推算相邻两帧之间IMU本体坐标系间的旋转四元数，如公式(16)所示：
其中，Tk和Tk+1分别为相机第k帧和第k+1帧图像的时间戳；t为IMU量测值的时间戳；表示时间戳为t的IMU量测值中的角速度；П表示四元数的连乘。
本实施例中，时间延迟td在th1和th2之间采样，采样间隔为δt。th1和th2分别取0ms,100ms,δt取3ms。
在步骤S2011中，结合已知的相机和IMU坐标系间的旋转四元数计算相邻两帧之间相机坐标系间的旋转四元数，如公式(17)所示：
在步骤S2012中，对于不同的时间延迟td，计算由相机信息和IMU信息获得的旋转四元数的差，并转换成旋转角度，在窗口内所有的相邻两帧之间取平均值，找到旋转角度平均值最小的td，如公式(18)所示：
若满足公式(19)所示的条件：
则记录一个时间延迟的估计值其中，K为滑动窗口中包含的帧的数目。本实施例中预设的旋转角度阈值为1.5°。
在步骤S2013中，判断是否记录了预设个数的所述时间延迟的估计值若是，则对所述预设个数(本实施例中为5)的所述时间延迟的估计值求中值，作为最终得到的时间延迟的初步估计值；否则，转至步骤S2001。
图3(a)-(b)是本发明视觉惯性测程法实施例一和实施例二中时间延迟标定值随时间变化的比较图，其中图3(a)表示在EuRoc MAV数据集的MH_01_easy数据上人工添加30ms的时间延迟的情况下对时间延迟的标定，图3(b)表示在安卓手机采集的数据上对时间延迟的标定。
我们在实施例二中以步骤S2000为时间延迟的初值，在实施例一中以0为时间延迟的初值，从图3(a)-(b)中可以看出两种策略下时间标定性能较为接近，而实施例二中使用步骤S2000的初步估计值作为初值在刚开始优化的时候时间延迟参数的值较少波动，能使算法更为稳定。作为代价的是步骤S2000需要一定时间，在此期间步骤S3000无法进行。
图4(a)-(d)是本发明视觉惯性测程法实施例一和现有的时间标定方法在EuRocMAV数据集上时间标定误差随时间变化的比较图，其中(a)、(b)、(c)、(d)分别表示在数据集上将图像的时间戳人工添加60ms、30ms、0ms、-30ms的时间延迟后时间标定的误差。可以看出，总体来说使用本方法的时间标定误差更小。如(c)所示，两种方法在事实上没有时间延迟的数据上都不会错误地估计时间延迟。
图5(a)-(b)是使用本发明视觉惯性测程法实施例一的VIO算法与使用现有的时间标定方法的VIO算法以及不进行时间标定的VIO算法在安卓手机采集的含有快速转弯和突然停止的数据上的轨迹比较图，轨迹是三维的，而图中表示的是轨迹在X轴和Y轴形成的平面上的投影，因为相机在Z方向的运动可以忽略不计。其中(a)、(b)分别对应含有突然停止和快速转弯的数据。图5(b)的圆点为时间间隔相等的相机位置，圆点的疏密程度反映相机运动的速度。图5(a)中，轨迹上A点为轨迹的起点，而使用三种方法分别计算出来的轨迹终点分别为B1，B2和B3。事实上，数据采集者绕着一个大约11m×21m的矩形区域走了一圈，且轨迹上A，B1，B2和B3四个点事实上应该重合。使用本方法的相机轨迹在相机经历突然停止后仍然能保持正确的尺度，而其他两种算法得出的相机轨迹的尺度会错误地缩小。图5(b)中，数据采集者事实上近似匀速运动，期间快速拐过一个接近90°的转角。使用本发明方法的算法在转弯之后估计出来的相机运动仍然近似匀速，而另外两种方法估计的相机速度在转弯后明显变快，即发生“漂走”现象。
本发明的一种存储设备的实施例，其存储有程序，所述程序适于由处理器加载并执行，以实现上面所述的使用在线时间标定的视觉惯性测程法。
本发明的一种处理设备的实施例，包括：处理器和存储器；
所述处理器，适于执行程序；所述存储器，适于存储该程序；所述程序适于由所述处理器加载并执行，以实现上面所述的使用在线时间标定的视觉惯性测程法。
图6是本发明的使用在线时间标定的视觉惯性测程设备实施例一的构成示意图。如图6所示，本实施例的视觉惯性测程设备包括：图像与IMU量测值获取模块10、时间延迟与相机姿态计算模块30；
所述图像与IMU量测值获取模块10，配置为：实时获取相机图像和IMU量测值，并根据IMU时间基准，调整图像的曝光时刻时间戳；将调整时间戳后的所述相机图像和IMU量测值加入滑动窗口；
所述时间延迟与相机姿态计算模块30，配置为：当新的图像进入滑动窗口时，根据所述新的图像和上一帧图像的曝光时刻时间戳以及上一帧图像对应的时间延迟，调整对IMU量测值进行预积分的时间区间，进而对IMU量测值进行预积分；计算包含重投影误差项、IMU误差项和边缘化误差项的代价函数，将所述时间延迟与相机位置、速度和姿态一起进行迭代优化，从而得到所述新的图像对应的所述时间延迟，以及相机位置、速度和姿态；其中，IMU误差项的残差向量中包含与所述时间延迟有关的变量。
图7是本发明的使用在线时间标定的视觉惯性测程设备实施例二的构成示意图。如图7所示，本实施例的视觉惯性测程设备还包括：时间延迟初值估计模块20。
所述时间延迟初值估计模块20，包括：分别根据所述相机图像和所述IMU量测值计算相机的姿态；计算出使得上述两组姿态偏差最小的时间延迟，作为所述时间延迟的初值。
本领域技术人员应该能够意识到，结合本文中所公开的实施例描述的各示例的方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明电子硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征做出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。
Claims (11)
1.一种使用在线时间标定的视觉惯性测程法，其特征在于，包括以下步骤：
步骤S1000，实时获取相机图像和IMU量测值，并根据IMU时间基准，调整图像的曝光时刻时间戳；将调整时间戳后的所述相机图像和IMU量测值加入滑动窗口；
步骤S3000，当新的图像进入滑动窗口时，根据所述新的图像和上一帧图像的曝光时刻时间戳以及上一帧图像对应的时间延迟，调整对IMU量测值进行预积分的时间区间，进而对IMU量测值进行预积分；计算包含重投影误差项、IMU误差项和边缘化误差项的代价函数，将所述时间延迟与相机位置、速度和姿态一起进行迭代优化，从而得到所述新的图像对应的所述时间延迟，以及相机位置、速度和姿态；
其中，
所述IMU误差项的残差向量中包含与所述时间延迟有关的变量。
2.根据权利要求1所述的视觉惯性测程法，其特征在于，步骤S1000具体包括：
步骤S1001，使用camera2 API实时获取相机图像，并获取相机时间基准下所述相机图像的曝光时刻时间戳；根据IMU时间基准，调整所述相机图像的曝光时刻时间戳；实时获取IMU量测值；
步骤S1002，将调整时间戳后的所述相机图像，以及获取的IMU量测值加入滑动窗口；
其中，
所述IMU量测值的获取频率高于所述相机图像的获取频率；
所述相机图像的曝光时刻时间戳根据下式调整：
表示调整后的第i帧所述相机图像的曝光时刻时间戳；表示在IMU时间基准下第1帧图像对应的回调函数执行时刻的时间戳；和分别表示在相机时间基准下第i帧和第1帧图像的曝光时刻时间戳。
3.根据权利要求2所述的视觉惯性测程法，其特征在于，
所述获取相机时间基准下所述相机图像的曝光时刻时间戳的方法为：
通过onImageAvailable()函数获取Image类型的对象，调用该对象的成员函数getTimestamp()获取相机时间基准下所述相机图像的曝光时刻时间戳；
所述回调函数执行时刻的时间戳的获取方法为：
使用SystemClock类的elapsedRealtime()函数获取IMU时间基准下所述回调函数执行时刻时间戳。
4.根据权利要求2所述的视觉惯性测程法，其特征在于，步骤S3000具体包括：
步骤S3001，若有一帧新的图像进入滑动窗口，则判断该图像加入窗口前窗口内最后一帧图像是否为关键帧；若其不是关键帧则将其丢弃，否则最前面一帧旧的图像滑出窗口，最新进入滑动窗口的图像的帧序号为K；如果最前面一帧旧的图像滑出窗口，那么其余各帧序号均减1，否则其余各帧序号不变；对时间戳位于和之间的IMU量测值进行预积分，得到和
其中，TK、TK-1分别为滑动窗口中最新图像和倒数第二帧图像的曝光时刻时间戳，均为经过步骤S1000调整后的值；为所述倒数第二帧图像的时间延迟，初值为0；
步骤S3002，按照基于非线性优化的VIO算法的通用做法在局部窗口中构造代价函数，使用Dogleg算法进行非线性优化求解，得到时间延迟和相机位置、速度、姿态的估计值；
其中，
所述代价函数中包括重投影误差项、IMU误差项和边缘化误差项；
所述重投影误差项和所述边缘化误差项使用基于非线性优化的VIO算法的通用做法进行构造；
所述IMU误差项使用下述方法进行构造：
其中，CIMU(x)表示IMU误差项；x由x1,x2,…,xk组成；k表示滑动窗口内图像的帧序号；xk是第k帧的状态向量，表示协方差矩阵，通过IMU预积分计算出；表示残差向量，与第k帧和第k+1帧的状态向量以及它们之间的IMU预积分有关：
为待估计的所述时间延迟，和分别为世界坐标系下第k帧图像曝光时刻相机的位置和速度；和分别为世界坐标系下第k+1帧曝光时刻相机的位置和速度；和分别为在第k帧IMU本体坐标系下加速度计和陀螺仪的偏移；和分别为在第k+1帧IMU本体坐标系下加速度计和陀螺仪的偏移；为用四元数表示的在第k帧曝光时刻从世界坐标系到IMU本体坐标系的旋转；为的旋转矩阵形式；为用四元数表示的在第k+1帧图像曝光时刻从IMU本体坐标系到世界坐标系的旋转；gw为世界坐标系中的重力加速度，是一个常量；Δtk为第k+1帧和第k帧曝光时刻时间戳的差值，Δtk＝Tk+1-Tk； 和的计算方法为：
表示在对当前滑动窗口内第k帧和第k+1帧之间的IMU量测值预积分的时候对时间偏移的估计值；ab,k和ae,k的计算方法分别为其中和分别为和时刻IMU量测值中的加速度；和分别为IMU第k帧和第k+1帧图像之间IMU预积分的结果；和分别表示为：
和分别为和时刻IMU量测值中的角速度；和是四元数和对应的旋转矩阵；
和均为待优化的参数；
步骤S3003，将非线性优化求解得到的所述时间延迟作为当前滑动窗口中最新图像对应的时间延迟的估计值：
所述时间延迟的估计值在下一帧图像进入滑动窗口后，将对应于步骤S3001中的值。
5.根据权利要求4所述的视觉惯性测程法，其特征在于，还包括一次性执行：
步骤S2000，分别根据所述相机图像和所述IMU量测值计算相机的姿态；计算出使得上述两组姿态偏差最小的时间延迟，作为时间延迟的初步估计值；
相应地，将步骤S2000计算得到的所述时间延迟的初步估计值作为步骤S3001中所述倒数第二帧图像的时间延迟的初值。
6.根据权利要求5所述的视觉惯性测程法，其特征在于，步骤S2000具体包括：
步骤S2001，检测是否有新的IMU量测值加入滑动窗口，若是，则转至步骤S2003；否则，转至步骤S2002；
步骤S2002，检测是否有新的一帧图像加入滑动窗口，若是，则转至步骤S2004；否则，转至步骤S2001；
步骤S2003，计算转角变化量Δq(t)并存入队列，更新非线性优化的滑动窗口中的所述累计转角q；转至步骤S2005；
其中，
q为用四元数表示的所述累计转角，是队列中全部所述转角变化量的累计值；为IMU量测值的角速度；Δt为IMU相邻两次观测的时间差；表示四元数的乘法；t为当前最新IMU量测值的时间戳；
步骤S2004，若有新的一帧图像加入滑动窗口，则判断该图像加入窗口前窗口内最后一帧图像是否为关键帧；若其不是关键帧则将其丢弃，否则窗口内最前面一帧旧的图像滑出窗口；计算滑动窗口中每一帧图像对应的第一个和最后一个IMU量测值中角速度差的模长的平均值用该平均值来衡量角速度变化的情况；
若滑出窗口的是最前面的一帧图像，则对当前队列中每个所述转角变化量分别判断该转角变化量对应的时间戳是否小于该滑出图像的时间戳；若是，则将该转角变化量出队列，并更新所述累计转角：
其中，Δq(t′)为根据时间戳为t′的IMU量测值计算出的所述转角变化量；t′小于该滑出图像的时间戳；
步骤S2005，计算所述累计转角q对应的沿最近转轴转动的角度θ(q)：
其中，qx，qy，qz和qw表示四元数q的四个分量；
步骤S2006，若θ1<θ(qa)<θ2且或则转至步骤S2007；否则，转至步骤S2001；
其中，阈值θ1和θ2用来筛选出具有较好几何结构的场景；阈值ω1和ω2用来平衡时间延迟估计的及时性和可靠性，若要提高及时性则减小ω1和ω2，若要提高可靠性则增大ω1和ω2；
步骤S2007，在应用本方法的VIO算法运行的过程中，滑动窗口中的图像特征点和IMU信息会共同参与优化得到滑动窗口中的相机位姿，将这个位姿作为本步骤中仅使用图像特征点的捆绑调整的初值。只使用图像上的标准化的特征点坐标作为观测值，对窗口内相机的K个位置和姿态进行捆绑调整；
步骤S2008，若捆绑调整失败，则继续转至步骤S2001；若成功，则转至步骤S2009；
步骤S2009，利用捆绑调整后的相机旋转计算两帧之间相机的相对旋转：
其中，和分别为通过捆绑调整得到的第k帧和第k+1帧相机坐标系到世界坐标系的旋转四元数，为从第k+1帧相机坐标系到第k帧相机坐标系的旋转四元数；
步骤S2010，将时间延迟在它的取值区间内进行离散采样；对于每一个采样得到的td，对于滑动窗口中每两帧相邻的图像，通过时间戳介于Tk-td和Tk+1-td之间的IMU量测值中的角速度推算相邻两帧之间IMU本体坐标系间的旋转四元数：
其中，Tk和Tk+1分别为相机第k帧和第k+1帧图像的时间戳；t为IMU量测值的时间戳；表示时间戳为t的IMU量测值中的角速度；П表示四元数的连乘；
步骤S2011，结合已知的相机和IMU坐标系间的旋转四元数计算相邻两帧之间相机坐标系间的旋转四元数：
步骤S2012，对于不同的时间延迟td，计算由相机信息和IMU信息获得的旋转四元数的差，并转换成旋转角度，在窗口内所有的相邻两帧之间取平均值，找到旋转角度平均值最小的td：
若
则记录一个时间延迟的估计值
其中，K为滑动窗口中包含的帧的数目；
步骤S2013，判断是否记录了预设个数的所述时间延迟的估计值若是，则对所述预设个数的所述时间延迟的估计值求中值，作为最终得到的时间延迟的初步估计值；否则，转至步骤S2001。
7.根据权利要求6所述的视觉惯性测程法，其特征在于，步骤S2007中“只使用图像上的标准化的特征点坐标作为观测值，对窗口内相机的K个位置和姿态进行捆绑调整”，具体为：
使用Ceres Solver库进行优化，使用huber范数作为损失函数，各个特征点的误差相互独立，每个特征点观测值的协方差矩阵均为：
其中，fx和fy分别为以x方向和y方向像素个数表示的相机焦距；
若捆绑调整完成后代价函数值小于预设的代价函数阈值，则认为捆绑调整成功，否则认为捆绑调整失败。
8.一种存储设备，其存储有程序，其特征在于，所述程序适于由处理器加载并执行，以实现权利要求1-7中任一项所述的使用在线时间标定的视觉惯性测程法。
9.一种处理设备，包括：处理器和存储器；
所述处理器，适于执行程序；
所述存储器，适于存储该程序；
其特征在于，所述程序适于由所述处理器加载并执行，以实现权利要求1-7中任一项所述的使用在线时间标定的视觉惯性测程法。
10.一种使用在线时间标定的视觉惯性测程设备，其特征在于，包括：图像与IMU量测值获取模块、时间延迟与相机姿态计算模块；
所述图像与IMU量测值获取模块，配置为：实时获取相机图像和IMU量测值，并根据IMU时间基准，调整图像的曝光时刻时间戳；将调整时间戳后的所述相机图像和IMU量测值加入滑动窗口；
所述时间延迟与相机姿态计算模块，配置为：当新的图像进入滑动窗口时，根据所述新的图像和上一帧图像的曝光时刻时间戳以及上一帧图像对应的时间延迟，调整对IMU量测值进行预积分的时间区间，进而对IMU量测值进行预积分；计算包含重投影误差项、IMU误差项和边缘化误差项的代价函数，将所述时间延迟与相机位置、速度和姿态一起进行迭代优化，从而得到所述新的图像对应的所述时间延迟，以及相机位置、速度和姿态；
其中，
所述IMU误差项的残差向量中包含与所述时间延迟有关的变量。
11.根据权利要求10所述的视觉惯性测程设备，其特征在于，还包括：时间延迟初值估计模块；
所述时间延迟初值估计模块，包括：分别根据所述相机图像和所述IMU量测值计算相机的姿态；计算出使得上述两组姿态偏差最小的时间延迟，作为所述时间延迟的初值。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810239278.2A CN108629793B (zh) | 2018-03-22 | 2018-03-22 | 使用在线时间标定的视觉惯性测程法与设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810239278.2A CN108629793B (zh) | 2018-03-22 | 2018-03-22 | 使用在线时间标定的视觉惯性测程法与设备 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108629793A true CN108629793A (zh) | 2018-10-09 |
| CN108629793B CN108629793B (zh) | 2020-11-10 |
Family
ID=63696233
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810239278.2A Active CN108629793B (zh) | 2018-03-22 | 2018-03-22 | 使用在线时间标定的视觉惯性测程法与设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108629793B (zh) |
Cited By (33)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109275048A (zh) * | 2018-11-21 | 2019-01-25 | 北京猎户星空科技有限公司 | 一种应用于机器人的数据处理方法、装置、设备及介质 |
| CN109544630A (zh) * | 2018-11-30 | 2019-03-29 | 南京人工智能高等研究院有限公司 | 位姿信息确定方法和装置、视觉点云构建方法和装置 |
| CN109764880A (zh) * | 2019-02-19 | 2019-05-17 | 中国科学院自动化研究所 | 紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统 |
| CN109785428A (zh) * | 2019-01-21 | 2019-05-21 | 苏州大学 | 一种基于多态约束卡尔曼滤波的手持式三维重建方法 |
| CN110084832A (zh) * | 2019-04-25 | 2019-08-02 | 亮风台（上海）信息科技有限公司 | 相机位姿的纠正方法、装置、系统、设备和存储介质 |
| CN110174120A (zh) * | 2019-04-16 | 2019-08-27 | 百度在线网络技术（北京）有限公司 | 用于ar导航模拟的时间同步方法及装置 |
| CN110207722A (zh) * | 2019-06-11 | 2019-09-06 | 远形时空科技（北京）有限公司 | 一种自动化标定相机里程计系统及方法 |
| CN110379017A (zh) * | 2019-07-12 | 2019-10-25 | 北京达佳互联信息技术有限公司 | 一种场景构建方法、装置、电子设备及存储介质 |
| CN110455309A (zh) * | 2019-08-27 | 2019-11-15 | 清华大学 | 具备在线时间校准的基于msckf的视觉惯性里程计 |
| CN110567453A (zh) * | 2019-08-21 | 2019-12-13 | 北京理工大学 | 仿生眼多通道imu与相机硬件时间同步方法和装置 |
| CN110595479A (zh) * | 2019-09-23 | 2019-12-20 | 云南电网有限责任公司电力科学研究院 | 一种基于icp算法的slam轨迹评估方法 |
| CN110702139A (zh) * | 2019-09-29 | 2020-01-17 | 百度在线网络技术（北京）有限公司 | 一种时延标定方法、装置、电子设备和介质 |
| CN110956665A (zh) * | 2019-12-18 | 2020-04-03 | 中国科学院自动化研究所 | 车辆拐弯轨迹双向计算方法、系统、装置 |
| CN110956666A (zh) * | 2019-11-12 | 2020-04-03 | 深圳市瑞立视多媒体科技有限公司 | 运动数据标定方法、装置、终端设备及存储介质 |
| CN111257853A (zh) * | 2020-01-10 | 2020-06-09 | 清华大学 | 一种基于imu预积分的自动驾驶系统激光雷达在线标定方法 |
| CN111307176A (zh) * | 2020-03-02 | 2020-06-19 | 北京航空航天大学青岛研究院 | 一种vr头戴显示设备中视觉惯性里程计的在线标定方法 |
| CN111308415A (zh) * | 2019-11-01 | 2020-06-19 | 华为技术有限公司 | 一种基于时间延迟的在线估计位姿的方法和设备 |
| CN111325803A (zh) * | 2020-02-12 | 2020-06-23 | 清华大学深圳国际研究生院 | 一种评估双目相机内外参与时间同步的标定的方法 |
| CN111383282A (zh) * | 2018-12-29 | 2020-07-07 | 杭州海康威视数字技术股份有限公司 | 位姿信息确定方法及装置 |
| CN111580596A (zh) * | 2020-05-19 | 2020-08-25 | 北京数字绿土科技有限公司 | 多个imu时间同步方法、装置、终端 |
| CN111854770A (zh) * | 2019-04-30 | 2020-10-30 | 北京初速度科技有限公司 | 一种车辆的定位系统和方法 |
| CN111998870A (zh) * | 2019-05-26 | 2020-11-27 | 北京初速度科技有限公司 | 一种相机惯导系统的标定方法和装置 |
| CN112097771A (zh) * | 2020-08-18 | 2020-12-18 | 青岛职业技术学院 | Gps延迟时间自适应的扩展卡尔曼滤波导航算法 |
| CN112129287A (zh) * | 2020-09-24 | 2020-12-25 | 北京华捷艾米科技有限公司 | 一种基于视觉惯性里程计处理的方法和相关装置 |
| CN112414400A (zh) * | 2019-08-21 | 2021-02-26 | 浙江商汤科技开发有限公司 | 一种信息处理方法、装置、电子设备和存储介质 |
| CN112923923A (zh) * | 2021-01-28 | 2021-06-08 | 深圳市瑞立视多媒体科技有限公司 | 一种imu与刚体姿态、位置对齐的方法、装置、设备以及可读存储介质 |
| CN112945231A (zh) * | 2021-01-28 | 2021-06-11 | 深圳市瑞立视多媒体科技有限公司 | 一种imu与刚体姿态对齐的方法、装置、设备以及可读存储介质 |
| CN113311463A (zh) * | 2020-02-26 | 2021-08-27 | 北京三快在线科技有限公司 | Gps延迟时间在线补偿方法、装置、电子设备和存储介质 |
| CN113591015A (zh) * | 2021-07-30 | 2021-11-02 | 北京小狗吸尘器集团股份有限公司 | 时间延迟的计算方法、装置、存储介质及电子设备 |
| CN114040128A (zh) * | 2021-11-24 | 2022-02-11 | 视辰信息科技(上海)有限公司 | 时间戳延时标定方法及系统、设备和计算机可读存储介质 |
| CN114459523A (zh) * | 2021-12-10 | 2022-05-10 | 红云红河烟草(集团)有限责任公司 | 一种在线质量检测仪器的标定预警方法 |
| WO2022179555A1 (zh) * | 2021-02-26 | 2022-09-01 | 影石创新科技股份有限公司 | 视频数据的防抖处理方法、装置、计算机设备和存储介质 |
| WO2023226156A1 (zh) * | 2022-05-24 | 2023-11-30 | 广东人工智能与先进计算研究院 | 时间戳校正方法、装置、设备、介质及计算机程序产品 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2014130854A1 (en) * | 2013-02-21 | 2014-08-28 | Regents Of The Univesity Of Minnesota | Extrinsic parameter calibration of a vision-aided inertial navigation system |
| CN105698765A (zh) * | 2016-02-22 | 2016-06-22 | 天津大学 | 双imu单目视觉组合测量非惯性系下目标物位姿方法 |
| US20160305784A1 (en) * | 2015-04-17 | 2016-10-20 | Regents Of The University Of Minnesota | Iterative kalman smoother for robust 3d localization for vision-aided inertial navigation |
| CN107193279A (zh) * | 2017-05-09 | 2017-09-22 | 复旦大学 | 基于单目视觉和imu信息的机器人定位与地图构建系统 |
| CN107564012A (zh) * | 2017-08-01 | 2018-01-09 | 中国科学院自动化研究所 | 面向未知环境的增强现实方法及装置 |
| CN107656545A (zh) * | 2017-09-12 | 2018-02-02 | 武汉大学 | 一种面向无人机野外搜救的自主避障与导航方法 |
| CN107767425A (zh) * | 2017-10-31 | 2018-03-06 | 南京维睛视空信息科技有限公司 | 一种基于单目vio的移动端AR方法 |
-
2018
- 2018-03-22 CN CN201810239278.2A patent/CN108629793B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2014130854A1 (en) * | 2013-02-21 | 2014-08-28 | Regents Of The Univesity Of Minnesota | Extrinsic parameter calibration of a vision-aided inertial navigation system |
| US20160305784A1 (en) * | 2015-04-17 | 2016-10-20 | Regents Of The University Of Minnesota | Iterative kalman smoother for robust 3d localization for vision-aided inertial navigation |
| CN105698765A (zh) * | 2016-02-22 | 2016-06-22 | 天津大学 | 双imu单目视觉组合测量非惯性系下目标物位姿方法 |
| CN107193279A (zh) * | 2017-05-09 | 2017-09-22 | 复旦大学 | 基于单目视觉和imu信息的机器人定位与地图构建系统 |
| CN107564012A (zh) * | 2017-08-01 | 2018-01-09 | 中国科学院自动化研究所 | 面向未知环境的增强现实方法及装置 |
| CN107656545A (zh) * | 2017-09-12 | 2018-02-02 | 武汉大学 | 一种面向无人机野外搜救的自主避障与导航方法 |
| CN107767425A (zh) * | 2017-10-31 | 2018-03-06 | 南京维睛视空信息科技有限公司 | 一种基于单目vio的移动端AR方法 |
Non-Patent Citations (1)
| Title |
|---|
| 程传奇等: "基于非线性优化的单目视觉/惯性组合导航算法", 《中国惯性技术学报》 * |
Cited By (47)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109275048A (zh) * | 2018-11-21 | 2019-01-25 | 北京猎户星空科技有限公司 | 一种应用于机器人的数据处理方法、装置、设备及介质 |
| CN109544630A (zh) * | 2018-11-30 | 2019-03-29 | 南京人工智能高等研究院有限公司 | 位姿信息确定方法和装置、视觉点云构建方法和装置 |
| CN111383282B (zh) * | 2018-12-29 | 2023-12-01 | 杭州海康威视数字技术股份有限公司 | 位姿信息确定方法及装置 |
| CN111383282A (zh) * | 2018-12-29 | 2020-07-07 | 杭州海康威视数字技术股份有限公司 | 位姿信息确定方法及装置 |
| CN109785428B (zh) * | 2019-01-21 | 2023-06-23 | 苏州大学 | 一种基于多态约束卡尔曼滤波的手持式三维重建方法 |
| CN109785428A (zh) * | 2019-01-21 | 2019-05-21 | 苏州大学 | 一种基于多态约束卡尔曼滤波的手持式三维重建方法 |
| CN109764880A (zh) * | 2019-02-19 | 2019-05-17 | 中国科学院自动化研究所 | 紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统 |
| CN110174120B (zh) * | 2019-04-16 | 2021-10-08 | 百度在线网络技术（北京）有限公司 | 用于ar导航模拟的时间同步方法及装置 |
| CN110174120A (zh) * | 2019-04-16 | 2019-08-27 | 百度在线网络技术（北京）有限公司 | 用于ar导航模拟的时间同步方法及装置 |
| CN110084832A (zh) * | 2019-04-25 | 2019-08-02 | 亮风台（上海）信息科技有限公司 | 相机位姿的纠正方法、装置、系统、设备和存储介质 |
| CN111854770A (zh) * | 2019-04-30 | 2020-10-30 | 北京初速度科技有限公司 | 一种车辆的定位系统和方法 |
| CN111998870A (zh) * | 2019-05-26 | 2020-11-27 | 北京初速度科技有限公司 | 一种相机惯导系统的标定方法和装置 |
| CN110207722A (zh) * | 2019-06-11 | 2019-09-06 | 远形时空科技（北京）有限公司 | 一种自动化标定相机里程计系统及方法 |
| CN110379017B (zh) * | 2019-07-12 | 2023-04-28 | 北京达佳互联信息技术有限公司 | 一种场景构建方法、装置、电子设备及存储介质 |
| CN110379017A (zh) * | 2019-07-12 | 2019-10-25 | 北京达佳互联信息技术有限公司 | 一种场景构建方法、装置、电子设备及存储介质 |
| CN110567453A (zh) * | 2019-08-21 | 2019-12-13 | 北京理工大学 | 仿生眼多通道imu与相机硬件时间同步方法和装置 |
| CN110567453B (zh) * | 2019-08-21 | 2021-05-25 | 北京理工大学 | 仿生眼多通道imu与相机硬件时间同步方法和装置 |
| CN112414400A (zh) * | 2019-08-21 | 2021-02-26 | 浙江商汤科技开发有限公司 | 一种信息处理方法、装置、电子设备和存储介质 |
| CN110455309A (zh) * | 2019-08-27 | 2019-11-15 | 清华大学 | 具备在线时间校准的基于msckf的视觉惯性里程计 |
| CN110595479A (zh) * | 2019-09-23 | 2019-12-20 | 云南电网有限责任公司电力科学研究院 | 一种基于icp算法的slam轨迹评估方法 |
| CN110595479B (zh) * | 2019-09-23 | 2023-11-17 | 云南电网有限责任公司电力科学研究院 | 一种基于icp算法的slam轨迹评估方法 |
| CN110702139B (zh) * | 2019-09-29 | 2021-08-27 | 百度在线网络技术（北京）有限公司 | 一种时延标定方法、装置、电子设备和介质 |
| CN110702139A (zh) * | 2019-09-29 | 2020-01-17 | 百度在线网络技术（北京）有限公司 | 一种时延标定方法、装置、电子设备和介质 |
| CN111308415A (zh) * | 2019-11-01 | 2020-06-19 | 华为技术有限公司 | 一种基于时间延迟的在线估计位姿的方法和设备 |
| CN110956666A (zh) * | 2019-11-12 | 2020-04-03 | 深圳市瑞立视多媒体科技有限公司 | 运动数据标定方法、装置、终端设备及存储介质 |
| CN110956665A (zh) * | 2019-12-18 | 2020-04-03 | 中国科学院自动化研究所 | 车辆拐弯轨迹双向计算方法、系统、装置 |
| CN110956665B (zh) * | 2019-12-18 | 2023-06-23 | 中国科学院自动化研究所 | 车辆拐弯轨迹双向计算方法、系统、装置 |
| CN111257853A (zh) * | 2020-01-10 | 2020-06-09 | 清华大学 | 一种基于imu预积分的自动驾驶系统激光雷达在线标定方法 |
| CN111325803A (zh) * | 2020-02-12 | 2020-06-23 | 清华大学深圳国际研究生院 | 一种评估双目相机内外参与时间同步的标定的方法 |
| CN111325803B (zh) * | 2020-02-12 | 2023-05-12 | 清华大学深圳国际研究生院 | 一种评估双目相机内外参与时间同步的标定的方法 |
| CN113311463A (zh) * | 2020-02-26 | 2021-08-27 | 北京三快在线科技有限公司 | Gps延迟时间在线补偿方法、装置、电子设备和存储介质 |
| CN111307176B (zh) * | 2020-03-02 | 2023-06-16 | 北京航空航天大学青岛研究院 | 一种vr头戴显示设备中视觉惯性里程计的在线标定方法 |
| CN111307176A (zh) * | 2020-03-02 | 2020-06-19 | 北京航空航天大学青岛研究院 | 一种vr头戴显示设备中视觉惯性里程计的在线标定方法 |
| CN111580596B (zh) * | 2020-05-19 | 2022-04-15 | 北京数字绿土科技股份有限公司 | 多个imu时间同步方法、装置、终端 |
| CN111580596A (zh) * | 2020-05-19 | 2020-08-25 | 北京数字绿土科技有限公司 | 多个imu时间同步方法、装置、终端 |
| CN112097771A (zh) * | 2020-08-18 | 2020-12-18 | 青岛职业技术学院 | Gps延迟时间自适应的扩展卡尔曼滤波导航算法 |
| CN112097771B (zh) * | 2020-08-18 | 2022-04-29 | 青岛职业技术学院 | Gps延迟时间自适应的扩展卡尔曼滤波导航方法 |
| CN112129287A (zh) * | 2020-09-24 | 2020-12-25 | 北京华捷艾米科技有限公司 | 一种基于视觉惯性里程计处理的方法和相关装置 |
| CN112945231A (zh) * | 2021-01-28 | 2021-06-11 | 深圳市瑞立视多媒体科技有限公司 | 一种imu与刚体姿态对齐的方法、装置、设备以及可读存储介质 |
| CN112923923A (zh) * | 2021-01-28 | 2021-06-08 | 深圳市瑞立视多媒体科技有限公司 | 一种imu与刚体姿态、位置对齐的方法、装置、设备以及可读存储介质 |
| WO2022179555A1 (zh) * | 2021-02-26 | 2022-09-01 | 影石创新科技股份有限公司 | 视频数据的防抖处理方法、装置、计算机设备和存储介质 |
| CN113591015A (zh) * | 2021-07-30 | 2021-11-02 | 北京小狗吸尘器集团股份有限公司 | 时间延迟的计算方法、装置、存储介质及电子设备 |
| CN114040128A (zh) * | 2021-11-24 | 2022-02-11 | 视辰信息科技(上海)有限公司 | 时间戳延时标定方法及系统、设备和计算机可读存储介质 |
| CN114040128B (zh) * | 2021-11-24 | 2024-03-01 | 视辰信息科技(上海)有限公司 | 时间戳延时标定方法及系统、设备和计算机可读存储介质 |
| CN114459523A (zh) * | 2021-12-10 | 2022-05-10 | 红云红河烟草(集团)有限责任公司 | 一种在线质量检测仪器的标定预警方法 |
| CN114459523B (zh) * | 2021-12-10 | 2024-04-30 | 红云红河烟草(集团)有限责任公司 | 一种在线质量检测仪器的标定预警方法 |
| WO2023226156A1 (zh) * | 2022-05-24 | 2023-11-30 | 广东人工智能与先进计算研究院 | 时间戳校正方法、装置、设备、介质及计算机程序产品 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108629793B (zh) | 2020-11-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108629793B (zh) | 使用在线时间标定的视觉惯性测程法与设备 | |
| CN104501814B (zh) | 一种基于视觉和惯性信息的姿态与位置估计方法 | |
| Li et al. | Real-time motion tracking on a cellphone using inertial sensing and a rolling-shutter camera | |
| Hanning et al. | Stabilizing cell phone video using inertial measurement sensors | |
| CN110411476B (zh) | 视觉惯性里程计标定适配及评价方法和系统 | |
| Kneip et al. | Deterministic initialization of metric state estimation filters for loosely-coupled monocular vision-inertial systems | |
| US9037411B2 (en) | Systems and methods for landmark selection for navigation | |
| CN109059907B (zh) | 轨迹数据处理方法、装置、计算机设备和存储介质 | |
| CN105953796A (zh) | 智能手机单目和imu融合的稳定运动跟踪方法和装置 | |
| CN109612471B (zh) | 一种基于多传感器融合的运动体姿态解算方法 | |
| US11223764B2 (en) | Method for determining bias in an inertial measurement unit of an image acquisition device | |
| CN110231028B (zh) | 飞行器导航方法、装置和系统 | |
| CN114623817B (zh) | 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法 | |
| CN111721288B (zh) | 一种mems器件零偏修正方法、装置及存储介质 | |
| CN110260861B (zh) | 位姿确定方法及装置、里程计 | |
| CN104318119B (zh) | 一种高动态下星点质心误差补偿方法 | |
| CN111707261A (zh) | 一种微型无人机高速感知和定位方法 | |
| WO2008024772A1 (en) | Image-based system and method for vehicle guidance and navigation | |
| GB2498177A (en) | Apparatus for determining a floor plan of a building | |
| CN110956665A (zh) | 车辆拐弯轨迹双向计算方法、系统、装置 | |
| CN107621266B (zh) | 基于特征点跟踪的空间非合作目标相对导航方法 | |
| CN109631894A (zh) | 一种基于滑动窗口的单目视觉惯性紧耦合方法 | |
| CN112113564A (zh) | 一种基于图像传感器和惯性传感器的定位方法及系统 | |
| CN114754768A (zh) | 一种点线融合的视觉惯性导航方法 | |
| Wenk et al. | Posture from motion |
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 | ||
| CB03 | Change of inventor or designer information | ||
| CB03 | Change of inventor or designer information |
Inventor after: Gao Wei Inventor after: Liu Jinxu Inventor after: Hu Zhanyi Inventor before: Liu Jinxu Inventor before: Gao Wei Inventor before: Hu Zhanyi |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |