本发明涉及多传感器时间标定算法领域，具体涉及一种使用在线时间标定的视觉惯性测程法与设备。
背景技术：
使用相机和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–inertialodometry，视觉惯性里程计)算法。
vins最新更新的开源代码是在此之前唯一公开的使用非线性优化的在线时间标定方法。这种方法用连续两帧图像上特征点坐标的差除以两帧之间的时间间隔计算出特征点在图像上移动的速度，再用它乘以时间延迟作为特征点坐标的改正量加到特征点坐标的观测值上。使用这种方法，时间延迟就成了非线性优化的参数之一。然而，图像采集的频率显著低于imu的量测值的频率，因此，使用上述方法计算出的特征点在图像上移动的速度也有较大的误差。
技术实现要素：
为了解决现有技术中的上述问题，本发明提出了一种使用在线时间标定的视觉惯性测程法与设备，提高了相机位姿计算的精度。
本发明的一方面，提出一种使用在线时间标定的视觉惯性测程法，包括以下步骤：
步骤s1000，实时获取相机图像和imu量测值，并根据imu时间基准，调整图像的曝光时刻时间戳；将调整时间戳后的所述相机图像和imu量测值加入滑动窗口；
步骤s3000，当新的图像进入滑动窗口时，根据所述新的图像和上一帧图像的曝光时刻时间戳以及上一帧图像对应的时间延迟，调整对imu量测值进行预积分的时间区间，进而对imu量测值进行预积分；计算包含重投影误差项、imu误差项和边缘化误差项的代价函数，将所述时间延迟与相机位置、速度和姿态一起进行迭代优化，从而得到所述新的图像对应的所述时间延迟，以及相机位置、速度和姿态；
其中，
所述imu误差项的残差向量中包含与所述时间延迟有关的变量。
优选地，步骤s1000具体包括：
步骤s1001，使用camera2api实时获取相机图像，并获取相机时间基准下所述相机图像的曝光时刻时间戳；根据imu时间基准，调整所述相机图像的曝光时刻时间戳；实时获取imu量测值；
步骤s1002，将调整时间戳后的所述相机图像，以及获取的imu量测值加入滑动窗口；
其中，
所述imu量测值的获取频率高于所述相机图像的获取频率；
所述相机图像的曝光时刻时间戳根据下式调整：
优选地，所述获取相机时间基准下所述相机图像的曝光时刻时间戳的方法为：
通过onimageavailable()函数获取image类型的对象，调用该对象的成员函数gettimestamp()获取相机时间基准下所述相机图像的曝光时刻时间戳；
所述回调函数执行时刻的时间戳的获取方法为：
使用systemclock类的elapsedrealtime()函数获取imu时间基准下所述回调函数执行时刻时间戳。
优选地，步骤s3000具体包括：
步骤s3001，若有一帧新的图像进入滑动窗口，则判断该图像加入窗口前窗口内最后一帧图像是否为关键帧；若其不是关键帧则将其丢弃，否则最前面一帧旧的图像滑出窗口，最新进入滑动窗口的图像的帧序号为k；如果最前面一帧旧的图像滑出窗口，那么其余各帧序号均减1，否则其余各帧序号不变；对时间戳位于
其中，tk、tk-1分别为滑动窗口中最新图像和倒数第二帧图像的曝光时刻时间戳，均为经过步骤s1000调整后的值；
步骤s3002，按照基于非线性优化的vio算法的通用做法在局部窗口中构造代价函数，使用dogleg算法进行非线性优化求解，得到时间延迟和相机位置、速度、姿态的估计值；
其中，
所述代价函数中包括重投影误差项、imu误差项和边缘化误差项；
所述重投影误差项和所述边缘化误差项使用基于非线性优化的vio算法的通用做法进行构造；
所述imu误差项使用下述方法进行构造：
其中，cimu(x)表示imu误差项；x由x1,x2,…,xk组成；k表示滑动窗口内图像的帧序号；xk是第k帧的状态向量，
步骤s3003，将非线性优化求解得到的所述时间延迟
所述时间延迟的估计值
优选地，所述视觉惯性测程法还包括一次性执行：
步骤s2000，分别根据所述相机图像和所述imu量测值计算相机的姿态；计算出使得上述两组姿态偏差最小的时间延迟，作为时间延迟的初步估计值；
相应地，将步骤s2000计算得到的所述时间延迟的初步估计值作为步骤s3001中所述倒数第二帧图像的时间延迟
优选地，步骤s2000具体包括：
步骤s2001，检测是否有新的imu量测值加入滑动窗口，若是，则转至步骤s2003；否则，转至步骤s2002；
步骤s2002，检测是否有新的一帧图像加入滑动窗口，若是，则转至步骤s2004；否则，转至步骤s2001；
步骤s2003，计算转角变化量δq(t)并存入队列，更新非线性优化的滑动窗口中的所述累计转角q；转至步骤s2005；
其中，
q为用四元数表示的所述累计转角，是队列中全部所述转角变化量的累计值；
步骤s2004，若有新的一帧图像加入滑动窗口，则判断该图像加入窗口前窗口内最后一帧图像是否为关键帧；若其不是关键帧则将其丢弃，否则窗口内最前面一帧旧的图像滑出窗口；计算滑动窗口中每一帧图像对应的第一个和最后一个imu量测值中角速度差的模长的平均值
若滑出窗口的是最前面的一帧图像，则对当前队列中每个所述转角变化量分别判断该转角变化量对应的时间戳是否小于该滑出图像的时间戳；若是，则将该转角变化量出队列，并更新所述累计转角：
其中，δq(t′)为根据时间戳为t′的imu量测值计算出的所述转角变化量；t′小于该滑出图像的时间戳；
步骤s2005，计算所述累计转角q对应的沿最近转轴转动的角度θ(q)：
其中，qx，qy，qz和qw表示四元数q的四个分量；
步骤s2006，若θ1<θ(qa)<θ2且
其中，阈值θ1和θ2用来筛选出具有较好几何结构的场景；阈值ω1和ω2用来平衡时间延迟估计的及时性和可靠性，若要提高及时性则减小ω1和ω2，若要提高可靠性则增大ω1和ω2；
步骤s2007，在应用本方法的vio算法运行的过程中，滑动窗口中的图像特征点和imu信息会共同参与优化得到滑动窗口中的相机位姿，将这个位姿作为本步骤中仅使用图像特征点的捆绑调整的初值。只使用图像上的标准化的特征点坐标作为观测值，对窗口内相机的k个位置和姿态进行捆绑调整；
步骤s2008，若捆绑调整失败，则继续转至步骤s2001；若成功，则转至步骤s2009；
步骤s2009，利用捆绑调整后的相机旋转计算两帧之间相机的相对旋转：
其中，
步骤s2010，将时间延迟在它的取值区间内进行离散采样；对于每一个采样得到的td，对于滑动窗口中每两帧相邻的图像，通过时间戳介于tk-td和tk+1-td之间的imu量测值中的角速度推算相邻两帧之间imu本体坐标系间的旋转四元数：
其中，tk和tk+1分别为相机第k帧和第k+1帧图像的时间戳；t为imu量测值的时间戳；
步骤s2011，结合已知的相机和imu坐标系间的旋转四元数
步骤s2012，对于不同的时间延迟td，计算由相机信息和imu信息获得的旋转四元数的差，并转换成旋转角度，在窗口内所有的相邻两帧之间取平均值，找到旋转角度平均值最小的td：
若
则记录一个时间延迟的估计值
其中，k为滑动窗口中包含的帧的数目；
步骤s2013，判断是否记录了预设个数的所述时间延迟的估计值
优选地，步骤s2007中“只使用图像上的标准化的特征点坐标作为观测值，对窗口内相机的k个位置和姿态进行捆绑调整”，具体为：
使用ceressolver库进行优化，使用huber范数作为损失函数，各个特征点的误差相互独立，每个特征点观测值的协方差矩阵均为：
其中，fx和fy分别为以x方向和y方向像素个数表示的相机焦距；
若捆绑调整完成后代价函数值小于预设的代价函数阈值，则认为捆绑调整成功，否则认为捆绑调整失败。
本发明的第二方面，提出一种存储设备，其存储有程序，所述程序适于由处理器加载并执行，以实现上面所述的使用在线时间标定的视觉惯性测程法。
本发明的第三方面，提出一种处理设备，包括：处理器和存储器；
所述处理器，适于执行程序；
所述存储器，适于存储该程序；
所述程序适于由所述处理器加载并执行，以实现上面所述的使用在线时间标定的视觉惯性测程法。
本发明的第四方面，提出一种使用在线时间标定的视觉惯性测程设备，包括：图像与imu量测值获取模块、时间延迟与相机姿态计算模块；
所述图像与imu量测值获取模块，配置为：实时获取相机图像和imu量测值，并根据imu时间基准，调整图像的曝光时刻时间戳；将调整时间戳后的所述相机图像和imu量测值加入滑动窗口；
所述时间延迟与相机姿态计算模块，配置为：当新的图像进入滑动窗口时，根据所述新的图像和上一帧图像的曝光时刻时间戳以及上一帧图像对应的时间延迟，调整对imu量测值进行预积分的时间区间，进而对imu量测值进行预积分；计算包含重投影误差项、imu误差项和边缘化误差项的代价函数，将所述时间延迟与相机位置、速度和姿态一起进行迭代优化，从而得到所述新的图像对应的所述时间延迟，以及相机位置、速度和姿态；
其中，所述imu误差项的残差向量中包含与所述时间延迟有关的变量。
优选地，所述视觉惯性测程设备还包括：时间延迟初值估计模块；
所述时间延迟初值估计模块，包括：分别根据所述相机图像和所述imu量测值计算相机的姿态；计算出使得上述两组姿态偏差最小的时间延迟，作为所述时间延迟的初值。
本发明的有益效果：
本发明所提出的使用在线时间标定的视觉惯性测程法，能够在对时间延迟没有任何先验知识的情况下，在基于非线性优化的视觉惯性测程法运行的同时估计出时间延迟，同时调整imu的预积分时间范围，使imu的预积分结果与图像数据在时间上保持一致。在eurocmav数据集上的测试结果显示，与已有的方法相比，本方法对时间延迟的估计更准确，而且当时间延迟存在时，使用本方法的vio算法得到的轨迹更准确。当不存在时间延迟时，本方法也不会对vio算法得到的轨迹产生明显的负面影响。在安卓手机采集的数据上，本方法相比已有的方法能更快地得到时间延迟的较准确的估计值，使得相机图像和imu数据的对应更准确，因此在设备快速转弯或突然停止的时候使用本方法的vio算法有更好的表现。
附图说明
图1是本发明的使用在线时间标定的视觉惯性测程法实施例一的流程示意图；
图2是本发明的使用在线时间标定的视觉惯性测程法实施例二的流程示意图；
图3(a)-(b)是视觉惯性测程法实施例一和视觉惯性测程法实施例二中时间延迟标定值随时间变化的比较图；
其中，图3(a)表示在eurocmav数据集的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)所示：
本实施例中，通过onimageavailable()函数获取image类型的对象，调用该对象的成员函数gettimestamp()获取相机时间基准下所述相机图像的曝光时刻时间戳；使用systemclock类的elapsedrealtime()函数获取imu时间基准下所述回调函数执行时刻时间戳。
在步骤s1002中，将调整时间戳后的所述相机图像，以及获取的imu量测值加入滑动窗口。
在步骤s3000中，进行时间延迟的精确标定：当新的图像进入滑动窗口时，根据所述新的图像和上一帧图像的曝光时刻时间戳以及上一帧图像对应的时间延迟，调整对imu量测值进行预积分的时间区间，进而对imu量测值进行预积分；计算包含重投影误差项、imu误差项和边缘化误差项的代价函数，将所述时间延迟与相机位姿一起进行迭代优化，从而得到所述新的图像对应的所述时间延迟，以及相机位姿；其中，所述imu误差项的残差向量中包含与所述时间延迟有关的变量。这种方法对imu误差项的表示比现有方法更加准确，而且利用到了采集的所有数据，因此对时间延迟的标定结果更加准确。
该步骤可以具体包括步骤s3001-s3003：
在步骤s3001中，若有一帧新的图像进入滑动窗口，则判断该图像加入窗口前窗口内最后一帧图像是否为关键帧；若其不是关键帧则将其丢弃，否则最前面一帧旧的图像滑出窗口，最新进入滑动窗口的图像的帧序号为k；如果最前面一帧旧的图像滑出窗口，那么其余各帧序号均减1，否则其余各帧序号不变；对时间戳位于
这里所说的“最后一帧”和“最前面一帧”，分别指滑动窗口中时间戳最新的和最旧的一帧图像。
其中，tk、tk-1分别为滑动窗口中最新图像和倒数第二帧图像的曝光时刻时间戳，均为经过步骤s1000调整后的值；
在步骤s3002中，按照基于非线性优化的vio算法的通用做法在局部窗口中构造代价函数，使用dogleg算法进行非线性优化求解，得到时间延迟和相机位置、速度、姿态的估计值。
其中，所述代价函数中包括重投影误差项、imu误差项和边缘化误差项；所述重投影误差项和所述边缘化误差项使用基于非线性优化的vio算法的通用做法进行构造；所述imu误差项使用如公式(2)所示的方法进行构造：
其中，cimu(x)表示imu误差项；x由x1,x2,…,xk组成；k表示滑动窗口内图像的帧序号；xk是第k帧的状态向量，
在步骤s3003中，将非线性优化求解得到的所述时间延迟
所述时间延迟的估计值
随着滑动窗口不断向前滑动，时间延迟的估计量
图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为用四元数表示的所述累计转角，是队列中全部所述转角变化量的累计值；
在步骤s2004中，若有新的一帧图像加入滑动窗口，则判断该图像加入窗口前窗口内最后一帧图像是否为关键帧；若其不是关键帧则将其丢弃，否则窗口内最前面一帧旧的图像滑出窗口；计算滑动窗口中每一帧图像对应的第一个和最后一个imu量测值中角速度差的模长的平均值
若滑出窗口的是最前面的一帧图像，则对当前队列中每个所述转角变化量分别判断该转角变化量对应的时间戳是否小于该滑出图像的时间戳；若是，则将该转角变化量出队列，并更新所述累计转角，如公式(12)所示：
其中，δq(t′)为根据时间戳为t′的imu量测值计算出的所述转角变化量；t′小于该滑出图像的时间戳。
在步骤s2005中，计算所述累计转角q对应的沿最近转轴转动的角度θ(q)，如公式(13)所示：
其中，qx，qy，qz和qw表示四元数q的四个分量；
在步骤s2006中，若θ1<θ(qa)<θ2且
其中，阈值θ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个位置和姿态进行捆绑调整的时候，使用ceressolver库进行优化，使用huber范数作为损失函数，各个特征点的误差相互独立，每个特征点观测值的协方差矩阵如公式(14)所示：
其中，fx和fy分别为以x方向和y方向像素个数表示的相机焦距。若捆绑调整完成后代价函数值小于预设的代价函数阈值0.07，则认为捆绑调整成功，否则认为捆绑调整失败。
在步骤s2008中，若捆绑调整失败，则继续转至步骤s2001；若成功，则转至步骤s2009。
在步骤s2009中，利用捆绑调整后的相机旋转计算两帧之间相机的相对旋转，如公式(15)所示：
其中，
在步骤s2010中，将时间延迟在它的取值区间内进行离散采样；对于每一个采样得到的td，对于滑动窗口中每两帧相邻的图像，通过时间戳介于tk-td和tk+1-td之间的imu量测值中的角速度推算相邻两帧之间imu本体坐标系间的旋转四元数，如公式(16)所示：
其中，tk和tk+1分别为相机第k帧和第k+1帧图像的时间戳；t为imu量测值的时间戳；
本实施例中，时间延迟td在th1和th2之间采样，采样间隔为δt。th1和th2分别取0ms,100ms,δt取3ms。
在步骤s2011中，结合已知的相机和imu坐标系间的旋转四元数
在步骤s2012中，对于不同的时间延迟td，计算由相机信息和imu信息获得的旋转四元数的差，并转换成旋转角度，在窗口内所有的相邻两帧之间取平均值，找到旋转角度平均值最小的td，如公式(18)所示：
若满足公式(19)所示的条件：
则记录一个时间延迟的估计值
在步骤s2013中，判断是否记录了预设个数的所述时间延迟的估计值
图3(a)-(b)是本发明视觉惯性测程法实施例一和实施例二中时间延迟标定值随时间变化的比较图，其中图3(a)表示在eurocmav数据集的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量测值计算相机的姿态；计算出使得上述两组姿态偏差最小的时间延迟，作为所述时间延迟的初值。
本领域技术人员应该能够意识到，结合本文中所公开的实施例描述的各示例的方法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明电子硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以电子硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
至此，已经结合附图所示的优选实施方式描述了本发明的技术方案，但是，本领域技术人员容易理解的是，本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下，本领域技术人员可以对相关技术特征做出等同的更改或替换，这些更改或替换之后的技术方案都将落入本发明的保护范围之内。