本发明公开了一种三维地图的创建方法及系统。其中，该方法包括：在RGB图像Is中选取相互之间的欧氏距离大于或者等于距离阈值的原始特征点放入集合Fs；将在RGB图像It中与原始特征点相匹配的对应特征点放入集合Ft；将Fs和Ft中的特征点投影到以RGB‑D相机为原点的三维空间相机坐标系中，得到三维点，并将得到的三维点分别放入配准点集Ps、Pt；基于公式计算出配准点集Ps和配准点集Pt之间的刚性变换矩阵Tt,s；根据刚性变换矩阵Tt,s得到相机的位姿；根据刚性变换矩阵Tt,s和相机的位姿创建三维地图。本发明通过引入特征点之间的欧氏距离作为阈值对特征点进行离散选取，有效解决了被选取特征点过于紧密易导致算法陷入局部最优而影响配准精度的问题，提高了配准精度。
一种三维地图的创建方法及系统
技术领域
[0001] 本发明涉及图像处理的技术领域，尤其涉及一种三维地图的创建方法及系统。
背景技术
[0002] SLAM(Simultaneous Localization And Mapping，同时定位与地图创建)是移动机器人的一项重要任务。随着传感器技术的发展和进步，研究者们提出了基于各种传感器的地图创建方法，且三维地图相对于二维地图来说，包含了更多有价值的形状信息和环境信息。因此，随着以Kinect为代表的RGB-D相机的问世，基于RGB-D相机的三维地图创建逐步成为近年来的研究热点。
[0003] 为了得到完整的三维地图信息，需要解决的问题是如何将相机从不同视角获得的数据通过坐标变换配准到同一坐标系下，而求解坐标变换参数的过程被称为运动估计。针对这个问题，研究者们提出了各种解决方案，其中最常用的是由Besl和McKay在1992年提出的迭代最近点(Iterative Closest Point,ICP)算法。然而该算法虽然具有较为精确的配准效果，但仍存在如下不足之处：
[0004] 随机选取的特征点可能出现过于紧密的情况，从而导致算法陷入局部最优，使得配准结果无法满足全局的配准要求，进而导致配准精度不高。
发明内容
[0005] 本发明通过提供一种三维地图的创建方法及系统，解决了现有技术中算法易陷入局部最优的技术问题，实现了提高配准精度的技术效果。
[0006] 本发明提供了一种三维地图的创建方法，至少包括：
[0007] 在RGB图像Is中选取相互之间的欧氏距离大于或者等于距离阈值的原始特征点放入集合Fs；
[0008] 将在RGB图像It中与所述原始特征点相匹配的对应特征点放入集合Ft；
[0009] 将所述集合Fs和所述集合Ft中的特征点投影到以RGB-D相机为原点的三维空间相机坐标系中，得到三维点，并将得到的三维点分别放入配准点集Ps、Pt；
[0010] 通过迭代方法基于公式 计算出所述配准点集Ps和所述配准点集Pt之间的刚性变换矩阵Tt,s；其中，N为点集中特征点的数量，λ为预设的比较阈值；
[0011] 根据所述刚性变换矩阵Tt,s得到所述RGB-D相机的位姿；
[0012] 根据所述刚性变换矩阵Tt,s和所述RGB-D相机的位姿创建三维地图。
[0013] 进一步地，所述将在RGB图像It中与所述原始特征点相匹配的对应特征点放入集合Ft，具体包括：
[0014] 根据所述原始特征点计算变换矩阵；
[0015] 基于所述变换矩阵在所述RGB图像It中找出与所述原始特征点相匹配的对应特征点；
[0016] 计算所述原始特征点与所述对应特征点之间的欧式距离d；
[0017] 将所述欧式距离d与比较距离阈值D进行比较；
[0018] 若所述欧式距离d大于或者等于所述比较距离阈值D，将所述对应特征点放入所述集合Ft。
[0019] 进一步地，所述根据所述刚性变换矩阵Tt,s得到所述RGB-D相机的位姿，具体包括：
[0020] 通过公式Pi+1＝Ti+1,iPi，计算得到所述RGB-D相机在i+1时刻的位姿Pi+1；其中，Ti+1,i为所述RGB-D相机在i+1时刻的位姿Pi+1与所述RGB-D相机在i时刻的位姿Pi之间的刚性变换矩阵，Pi为所述RGB-D相机在i时刻的位姿；
[0021] 通过公式 计算得到所述RGB-D相机在初始时刻的位姿Po；其中，Tn-1,n为所述RGB-D相机在n时刻的位姿与所述RGB-D相机在n-1时刻的位姿之间的刚性变换矩阵；其中，n≤i。
[0022] 进一步地，所述根据所述刚性变换矩阵Tt,s和所述RGB-D相机的位姿创建三维地图，具体包括：
[0023] 基于所述刚性变换矩阵Tt,s和所述RGB-D相机的位姿，将所述配准点集Ps和所述配准点集Pt中的三维点经过旋转和平移变换，从而将每一时刻得到的点云信息对齐到同一坐标系中，得到所述三维地图。
[0024] 进一步地，在所述通过迭代方法基于公式 计算出所述配准点集Ps和所述配准点集Pt之间的刚性变换矩阵Tt,s之后，通过代价函数对所述刚性变换矩阵Tt,s进行优化；其中，Pi为所述
RGB-D相机在i时刻的位姿，Pj为所述RGB-D相机在j时刻的位姿，C为时刻的总数，Tij为Pi和Pj之间的约束，Ωij为Pi和Pj之间的期望和信息矩阵，e(Pi,Pj,Tij)为误差函数向量。
[0025] 本发明提供的三维地图的创建系统，至少包括：
[0026] 第一数据获取模块，用于在RGB图像Is中选取相互之间的欧氏距离大于或者等于距离阈值的原始特征点放入集合Fs；
[0027] 第二数据获取模块，用于将在RGB图像It中与所述原始特征点相匹配的对应特征点放入集合Ft；
[0028] 第三数据获取模块，用于将所述集合Fs和所述集合Ft中的特征点投影到以RGB-D相机为原点的三维空间相机坐标系中，得到三维点，并将得到的三维点分别放入配准点集Ps、Pt；
[0029] 运算模块，用于通过迭代方法基于公式 计算出所述配准点集Ps和所述配准点集Pt之间的刚性变换矩阵Tt,s；其中，N为点集中特征点的数量，λ为预设的比较阈值；
[0030] 数据处理模块，用于根据所述刚性变换矩阵Tt,s得到所述RGB-D相机的位姿；
[0031] 地图创建模块，用于根据所述刚性变换矩阵Tt,s和所述RGB-D相机的位姿创建三维地图。
[0032] 进一步地，所述第二数据获取模块，具体包括：
[0033] 第一计算单元，用于根据所述原始特征点计算变换矩阵；
[0034] 匹配单元，用于基于所述变换矩阵在所述RGB图像It中找出与所述原始特征点相匹配的对应特征点；
[0035] 第二计算单元，用于计算所述原始特征点与所述对应特征点之间的欧式距离d；
[0036] 比较单元，用于将所述欧式距离d与比较距离阈值D进行比较；
[0037] 存储单元，用于若所述比较单元的结果为所述欧式距离d大于或者等于所述比较距离阈值D，将所述对应特征点放入所述集合Ft。
[0038] 进一步地，所述数据处理模块，具体包括：
[0039] 第一数据处理单元，用于通过公式Pi+1＝Ti+1,iPi，计算得到所述RGB-D相机在i+1时刻的位姿Pi+1；其中，Ti+1,i为所述RGB-D相机在i+1时刻的位姿Pi+1与所述RGB-D相机在i时刻的位姿Pi之间的刚性变换矩阵，Pi为所述RGB-D相机在i时刻的位姿；
[0040] 第二数据处理单元，用于通过公式 计算得到所述RGB-D相机在初始时刻的位姿Po；其中，Tn-1,n为所述RGB-D相机在n时刻的位姿与所述RGB-D相机在n-1时刻的位姿之间的刚性变换矩阵；其中，n≤i。
[0041] 进一步地，所述地图创建模块，具体用于基于所述刚性变换矩阵Tt,s和所述RGB-D相机的位姿，将所述配准点集Ps和所述配准点集Pt中的三维点经过旋转和平移变换，从而将每一时刻得到的点云信息对齐到同一坐标系中，得到所述三维地图。
[0042] 进一步地，还至少包括：
[0043] 数据优化模块，用于通过代价函数 对所述刚性变换矩阵Tt,s进行优化；其中，Pi为所述RGB-D相机在i时刻的位姿，Pj为所述RGB-D相机在j时刻的位姿，C为时刻的总数，Tij为Pi和Pj之间的约束，Ωij为Pi和Pj之间的期望和信息矩阵，e(Pi,Pj,Tij)为误差函数向量。
[0044] 本发明中提供的一个或多个技术方案，至少具有如下技术效果或优点：
[0045] 通过引入特征点之间的欧氏距离作为阈值对特征点进行离散选取，有效解决了被选取特征点过于紧密易导致算法陷入局部最优而影响配准精度的问题，提高了配准精度。
附图说明
[0046] 图1为本发明实施例提供的三维地图的创建方法的流程图；
[0047] 图2为通过本发明实施例提供的三维地图的创建方法对匹配点进行初始配准前后的对比图；
[0048] 图3为通过本发明实施例提供的三维地图的创建方法创建的三维地图的效果图；
[0049] 图4为本发明实施例提供的三维地图的创建系统的模块图。
具体实施方式
[0050] 本发明实施例通过提供一种三维地图的创建方法及系统，解决了现有技术中算法易陷入局部最优的技术问题，实现了提高配准精度的技术效果。
[0051] 本发明实施例中的技术方案为解决上述技术问题，总体思路如下：
[0052] 通过引入特征点之间的欧氏距离作为阈值对特征点进行离散选取，有效解决了被选取特征点过于紧密易导致算法陷入局部最优而影响配准精度的问题，提高了配准精度。
[0053] 为了更好地理解上述技术方案，下面将结合说明书附图以及具体的实施方式对上述技术方案进行详细的说明。
[0054] 参见图1，本发明实施例提供的三维地图的创建方法，至少包括：
[0055] 步骤S110：在RGB图像Is中选取相互之间的欧氏距离dist大于或者等于距离阈值β×distmax的原始特征点放入集合Fs；其中，β为阈值参数，distmax为特征点之间最大的欧式距离。
[0056] 在本实施例中，若阈值参数β设置过小，则会导致无法对特征点进行有效的离散选取。若阈值参数β设置过大，则会导致满足规则的特征点数目不足，从而影响算法二次配准的精度，因此阈值参数β的取值至关重要。在本实施例中，β的取值在0.3-0.5之间。
[0057] 步骤S120：将在RGB图像It中与原始特征点相匹配的对应特征点放入集合Ft；
[0058] 对本步骤进行说明，步骤S120具体包括：
[0059] 根据原始特征点计算变换矩阵；
[0060] 基于变换矩阵在RGB图像It中找出与原始特征点相匹配的对应特征点；
[0061] 计算原始特征点与对应特征点之间的欧式距离d；
[0062] 将欧式距离d与比较距离阈值D进行比较；
[0063] 若欧式距离d大于或者等于比较距离阈值D，将对应特征点放入集合Ft，这些特征点被称为内点(匹配正确的特征点)。利用内点数据比较准确的特点，实现了对特征点集间的初始位姿的优化，消除了误匹配。
[0064] 若欧式距离d小于比较距离阈值D，将对应特征点丢弃，去除误匹配的特征点。
[0065] 参见图2，上方为未经过初始配准的匹配点，下方为经过初始配准之后的匹配点，可以看出经初始配准之后，有效地去除了特征点中的误匹配。
[0066] 需要说明的是，还根据对应特征点之间的欧氏距离进一步判断其是否匹配；
[0067] 具体地，设U1V1和U2V2分别代表特征点P1和特征点P2在图像坐标系UV中的坐标，有欧氏距离计算公式：
[0068]
[0069] 欧氏距离越小，表示特征点的差异程度越低。首先计算特征点匹配对之间的最小欧氏距离distmin，然后根据劳氏算法(Lowe’s algorithm)计算每个特征点匹配对之间的欧氏距离dist。接着将特征点匹配对之间的欧氏距离dist与预设阈值a*distmin进行比较。若dist≤α*distmin，则说明该特征点对匹配成功，反之说明该特征点对匹配失败，需要重新匹配。在本实施例中，劳氏算法中的参数α为4。
[0070] 还需要说明的是，本发明实施例分别在5个不同数据集环境下进行实验，且数据集包含由Kinect传感器采集的分辨率为640×480的彩色图像和深度图像序列、相机的内参以及由高精度运动捕捉系统获取的相机真实运动轨迹，且提供了工具计算相机真实位姿和算法估计位姿之间的均方根误差(RSME)，从而有效地评估了算法的准确性。
[0071] 步骤S130：将集合Fs和集合Ft中的特征点投影到以RGB-D相机为原点的三维空间相机坐标系中，得到三维点，并将得到的三维点分别放入配准点集Ps、Pt；
[0072] 步骤S140：通过迭代方法基于公式 计算出配准点集Ps和配准点集Pt之间的刚性变换矩阵Tt,s；其中，N为点集中特征点的数量，λ为预设的比较阈值。
[0073] 对本步骤进行说明，ICP算法根据最小二乘算法最小化目标函数E，直到E小于预设的比较阈值λ时停止迭代，最终得到两个点集间的刚性变换矩阵Tt,s。
[0074] 需要说明的是，若预设的比较阈值λ设置过大，则会使算法无法得到精确的配准效果。若λ设置过小，则会使算法无法快速收敛。在本实施例中，λ的取值在0.05到0.08之间，从而使算法的准确性和时间消耗达到一个较好的平衡点。
[0075] 为了减少累计误差，进一步优化位姿，通过代价函数对刚性变换矩阵Tt,s进行优化；其中，Pi为RGB-D相机
在i时刻的位姿，Pj为RGB-D相机在j时刻的位姿，C为时刻的总数，Tij为Pi和Pj之间的约束，Ωij为Pi和Pj之间的期望和信息矩阵，e(Pi,Pj,Tij)为误差函数向量。
[0076] 对本步骤进行说明：
[0077] 先通过非线性最小二乘方法得到该代价函数的最小值，即得到不同时刻之间最小误差时的各位姿，再通过不同时刻之间最小误差时的各位姿计算得到各位姿之间的刚性变换矩阵Tt,s，从而实现了对刚性变换矩阵Tt,s的优化。
[0078] 步骤S150：根据刚性变换矩阵Tt,s得到RGB-D相机的位姿；
[0079] 对本步骤进行说明，步骤S150具体包括：
[0080] 通过公式Pi+1＝Ti+1,iPi，计算得到RGB-D相机在i+1时刻的位姿Pi+1；其中，Ti+1,i为RGB-D相机在i+1时刻的位姿Pi+1与RGB-D相机在i时刻的位姿Pi之间的刚性变换矩阵，Pi为RGB-D相机在i时刻的位姿；
[0081] 通过公式 计算得到RGB-D相机在初始时刻的位姿Po；其中，Tn-1,n为RGB-D相机在n时刻的位姿与RGB-D相机在n-1时刻的位姿之间的刚性变换矩阵；其中，n≤i。
[0082] 步骤S160：根据刚性变换矩阵Tt,s和RGB-D相机的位姿创建三维地图。
[0083] 对本步骤进行说明，步骤S160具体包括：
[0084] 基于刚性变换矩阵Tt,s和RGB-D相机的位姿，将配准点集Ps和配准点集Pt中的三维点经过旋转和平移变换，从而将每一时刻得到的点云信息对齐到同一坐标系中，得到三维地图。
[0085] 参见图3，通过本发明实施例获得了一致性较好的地图，有效地恢复了实验场景，且轮廓清晰，建立了与数据集环境基本一致的三维点云地图。
[0086] 参见图4，本发明实施例提供的三维地图的创建系统，至少包括：
[0087] 第一数据获取模块100，用于在RGB图像Is中选取相互之间的欧氏距离大于或者等于距离阈值的原始特征点放入集合Fs；
[0088] 第二数据获取模块200，用于将在RGB图像It中与原始特征点相匹配的对应特征点放入集合Ft；
[0089] 在本实施例中，第二数据获取模块200，具体包括：
[0090] 第一计算单元，用于根据原始特征点计算变换矩阵；
[0091] 匹配单元，用于基于变换矩阵在RGB图像It中找出与原始特征点相匹配的对应特征点；
[0092] 第二计算单元，用于计算原始特征点与对应特征点之间的欧式距离d；
[0093] 比较单元，用于将欧式距离d与比较距离阈值D进行比较；
[0094] 存储单元，用于若比较单元的结果为欧式距离d大于或者等于比较距离阈值D，将对应特征点放入集合Ft，这些特征点被称为内点(匹配正确的特征点)。
[0095] 丢弃单元，用于若比较单元的结果为欧式距离d小于比较距离阈值D，将对应特征点丢弃，去除误匹配的特征点。
[0096] 第三数据获取模块300，用于将集合Fs和集合Ft中的特征点投影到以RGB-D相机为原点的三维空间相机坐标系中，得到三维点，并将得到的三维点分别放入配准点集Ps、Pt；
[0097] 运算模块400，用于通过迭代方法基于公式 计算出配准点集Ps和配准点集Pt之间的刚性变换矩阵Tt,s；其中，N为点集中特征点的数量，λ为预设的比较阈值。
[0098] 需要说明的是，若预设的比较阈值λ设置过大，则会使算法无法得到精确的配准效果。若λ设置过小，则会使算法无法快速收敛。在本实施例中，λ的取值在0.05到0.08之间，从而使算法的准确性和时间消耗达到一个较好的平衡点。
[0099] 数据优化模块500，用于通过代价函数 对刚性变换矩阵Tt,s进行优化；其中，Pi为RGB-D相机在i时刻的位姿，Pj为RGB-D相机在j时刻的位姿，C为时刻的总数，Tij为Pi和Pj之间的约束，Ωij为Pi和Pj之间的期望和信息矩阵，e(Pi,Pj,Tij)为误差函数向量。
[0100] 在本实施例中，数据优化模块500，具体用于先通过非线性最小二乘方法得到该代价函数的最小值，即得到不同时刻之间最小误差时的各位姿，再通过不同时刻之间最小误差时的各位姿计算得到各位姿之间的刚性变换矩阵Tt,s，从而实现了对刚性变换矩阵Tt,s的优化。
[0101] 数据处理模块600，用于根据刚性变换矩阵Tt,s得到RGB-D相机的位姿；
[0102] 在本实施例中，数据处理模块600，具体包括：
[0103] 第一数据处理单元，用于通过公式Pi+1＝Ti+1,iPi，计算得到RGB-D相机在i+1时刻的位姿Pi+1；其中，Ti+1,i为RGB-D相机在i+1时刻的位姿Pi+1与RGB-D相机在i时刻的位姿Pi之间的刚性变换矩阵，Pi为RGB-D相机在i时刻的位姿；
[0104] 第二数据处理单元，用于通过公式 计算得到RGB-D相机在初始时刻的位姿Po；其中，Tn-1,n为RGB-D相机在n时刻的位姿与RGB-D相机在n-1时刻的位姿之间的刚性变换矩阵；其中，n≤i。
[0105] 地图创建模块700，用于根据刚性变换矩阵Tt,s和RGB-D相机的位姿创建三维地图。
[0106] 在本实施例中，地图创建模块700，具体用于基于刚性变换矩阵Tt,s和RGB-D相机的位姿，将配准点集Ps和配准点集Pt中的三维点经过旋转和平移变换，从而将每一时刻得到的点云信息对齐到同一坐标系中，得到三维地图。
[0107] 【技术效果】
[0108] 1、通过引入特征点之间的欧氏距离作为阈值对特征点进行离散选取，有效解决了被选取特征点过于紧密易导致算法陷入局部最优而影响配准精度的问题，提高了配准精度。
[0109] 2、通过优化特征点集间的初始位姿并消除了误匹配的特征点，进一步提高了配准精度。
[0110] 3、对刚性变换矩阵Tt,s进行了优化，从而减小了地图创建的误差。
[0111] 本发明实施例能够提高配准精度，可以有效地恢复实验场景，且轮廓清晰，建立了与实验环境基本一致的三维地图。
[0112] 本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
[0113] 本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0114] 这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0115] 这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0116] 尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。
[0117] 显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。
