视觉里程计的实现方法和实现装置
技术领域
本发明涉及视觉里程计技术领域，尤其涉及一种视觉里程计的实现方法和实现装置。
背景技术
视觉里程计的目标是根据摄像机拍摄的图像估计摄像机的运动轨迹。当前视觉里程计的实现方法中，没有考虑摄像机采集的每帧图像之间的时域约束关系，独立求解每帧图像的摄像机姿态，忽略了连续帧图像之间摄像机运动的物理约束，这样导致最终的摄像机姿态计算结果会在实际位置周围抖动，不具有连续性且不符合轨迹应该满足的物理约束，会对机器人、自动驾驶的控制带来致命问题。
发明内容
有鉴于此，本发明提供一种视觉里程计的实现方法和实现装置，用于解决现有的视觉里程计得到的摄像机姿态的计算结果不准确的问题。
为解决上述技术问题，本发明提供一种视觉里程计的实现方法，包括：
建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；
建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；
根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；
对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。
可选的，作用在所述摄像机上的力能够分解成：世界坐标系下方向和大小保持不变的力Fw，摄像机坐标系下大小保持不变的力Fb，和，世界坐标系下在时刻tu新增加的力Fu；
所述对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度包括：
对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由Fu确定。
可选的，所述第一模型函数为：
其中，s(t)是所述摄像机在时刻t在三维空间中的位置，ti为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与ti的时间差，m为所述摄像机的质量，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。
可选的，所述第二模型函数为：
其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，ti为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与ti的时间差，°为四元数的乘法运算。
可选的，所述目标函数为：
其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，nk为第k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，
可选的，所述对所述目标函数进行优化之后，还包括：
若优化后的Fu大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为tu。
本发明还提供一种视觉里程计的实现装置，包括：
第一建立模块，用于建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；
第二建立模块，用于建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；
目标函数建立模块，用于根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；
优化模块，用于对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。
可选的，作用在所述摄像机上的力能够分解成：世界坐标系下方向和大小保持不变的力Fw，摄像机坐标系下大小保持不变的力Fb，和，世界坐标系下在时刻tu新增加的力Fu；
所述优化模块，还用于对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由Fu确定。
可选的，所述第一模型函数为：
其中，s(t)是所述摄像机在时刻t在三维空间中的位置，ti为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与ti的时间差，m为所述摄像机的质量，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。
可选的，所述第二模型函数为：
其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，ti为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与ti的时间差，°为四元数的乘法运算。
可选的，所述目标函数为：
其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，nk为第k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，
可选的，所述视觉里程计的实现装置还包括：
时间窗确定模块，用于若优化后的Fu大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为tu。
本发明还提供一种视觉里程计的实现装置，包括处理器、存储器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现上述视觉里程计的实现方法的步骤。
本发明还提供一种计算机可读存储介质，所述计算机可读存储介质上存储计算机程序，所述计算机程序被处理器执行时实现上述视觉里程计的实现方法的步骤。
本发明的上述技术方案的有益效果如下：
本发明实施例中，采用动力学模型对摄像机的运动轨迹进行连续建模，故在每一个时刻都可以得到摄像机的姿态(包括速度、加速度和/或角速度等)，考虑了连续帧图像之间摄像机运动的物理约束，可以保证求解得到的摄像机轨迹是满足实际的物理规律的且具有速度连续的二阶连续性质的轨迹，从而解决了相关的逐帧求解摄像机姿态的方法中，没有考虑帧与帧之间的时域约束导致最终的摄像机姿态计算结果会在实际位置周围抖动，对机器人或无人驾驶车的控制带来危害的问题。
同时，由于在每一个时刻都可以得到摄像机的姿态，故可以在任意时刻与惯性传感器的数据进行融合，这样能够充分利用惯性传感器的高帧率的特点，增强了视觉里程计的稳定性，减小了误差累积的问题。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例的描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例的视觉里程计的实现方法的流程示意图；
图2为本发明实施例的作用在摄像机上的力的分解示意图；
图3为本发明实施例的摄像机的角速度与摄像机的朝向的关系示意图；
图4为本发明实施例的动力学模型的优化方法的示意图；
图5为本发明实施例的对摄像机运动轨迹的分段方法示意图；
图6为本发明实施例的视觉里程计的实现装置的结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例的附图，对本发明实施例的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于所描述的本发明的实施例，本领域普通技术人员所获得的所有其他实施例，都属于本发明保护的范围。
请参考图1，图1为本发明实施例的视觉里程计的实现方法的流程示意图，该视觉里程计的实现方法包括：
步骤11：建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；
即采用作用在摄像机上的力建模摄像机的位置变化(即平移轨迹)。
步骤12：建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；
即采用摄像机的角速度建模摄像机的朝向变化。
步骤13：根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；
步骤14：对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。
作用在摄像机上的力与摄像机的加速度相关，加速度等于作用在摄像机上的合力除以摄像机的质量。因此得到作用在摄像机的力，即可得到摄像机的加速度。
本发明实施例中，采用动力学模型对摄像机的运动轨迹进行连续建模，故在每一个时刻都可以得到摄像机的姿态(包括速度、加速度和/或角速度等)，考虑了连续帧图像之间摄像机运动的物理约束，可以保证求解得到的摄像机轨迹是满足实际的物理规律的且具有速度连续的二阶连续性质的轨迹，从而解决了相关的逐帧求解摄像机姿态的方法中，没有考虑帧与帧之间的时域约束导致最终的摄像机姿态计算结果会在实际位置周围抖动，对机器人或无人驾驶车的控制带来危害的问题。
同时，由于在每一个时刻都可以得到摄像机的姿态，故可以在任意时刻与惯性传感器的数据进行融合，这样能够充分利用惯性传感器的高帧率的特点，增强了视觉里程计的稳定性，减小了误差累积的问题。
在本发明的一些实施例中，请参考图2，可以将作用在所述摄像机上的力分解成：世界坐标系下方向和大小保持不变的力Fw，摄像机坐标系下大小保持不变的力Fb，和，世界坐标系下在时刻tu新增加的力Fu。图2所示的实施例中，使用三个力Fw、Fb和Fu的合力近似摄像机的平移轨迹，图2所示的实施例中，假设摄像机架设在汽车上，故图2中摄像机采用汽车表示。图2中Cw为世界坐标系，Cc为摄像机坐标系，V(ti)为摄像机在ti时刻的速度，V(ti+Δt)为摄像机在ti+Δt时刻的速度，ω为摄像机的角速度。
本发明实施例中，可以根据作用在所述摄像机上的Fu，将摄像机的运动轨迹划分为多个时间窗下的多个分段，在同一个时间窗内，摄像机的运动具有一致性，可以采用相同的优化后的作用在所述摄像机上的力和所述摄像机的角速度描述摄像头的姿态。
即，所述对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度包括：对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由Fu确定。
当然，在本发明的其他一些实施例中，也可以只考虑作用在所述摄像机上Fw，不考虑Fb和Fu，从而使得动力学模型更加简化。
下面对如何建立用于表述摄像机的位置变化的第一模型函数进行说明。
本发明实施例中，可选的，所述第一模型函数为：
其中，s(t)是所述摄像机在时刻t在三维空间中的位置，ti是第i个时间窗内的开始时刻，Δt是第i个时间窗内的时刻t与ti的时间差，m是所述摄像机的质量，q(t)是所述摄像机在时刻t的朝向的四元数表示，R[q(t)是在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵，第i个时间窗内的t＝ti+Δt，t是第i个时间窗内的任意时刻。
上述公式中，摄像机的位置变化(即摄像机的平移轨迹)采用各个力(Fw、Fb和Fu)的积分进行表示。
下面对如何建立用于表述所述摄像机的朝向变化的第二模型函数进行说明。
本发明实施例中，请参考图3，可以对摄像机的角速度进行线性近似，使用角速度和朝向之间的关系，得到摄像机的朝向随时间的变化函数(即第二模型函数)。
摄像机的角速度满足以下公式：
ω(ti+Δt)＝ω(ti)+ω′(ti)Δt……(1)
其中，ω(t)表示摄像机在时刻t的角速度，ω′(t)为摄像机在时刻t的角加速度，ti是第i个时间窗内的开始时刻，Δt是第i个时间窗内的时刻t与ti的时间差。
摄像机的朝向q(t)与角速度满足以下公式：
其中，q’(t)数学上指的是q(t)的一阶导数或一阶微分，表示朝向变化趋势，°为指四元数的乘法运算，t＝ti+Δt。
对q(t)进行二阶泰勒展开并代入上述公式(1)，在去掉高阶项后可得摄像机朝向的函数表达(即第二模型函数)：
其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，ti是第i个时间窗内的开始时刻，Δt是第i个时间窗内的时刻t与ti的时间差，°为四元数的乘法运算。
下面对如何获得目标函数进行说明。
本发明实施例中，可以采用上述第一模型函数(s(t))、第二模型函数(q(t))对每帧图像的摄像机姿态Tcw(t)进行计算：
Tcw(t)＝[R(q(t)),s(t)]
Tcw(t)为从世界坐标系变换到摄像机坐标系的变换，s(t)是所述摄像机在时刻t在三维空间中的位置，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。
本发明实施例中，可以采用多种目标函数，例如可以采用最小化重投影误差得到的目标函数。
如果是采用最小化重投影误差得到的目标函数，上述计算出Tcw(t)之后，还需要计算重投影误差π(T(t)X)-p。
然后，得到目标函数：
其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，即θ＝{Fw,Fb,Fu}，φ为所述摄像机的角速度的集合，即φ＝{ω,ω′}，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，nk为第k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，
得到上述目标函数之后，可以采用优化算法对目标函数进行优化，得到优化后的作用在摄像机上的力和摄像机的角速度。本发明实施例中，可以采用多种类型的优化算法，例如Levenberg-Marquardt算法等。
上述公式中的后两项
本发明实施例中，由于在每一个时刻都可以得到摄像机的姿态(包括速度、加速度和/或角速度等)，故可以直接与惯性传感器的观测数据(加速度和角速度)做比较，惯性传感器的数据直接与视觉定位过程深度融合，请参考图4，不需要再将惯性传感器的数据进行积分得到摄像机姿态再做融合，这样能够充分利用惯性传感器的高帧率的特点，增强了视觉里程计的稳定性，减小了误差累积的问题。
图4中，θ为作用在所述摄像机上的力的集合，即θ＝{Fw,Fb,Fu}，φ为所述摄像机的角速度的集合，即φ＝{ω,ω′}，
本发明实施例中，由于真实世界中的力和角速度应当在合理的范围内，可选的，还可以加入了这些动力学参数(力和角速度)的范围约束：
其中，Fmin为人为指定的力的最小值，Fmax为人为指定的力的最大值，ωmin为人为指定的角速度的最小值，ωmax为人为指定的角速度的最大值。
本发明实施例中，可选的，所述对所述目标函数进行优化之后，还包括：若优化后的Fu大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为tu。即如果优化后的Fu大于或等于指定阈值，说明新增力Fu具有显著的作用，则认为当前的运行一致性被破坏，需要重新开始一个时间窗，并从Fu出现的时刻tu开始，利用新的动力学参数描述摄像机的运动轨迹，如图5所示。图5中的状态变化函数即上述Tcw(t)＝[R(q(t)),s(t)]，将时间窗开始时刻的角速度、位置等信息，变化到t时刻的角速度、位置等信息。从图5中可以看出，将摄像机的运动轨迹划分为多个分段，每一分段对应一个时间窗(时间窗i、j和k)。
请参考图6，本发明实施例还提供一种视觉里程计的实现装置，包括：
第一建立模块61，用于建立用于表述摄像机的位置变化的第一模型函数，所述第一模型函数中采用作用在摄像机上的力表述所述摄像机的位置变化；
第二建立模块62，用于建立用于表述所述摄像机的朝向变化的第二模型函数，所述第二模型函数中采用所述摄像机的角速度表述所述摄像机的朝向变化；
目标函数建立模块63，用于根据所述第一模型函数、所述第二模型函数以及惯性传感器观测到的所述摄像机的加速度和角速度，得到目标函数；
优化模块64，用于对所述目标函数进行优化，得到优化后的作用在所述摄像机上的力和所述摄像机的角速度。
可选的，作用在所述摄像机上的力能够分解成：世界坐标系下方向和大小保持不变的力Fw，摄像机坐标系下大小保持不变的力Fb，和，世界坐标系下在时刻tu新增加的力Fu；
所述优化模块，还用于对所述目标函数进行优化，得到一时间窗内的优化后的作用在所述摄像机上的力和所述摄像机的角速度，所述时间窗的开始时刻由Fu确定。
可选的，所述第一模型函数为：
其中，s(t)是所述摄像机在时刻t在三维空间中的位置，ti为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与ti的时间差，m为所述摄像机的质量，q(t)为所述摄像机在时刻t的朝向的四元数表示，R[q(t)为在时刻t摄像机坐标系变换到世界坐标系下的变换矩阵。
可选的，所述第二模型函数为：
其中，q(t)为所述摄像机在时刻t的朝向的四元数表示，ω(t)为所述摄像机在时刻t的角速度，ti为第i个时间窗内的开始时刻，Δt为第i个时间窗内的时刻t与ti的时间差，°为四元数的乘法运算。
可选的，所述目标函数为：
其中，i为时间窗的计数值，θ为作用在所述摄像机上的力的集合，φ为所述摄像机的角速度的集合，N为第i个时间窗内的图像的帧数，k为第i个时间窗内的图像的帧的计数值，j为第k帧图像中的特征点的计数值，nk为第k帧图像中的特征点的总数，π(·)为三维点X投影到二维的函数，
可选的，所述视觉里程计的实现装置还包括：
时间窗确定模块，用于若优化后的Fu大于或等于指定阈值，确定新的时间窗开始，所述新的时间窗的开始时刻为tu。
在本发明的实施例中，为了便于理解和描述，使用了与所要执行的功能对应的模块的描述方式，容易理解，这些模块是功能实体，不一定必须与物理或逻辑上独立的实体相对应。可以通过通用处理器运行对应功能的软件以执行计算机指令的形式来实现这些功能实体，或在一个或多个硬件模块或集成电路中可编程地实现这些功能实体，或设计为专门执行对应功能的集成电路来实现这些功能实体。
本发明实施例还提供一种视觉里程计的实现装置，包括处理器、存储器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现上述视觉里程计的实现方法的步骤。
本发明实施例还提供一种计算机可读存储介质，所述计算机可读存储介质上存储计算机程序，所述计算机程序被处理器执行时实现上述视觉里程计的实现方法的步骤。
在本发明的实施例中，处理器可以是中央处理器(CPU)、现场可编程逻辑阵列(FPGA)、单片机(MCU)、数字信号处理器(DSP)、专用集成电路(ASIC)等具有数据处理能力和/或程序执行能力的逻辑运算器件。
在本发明的实施例中，所述计算机可读存储介质包括但不限于例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(RAM)和/或高速缓冲存储器(Cache)等。所述非易失性存储器例如可以包括只读存储器(ROM)、硬盘、闪存等。
除非另作定义，本发明中使用的技术术语或者科学术语应当为本发明所属领域内具有一般技能的人士所理解的通常意义。本发明中使用的“第一”、“第二”以及类似的词语并不表示任何顺序、数量或者重要性，而只是用来区分不同的组成部分。“连接”或者“相连”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电性的连接，不管是直接的还是间接的。“上”、“下”、“左”、“右”等仅用于表示相对位置关系，当被描述对象的绝对位置改变后，则该相对位置关系也相应地改变。
以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。
