CN113884081B - 测定定位点三维坐标的方法及设备 - Google Patents

Abstract

本发明涉及一种测定空间环境中多个定位点的三维坐标的方法及设备，方法包括：利用标定好的相机对多个定位点在不同位置形成多张二维图像；提取每张二维图像中的每个码点的图像坐标；找出相机位姿可确定的二维图像，以及基于相机位姿可确定的二维图像中三维坐标已知的至少四个定位点对应的码点的二维坐标及对应的三维坐标已知的至少四个定位点的三维坐标，获得相机位姿可确定的二维图像对应的相机位姿；对于已知位姿的两张二维图像中三维坐标未知的同一定位点所对应的码点，以相机所对应的两个三维位置分别与对应两张二维图像中的同一定位点所对应的码点的连线的交点的三维坐标作为同一定位点的三维坐标。

Description

测定定位点三维坐标的方法及设备

技术领域

本发明涉及定位追踪领域，尤其涉及空间环境中定位点的布置方法及装置，以及测定空间环境中的定位点的三维坐标的方法及设备。

背景技术

在视觉定位系统中，尤其是Inside-out方式的定位系统，常常把相机放置于待定位物体上，然后在环境中固定一些标记，相机观测到标记后，可以根据标记的位置坐标，计算出自身的坐标，因此在任何一种基于标记的inside-out方式的视觉定位系统中，要解决的第一个问题就是标定每一个标记的三维坐标位置，已知标记的位置之后才能定位相机的位置。这个过程可以称为标定、也可以称为三维重建、或者建模。

传统的建模利用采用SFM(Structure From Motion)，简单来说流程是先贴标记，然后拍一圈视频计算出来每个标记的位置。具体的，包括如下步骤：

(1)先在环境部署标记；

(2)用已知内参(标定好)的相机从不同位置和角度拍摄包含标记的图片或视频；

(3)提取每张图片中标记点的图像坐标；

(4)通过算法找到不同图片中相同的标记点，或者说将不同图像中的标记点关联起来；

(5)根据标记的位置，使得标记点的坐标能够尽量符合约束方程，使得最后标记点重投影到不同视角的多张图片上时，重投影误差最小。

不过，上述方法建模精度差，且建模的范围越大，误差越大。

发明内容

为解决或缓解上述技术问题的至少一个方面，提出本发明。

根据本发明的实施例的一个方面，本发明提出一种定位点布置方法，包括：

步骤1：利用在第一位置的光电测量仪器在第一平面内标定至少三个第一区域点，并记录每个第一区域点的三维坐标，所述至少三个第一区域点限定第一打点区域；

步骤2：光电测量仪器的控制模块中在所述第一打点区域内模拟生成多个定位点；

步骤3：所述控制模块控制光电测量仪器基于所述第一打点区域内模拟生成的定位点逐一激光打点到第一区域内且获取激光点的三维坐标。

可选的，该方法还包括：

步骤4：利用在第一位置的光电测量仪器在第二平面内标定至少三个第二区域点，并记录每个第二区域点的三维坐标，所述至少三个第二区域点限定第二打点区域；

步骤5：光电测量仪器的控制模块中在所述第二打点区域内模拟生成多个定位点；和

步骤6：所述控制模块控制光电测量仪器基于所述第二打点区域内模拟生成多个定位点将激光逐一打在第二打点区域内且获取激光点的三维坐标。

或者可选的，该方法还包括：

步骤4：利用从第一位置移动到第二位置的光电测量仪器在第二平面内标定至少三个第二区域点，并记录每个第二区域点的三维坐标，所述至少三个第二区域点限定第二打点区域；

步骤5：光电测量仪器的控制模块中在所述第二打点区域内模拟生成多个定位点；和

步骤6：所述控制模块控制光电测量仪器基于在第二打点区域内模拟生成的定位点将激光逐一打点在第二打点区域内且获取激光点的三维坐标；和

步骤7：将在第二位置的坐标系中的定位点坐标转化到第一位置的坐标系中，或者将第一位置的坐标系中的定位点坐标转化到第二位置的坐标系中。

进一步的，步骤7包括：

将第二位置的光电测量仪器的激光打向第一位置上已经标定的至少三个定位点，并记录其在第二位置的坐标系中的三维坐标；

基于所述至少三个定位点在第一位置的坐标系中的三维坐标以及在第二位置的坐标系中的三维坐标，求出两个坐标系的转换关系。

根据本发明的实施例的另一方面，提出了一种光学定位标记的布置方法，所述光学定位标记包括定位部，所述方法包括：

利用上述方法布置定位点；和

将光学定位标记逐一贴附在光电测量仪器发出的激光所打的定位点上，且所述定位部的形心与对应的定位点对齐。

可选的，所述光学定位标记还包括ID部，每一个光学定位标记对应一个ID，所述方法还包括步骤：在逐一贴附光学定位标记时将光学定位标记的ID与对应的定位点的三维坐标形成对应关系。进一步的，所述ID部包括由多个色块构成的颜色编码区域，所述多个色块为可见光色块且依序排布以形成颜色编码。

根据本发明的实施例的还一方面，提出了一种定位点布置装置，包括：

利用在第一位置的光电测量仪器在第一平面内标定至少三个第一区域点、并记录每个第一区域点的三维坐标的区域圈定装置，所述至少三个第一区域点限定第一打点区域；

控制光电测量仪器在所述第一打点区域内模拟生成多个定位点的控制模块；和

控制光电测量仪器基于所述第一打点区域内模拟生成的定位点逐一激光打点到第一打点区域内且获取激光点的三维坐标的控制模块。

根据本发明的实施例的再一方面，提出了一种测定空间环境中多个定位点的三维坐标的方法，每一个定位点均具有不同的ID，ID与定位点的三维坐标对应，所述多个定位点包括至少四个三维坐标预先已知的定位点，所述方法包括：

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

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

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

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

可选的，该方法还包括：

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

根据本发明的实施例的又一方面，提出了一种测定空间环境中多个定位点的三维坐标的方法，每一个定位点均具有不同的ID，ID与定位点的三维坐标对应，所述多个定位点包括至少四个三维坐标预先已知的定位点，所述方法包括：

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

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

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

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

可选的，该方法还包括：

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

本发明的实施例中还涉及一种测定空间环境中多个定位点的三维坐标的设备，每一个定位点均具有不同的ID，ID与定位点的三维坐标对应，所述多个定位点包括至少四个三维坐标预先已知的定位点，所述设备包括：

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

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

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

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

本发明的实施例中也涉及一种测定空间环境中多个定位点的三维坐标的设备，每一个定位点均具有不同的ID，ID与定位点的三维坐标对应，所述多个定位点包括至少四个三维坐标预先已知的定位点，所述设备包括：

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

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

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

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

根据本发明的实施例，还提出了一种测定空间环境中多个定位点的三维坐标的方法，所述方法包括：

步骤1：提供摄像头、光电测量仪器以及控制光电测量仪器的运动的驱动装置；

步骤2：在第一位置利用摄像头获取定位点的二维图像，确定二维图像中定位点对应的形心位置；

步骤3：在第一位置利用光电测量仪器打出激光点；

步骤4：由驱动装置驱动光电测量仪器以使得激光点在第一位置获得的二维图像中与定位点对应的形心位置重合；

步骤5：将光电测量仪器在第一位置打出激光点的三维坐标作为定位点的三维坐标。

可选的，在步骤5中，定位点的三维坐标与ID形成对应关系。

可选的，该方法还包括：

步骤6：重复上述步骤2-5，以获取其他三维坐标未知的定位点的三维坐标。

可选的，步骤1与步骤2之间还包括标定步骤：标定摄像头以及光电测量仪器，使得摄像头坐标系与光电测量仪器的坐标系对齐。

上述方法还包括：

步骤7：在第二位置利用摄像头获取定位点的二维图像，确定二维图像中定位点对应的形心位置；

步骤8：在所述第二位置利用光电测量仪器打出激光点；

步骤9：由驱动装置驱动光电测量仪器以使得激光点在所述第二位置所获取的二维图像中与定位点对应的形心位置重合；

步骤10：将光电测量仪器在所述第二位置打出激光点的三维坐标作为定位点的三维坐标；

步骤11：将在所述第二位置的坐标系中的定位点坐标转化到第一位置的坐标系中，或者将第一位置的坐标系中的定位点坐标转化到第二位置的坐标系中。

可选的，步骤11包括：

将第二位置的光电测量仪器的激光打向第一位置上已经标定的至少三个定位点，并记录其在第二位置的坐标系中的三维坐标；

基于所述至少三个定位点在第一位置的坐标系中的三维坐标以及在第二位置的坐标系中的三维坐标，求出两个坐标系的转换关系。

本发明的实施例还提出了一种测定空间环境中多个定位点的三维坐标的设备，所述设备包括：

摄像头、光电测量仪器，其中摄像头坐标系与光电测量仪器的坐标系对齐；

确定摄像头获取的二维图像中定位点对应的形心位置的装置；

驱动装置，驱动光电测量仪器以使得光电测量仪器打出的激光点在二维图像中与定位点对应的形心位置重合；

定位点三维坐标获取装置，用于将光电测量仪器打出激光点的三维坐标作为定位点的三维坐标。

本发明的实施例还提出了一种测定空间环境中定位点的三维坐标的设备，包括：

光电测量仪器，光电测量仪器适于打出激光点，所述光电测量仪器具有驱动装置，用于驱动光电测量仪器以调整打出的激光点的位置；

摄像头，适于捕捉空间环境中的定位点以及光电测量仪器所打出的激光点的二维图像，摄像头的坐标系与光电测量仪器的坐标系对齐；和

控制装置，用于控制所述驱动装置以便光电测量仪器打出的激光点在二维图像中与定位点对应的形心位置重合。

可选的，所述光电测量仪器包括测距传感器和测角传感器，测距传感器适于打出激光点并测量光电测量仪器与激光点之间的距离，测角传感器用于测量测距传感器的航偏角和俯仰角，其中：所述光电测量仪器基于测得的距离以及航偏角和俯仰角，获得激光点相对于光电测量仪器的三维坐标。

本发明的实施例也提出了一种光电测量仪器，包括：

测距传感器，适于打出激光点并测量光电测量仪器与激光点之间的距离；

测角传感器，用于测量测距传感器的航偏角和俯仰角；

驱动装置，用于驱动测距传感器以调整打出的激光点的位置。

可选的，所述测角传感器为两轴测角传感器。

附图说明

图1为根据本发明的一个示例性实施例的定位点布置方法的流程图；

图2为根据本发明的一个示例性实施例的光学定位标记的布置方法的流程图；

图3为根据本发明的一个示例性实施例的光学定位标记的示意图；

图4为根据本发明的一个示例性实施例的光学定位标记的示意图；

图5为根据本发明的一个示例性实施例的测定空间环境中多个定位点的三维坐标的方法的流程图；

图6为根据本发明的一个示例性实施例的测定空间环境中多个定位点的三维坐标的方法的流程图；

图7为根据本发明的一个示例性实施例的测定空间环境中多个定位点的三维坐标的方法的流程图；

图8为根据本发明的一个示例性实施例的测定空间环境中定位点的三维坐标的设备的结构示意图；

图9为根据本发明的一个示例性实施例的光电测量仪器的结构示意图。

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。在说明书中，相同或相似的附图标号指示相同或相似的部件。下述参照附图对本发明实施方式的说明旨在对本发明的总体发明构思进行解释，而不应当理解为对本发明的一种限制。

实施例1

如图1所示，本发明提出了一种定位点布置方法，包括：

步骤1：利用在第一位置的光电测量仪器在第一平面内标定至少三个第一区域点，并记录每个第一区域点的三维坐标，所述至少三个第一区域点限定第一打点区域；

步骤2：光电测量仪器的控制模块中在所述第一打点区域内模拟生成多个定位点；

步骤3：所述控制模块控制光电测量仪器基于所述第一打点区域内模拟生成的定位点逐一激光打点到第一打点区域内且获取激光点的三维坐标。

在本发明中，光电测量仪器只要可以激光测距和测角即可，光电测量仪器可以是全站仪，主要包括了激光测距设备和经纬仪(测角)，因此可以精确测量一定范围内的三维坐标。光电测量仪器可以将激光点打到环境中，打在每一处都可以得到该点的三维坐标。可以利用伺服电机和控制器控制光电测量仪器。如本领域技术人员所理解的，在本发明中，将定位点的三维坐标测量好了之后，可以将其保存或传输。

光电测量仪器的位置可以不变，但是为了在另一个平面内布置定位点，上述方法还可以包括：

步骤4：利用在第一位置的光电测量仪器在第二平面内标定至少三个第二区域点，并记录每个第二区域点的三维坐标，所述至少三个第二区域点限定第二打点区域；

步骤5：光电测量仪器的控制模块中在所述第二打点区域内模拟生成多个定位点；和

步骤6：所述控制模块控制光电测量仪器基于所述第二打点区域内模拟生成多个定位点将激光逐一打在第二打点区域内且获取激光点的三维坐标。

在需要移动光电测量仪器到另一个位置以在另一个平面内布置定位点的情况下，上述方法还可以包括：

步骤4：利用从第一位置移动到第二位置的光电测量仪器在第二平面内标定至少三个第二区域点，并记录每个第二区域点的三维坐标，所述至少三个第二区域点限定第二打点区域；

步骤5：光电测量仪器的控制模块中在所述第二打点区域内模拟生成多个定位点；和

步骤6：所述控制模块控制光电测量仪器基于在第二打点区域内模拟生成的定位点将激光逐一打点在第二打点区域内且获取激光点的三维坐标；和

步骤7：将在第二位置的坐标系中的定位点坐标转化到第一位置的坐标系中，或者将第一位置的坐标系中的定位点坐标转化到第二位置的坐标系中。

具体的，坐标转化可以包括如下步骤：将第二位置的光电测量仪器的激光打向第一位置上已经标定的至少三个定位点，并记录其在第二位置的坐标系中的三维坐标；基于所述至少三个定位点在第一位置的坐标系中的三维坐标以及在第二位置的坐标系中的三维坐标，例如通过SVD或ICP算法求出两个坐标系的转换关系。

相应的，本发明涉及一种定位点布置装置，包括：

利用在第一位置的光电测量仪器在第一平面内标定至少三个第一区域点、并记录每个第一区域点的三维坐标的区域圈定装置，所述至少三个第一区域点限定第一打点区域；

控制光电测量仪器在所述第一打点区域内模拟生成多个定位点的控制模块；和

控制光电测量仪器基于所述第一打点区域内模拟生成的定位点逐一激光打点到第一打点区域内且获取激光点的三维坐标的控制模块。

相应的，如图2所示，本发明还提出了一种光学定位标记的布置方法，所述光学定位标记包括定位部，所述方法包括：

利用上述方法布置定位点；和

将光学定位标记逐一贴附在光电测量仪器发出的激光所打的定位点上，且所述定位部的形心与对应的定位点对齐。

这样，光电测量仪器自动在环境中打点，而部署人员则可以在打了激光点的地方贴标记。

可选的，光学定位标记还包括ID部，每一个光学定位标记对应一个ID，所述方法还包括步骤：

在逐一贴附光学定位标记时将光学定位标记的ID与对应的定位点的三维坐标形成对应关系。

布置好三维坐标已知的定位点，最终是为了形成点云。

下面具体描述布置定位点的方法一个示例性实施例。该方法包括如下步骤：

(1)将光电测量仪器放在环境中，人工用激光点在一个平面内指定4个点，并记录4个点的三维坐标，圈定一个区域，如本领域技术人员已知的，三个点也可以圈定一个区域。

(2)光电仪器会在4个点圈定的范围内，用采样算法，随机产生一些采样点。光电测量仪器将激光点逐个打在采样点上，并获取三维坐标。这里的随机可以根据需要要求采样点之间的距离大于预定阈值。

(3)用户将标记贴在光电仪器打在激光点的位置上。如果该标记有ID，则将ID和三维坐标一同记录，否则只记录三维坐标。

(4)如果需要在多个平面部署标记，则重复步骤(1)到步骤(3)。

(5)如果中途由于遮挡，需要更换光电测量仪器摆放位置，则更换完位置后，光电测量仪器仍然重复步骤(1)到步骤(4)。但是测量出来的坐标点，属于新的坐标系，需要将新的坐标系与原坐标系的转换关系求出，具体方法是：将光电测量仪器的激光点打向变更光电测量仪器位置之前测量的至少三个标记，并记录标记ID和三维坐标。用相同至少三个标记在两个不同坐标系内的坐标作为输入，通过SVD或者ICP算法，可以求出新旧坐标系的转换关系，因此可以将新坐标系测量出来的标记点坐标，转化到原坐标系中。

在本发明的上述实施例中，建模精度高，没有全局累积误差。不过，该方法需要光电测量仪器，也需要专人建模。

在本发明中，所述ID部包括由多个色块构成的颜色编码区域，所述多个色块为可见光色块且依序排布以形成颜色编码。关于ID部的描述同样适用于本发明公开的其他实施例。

下面参照附图3和4描述光学定位标记100。

如图3所示，中间包含十字的小圆为反光点或光反射部或定位部10的位置，该小圆为颜色编码区域的内边界，最外层的黑色圆环为颜色编码区域的外边界，中间的含有不同颜色扇环的圆环则为颜色编码区域20。不同的颜色表示不同的码。需要指出的是，该十字也可以并不存在；另外，定位部也可以设置为覆盖小圆的圆心即可；最外层的圆环可以是黑色，也可以是其他颜色，当然最好是与颜色编码区域中的颜色不同的颜色，有利的，最外层的圆环的颜色与背景环境的颜色不同。

最外圈的颜色信息可以快速、准确的确认光学定位标记的外形和轮廓，便于快速、准确的确认光学定位标记的圆心位置。

在解码时，首先可以找到基准色块，例如图3中的白色区域块，然后按照顺时针(当然也可以按照逆时针)的方向依次读取其它颜色块的颜色。以图3为例，中间的圆环被划分成了6份，每个码所占的区域为1/6个圆环(60度扇环)。图中有红(R)、蓝(B)、绿(G)、白(W)4种颜色，分别表示0，1，2，3四种码。从白色区域开始，顺时针进行编码，则图3中的颜色编码为320100。关于颜色编码的设计，如果确定了中间的颜色块的数目n，颜色的组合数目c，则总共可以产生c^n-1个编码，因为每个颜色块可以取c种颜色，而基准颜色块的颜色则为固定的一种特殊的颜色，主要是用来确定解码的顺序，在图4的实施例中，基准块的颜色不能与其它颜色块的颜色相同，否则会产生一个颜色编码产生多种解码方式，破坏了颜色编码解码时的唯一性。需要说明的是，之所以需要明确基准色块的颜色不同于其他色块，是因为色块构成了一个环形时基准色块并不适于用位置来确定，换言之，在色块构成环形的颜色编码区域的情况下，基准色块通过颜色来选定。

在图3的实施例中，中间的颜色块的设计是可扩展的，可以体现在以下方面：

1.颜色的选取，可以选取多种分辨度比较高的颜色组合进行编码。可以在多种颜色空间里进行选择，例如RGB，HSV颜色空间选取。例如选取纯红(R＝255， G＝0，B＝0)，纯蓝(R＝0，G＝0，B＝255)，纯绿(R＝0，G＝255，B＝0)三种颜色组合，基准颜色例如选取纯白(R＝255，G＝255，B＝255)。

2.中间颜色块的划分数目，数目越多，最终产生的编码数目也越多，主要取决于实际三维场景需要部署的编码点数目。例如，颜色块为4，颜色组合数为3，则产生27个编码，当颜色块数目增加到6，颜色组合数为3，则产生243个编码。

还需要说明的是，图3中的扇形色块也可以是不等分圆环的形式，此时，每一个色块或每一个编码点并非以角度确定，而是直接以色块的颜色确定。例如，在此情况下，图3中右上角的两个红色块就看做是一个色块。

下面参照图3示例性简单描述如何获得光学定位标记的反光点。首先，红外相机得到的是灰度图像，反光点位置处比较亮，灰度值高，利用阈值法可以把不是反光点的像素给过滤掉。然后，对反光点的像素进行聚类，得到反光点在图像中的连通区域。最后，根据连通区域，拟合圆形或椭圆，利用圆心近似求解反光点的质心。

图4为根据本发明的一个示例性实施例的光学定位标记的示意图。在图4 中，整个光学定位标记大致为长条形，在颜色编码区域20，每一个色块为直的色条。相邻的色条之间可彼此邻接且颜色不同。还可以利用色条的宽度来判别不同的色条。定位部10可以设置在光学定位标记的任意位置，可选的，设置在长条形光学定位标记的一端，如图4所示。其中，颜色编码区域包括位于光学定位标记长度方向上的一端的单个基准色块，颜色编码自所述单个基准色块按预定顺序开始。图4中，其颜色编码可以为0212。

实施例2

也可以在有初始化值的情况下建模或者获得定位点的点云。

如图5所示，本发明涉及一种测定空间环境中多个定位点的三维坐标的方法，每一个定位点均具有不同的ID，ID与定位点的三维坐标对应，所述多个定位点包括至少四个三维坐标预先已知的定位点，所述方法包括：

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

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

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

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

需要注意的是，ID与定位点的三维坐标对应不仅包括了每一个定位点的三维坐标对应一个ID的情形，还包括了有可能一个ID对应多个三维位置(原因是编码资源不够，比如一共有200个不同的编码ID，但是需要部署3000个点，就必须有重复的)的情形，不过在后情形中，在具体某一图像中，非歧义的通过ID 得到唯一的三维位置，比如同一图像中的码点对应的三维位置都很近。

上述方法中，还可以包括步骤5：重复步骤3和步骤4以获知其他三维坐标未知的定位点的三维坐标。

需要说明的是，获取二维图像可以是图片的形式，也可以是视频的形式，这均在本发明的保护范围内。

需要注意的是，在较为理想的情况下，两条连线可以相交从而形成上述交点，但是，实际操作中，由于误差的存在，两条连线不一定相交，此时，需要多于两张的已知位姿的二维图像，从而形成多于两条的连线，多条连线会取得一个近似相交的“交点”。在本发明中，这种多于两条连线获取所谓交点的情形也包含在特征“以相机所对应的两个三维位置分别与对应两张二维图像中的所述同一定位点所对应码点的连线的交点”的范围之内。

还需要说明的是，在本发明中，标定后的相机实际上是小孔成像模型。已知相机的位姿实际上是已知镜头光心的三维位置以及成像平面的姿态，而上述的连线则是镜头光心与定位点成像(即码点)中心的一条线。

下面具体描述测定空间环境中多个定位点的三维坐标的方法，该方法包括步骤：

(1)环境中贴标记点，每个标记点具有不同的ID，其中一部分标记点的三维坐标已知，将这些坐标点的集合作为S1，剩余的标记点集合为X。

(2)用已知内参(标定好)的相机从不同位置和角度拍摄包含标记的图片或视频。

(3)提取每张图片中标记点的图像坐标，以及标记的ID。

(4)通过这些标记的ID，找出属于S1的标记，如果一张图片中，有4个或者4个以上的标记属于S1，则称这幅图像是已知姿态的。因为标记属于S1，因此标记的三维坐标已知，结合该标记在图像中对应的二维坐标，例如，通过 PNP算法，可以求出该相机的姿态。

(5)将所有已知姿态的图片中，不属于S1的标记，从图像投影到现实世界，每个标记得到一条射线。同一个标记在不同已知姿态的图片中对应不同射线，这些射线的交点即是该标记的三维坐标，将新计算出三维坐标的标记，全部或者部分择优的和S1中的标记一并设为S2。

(6)步骤3到步骤5，可以从S1得到S2，重复迭代步骤3到步骤5，可以从Sn得到Sn+1,如果Sn中的标记数目与Sn+1中的标记数目相等或者两者之差小于预定阈值，则停止迭代。此时Sn中的所有标记都是已知三维坐标的，建模结束。

在上面提及的方法中，首先获取了多张二维图像，之后在这些图像的基础上，从已知推定未知，获得所有定位点的三维坐标。不过，也可以每次相应的获得两张二维图像，获得一些定位点的三维坐标，然后再以两张二维图像为基础获得另外一些定位点的三维坐标。相应的，本发明还提出了一种测定空间环境中多个定位点的三维坐标的方法，每一个定位点均具有不同的ID，ID与定位点的三维坐标对应，所述多个定位点包括至少四个三维坐标预先已知的定位点，如图6所示，所述方法包括：

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

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

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

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

需要注意的是，在步骤1中，“选取对至少四个三维位置已知的定位点形成的两张二维图像”也包括新拍一张二维图像以及选择一张旧二维图像的情形。可以重复步骤1-4以获知其他三维坐标未知的定位点的三维坐标。

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

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

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

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

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

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

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

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

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

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

在实施例2中，可以用普通相机建模，不需要专门设备和专人建模，建模精度比SFM高。不过，其精度比光电测量仪器低，而且也存在全局累积误差。

需要说明的是，在本发明中，“获取相机的位姿”可以利用现有技术中已知的方法，例如使用solvePnP算法或者POSIT算法，只要是能够用二维到三维的映射关系，求出相机在当前位置的位姿的方法都在本发明的保护范围之内。

实施例3

可以将光电测量仪器和视觉测量结合在一起。

如图7所示，本发明提出了一种测定空间环境中多个定位点的三维坐标的方法，所述方法包括：

步骤1：提供摄像头、光电测量仪器以及控制光电测量仪器的运动的驱动装置；

步骤2：在第一位置利用摄像头获取定位点的二维图像，确定二维图像中定位点对应的形心位置；

步骤3：在第一位置利用光电测量仪器打出激光点；

步骤4：由驱动装置驱动光电测量仪器以使得激光点在第一位置所获得的二维图像中与定位点对应的形心位置重合；

步骤5：将光电测量仪器在第一位置打出激光点的三维坐标作为定位点的三维坐标。

可选的，在步骤5中，定位点的三维坐标与ID形成对应关系。

可选的，该方法还包括：

步骤6：重复上述步骤2-5，以获取其他三维坐标未知的定位点的三维坐标。

可选的，步骤1与步骤2之间还包括标定步骤：标定摄像头以及光电测量仪器，使得摄像头坐标系与光电测量仪器的坐标系对齐。如果标定了就可以根据三角测量方法、摄像头、激光测距仪的相对位置、姿态可以得到激光测量点映射到二维图像中的坐标。不过，也可以不包括标定步骤，此时，可以直接从二维图像中，用图像处理的方法，直接获得激光点的图像坐标，然后将激光点在二维图像中的坐标调整到定位点在二维图像中的坐标。

下面具体描述测定空间环境中多个定位点的三维坐标的方法，该方法包括步骤：

(1)摄像头采集图像，在图像中找到标记的图像中心A。

(2)根据三角测量方法、摄像头、激光测距仪的相对位置、距离，可以得到激光测量点映射到图像中的坐标B。当然，也可以直接从图像中，用图像处理的方法，直接获得激光点的图像坐标B。

(3)如果B与A没有对齐，则调整激光测距仪的航向电机和俯仰电机使B 更接近A。

(4)重复步骤(2)、(3)，直到A与B重合，或者距离小于阈值。

(5)转到步骤1测量下一个标记，可选的，在存在ID部的情况下，记录该标记的编号。

重复步骤(1)-(5)会生成一个点云，但是如果标记点部署的范围很大，测量设备固定在一个位置，并不能测量到所有的标记，因此需要将设备移动到下一个地方，测量，最终会得到多个点云，然后再通过算法将这些点云拼接在一起，形成同一坐标系下的建模。

相应的，上述方法还可包括：

步骤7：在第二位置利用摄像头获取定位点的二维图像，确定二维图像中定位点对应的形心位置；

步骤8：在所述第二位置利用光电测量仪器打出激光点；

步骤9：由驱动装置驱动光电测量仪器以使得激光点在所述第二位置所获取的二维图像中与定位点对应的形心位置重合；

步骤10：将光电测量仪器在所述第二位置打出激光点的三维坐标作为定位点的三维坐标；

步骤11：将在所述第二位置的坐标系中的定位点坐标转化到第一位置的坐标系中，或者将第一位置的坐标系中的定位点坐标转化到第二位置的坐标系中。

可选的，步骤11包括：

将第二位置的光电测量仪器的激光打向第一位置上已经标定的至少三个定位点，并记录其在第二位置的坐标系中的三维坐标；

基于所述至少三个定位点在第一位置的坐标系中的三维坐标以及在第二位置的坐标系中的三维坐标，求出两个坐标系的转换关系。

相应的，本发明提出了一种测定空间环境中多个定位点的三维坐标的设备，所述设备包括：

摄像头、光电测量仪器，其中摄像头坐标系与光电测量仪器的坐标系对齐；

确定摄像头获取的二维图像中定位点对应的形心位置的装置；

驱动装置，驱动光电测量仪器以使得光电测量仪器打出的激光点在二维图像中与定位点对应的形心位置重合；

定位点三维坐标获取装置，用于将光电测量仪器打出激光点的三维坐标作为定位点的三维坐标。

相应的，如图8、9所示，本发明还提出了一种测定空间环境中定位点的三维坐标的设备1000包括：

光电测量仪器200，光电测量仪器适于打出激光点，所述光电测量仪器具有驱动装置210，用于驱动光电测量仪器以调整打出的激光点的位置；

摄像头300，适于捕捉空间环境中的定位点以及光电测量仪器所打出的激光点的二维图像，摄像头的坐标系与光电测量仪器的坐标系对齐；和

控制装置400，用于控制所述驱动装置以便光电测量仪器打出的激光点在二维图像中与定位点对应的形心位置重合。

可选的，所述光电测量仪器包括测距传感器220和测角传感器230，测距传感器220适于打出激光点并测量光电测量仪器与激光点之间的距离，测角传感器230用于测量测距传感器的航偏角和俯仰角，其中：所述光电测量仪器基于测得的距离以及航偏角和俯仰角，获得激光点相对于光电测量仪器的三维坐标。

如图9所示，本发明提出了一种光电测量仪器200，包括：

测距传感器220，适于打出激光点并测量光电测量仪器与激光点之间的距离；

测角传感器230，用于测量测距传感器的航偏角和俯仰角；

驱动装置210，用于驱动测距传感器以调整打出的激光点的位置。

可选的，所述测角传感器为两轴测角传感器。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行变化，本发明的范围由所附权利要求及其等同物限定。

Claims (6)

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形成对应的关系。