坐标系，将首帧全部特征点的三维坐标与特 征描述子加入世界地图；在非首帧计算时，根据所述变换矩阵将当前帧成功匹配的特征点 的三维坐标CoorCamera逆映射到世界坐标系CoorGlobal'，其中，转换公式为：
[0023] CoorGlobal7 =inv(R) X (CoorCamera-T)；
[0024] 将所述成功匹配特征点在世界坐标系下的坐标更新为多次观察的均值，其中，当 第n+1次观察到该特征点，其世界地图中的坐标更新公式为：
[0025]
[0026] 将所述成功匹配特征点的特征点描述子更新为最近一次观察的特征点描述子，并 将当前帧新观察到的特征点三维坐标与特征点描述子加入所述世界地图。
[0027] 根据本发明实施例的基于视觉测程的同时定位与地图构建方法，通过双目相机定 位系统，可以在无先验知识的室内环境中恢复空间特征点的真实深度;采用视觉测程方法 进行位姿估计，有效的降低了同时定位与地图构建算法的计算复杂度，使得计算速度与世 界地图规模得到提升，适用于实时定位移动平台，且可以工作在更大范围的室内空间中；同 时，通过随机抽样一致方法有效的消除了错误匹配的特征点，避免为位姿估计引入误差信 息;此外，通过建立并更新世界地图有效消除了测量误差的累计，保证了室内定位精度在厘 米级别且不随时间发生漂移;此外，所构建的世界地图包含稠密的空间特征信息，可用于后 续障碍躲避，路径规划工作。
[0028] 本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变 得明显，或通过本发明的实践了解到。
【附图说明】
[0029] 本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得 明显和容易理解，其中：
[0030] 图1是根据本发明一个实施例的基于视觉测程的同时定位与地图构建方法的流程 图；
[0031] 图2是根据本发明一个实施例的三维重建空间几何原理示意图；
[0032] 图3是根据本发明一个实施例的随机抽样一致方法的流程图；
[0033] 图4是根据本发明一个实施例的世界坐标系与相机坐标系关系示意图；
[0034] 图5是根据本发明一个实施例的室内定位运动轨迹及世界地图示意图；
[0035] 图6是根据本发明一个实施例的室内无偏测试结果示意图。
【具体实施方式】
[0036] 下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终 相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附 图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
[0037] 以下结合附图描述根据本发明实施例的基于视觉测程的同时定位与地图构建方 法。
[0038] 图1是根据本发明一个实施例的基于视觉测程的同时定位与地图构建方法的流程 图。如图1所示，该方法包括以下步骤：
[0039] 步骤S1:通过双目图像捕获系统采集双目图像，并对双目图像进行校正，以得到无 失真的双目图像。
[0040] 在具体示例中，双目图像捕获系统为双目相机，其由两个平行放置的摄像头组成， 两摄像头间相距一定距离，该距离即为基线。具体地，双目摄像头受相同信号触发，保证所 拍摄图片的时刻相同。在具体示例中，双目图像捕获系统在移动平台运动中以每秒30帧的 速度捕捉一系列照片流，以作为定位系统的输入。
[0041] 进一步地，电荷親合器件(Charge Coupled Device，CO))相机会在成像过程中产 生由透镜成像造成的失真，如径向失真，倾斜失真等。对于双目图像捕获系统，每个单相机 都存在光学畸变，同时两个相机摆放角度不完全一致也会给后续图像数据处理带来困难。 更为具体地，两个相机参考系间存在旋转和平移的关系，该关系使得同一空间中的特征点 在两个相机中的像素坐标各自不同，从而增加了三维重建恢复真实深度的计算复杂度。因 此，在本发明的一个实施例中，例如通过matlab提供的双目相机校正工具箱提取双目相机 的成像参数，并根据该成像参数对所拍摄到的双目图像进行预先校正，使得校正后的图片 不存在光学畸变，且相同特征点在双目图像中的像素坐标具有相同的纵坐标，只在横坐标 方向存在差异，即得到无失真的双目图像。
[0042]步骤S2:对无失真的双目图像进行旋转、伸缩、光照不变的特征点提取，并根据提 取的特征点生成特征点描述子。
[0043]具体地，该步骤从校正后的双目图像中分别进行特征点检测，获得特征点在二维 图像中的像素坐标，并提取特征点的描述子。特征提取可能包括尺度不变特征（scale-invariant feature transform，SIFT) 和加速稳健特征 （Speeded Up Robust Features， SURF)等。
[0044] 在本发明的一个实施例中，例如采用尺度不变特征转换方法对无失真的双目图像 进行特征点提取。尺度不变特征转换方法是进行特征点提取并建立特征点间匹配关系的算 法。这种特征匹配算法在图像发生旋转、尺度缩放以及亮度改变时保持相对稳定。考虑到室 内属于近场环境，随着拍摄平台的移动空间物体的尺度缩放效应较为明显；同时室内存在 复杂的光照条件。因此，该算法适用于室内双目图像、帧间图像的特征点提取与匹配。相同 的特征点可从不同的拍摄条件和拍摄视角下提取出并建立特征匹配关系。在尺度不变特征 转换中，每个特征点描述子用128维的向量描述，两个特征点的匹配关系是通过判断其特征 点描述子的向量夹角是否小于特定阈值所决定。需要说明的是，本发明的实施例不仅限于 采用尺度不变特征转换方法对无失真的双目图像进行特征点提取，也可以为其它特征点提 取方法，此处仅是示例性的展示。
[0045] 步骤S3:根据特征点描述子建立双目图像的特征点匹配关系。该步骤即双目匹配 步骤，其中，在双目匹配中成功匹配的特征点可以根据三维重建步骤恢复其空间深度，而未 成功建立匹配的特征点在这一步骤淘汰。
[0046] 进一步地，双目匹配步骤通过判断特征点描述子间的向量夹角是否小于特定阈值 来确定特征点间的匹配关系，其具体操作步骤与之后的步骤S5类似。
[0047] 具体地，假设左图提取到的特征点的描述子按行存储在矩阵"[DscpU， DscpL2, · · ·，DscpLm]T)中，右图中的特征点的描述子存储在矩阵RQDscpRhDscpfc，· · ·， DscpRn]T)中。对于左图中的每个特征点，遍历右图的全部特征点，寻找与其最相似的特征点 建立从左图到右图的特征点映射关系。进一步地，相似性的度量由特征点描述子向量间的 夹角给出：向量间的夹角越小，两向量相似度越高;两特征点特征描述子的相似度越高，特 征点的匹配置信度越大。
[0048]步骤S4:根据双目图像的特征点匹配关系得到匹配特征点的水平视差，并根据匹 配特征点的水平视差和双目图像捕获系统(双目相机）的参数计算匹配特征点的空间真实 深度。其中，双目相机的参数例如包括:双目相机的焦距、基线及中心点等。
[0049]在本发明的一个实施例中，图像捕获系统包括左相机和右相机，步骤S4进一步包 括:分别获取匹配特征点在左相机和右相机中的成像点的像素坐标;根据相机中心点的坐 标计算左相机中的成像点和右相机中的成像点的水平视差;根据左相机中的成像点和右相 机中的成像点的水平视差及空间几何关系得到匹配特征点的空间真实深度，其中，空间真 实深度的计算公式如下：
[0050]
[0051] 其中，ZP为匹配特征点的空间真实深度，Bl为基线，fc为焦距，Disparity为水平视 差。
[0052] 具体地说，在以往的工作中，计算特征点的真实深度往往需要对环境有一定先验 知识，例如:场景中已知深度的物体，置入环境的特征标识等等。这限制了算法在未知环境 中的应用范围。在本发明的一个实施例中，无需预先获知空间中物体的深度或提前干涉环 境，通过双目相机的成像特点对空间中捕获到的特征点进行三维重建工作，可获得其相对 于拍摄时刻的相机参考系的空间三维坐标。
[0053] 进一步地，参照图2所示，给出了三维重建空间几何原理示意图（俯视）。校正后的 双目相机的相机参考系由（X1，yi，Z1)和(X2，y 2，Z2)表示，其具有相同的成像平面。空间中点P