[发明专利]一种基于语义误差图像的视觉定位方法及装置

权利要求书

1.一种基于语义误差图像的视觉定位方法，其特征在于，包括：

对目标图像进行特征提取，将提取出的各特征点与构建的三维场景模型的各三维点进行特征匹配，得到至少一个匹配对，每个匹配对为特征匹配的所述目标图像的像素点与所述三维场景模型的三维点；

对所述目标图像进行语义分割处理，得到所述目标图像的二维语义图像，所述二维语义图像的每个像素点具有对应的语义信息，根据所述二维语义图像的各像素的语义信息，确定各匹配对的语义信息；

根据至少一个匹配对，构建包括至少一个假设位姿的假设位姿池；

对于所述假设位姿池中的每个假设位姿，构建重投影误差图像和语义误差图像；所述语义误差图像为：由所有匹配对中的三维点构成三维语义图像，将所述三维语义图像按照当前假设位姿重投影得到二维图像，将所述二维图像的每个理论像素点的语义信息赋予所述二维语义图像的对应像素点的语义信息后，由所述二维图像的各理论像素点的语义信息与对应匹配的三维点的语义信息之间的语义误差构成的图像；

根据每个假设位姿的重投影误差图像和语义误差图像，确定出重投影误差最小且语义误差最小的假设位姿作为位姿估计。

2.根据权利要求1所述的方法，其特征在于，构建所述假设位姿池的方法是：

从所有匹配对中随机选取四个匹配对，根据PNP算法和选取出的四个匹配对，计算得到一个假设位姿，由所有匹配对的随机组合所计算得到的所有假设位姿，构成所述假设位姿池。

3.根据权利要求2所述的方法，其特征在于，所述假设位姿的计算方法为：

（5）

其中，R为旋转矩阵，t为平移矩阵。

4.根据权利要求1所述的方法，其特征在于，所述根据每个假设位姿的重投影误差图像和语义误差图像，选取出重投影误差最小且语义误差最小的假设位姿作为位姿估计包括：

根据每个假设位姿对应的重投影误差图像，统计正确位置总数；

根据每个假设位姿对应的语义误差图像，统计正确语义总数；

选取出正确位置总数最大且正确语义总数最大的假设位姿作为最佳的位姿估计。

5.根据权利要求4所述的方法，其特征在于，根据每个假设位姿对应的重投影误差图像，统计正确位置总数，包括：

对于每个假设位姿，j=1,2…n，n为假设位姿池中假设位姿的数量，将三维语义图像按照假设位姿重投影为二维图像，任意一个三维点的位置坐标，投影得到的二维图像的理论像素点的理论位置坐标表示为：

（6）

其中，、、为三维点在x、y、z方向上的位置坐标，C为相机投影矩阵；

二维图像的理论像素点的理论位置坐标，与所述二维语义图像的像素点的实际位置坐标存在重投影误差为：

（7）

根据重投影误差构建得到重投影误差图像，设置匹配对内层阈值，有：

（8）

若重投影误差小于内层阈值，该假设位姿投影得到的二维图像的理论像素点与所述二维语义图像的对应像素点位置一致，为正确位置；

对于每个假设位姿对应的重投影误差图像，计算内点总数，统计正确位置总数为：

（9）。

6.根据权利要求5所述的方法，其特征在于，根据根据每个假设位姿对应的语义误差图像，统计正确语义总数的方法是：

判断所述二维图像的理论像素点的语义信息与三维点的语义信息之间存在语义误差：

（10）

对于每个假设位姿对应的语义误差图像，统计正确语义总数：

（11）。