CN102289809A

CN102289809A - 估计摄像机位姿的方法及装置

Info

Publication number: CN102289809A
Application number: CN2011102091711A
Authority: CN
Inventors: 戴琼海; 叶亘之; 刘烨斌
Original assignee: Tsinghua University
Current assignee: Tsinghua University
Priority date: 2011-07-25
Filing date: 2011-07-25
Publication date: 2011-12-21

Abstract

本发明公开了一种估计摄像机位姿的方法及装置。估计摄像机位姿的方法包括以下步骤：1)使用三轴陀螺仪和三轴加速计获取获取所述摄像机的运动参数；2)根据所述运动参数更新多个粒子的位置；3)获取地图特征点和图像特征点；以及4)将所述地图特征点与所述图像特征点进行匹配以生成观测概率模型并根据观测特征点更新所述地图特征点。根据本发明实施例的估计摄像机位姿的方法及装置，定位精度得到很大提高，粒子的重新分布更具针对性，减少了粒子数量，提高了算法的效率。

Description

估计摄像机位姿的方法及装置

技术领域

本发明涉及计算机视觉技术领域，特别地，本发明涉及一种估计摄像机位姿的方法及装置。

背景技术

摄像机位姿跟踪是计算机视觉中的重要问题。精确的摄像机位姿估计对于多视图三维重建、移动机器人定位具有重要意义。

在位姿估计问题中，一般可以分为地图已知和地图未知两种情况处理。地图已知的问题比较简单，只需将图像中识别到的路标与已知三维空间未知的真实路标进行比对即可。而与之对应，地图未知的问题相对困难，整个流程中，不但需要对摄像机的位姿进行估计，还要实时的完成对地图置信度的更新。

现今比较常用的针对地图未知情况下的摄像机位姿估计方法为SLAM(同步定位与地图创建)。该系统应用扩展卡尔曼滤波同步的完成机器人位姿的估计与地图的更新，但是定位精度较差，并且算法效率低。

发明内容

本发明的目的旨在至少解决上述技术缺陷之一。为此，本发明需要提供一种估计摄像机位姿的方法及装置，该估计摄像机位姿的方法及装置的优点是：观测概率模型更加精确、定位精度较高并且算法效率较高。

根据本发明的一个方面，提供了一种估计摄像机位姿的方法，包括以下步骤：1)使用三轴陀螺仪和三轴加速计获取所述摄像机的运动参数；2)根据所述运动参数更新多个粒子的位置；3)获取地图特征点和图像特征点；以及4)将所述地图特征点与所述图像特征点进行匹配以生成观测概率模型并根据观测特征点更新所述地图特征点。

根据本发明实施例的估计摄像机位姿的方法，融入了物理感应器(三轴陀螺仪和三轴加速计)的信息，实现了对摄像机位置及姿态的准确估计，使观测概率模型更加精确，定位精度得到提高。

根据本发明的一个实施例，将每个粒子定义为：p＝(α，β，γ，x，y，z)，其中p等价为四维变换矩阵

[\begin{matrix} R_{p} & T_{p} \\ 0 & 1 \end{matrix}] = [\begin{matrix} \cos β \cos γ & \cos β \sin γ & \sin β & x \\ \sin α \sin β \cos γ + \cos α \sin γ & - \sin α \sin β \sin γ + \cos α \cos γ & - \sin α \cos β & y \\ - \cos α \sin β \cos γ + \sin α \sin γ & \cos α \sin β \sin γ + \sin α \cos γ & \cos α \cos β & z \\ 0 & 0 & 0 & 1 \end{matrix}]

其中，α，β，γ是所述摄像机绕X、Y和Z轴旋转的角度，x，y，z是所述摄像机光心在X、Y和Z轴方向上的位移，R_p是粒子的旋转矩阵，T_p是粒子的位置向量。

根据本发明的一个实施例，所述步骤2)进一步包括步骤：读取所述三轴加速度计返回的在三轴方向检测到的加速度a_raw＝(a_x，a_y，a_z)；读取所述三轴陀螺仪返回的在三轴方向的旋转的角速度w_raw＝(w_α，w_β，w_γ)；根据从所述三轴加速度计和所述三轴陀螺仪返回所述运动参数的时间间隔，更新所述多个粒子的旋转矩阵，得到R_p＝R_p(w_rawΔt)+ΔR，其中，α，β，γ是所述摄像机绕X、Y和Z轴旋转的角度，x，y，z是所述摄像机光心在X、Y和Z轴方向上的位移，Δt是所述三轴陀螺仪两次返回角速度的时间间隔，ΔR是角度微扰；以及根据T_p＝T_p+∫∫(R_p×a_raw-g)dtdt+ΔT更新粒子的位置向量，其中ΔT为位移微扰。

根据本发明的一个实施例，所述步骤4)进一步包括步骤：对于所述多个粒子中的每个粒子，将所述地图特征点投影至所述每个粒子的视野内，得到在所述每个粒子上观测到的图像特征点；将在所述每个粒子上观测到的图像特征点与在真实图像上利用尺度不变特征变换检测出的特征点进行匹配，根据匹配度生成所述观测概率模型；以及根据观测特征点更新地图特征点。

根据本发明的一个实施例，将所述地图特征点的世界坐标转换至在所述多个粒子的坐标系中的坐标。

根据本发明的一个实施例，在所述多个粒子的坐标系中，将光心作为原点。

根据本发明的另一个方面，提供了一种估计摄像机位姿的装置，包括：参数获取模块，所述参数获取模块用于使用三轴陀螺仪和三轴加速计获取所述摄像机的运动参数；更新模块，所述更新模块用于根据所述运动参数更新多个粒子的位置；特征点获取模块，所述特征点获取模块用于获取地图特征点和图像特征点；以及匹配模块，所述匹配模块用于将所述地图特征点与所述图像特征点进行匹配以生成观测概率模型并根据观测特征点更新所述地图特征点。

根据本发明实施例的估计摄像机位姿的装置，融入了物理感应器(三轴陀螺仪和三轴加速计)的信息，实现了对摄像机位置及姿态的准确估计，使观测概率模型更加精确，定位精度得到提高。

[\begin{matrix} R_{p} & T_{p} \\ 0 & 1 \end{matrix}] = [\begin{matrix} \cos β \cos γ & \cos β \sin γ & \sin β & x \\ \sin α \sin β \cos γ + \cos α \sin γ & - \sin α \sin β \sin γ + \cos α \cos γ & - \sin α \cos β & y \\ - \cos α \sin β \cos γ + \sin α \sin γ & \cos α \sin β \sin γ + \sin α \cos γ & \cos α \cos β & z \\ 0 & 0 & 0 & 1 \end{matrix}]

根据本发明的一个实施例，所述更新模块用于：读取所述三轴加速度计返回的在三轴方向检测到的加速度a_taw＝(a_x，a_y，a_z)；读取所述三轴陀螺仪返回的在三轴方向的旋转地角速度w_raw＝(w_α，w_β，w_γ)；根据从所述三轴加速度计和所述三轴陀螺仪返回所述运动参数的时间间隔，更新所述多个粒子的旋转矩阵，得到R_p＝R_p(w_rawΔt)+ΔR，其中，α，β，γ是所述摄像机绕X、Y和Z轴旋转的角度，x，y，z是所述摄像机光心在X、Y和Z轴方向上的位移，Δt是所述三轴陀螺仪两次返回角速度的时间间隔，ΔR是角度微扰；以及根据T_p＝T_p+∫∫(R_p×a_raw-g)dtdt+ΔT更新粒子的位置向量，其中ΔT为位移微扰。

根据本发明的一个实施例，所述匹配模块用于：对于所述多个粒子中的每个粒子，将所述地图特征点投影至所述每个粒子的视野内，得到在所述每个粒子上观测到的图像特征点；将在所述每个粒子上观测到的图像特征点与在真实图像上利用尺度不变特征变换检测出的特征点进行匹配，根据匹配度生成所述观测概率模型；以及根据观测特征点更新地图特征点。

根据本发明的一个实施例，所述匹配模块将所述地图特征点的世界坐标转换至在所述多个粒子的坐标系中的坐标。

根据本发明实施例的估计摄像机位姿的方法及装置，由于融入了物理感应器(陀螺仪、加速度计)的信息，使粒子滤波器中的运动概率模型更加精确，定位精度得到很大提高；也使粒子的重新分布更具针对性，从而减少了粒子数量，提高了算法的效率。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1是根据本发明一个实施例的估计摄像机位姿的方法的流程图；

图2是根据本发明一个实施例的根据运动参数更新多个粒子的位置的方法的流程图；

图3是根据本发明一个实施例的生成观测概率模型的方法的流程图；以及

图4是根据本发明一个实施例的估计摄像机位姿的装置的框图。

具体实施方式

下面详细描述本发明的实施例，实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

需要说明的是，此外，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”、“第三”的特征可以明示或者隐含地包括一个或者更多个该特征。进一步地，在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。

下面参考附图描述本发明的具体实施例。

图1是根据本发明一个实施例的估计摄像机位姿的方法的流程图。如图1所示，估计摄像机位姿的方法包括下述步骤。

步骤S101，使用三轴陀螺仪和三轴加速计获取获取摄像机的运动参数。

步骤S102，根据运动参数更新多个粒子的位置。将每个粒子定义为：

p＝(α，β，γ，x，y，z)，其中p等价为四维变换矩阵

[\begin{matrix} R_{p} & T_{p} \\ 0 & 1 \end{matrix}] = [\begin{matrix} \cos β \cos γ & \cos β \sin γ & \sin β & x \\ \sin α \sin β \cos γ + \cos α \sin γ & - \sin α \sin β \sin γ + \cos α \cos γ & - \sin α \cos β & y \\ - \cos α \sin β \cos γ + \sin α \sin γ & \cos α \sin β \sin γ + \sin α \cos γ & \cos α \cos β & z \\ 0 & 0 & 0 & 1 \end{matrix}]

其中，α，β，γ是摄像机绕X、Y和Z轴旋转的角度，x，y，z是摄像机光心在X、Y和Z轴方向上的位移，R_p是粒子的旋转矩阵，T_p是粒子的位置向量。

图2是根据本发明一个实施例的根据运动参数更新多个粒子的位置的方法的流程图。如图2所示，步骤S102包括下述步骤。

步骤S1021，读取三轴加速度计返回的在三轴方向检测到的加速度a_raw＝(a_x，a_y，a_z)。

步骤S1022，读取三轴陀螺仪返回的在三轴方向的旋转的角速度w_raw＝(w_α，w_β，w_γ)。

步骤S1023，根据从三轴加速度计和三轴陀螺仪返回运动参数的时间间隔，更新多个粒子的旋转矩阵，得到R_p＝R_p(w_rawΔt)+ΔR，其中，α，β，γ是摄像机绕X、Y和Z轴旋转的角度，x，y，z是摄像机光心在X、Y和Z轴方向上的位移，Δt是三轴陀螺仪两次返回角速度的时间间隔，ΔR是角度微扰。

步骤S1024，根据T_p＝T_p+∫∫(R_p×a_raw-g)dtdt+ΔT更新粒子的位置向量，其中ΔT为位移微扰。

步骤S103，获取地图特征点和图像特征点。

步骤S104，将地图特征点与图像特征点进行匹配以生成观测概率模型并根据观测特征点更新地图特征点。

图3是根据本发明一个实施例的生成观测概率模型的方法的流程图。如图3所示，步骤S104包括以下步骤：

步骤S1041，对于多个粒子中的每个粒子，将地图特征点投影至每个粒子的视野内，得到在每个粒子上观测到的图像特征点。

步骤S1042，将在每个粒子上观测到的图像特征点与在真实图像上利用尺度不变特征变换检测出的特征点进行匹配，根据匹配度生成观测概率模型。

步骤S1043，根据观测特征点更新地图特征点。

具体地，对每一个图像特征点，找到与该图像特征点最匹配的虚拟图像特征点。具体地，对每个虚拟图像特征点进行遍历，定义两特征点的惩罚函数：

MatchCost(i，j)＝ε_D|D_i-D_j|+ε_L|L_i-L_j|

其中，|Di-Dj|是两个虚拟图像特征点的SIFT特征描述子的欧氏距离，|Li-Lj|是两个虚拟图像特征点的图像位置坐标的欧氏距离。对所有的虚拟图像特征点进行遍历后，与所求图像特征点惩罚最小的即为与之最为匹配的虚拟特征点。将该惩罚累计进总惩罚中。

TotalMatchCost+＝BestMatchCost(i)

对每个虚拟图像特征点做遍历，如果在之前的匹配过程中，没有相应的图像特征点与之对应，则将其作为漏匹配点。对每个这样的漏匹配点，将漏匹配惩罚累计进总惩罚中。

TotalMatchCost+＝NoMatchCost

最后，将总惩罚转换成概率，即为观测图像特征点与图像特征点的匹配度。设置惩罚截断常数CostTruncationValue，若TotalMatchCost＞CostTruncationValue，则视觉概率模型为：

VisualModelProbability＝0

若总惩罚小于等于惩罚截断常数，则视觉概率模型为：

VisualModelProbability＝

(CostTruncationValue-TotalMatchCost)/CostTruncationValue

图像上观察到的特征点，如果没有已确定的地图特征点与之对应，由于其深度未知，无法准确预知它的深度位置，但可以确定它一定位于光心与图像坐标的连线的延长线上。将该延长线上的特征点点离散取样，取样点的计数值加1。若某特征点的计数值超过5，则将其归类为已确定的地图特征点。将超过10帧未达到5个计数值的待定特征点直接删除。

根据本发明的一个实施例，将地图特征点的世界坐标转换至在多个粒子的坐标系中的坐标。在多个粒子的坐标系中，将光心作为原点。

图4是根据本发明一个实施例的估计摄像机位姿的装置的框图。如图4所示，估计摄像机位姿的装置10包括：参数获取模块110，更新模块120，特征点获取模块130和匹配模块140。

具体地，参数获取模块110用于使用三轴陀螺仪和三轴加速计获取摄像机的运动参数。更新模块120用于根据运动参数更新多个粒子的位置。特征点获取模块130用于获取地图特征点和图像特征点。匹配模块140用于将地图特征点与图像特征点进行匹配以生成观测概率模型并根据观测特征点更新地图特征点。

[\begin{matrix} R_{p} & T_{p} \\ 0 & 1 \end{matrix}] = [\begin{matrix} \cos β \cos γ & \cos β \sin γ & \sin β & x \\ \sin α \sin β \cos γ + \cos α \sin γ & - \sin α \sin β \sin γ + \cos α \cos γ & - \sin α \cos β & y \\ - \cos α \sin β \cos γ + \sin α \sin γ & \cos α \sin β \sin γ + \sin α \cos γ & \cos α \cos β & z \\ 0 & 0 & 0 & 1 \end{matrix}]

根据本发明的一个实施例，更新模块120用于：读取三轴加速度计返回的在三轴方向检测到的加速度a_raw＝(a_x，a_y，a_z)；读取三轴陀螺仪返回的在三轴方向的旋转地角速度w_raw＝(w_α，w_β，w_γ)；根据从三轴加速度计和三轴陀螺仪返回运动参数的时间间隔，更新多个粒子的旋转矩阵，得到R_p＝R_p(w_rawΔt)+ΔR，其中，α，β，γ是摄像机绕X、Y和Z轴旋转的角度，x，y，z是摄像机光心在X、Y和Z轴方向上的位移，Δt是三轴陀螺仪两次返回角速度的时间间隔，ΔR是角度微扰；以及根据T_p＝T_p+∫∫(R_p×a_raw-g)dtdt+ΔT更新粒子的位置向量，其中ΔT为位移微扰。

根据本发明的一个实施例，匹配模块130用于：对于多个粒子中的每个粒子，将地图特征点投影至每个粒子的视野内，得到在每个粒子上观测到的图像特征点；将在每个粒子上观测到的图像特征点与在真实图像上利用尺度不变特征变换检测出的特征点进行匹配，根据匹配度生成观测概率模型；以及根据观测特征点更新地图特征点。

MatchCost(i，j)＝ε_D|D_i-D_j|+ε_L|L_i-L_j|

TotalMatchCost+＝BestMatchCost(i)

TotalMatchCost+＝NoMatchCost

VisualModelProbability＝0

若总惩罚小于等于惩罚截断常数，则视觉概率模型为：

VisualModelProbability＝

(CostTruncationValue-TotalMatchCost)/CostTruncationValue

根据本发明的一个实施例，匹配模块130将地图特征点的世界坐标转换至在多个粒子的坐标系中的坐标。根据本发明的一个实施例，在多个粒子的坐标系中，将光心作为原点。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同限定。

Claims

1.一种估计摄像机位姿的方法，其特征在于，包括以下步骤：

1)使用三轴陀螺仪和三轴加速计获取获取所述摄像机的运动参数；

2)根据所述运动参数更新多个粒子的位置；

3)获取地图特征点和图像特征点；以及

4)将所述地图特征点与所述图像特征点进行匹配以生成观测概率模型并根据观测特征点更新所述地图特征点。

2.根据权利要求1所述的方法，其特征在于，将每个粒子定义为：

p＝(α，β，γ，x，y，z)，其中p等价为四维变换矩阵

[\begin{matrix} R_{p} & T_{p} \\ 0 & 1 \end{matrix}] = [\begin{matrix} \cos β \cos γ & \cos β \sin γ & \sin β & x \\ \sin α \sin β \cos γ + \cos α \sin γ & - \sin α \sin β \sin γ + \cos α \cos γ & - \sin α \cos β & y \\ - \cos α \sin β \cos γ + \sin α \sin γ & \cos α \sin β \sin γ + \sin α \cos γ & \cos α \cos β & z \\ 0 & 0 & 0 & 1 \end{matrix}]

3.根据权利要求2所述的方法，其特征在于，所述步骤2)进一步包括步骤：

读取所述三轴加速度计返回的在三轴方向检测到的加速度a_raw＝(a_x，a_y，a_z)；

读取所述三轴陀螺仪返回的在三轴方向的旋转的角速度w_raw＝(w_α，w_β，w_γ)；

根据从所述三轴加速度计和所述三轴陀螺仪返回所述运动参数的时间间隔，更新所述多个粒子的旋转矩阵，得到R_p＝R_p(w_rawΔt)+ΔR，其中Δt是所述三轴陀螺仪两次返回角速度的时间间隔，ΔR是角度微扰；以及

根据T_p＝T_p+∫∫(R_p×a_raw-g)dtdt+ΔT更新粒子的位置向量，其中ΔT为位移微扰。

4.根据权利要求1所述的方法，其特征在于，所述步骤4)进一步包括步骤：

对于所述多个粒子中的每个粒子，将所述地图特征点投影至所述每个粒子的视野内，得到在所述每个粒子上观测到的图像特征点；

将在所述每个粒子上观测到的图像特征点与在真实图像上利用尺度不变特征变换检测出的特征点进行匹配，根据匹配度生成所述观测概率模型；以及

根据观测特征点更新地图特征点。

5.根据权利要求4所述的方法，其特征在于，将所述地图特征点的世界坐标转换至在所述多个粒子的坐标系中的坐标。

6.根据权利要求5所述的方法，其特征在于，在所述多个粒子的坐标系中，将光心作为原点。

7.一种估计摄像机位姿的装置，其特征在于，包括：

参数获取模块，所述参数获取模块用于使用三轴陀螺仪和三轴加速计获取所述摄像机的运动参数；

更新模块，所述更新模块用于根据所述运动参数更新多个粒子的位置；

特征点获取模块，所述特征点获取模块用于获取地图特征点和图像特征点；以及

匹配模块，所述匹配模块用于将所述地图特征点与所述图像特征点进行匹配以生成观测概率模型并根据观测特征点更新所述地图特征点。

8.根据权利要求7所述的装置，其特征在于，将每个粒子定义为：

p＝(α，β，γ，x，y，z)，其中p等价为四维变换矩阵

[\begin{matrix} R_{p} & T_{p} \\ 0 & 1 \end{matrix}] = [\begin{matrix} \cos β \cos γ & \cos β \sin γ & \sin β & x \\ \sin α \sin β \cos γ + \cos α \sin γ & - \sin α \sin β \sin γ + \cos α \cos γ & - \sin α \cos β & y \\ - \cos α \sin β \cos γ + \sin α \sin γ & \cos α \sin β \sin γ + \sin α \cos γ & \cos α \cos β & z \\ 0 & 0 & 0 & 1 \end{matrix}]

9.根据权利要求8所述的装置，其特征在于，所述更新模块用于：

读取所述三轴陀螺仪返回的在三轴方向的旋转地角速度w_raw＝(w_α，w_β，w_γ)；以及

根据从所述三轴加速度计和所述三轴陀螺仪返回所述运动参数的时间间隔，更新所述多个粒子的旋转矩阵，得到R_p＝R_p(w_rawΔt)+ΔR，其中，Δt是所述三轴陀螺仪两次返回角速度的时间间隔，ΔR是角度微扰；以及

10.根据权利要求7所述的装置，其特征在于，所述匹配模块用于：

根据观测特征点更新地图特征点。

11.根据权利要求10所述的装置，其特征在于，所述匹配模块将所述地图特征点的世界坐标转换至在所述多个粒子的坐标系中的坐标。

12.根据权利要求11所述的装置，其特征在于，在所述多个粒子的坐标系中，将光心作为原点。