在左相机中的成像点为a，在右相机中的成像点为PR。在步骤S2中可以获得双目相机参数如 焦距(fc)，基线(BL)，中心点等。特别地，对于双目图像中匹配的特征点，由于双目图像已经 过预先校正，其成像点Pl与Pr只在水平方向上存在像素差值，在竖直方向上像素坐标相同。 具体地，通过相机成像分辨率及中心点的坐标计算得到左相机中的成像点和右相机中的成 像点所对应的实际空间中的距离，即两个成像点的水平视差(Disparity)。进一步地，根据 空间几何关系计算得到匹配特征点（如点P)的空间真实深度2 [)，其中，空间真实深度的计算 公式如下：
[0054]
[0055] 进一步地，根据相机小孔成像原理，可由空间真实深度ZP，像素坐标(xP，yP)及相机 焦距fc计算出特征点在左相机参考系（后续计算以左相机为基准相机）中的三维坐标(X P， Yp，Zp)，具体计算方法为：
[0056]
[0057] 步骤S5:获取当前帧双目图像的特征点，并将当前帧双目图像的特征点与世界地 图中的特征点进行匹配。换言之，该步骤即世界地图匹配步骤，指在当前帧中提取到的特征 点与世界地图中所包含的特征点间建立匹配关系。
[0058] 在本发明的一个实施例中，该步骤S5进一步包括:将当前帧特征点的描述子按行 存储在第一矩阵中，并将世界地图中的特征点的描述子按行存储在第二矩阵中；遍历世界 地图中的全部特征点，从世界地图中分别提取与当前帧中的每个特征点相似度最高的特征 点;根据相似度最高的特征点建立从当前帧特征点到世界地图的特征点映射关系表。
[0059]具体地说，假设当前帧提取到的特征点的描述子按行存储在第一矩阵AUDscpQ， DscpC2, . . .，DscpCm]T)中，世界地图中包含的全部特征点的描述子存储在第二矩阵B ([DscpGi，DscpG2,. . .，DscpGn]T)中。对于当前帧的每个特征点，遍历世界地图中的全部特 征点，寻找与其最相似的特征点建立起从当前帧到世界地图的特征点映射关系。具体地，特 征点的相似度根据特征点描述子向量间的夹角确定，向量间的夹角越小，两向量相似度越 高，当特征点描述子向量间的夹角最小时，特征点的相似度最高。
[0060]进一步地，世界地图匹配包含以下操作：第二矩阵B与特征点i的描述子列向量 DscpCi相乘，结果为nX 1列向量，向量中的η个元素分别代表DscpCi和DscpGj( j = l. . .η)的 向量夹角的余弦值。进一步地，通过反余弦公式计算可得向量夹角，存储在η X 1列向量Resi 中，其中元素 ReSl(l)(l = l...n)代表当前帧提取到的第i个特征点与世界地图中第1个特 征点的特征点描述子的向量夹角。
[0061 ] 进一步地，遍历Resi中的每个元素，寻找其中最小的元素记作Residi)，及第二小 的元素记作ReSi(l2)，其中，1!和12表示最小及第二小元素在Re Si中的位置（即在世界地图所 包含特征点中的位置）。进一步地，当且仅当满足以下条件时当前帧的第i个特征点与世界 地图中第h个特征点成功匹配：
[0062] Resi(li)<〇.8Resi(l2)。
[0063] 进一步地，依次将世界地图特征点描述子存储矩阵B与当前帧每个特征点描述子 DscpCi相乘，相乘的过程可以通过矩阵运算加速。具体地，通过矩阵乘法B X Ατ，可以由一条 指令计算出当前帧全部特征点描述子与世界地图中各个特征点描述子的向量夹角的余弦 矩阵。进一步地，反余弦的计算例如可以通过麦克劳林展开式近似为一系列矩阵乘法、矩阵 加法运算。通过相关的矩阵运算库如MKL(Intel Math Kernel Library，英特尔数学核心函 数库），cuBLAS(NVIDIA Cuda Basic Linear Algebra Subroutines，英伟达基础线性代数 子程序库）可实现基于CPU(Central Processing Unit，中央处理器）及GPU(Graphic Processing Unit，图形处理器）的高性能矩阵运算。考虑到世界地图维数较大，因此，该方 法有效提升了世界地图匹配及双目匹配步骤在软件端的算法运行效率，提高了算法处理图 像的帧率，使移动平台的实时定位成为可能。
[0064] 步骤S6:对当前帧双目图像的特征点与世界地图中的特征点的匹配结果进行分 析，以去除其中错误匹配的特征点，得到成功匹配的特征点。具体地，世界地图匹配中会形 成一些错误匹配的特征点，这些错误匹配的特征点具有相似的特征点描述子，但并不属于 同一像点。错误匹配的特征点会给位姿估计引入误差，使得定位结果不准确。因此，在本发 明的一个实施例中，采用随机抽样一致方法对当前帧双目图像的特征点与世界地图中的特 征点的匹配结果进行分析，以去除其中错误匹配的特征点。具体地说，参照图3所示，给出了 随机抽样一致方法的流程图。随机抽样一致方法假设正确匹配的特征点（局内点)在世界系 下的坐标与当前相机参考系中的坐标间的坐标变换关系可由统一的模型描述，而错误匹配 的特征点（局外点)不符合该坐标变换模型。同时，错误匹配的特征点属于测量中的异常，在 整个特征点集中属小概率事件。
[0065] 具体地，在本发明的一个实施例中，通过随机抽样一致方法去除错误匹配特征点， 包括以下步骤：
[0066] S61:随机从N组成功匹配的特征点中选取m组匹配特征点，并建立全局配准方程， 根据全局配准方程计算所述m组匹配特征点的坐标从世界坐标系变换到当前相机参考系的 变换矩阵。
[0067] S62:将步骤S61中未选取的N-m组匹配特征点在当前相机参考系下的坐标根据步 骤S61中计算的变换矩阵逆映射到世界坐标系，得到坐标逆映射结果CoorTemp，并计算 CoorTemp与特征点在世界坐标系下的实际观察坐标CoorGlobal的欧氏距离，称为匹配代 价。
[0068] S63:判断步骤S62中欧式距离（匹配代价)是否大于预设阈值，以及如果该欧氏距 离大于预设阈值，则判定对应的特征点为局外点，即不符合步骤S61中的坐标变换矩阵;如 果该欧式距离小于预设阈值，则判定对应的特征点为局内点，并统计本次计算中局内点的 个数。其中，该预设阈值既可以通过在环境中预先设定实验测量，也可以通过经验给出，并 且在设置预设阈值时要综合考虑实验环境与测量的过程噪声。
[0069] S64:迭代执行S61至S63(即重复执行步骤S61至S63)，并记录每次迭代过程中局内 点的个数及坐标变换矩阵；
[0070] S65:从多次迭代过程中选取包含局内点个数最多的一次迭代，并将该次迭代中的 局内点作为成功匹配的特征点，将局外点作为错误匹配的特征点，在后续计算中去除错误 匹配的特征点（局外点）。
[0071] 步骤S7:计算成功匹配的特征点在世界坐标系下的坐标与当前参考坐标系下的三 维坐标的变换矩阵，根据变换矩阵得到双目图像捕获系统(双目相机)相对初始位置的位姿 变化估计值。
[0072] 具体地，参照图4所示，给出了世界坐标系与相机坐标系关系示意图。世界坐标系 确定为图像拍摄首帧时刻的相机坐标系，在后续计算中不再发生改变。相机参考系随着载 有双目图像捕获系统的移动平台的运动，相对世界坐标系发生旋转与平移，位姿估计的目 标是获得两个坐标系间的变换关系，即为移动平台相对初始位置所发生的运动。进一步地， 通过步骤S1至步骤S6,获得成功匹配的特征点在世界坐标系下的坐标及其在相机参考系中 的坐标。根据几何原理，两坐标系的坐标系变换与同一点在两个坐标系下的坐标变换互为 逆过程，这是进行位姿估计的基础。
[0073]进一步地，获取成功匹配的特征点在世界坐标系下的坐标CoorGlobal及其在当前 图像捕获系统参考系下的坐标CoorCamera。三维坐标间的变换关系可以用旋转矩阵R和平 移矩阵T来描述，因此，可以计算成功匹配的特征点在三维坐标间的旋转矩阵R和平移矩阵 T，即得到双目图像捕获系统相对初始位置的位姿变化估计值。具体地，位姿估计的过程为 找到一组坐标变换关系R和T，使得通过该组坐标变换关系将匹配的特征点集从世界坐标系 转换到相机参考系后，与其在当前帧相机参考系下的实际观察坐标的均方误差之和最小， 也即旋转矩阵R和平移矩阵T满足如下关系式：
[0074]
[0075] 步骤S8:在首