[发明专利]同时定位建图方法及装置

权利要求书

1.一种同时定位建图方法，其特征在于，包括：

对双目图像序列进行特征点提取、匹配和三角化，获取每帧双目图像的三维路标点云；

根据各帧双目图像的三维路标点云，获取亲和度矩阵；

根据所述亲和度矩阵进行层次聚类，确定每一路标所属的刚体；

根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图；

其中，亲和度用于反映两个路标属于同一刚体的对数概率；

所述根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图的具体步骤包括：

对每帧双目图像的三维路标点云进行鲁棒初始化，获取所述每帧双目图像的三维路标点云中各路标的初始化位置；

对于所述每帧双目图像的三维路标点云中每一刚体包括的路标，利用因子图对所述刚体包括的路标的位置进行更新，获取所述每帧双目图像新的三维路标点云；

对每帧双目图像的三维路标点云进行鲁棒初始化，获取所述每帧双目图像的三维路标点云中各路标的初始化位置的具体步骤包括：

对于当前帧双目图像的三维路标点云中的任一路标，根据所述任一路标的高斯混合模型，确定使得所述任一路标的注册残差最小的两帧变换；

其中，所述两帧变换指所述当前帧双目图像与上一帧双目图像之间的变换。

2.根据权利要求1所述的同时定位建图方法，其特征在于，所述根据各帧双目图像的三维路标点云，获取亲和度矩阵的具体步骤包括：

根据各帧双目图像的三维路标点云，获取每两个路标间的亲和度；

根据所述每两个路标间的亲和度，获取亲和度矩阵。

3.根据权利要求1所述的同时定位建图方法，其特征在于，所述根据所述各帧双目图像的三维路标点云和每一刚体包括的路标，进行同时定位建图之后，还包括：

根据各帧双目图像新的三维路标点云，获取新的亲和度矩阵；

对所述新的亲和度矩阵进行层次聚类，确定各刚体包括的路标；

根据鲁棒初始化方法和因子图对新的每帧的三维路标点云进行更新。

4.根据权利要求1所述的同时定位建图方法，其特征在于，对于所述每帧双目图像的三维路标点云中每一刚体包括的路标，利用因子图对所述刚体包括的路标的位置进行更新的具体步骤包括：

根据所述上一帧双目图像的三维路标点云中所述任一路标的位置更新结果和所述两帧变换，获取所述当前帧双目图像的三维路标点云中所述任一路标的初始化位置；

根据所述当前帧双目图像的三维路标点云中各路标的初始化位置，构建因子图；

对所述因子图的能量函数进行解耦式求解，获取所述当前帧双目图像的三维路标点云中每一刚体包括的路标更新后的位置。

5.根据权利要求1至4任一项所述的同时定位建图方法，其特征在于，对所述亲和度矩阵进行层次聚类，确定每一路标所属的刚体的具体步骤包括：

根据最长距离法进行层次聚类，根据聚类结果确定各刚体，并确定每一路标所属的刚体。