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

专利主权项内容

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