【技术实现步骤摘要】
本专利技术涉及视觉定位与地图构建领域，尤其涉及一种坐标系变换方法及存储介质。
技术介绍
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对应的内点集合的序号的最大值；所述残差方程计算步骤是基于不同的匹配本文档来自技高网...
【技术保护点】
1.一种坐标系变换方法，其特征在于，包括：
2.如权利要求1所述的坐标系变换方法，其特征在于，所述第一投影匹配步骤包括：
3.如权利要求1所述的坐标系变换方法，其特征在于，所述内点集合计算步骤包括：
4.如权利要求3所述的坐标系变换方法，其特征在于，所述残差计算步骤包括：
5.如权利要求1所述的坐标系变换方法，其特征在于，所述最优解计算步骤包括：
6.如权利要求5所述的坐标系变换方法，其特征在于，所述雅可比计算步骤包括：
7.如权利要求1所述的坐标系变换方法，其特征在于，所述差异统计步骤包括：
8.如权利要求1所述的坐标系变换方法，其特征在于，所述第一判断步骤还包括：
9.如权利要求1所述的坐标系变换方法，其特征在于，所述第二判断步骤还包括：
10.一种存储介质，其特征在于，包括：
【技术特征摘要】
1.一种坐标系变换方法，其特征在于，包括：
2.如权利要求1所述的坐标系变换方法，其特征在于，所述第一投影匹配步骤包括：
3.如权利要求1所述的坐标系变换方法，其特征在于，所述内点集合计算步骤包括：
4.如权利要求3所述的坐标系变换方法，其特征在于，所述残差计算步骤包括：
5.如权利要求1所述的坐标系变换方法，其特征在于，所述最优解计算步骤包括：
【专利技术属性】
技术研发人员：张逸伦，李卓，庄羽，王俊，周子越，张腾，
申请(专利权)人：玩出梦想上海科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。