"可是第一特征在第二特征正上方或斜上方，或仅仅表示 第一特征水平高度高于第二特征。第一特征在第二特征"之下"、"下方"和"下面"可以是第 一特征在第二特征正下方或斜下方，或仅仅表示第一特征水平高度小于第二特征。
[0098] 在本说明书的描述中，参考术语"一个实施例"、"一些实施例"、"示例"、"具体示 例"、或"一些示例"等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特 点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不 必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任 一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技 术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结 合和组合。
[0099] 尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例 性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述 实施例进行变化、修改、替换和变型。
【主权项】
1. 一种基于视觉测程的同时定位与地图构建方法，其特征在于，包括以下步骤： S1:通过双目图像捕获系统同步采集双目图像，并对所述双目图像进行校正，以得到无 失真的双目图像； S2:对所述无失真的双目图像进行特征点提取，并根据提取的特征点生成特征点描述 子； S3:根据所述特征点描述子建立所述双目图像的特征点匹配关系； S4:根据所述双目图像的特征点匹配关系得到匹配特征点的水平视差，并根据所述匹 配特征点的水平视差和双目图像捕获系统的参数计算所述匹配特征点的空间真实深度； S5:获取当前帧双目图像的特征点，并将所述当前帧双目图像的特征点与世界地图中 的特征点进行匹配； S6:对所述当前帧双目图像的特征点与世界地图中的特征点的匹配结果进行分析，以 去除其中错误匹配的特征点，得到成功匹配的特征点； S7:计算所述成功匹配的特征点在世界坐标系下的坐标与当前参考坐标系下的三维坐 标的变换矩阵，根据所述变换矩阵得到所述双目图像捕获系统相对初始位置的位姿变化估 计值； S8:在首帧计算时，初始化所述世界地图，并在非首帧计算时，根据所述变换矩阵更新 世界地图中与当前帧成功匹配的特征点的三维坐标与特征描述子，并向世界地图中加入当 前帧新观察到的特征点及其特征描述子。2. 根据权利要求1所述的基于视觉测程的同时定位与地图构建方法，其特征在于，所述 S1进一步包括： 提取所述双目图像捕获系统的成像参数，并根据所述成像参数对所述双目图像进行校 正，以得到无失真的双目图像。3. 根据权利要求1所述的基于视觉测程的同时定位与地图构建方法，其特征在于，在所 述S2中，通过尺度不变特征转换方法对所述无失真的双目图像进行特征点提取。4. 根据权利要求1所述的基于视觉测程的同时定位与地图构建方法，其特征在于，在所 述S3中，通过判断所述双目图像的特征点描述子间的向量夹角是否小于特定阈值建立所述 双目图像的特征点间的匹配关系。5. 根据权利要求1所述的基于视觉测程的同时定位与地图构建方法，其特征在于，其 中，所述双目图像捕获系统包括左相机和右相机，所述S4进一步包括： 分别获取所述匹配特征点在左相机和右相机中的成像点的像素坐标； 根据相机成像分辨率及所述中心点的坐标计算所述左相机中的成像点和右相机中的 成像点的水平视差； 根据所述左相机中的成像点和右相机中的成像点的水平视差及空间几何关系得到所 述匹配特征点的空间真实深度，其中，所述空间真实深度的计算公式如下： BiXfc p Disparity 其中，心为所述匹配特征点的空间真实深度，Bl为基线，fc为焦距，Disparity为水平视 差。6. 根据权利要求1所述的基于视觉测程的同时定位与地图构建方法，其特征在于，所述 S5进一步包括： 将当前帧特征点的描述子按行存储在第一矩阵中，并将所述世界地图中的特征点的描 述子按行存储在第二矩阵中； 遍历所述世界地图中的全部特征点，从所述世界地图中分别提取与所述当前帧中的每 个特征点相似度最高的特征点； 根据所述相似度最高的特征点建立从所述当前帧特征点到所述世界地图的特征点映 射关系表。7. 根据权利要求6所述的基于视觉测程的同时定位与地图构建方法，其特征在于，所述 特征点的相似度根据所述特征点描述子向量间的夹角确定，其中，当所述特征点描述子向 量间的夹角最小时，所述特征点的相似度最高。8. 根据权利要求1所述的基于视觉测程的同时定位与地图构建方法，其特征在于，在所 述S6中，通过随机抽样一致方法对所述当前帧的特征点与世界地图中的特征点的匹配结果 进行分析，以去除其中错误匹配的特征点，得到成功匹配的特征点，具体包括： S61:随机从N组成功匹配的特征点中选取m组匹配特征点，并建立全局配准方程，根据 所述全局配准方程计算所述m组匹配特征点的坐标从世界坐标系变换到当前相机参考系的 变换矩阵； S62:将未选取的N-m组匹配特征点的当前图像捕获系统参考系下的坐标根据所述变换 矩阵逆映射到世界坐标系，得到坐标逆映射结果CoorTemp，并计算所述CoorTemp与特征点 在世界坐标系下的实际观察坐标CoorGlobal的欧氏距离； S63:判断所述欧式距离是否大于预设阈值，如果所述欧氏距离大于预设阈值，则判定 对应的特征点为局外点，如果所述欧式距离小于所述预设阈值，则判定对应的特征点为局 内点，并统计局内点的个数； S64:迭代执行所述S61至S63，并记录每次迭代过程中局内点的个数及坐标变换矩阵； S65:从多次迭代过程中选取包含局内点个数最多的一次迭代，并将该次迭代中的局内 点作为成功匹配的特征点，将局外点作为错误匹配的特征点，并去除所述错误匹配的特征 点。9. 根据权利要求1所述的基于视觉测程的同时定位与地图构建方法，其特征在于，所述 S7进一步包括： 获取所述成功匹配的特征点在世界坐标系下的坐标CoorGlobal及在当前图像捕获系 统参考系下的坐标CoorCamera; 计算所述成功匹配的特征点在三维坐标间的旋转矩阵R和平移矩阵T，根据所述旋转矩 阵R和平移矩阵T得到所述双目图像捕获系统相对初始位置的位姿变化估计值，其中，所述 旋转矩阵R和平移矩阵T满足如下关系式： min ^ 11 CoorCamera-T-RxCoorGlobal \ f SX mi ters 010. 根据权利要求9所述的基于视觉测程的同时定位与地图构建方法，其特征在于，所 述S8进一步包括： 在首帧计算时，初始化所述世界地图，将首帧的图像捕获系统参考系确定为本次定位 的世界坐标系，将首帧全部特征点的三维坐标与特征描述子加入世界地图； 在非首帧计算时，根据所述变换矩阵将当前帧成功匹配的特征点的三维坐标 CoorCamera逆映射到世界坐标系CoorGlobal'，其中，转换公式为： CoorGlobal7 = inv(R)X(CoorCamera-T)； 将所述成功匹配的特征点在世界坐标系下的坐标更新为多次观察的均值，其中，当第η +1次观察到该特征点，其世界地图中的坐标更新公式为： €戀沿議！；-1- m + i . 将所述成功匹配特征点的特征点描述子更新为最近一次观察的特征点描述子，并将当 前帧新观察到的特征点三维坐标与特征点描述子加入所述世界地图。
【专利摘要】本发明提出一种基于视觉测程的同时定位与地图构建方法，包括：采集双目图像并校正，得到无失真的双目图像；对无失真的双目图像进行特征点提取，生成特征点描述子；建立双目图像的特征点匹配关系；根据该匹配关系得到匹配特征点的水平视差，并结合双目图像捕获系统的参数计算空间真实深度；计算当前帧的特征点与世界地图中特征点匹配结果；去除其中错误匹配的特征点，得到成功匹配的特征点；计算成功匹配的特征点在世界坐标系下的坐标与当前参考坐标系下的三维坐标的变换矩阵，根据变换矩阵得到双目图像捕获系统相对初始位置的位姿变化估计值；建立及更新世界地图。本发明具有较低的计算复杂度，厘米级的定位精度及对位置估计的无偏特性。
【IPC分类】G06T7/00
【公开号】CN105469405
【申请号】CN201510845988
【发明人】谷梦媛, 郭开元, 汪玉, 王文强, 杨华中
【申请人】清华大学
【公开日】2016年4月6日
【申请日】2015年11月26日