一种坐标系变换方法及存储介质与流程

本发明涉及视觉定位与地图构建领域，尤其涉及一种坐标系变换方法及存储介质。

背景技术：

1、xr设备是一种可以提供虚拟世界沉浸式体验的电子产品。在xr设备中，视觉slam(simultaneous localization and mapping，即时定位与地图构建)是其主要依赖的6dof技术，指的是设备在自身位置不确定的条件下，在完全未知环境中根据相机采集的图像创建地图，并利用地图进行自主定位。而重定位技术是slam技术中的一个子方向。重定位技术主要指的是，利用以图像为主的传感器信息，计算两个slam地图间的坐标系变换，以达到地图融合、坐标系合并、三维数据长久化等目的。

2、现有重定位技术计算坐标系变换的方式往往通过ransac+pnp进行，而pnp算出来的结果仅考虑了少量内点匹配对，并不能获得基于最大似然概率的结果。有少部分策略会在pnp后增加非线性优化环节，但由于重定位的基本单位是单个相机帧，计算的位姿是单个相机帧的位姿，故无法合理的利用更多相机帧的信息。且现存所使用的视觉关联往往是2d-3d形式，忽略了两个地图间能获得的3d-3d、3d-2d、2d-2d信息，导致计算出来的坐标系变换精度较低，或者达不到当前信息总量下最优的结果。

技术实现思路

1、本发明提供一种坐标系变换方法，用以解决现有重定位中的坐标系变换方法坐标系变换精度较低以及达不到当前信息总量下最优的结果的问题。

2、为了解决上述技术问题，本发明公开了如下技术方案：

3、本发明提供了一种坐标系变换方法，包括数据准备步骤，搜索步骤，位姿解算步骤，初始值获取步骤，第一投影匹配步骤，内点集合计算步骤，最优解计算步骤，差异统计步骤，第一判断步骤，第二投影匹配步骤以及第二判断步骤。

4、所述数据准备步骤是为了准备至少一组数据，所述数据包括至少一个地图，所述地图包括至少一个3d点，至少一个相机帧以及至少一个2d点；所述搜索步骤是对所述地图进行重定位的启发式搜索，获取第一视觉观测集合，所述第一视觉观测集合包括至少一个匹配对；所述位姿解算步骤是基于所述第一视觉观测集合进行初步位姿解算，获得世界坐标系下位姿的李代数的第一初始值以及第二视觉观测集合；所述初始值获取步骤用以获取用于非线性优化的内点残差阈值，记为第二初始值，获取用于二次投影匹配的重投影阈值，记为第三初始值；所述第一投影匹配步骤是基于所述第一初始值与所述第三初始值，对所述地图执行二次投影匹配，获得第一新增视觉观测集合，将所述第二视觉观测集合与所述第一新增视觉观测集合取并集，获得第三视觉观测集合；所述内点集合计算步骤是基于所述第一初始值与所述第二初始值，遍历所述第三视觉观测集合，计算所述第三视觉观测集合中的匹配对的残差值，判断所述残差值是否大于所述第二初始值，得到所述第三视觉观测集合的第一内点集合；所述最优解计算步骤是基于所述第一初始值与所述第一内点集合计算所述第一内点集合中的匹配对的雅可比，得到残差方程，通过非线性优化求解所述残差方程得到最优解；所述差异统计步骤是基于所述最优解与所述第二初始值，对所述第三视觉观测集合再执行一次所述内点集合计算步骤，得到第二内点集合，统计所述第一内点集合与所述第二内点集合之间的差异，获得差异值；所述第一判断步骤用以判断所述差异值是否大于最大差异值，若否，执行下一步骤；所述第二投影匹配步骤是为了获取用于二次投影匹配的重投影阈值的第五初始值，基于所述最优解与所述第五初始值，对所述地图再一次执行二次投影匹配，获得第二新增视觉观测集合，统计所述第二新增视觉观测集合中的元素数量；所述第二判断步骤是为了判断所述元素数量是否大于最大新增观测元素数量，若否，输出所述位姿的最优解以及所述第二内点集合。

5、进一步地，所述第一投影匹配步骤包括还原步骤，第一遍历步骤以及第二遍历步骤。

6、所述还原步骤是基于所述第一初始值还原所述地图之间的坐标变换矩阵，其公式为

7、

8、

9、其中，表示所述第一初始值的估计值，t表示平移，r表示旋转向量，r表示旋转矩阵，exp()表示旋转向量到旋转矩阵的指数映射，0、1表示不同的地图，t(x)表示坐标变换矩阵；所述第一遍历步骤用以遍历第一地图下的第一3d点，利用一个第一坐标变换矩阵将所述第一地图下的第一3d点投影至第二地图下的各个第二相机帧下，获得投影后的第一像素坐标，搜索获得所述第一像素坐标搜索像素距离不超过所述第三初始值的第一2d点，对所述第一3d点与所述第一2d点进行匹配，匹配成功则将所述第一3d点与所述第一2d点添加至所述第一新增视觉观测集合；所述第二遍历步骤用以遍历第二地图下的第二3d点，利用一个第二坐标变换矩阵将所述第二地图下的第二3d点投影至所述第一地图下的各个第一相机帧下，获得投影后的第二像素坐标，搜索获得所述第二像素坐标搜索像素距离不超过所述第二初始值的第二2d点，对所述第二3d点与所述第二2d点进行匹配，匹配成功则将所述第二3d点与所述第二2d点添加至所述第一新增视觉观测集合。

10、进一步地，所述内点集合计算步骤包括残差计算步骤以及内点判断步骤。

11、所述残差计算步骤用以计算所述第三视觉观测集中不同匹配对的残差值；所述内点判断步骤用以判断所述残差值是否大于所述第二初始值，若否，将所述残差值对应的匹配对添加至所述第一内点集合。

12、进一步地，所述残差计算步骤包括：

13、当所述匹配对为两个3d点时，计算第一残差值，其公式为

14、

15、其中，e33表示第一残差值，f表示目标函数的调用，vm33表示第一地图下的3d点与第二地图下的3d点的匹配对，t表示向量的转置；

16、当所述匹配对为一个3d点与一个2d点时，计算第二残差值，其公式为

17、

18、

19、其中，e23表示第二残差值，vm23表示第一相机帧下的2d点与第二地图下的3d点的匹配对，表示第三残差值，表示第二相机帧下的2d点与第一地图下的3d点的匹配对；以及

20、当所述匹配对为两个2d点时，计算第四残差值，其公式为

21、

22、其中，e22表示第四残差值，vm22表示第一相机帧下的2d点与第二相机帧下2d点的匹配对。

23、进一步地，所述最优解计算步骤包括雅可比计算步骤，权重因子计算步骤，残差方程计算步骤以及最优解计算步骤。

24、所述雅可比计算步骤用以计算所述第二视觉观测集中不同所述匹配对的雅可比；所述权重因子计算步骤用以计算用来调整纯2d项以及含3d项的权重因子，其公式为

25、

26、其中，λmax为给定的阈值，取值范围为0.1～0.3，g表示匹配对vm33对应的内点集合的序号的最大值，n表示匹配对vm23对应的内点集合的序号的最大值，q表示匹配对对应的内点集合的序号的最大值，v表示匹配对vm22对应的内点集合的序号的最大值；所述残差方程计算步骤是基于不同的匹配对对应的内点集合，得到完成的残差方程，其公式为

27、

28、其中，f表示残差方程，g表示匹配对vm33对应的内点集合的序号，n表示匹配对vm23对应的内点集合的序号，q表示匹配对对应的内点集合的序号，v表示匹配对vm22对应的内点集合的序号，x表示第一初始值；所述最优解计算步骤是基于所述残差方程计算x的最优估计值，其公式为

29、

30、其中，表示x的最优估计值，即为最优解，argmin表示一个函数在其定义域中取得最小值的参数值。

31、进一步地，所述雅可比计算步骤包括：

32、当所述匹配对为两个3d点，匹配对为vm33时，采取第一目标函数，其公式为

33、

34、其雅可比为

35、

36、其中，表示第一坐标变换矩阵，表示第一地图内的3d点，表示第二地图内的3d点，i表示3x3的单位矩阵，表示偏微分，w0表示第一地图，w1表示第二地图，表示第一地图与第二地图之间的旋转矩阵，表示第一地图与第二地图之间的平移；

37、当所述匹配对为一个3d点与一个2d点，匹配对为vm23时，采取第二目标函数，其公式为

38、

39、其雅可比为

40、

41、

42、

43、其中，cm表示第一相机，表示第一相机下的2d点，表示第一相机与第一地图之间的坐标变换矩阵，表示第一相机与第一地图之间的转换矩阵；

44、当所述匹配对为一个3d点与一个2d点，匹配对为时，采取第三目标函数，其公式为

45、

46、其雅可比为

47、

48、其中，ck表示第二相机，表示第二相机与第二地图之间的旋转矩阵，表示第一地图与第二地图之间的旋转矩阵的转置矩阵，表示第二相机与第二地图之间的坐标变换矩阵，表示第二相机下的2d点；

49、当所述匹配对为两个2d点，匹配对为vm22时，采取第四目标函数，其公式为

50、

51、其雅可比为

52、

53、其中，表示第一相机下的2d点的转置，表示第一相机与第二相机之间的旋转矩阵，表示第一地图与第二相机之间的平移，表示第一相机与第二相机之间的平移，表示第一地图与第二相机之间的旋转矩阵。

54、进一步地，所述差异统计步骤包括重置步骤以及第三遍历步骤。

55、所述重置步骤是将所述差异值重置为0；所述第三遍历步骤用以遍历所述第三视觉观测集合，判断所述第一内点集合与所述第二内点集合是否存在相同的内点，若否，则将所述差异值累加1。

56、进一步地，所述第一判断步骤还包括：

57、当所述差异值大于所述最大差异值时，获取用于非线性优化的内点残差阈值的第四初始值，重新执行所述内点集合计算步骤，所述最优解计算步骤以及所述差异统计步骤，直至所述差异值小于所述最大差异值。

58、进一步地，所述第二判断步骤还包括：

59、当判断所述元素数量大于所述最大新增观测元素数量时，取所述第二新增视觉观测集合与所述第三视觉观测集合的并集，得到第四视觉观测集合，基于所述第二初始值与所述第四视觉观测集合，重新执行所述内点集合计算步骤，所述最优解计算步骤，所述差异统计步骤，所述第一判断步骤以及所述第二投影匹配步骤，直至所述元素数量小于所述最大新增观测元素数量。

60、本发明还提供一种存储介质，包括存储器以及处理器。

61、所述存储器用以存储可执行程序代码；所述处理器用以读取所述可执行程序代码，以运行与所述可执行程序代码对应的计算机程序，以执行所述坐标系变换方法中的至少一步骤。

62、与现有技术相比，本发明至少具备以下技术效果：

63、本发明提供一种坐标系变换方法，提出了可涵盖多个相机帧的，直接针对世界坐标系变换进行非线性优化的方法；推导了3d-3d、2d-3d、3d-2d、2d-2d关联的雅可比形式，可以在非线性优化环节充分利用所有的视觉关联；设计了基于二次投影匹配、内点同进同出策略的双重迭代优化策略，可以根据优化的结果在地图中多次搜索，充分利用所有的信息并反复优化，使得计算出来的坐标系变换精度更高，达到当前信息总量下最优的结果。