买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：玩出梦想(上海)科技有限公司

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

主权项：1.一种坐标系变换方法，其特征在于，包括：数据准备步骤，准备两组以上的数据，所述数据包括至少一个地图，所述地图包括至少一个3D点，至少一个相机帧以及至少一个2D点；搜索步骤，对所述地图进行重定位的启发式搜索，获取第一视觉观测集合，所述第一视觉观测集合包括至少一个匹配对；位姿解算步骤，基于视觉信息获得相应位姿变换的算法，对所述第一视觉观测集合进行初步位姿解算，获得世界坐标系下位姿的李代数的第一初始值以及第二视觉观测集合；初始值获取步骤，获取用于非线性优化的内点残差阈值，记为第二初始值，获取用于二次投影匹配的重投影阈值，记为第三初始值；第一投影匹配步骤，基于所述第一初始值与所述第三初始值，对所述地图执行二次投影匹配，获得第一新增视觉观测集合，将所述第二视觉观测集合与所述第一新增视觉观测集合取并集，获得第三视觉观测集合；所述第一投影匹配步骤包括：还原步骤，基于所述第一初始值还原所述地图之间的坐标变换矩阵，其公式为 其中，表示所述第一初始值的估计值，t表示平移向量，r表示旋转向量，R表示旋转矩阵，exp表示旋转向量到旋转矩阵的指数映射，0、1表示不同的地图，Tx表示坐标变换矩阵；第一遍历步骤，遍历第一地图下的第一3D点，利用一个第一坐标变换矩阵将所述第一地图下的第一3D点投影至第二地图下的各个第二相机帧下，获得投影后的第一像素坐标，搜索获得所述第一像素坐标搜索像素距离不超过所述第三初始值的第一2D点，对所述第一3D点与所述第一2D点进行匹配，匹配成功则将所述第一3D点与所述第一2D点添加至所述第一新增视觉观测集合；以及第二遍历步骤，遍历第二地图下的第二3D点，利用一个第二坐标变换矩阵将所述第二地图下的第二3D点投影至所述第一地图下的各个第一相机帧下，获得投影后的第二像素坐标，搜索获得所述第二像素坐标搜索像素距离不超过所述第二初始值的第二2D点，对所述第二3D点与所述第二2D点进行匹配，匹配成功则将所述第二3D点与所述第二2D点添加至所述第一新增视觉观测集合；内点集合计算步骤，基于所述第一初始值与所述第二初始值，遍历所述第三视觉观测集合，计算所述第三视觉观测集合中的匹配对的残差值，判断所述残差值是否大于所述第二初始值，得到所述第三视觉观测集合的第一内点集合；最优解计算步骤，基于所述第一初始值与所述第一内点集合计算所述第一内点集合中的匹配对的雅可比，得到残差方程，通过非线性优化求解所述残差方程得到最优解；所述最优解计算步骤包括：雅可比计算步骤，计算所述第二视觉观测集中不同所述匹配对的雅可比；权重因子计算步骤，计算用来调整纯2D项以及含3D项的权重因子，其公式为 其中，λmax为给定的阈值，取值范围为0.1～0.3，G表示匹配对VM33对应的内点集合的序号的最大值，N表示匹配对VM23对应的内点集合的序号的最大值，Q表示匹配对对应的内点集合的序号的最大值，V表示匹配对VM22对应的内点集合的序号的最大值；残差方程计算步骤，基于不同的匹配对对应的内点集合，得到完成的残差方程，其公式为 其中，F表示残差方程，g表示匹配对VM33对应的内点集合的序号，匹配对VM33表示第一地图下的3D点与第二地图下的3D点的匹配对，n表示匹配对VM23对应的内点集合的序号，匹配对VM23表示第一相机帧下的2D点与第二地图下的3D点的匹配对，q表示匹配对对应的内点集合的序号，匹配对表示第二相机帧下的2D点与第一地图下的3D点的匹配对，v表示匹配对VM22对应的内点集合的序号，匹配对VM22表示第一相机帧下的2D点与第二相机帧下2D点的匹配对，x表示第一初始值；以及最优解计算步骤，基于所述残差方程计算x的最优估计值，其公式为 其中，表示x的最优估计值，即为最优解，argmin表示一个函数在其定义域中取得最小值的参数值；差异统计步骤，基于所述最优解与所述第二初始值，对所述第三视觉观测集合再执行一次所述内点集合计算步骤，得到第二内点集合，统计所述第一内点集合与所述第二内点集合之间的差异，获得差异值；第一判断步骤，判断所述差异值是否大于最大差异值，若否，执行下一步骤；第二投影匹配步骤，获取用于二次投影匹配的重投影阈值的第五初始值，基于所述最优解与所述第五初始值，对所述地图再一次执行二次投影匹配，获得第二新增视觉观测集合，统计所述第二新增视觉观测集合中的元素数量；以及第二判断步骤，判断所述元素数量是否大于最大新增观测元素数量，若否，输出所述位姿的最优解以及所述第二内点集合。

全文数据：

权利要求：

百度查询： 玩出梦想(上海)科技有限公司 一种坐标系变换方法及存储介质