[发明专利]基于3D地图的球机位置与姿态参数校准方法、存储介质

权利要求书

1.一种基于3D地图的球机位置与姿态参数校准方法，其特征在于，包括以下步骤：

S1、根据地图中和球机拍摄的画面中的特征点，获取多个所述特征点对应的地图中的坐标和球机的PT坐标；

S2、根据多个所述特征点的坐标计算球机的三维坐标点B；

S3、将以所述三维坐标点B为中心，N为边长的正方体划分为M×M×M个区域，计算每个所述区域节点坐标的误差值，并将误差值最小的节点作为预设球机位置；

S4、判断所述区域的尺寸是否小于预设阈值，如果是，则将所述预设球机位置作为球机位置B_n，否则，执行步骤S5；

S5、将以所述预设球机位置为中心，N/M为边长的正方体划分为M×M×M个区域，计算每个所述区域节点坐标的误差值，并将误差值最小的节点为预设球机位置，执行步骤S4。

2.根据权利要求1所述的基于3D地图的球机位置与姿态参数校准方法，其特征在于，在步骤S1中，获取至少4个所述特征点，每个所述特征点分别记为A_i(X_i，Y_i，Z_i，P_i，T_i)(i＝1，2，3…n)，其中，X_i，Y_i，Z_i分别为多个所述特征点对应的3D地图中的坐标，P_i，T_i分别为多个所述特征点对应的球机的PT坐标。

3.根据权利要求2所述的基于3D地图的球机位置与姿态参数校准方法，其特征在于，步骤S1还包括以下步骤：

S11、令i＝k，k＝1；

S12、调整所述球机的视频画面，当所述球机画面中心对准一个所述特征点时，记录所述球机的PT坐标X_i，Y_i，Z_i；

S13、在所述3D地图中找到所述特征点，并记录该特征点的在所述3D地图中的坐标P_i，T_i；

S14、记录所述特征点的坐标为A_i(X_i，Y_i，Z_i，P_i，T_i)；

S15、i＝k+1，若i＜5，则执行S12。

4.根据权利要求1所述的基于3D地图的球机位置与姿态参数校准方法，其特征在于，在步骤S2中，利用牛顿迭代法计算球机的三维坐标点B。

5.根据权利要求4所述的基于3D地图的球机位置与姿态参数校准方法，其特征在于，在步骤S3中，N＝1米，M＝4，所述区域为正方体，所述误差值为：

其中，η为误差值，∑∠A_iB_nA_j分别为在三维坐标系中和球机坐标系中点Bn与两个点之间的连线形成的夹角。

6.根据权利要求5所述的基于3D地图的球机位置与姿态参数校准方法，其特征在于，在步骤S4中，所述误差值小于1cm时，将所述预设球机位置作为球机位置B_n。