[发明专利]测定定位点三维坐标的方法及设备

权利要求书

1.一种测定空间环境中多个定位点的三维坐标的方法，每一个定位点均具有不同的ID，ID与定位点的三维坐标对应，所述多个定位点包括至少四个三维坐标预先已知的定位点，所述方法包括：

步骤1：利用标定好的相机对所述多个定位点在不同位置形成多张二维图像，定位点在二维图像中构成码点，所述多张二维图像中包括至少两个相机位姿可确定的二维图像，相机位姿可确定的二维图像中包含了至少四个三维坐标已知的定位点所对应的码点；

步骤2：提取每张二维图像中的每个码点对应的定位点的ID以及每个码点的图像坐标；

步骤3：找出相机位姿可确定的二维图像，以及基于相机位姿可确定的二维图像中三维坐标已知的至少四个定位点对应的码点的二维坐标及对应的三维坐标已知的至少四个定位点的三维坐标，获得相机位姿可确定的二维图像对应的相机位姿；

步骤4：对于已知位姿的两张二维图像中三维坐标未知的同一定位点所对应的码点，以相机所对应的两个三维位置分别与对应两张二维图像中的所述同一定位点所对应的码点的连线的交点的三维坐标作为所述同一定位点的三维坐标，并将该三维坐标与对应的ID形成对应的关系。

2.根据权利要求1所述的方法，还包括：

步骤5：重复步骤3和步骤4以获知其他三维坐标未知的定位点的三维坐标。

3.一种测定空间环境中多个定位点的三维坐标的方法，每一个定位点均具有不同的ID，ID与定位点的三维坐标对应，所述多个定位点包括至少四个三维坐标预先已知的定位点，所述方法包括：

步骤1：利用标定好的相机对至少四个三维位置已知的定位点形成至少两张二维图像或者选取对至少四个三维位置已知的定位点形成的至少两张二维图像，定位点在二维图像中构成码点，所述至少两张二维图像中的每一张中还包括了三维坐标未知的同一定位点对应的码点；

步骤2：提取二维图像中的每个码点对应的定位点的ID以及每个码点的图像坐标；

步骤3：基于至少四个三维位置已知的定位点的三维坐标以及至少四个三维位置已知的定位点对应的码点的图像坐标，获得所述两张二维图像对应的相机位姿；

步骤4：对于已知位姿的两张二维图像中三维坐标未知的同一定位点所对应的码点，以相机所对应的两个三维位置分别与对应两张二维图像中的所述同一定位点所对应的码点的连线的交点的三维坐标作为所述同一定位点的三维坐标，并将该三维坐标与对应的ID形成对应的关系。

4.根据权利要求3所述的方法，还包括：

步骤5：重复步骤1-4以获知其他三维坐标未知的定位点的三维坐标。

5.一种测定空间环境中多个定位点的三维坐标的设备，每一个定位点均具有不同的ID，ID与定位点的三维坐标对应，所述多个定位点包括至少四个三维坐标预先已知的定位点，所述设备包括：

标定好的相机，所述相机对所述多个定位点在不同位置形成多张二维图像，定位点在二维图像中构成码点，所述多张二维图像中包括至少两个相机位姿可确定的二维图像，每一张相机位姿可确定的二维图像中包含了至少四个三维坐标已知的定位点所对应的码点；

提取每张二维图像中的每个码点对应的定位点的ID以及每个码点的图像坐标的装置；

相机位姿确定装置，用于找出相机位姿可确定的二维图像，以及基于相机位姿可确定的二维图像中三维坐标已知的至少四个定位点对应的码点的二维坐标及对应的三维坐标已知的至少四个定位点的三维坐标、获得相机位姿可确定的二维图像对应的相机位姿；

定位点三维坐标获取装置，用于：对于已知位姿的两张二维图像中三维坐标未知的同一定位点所对应的码点，以相机所对应的两个三维位置分别与对应两张二维图像中的所述同一定位点所对应的码点的连线的交点的三维坐标作为所述同一定位点的三维坐标，并将该三维坐标与对应的ID形成对应的关系。

6.一种测定空间环境中多个定位点的三维坐标的设备，每一个定位点均具有不同的ID，ID与定位点的三维坐标对应，所述多个定位点包括至少四个三维坐标预先已知的定位点，所述设备包括：

标定好的相机，所述相机对至少四个三维位置已知的定位点形成至少两张二维图像，定位点在二维图像中构成码点，所述至少两张二维图像中的每一张中还包括了三维坐标未知的同一定位点对应的码点；

提取二维图像中的每个码点对应的定位点的ID以及每个码点的图像坐标的装置；

获取相机位姿的装置，用于基于至少四个三维位置已知的定位点的三维坐标以及至少四个三维位置已知的定位点对应的码点的图像坐标、获得所述两张二维图像对应的相机位姿；

定位点三维坐标获取装置，用于：对于已知位姿的两张二维图像中三维坐标未知的同一定位点所对应的码点，以相机所对应的两个三维位置分别与对应两张二维图像中的所述同一定位点对应的码点的连线的交点的三维坐标作为所述同一定位点的三维坐标，并将该三维坐标与对应的ID形成对应的关系。