本发明提出了一种高精度的单线激光器和光学相机的快速标定方法，首先安装激光器和相机，将相机和单线激光器固定在同一个测量装置上，然后在房间中布设摄影测量控制场，得到控制点的准确坐标值，之后将测量装置放置在摄影测量控制场中采集一组有效数据，包括相机影像数据和激光距离数据，接下来根据相机影像数据标定相机的位置和姿态，紧接着根据激光距离数据标定激光的位置和姿态，最后将控制场坐标系作为中介得到单线激光器和相机的外参值。与传统的标定方法相比，该方法不仅能简化标定流程，显著减少观测次数，而且能够取得足够高的精度和稳定性。
1.一种高精度的单线激光器和光学面阵相机的快速标定方法，其特征在于包括：首先安装激光器和相机，将相机和单线激光器固定在同一个测量装置上，然后在房间中布设摄影测量控制场，得到控制点的准确坐标值，之后将测量装置放置在摄影测量控制场中采集一组有效数据，包括相机影像数据和激光距离数据，接下来根据相机影像数据标定相机的位置和姿态，紧接着根据激光距离数据标定激光的位置和姿态，最后将控制场坐标系作为中介得到单线激光器和相机的外参值；具体包括如下步骤：步骤(1)，安装激光器和相机，将相机和单线激光器固定在同一个测量装置上，并保证相机和单线激光器之间的位置不发生变化，并定义激光坐标系和相机坐标系；依据线激光的工作原理，设置激光坐标系是以激光扫描中心为原点，扫描平面为x-o-z平面建立右手系；相机坐标系按照小孔成像模型建立，以相机光心为原点，与成像平面平行建立x-o-y平面，光心前方为z轴建立右手系；
步骤(2)，在房间中布设摄影测量控制场，在三维空间中放置大量靶标，用全站仪测量控制点在控制场坐标系下的精确坐标；其中，控制场坐标系是以摄影测量控制场墙角顶点为原点，墙角三条棱边分x轴、y轴和z轴建立的右手系；
步骤(3)，将测量装置放置在摄影测量控制场中，打开激光采集设备和相机设备，调整设备位置，使得激光能够同时发射到摄影测量控制场墙角的三个面上，同时相机能够拍摄到足够多的靶标点；当设备稳定工作后，采集一组观测数据，包括相机影像数据和激光距离数据；
步骤(4)，根据相机影像数据标定相机的位置和姿态；在影像中选取出控制点的像素坐标，根据相机光心、控制点像点和三维空间中控制点共线的条件建立共线条件方程，将共线方程中的相机内参和外参元素线性化，得到共线方程的线性化形式，解该线性化形式的共线方程最终得到相机在控制场坐标系中的位置和姿态；
步骤(5)，根据激光距离数据标定激光的位置和姿态；从激光距离数据中选取出打到墙角三个平面上的激光点，分别拟合这些激光点，得到三条激光直线方程，求三条激光直线两两之间的交点，得到三个交点在激光坐标系下的坐标，计算三个交点两两之间距离，得到激光点到墙角顶点的距离，即得到三个交点在控制场坐标系下的坐标；最后，利用三个交点在激光坐标系和控制场坐标系下的不同坐标值求解激光器在控制场坐标系中的位置和姿态；
设墙角的三个平面分别表示为Π1、Π2和Π3，单线激光器的扫描面与墙角的三条棱边分别相交于P1、P2和P3三个点，其中P1和P2将激光点分为三段，分别记为L1、L2和L3；
拟合打到三个平面上的激光点得到三条激光线段的直线方程L1、L2和L3，两两做叉乘计算直线交点，得到三个交点在激光坐标系下的坐标为：三个交点和控制场坐标系原点O构成了一个垂直三棱锥，记三棱锥三条底边的长度为d1，d2，d3，计算公式如下所示：设OP1、OP2和OP3长度分别为λ1、λ2和λ3，由于三条棱边两两垂直，依据勾股定理得出：上式存在八组可能的解，但是在控制场中三条棱边的长度都是大于零的，推导出此式的唯一实数解：在控制场坐标系中，三个交点位于三条坐标轴上，三条棱边的长度已知，故三个交点在控制场坐标系中的坐标表示为：(P1，P2，P3)和(Q1，Q2，Q3)分别是三个交点在激光坐标系下和控制场坐标系下的坐标，因此求解两个坐标系之间的旋转矩阵RLW和平移向量TLW构成了一个三点对应问题，这个问题的求解如下：
1)设置P1为原点，x轴设置为P2P1的单位向量：
2)y轴设置为三个点构成的平面上垂直于x轴的单位向量：
5)对Q1、Q2、Q3重复同样的过程计算控制场坐标系下的旋转矩阵R2；
至此，得到了激光坐标系在控制场坐标系下的外参(RLW，TLW)；
步骤(6)，激光器和相机的联合标定；以控制场坐标系为中介，经过坐标变换得到激光坐标系和相机坐标系之间的转换关系，即标定的激光器和相机之间的外参。
2.根据权利要求1所述的单线激光器和光学面阵相机的快速标定方法，其特征在于：步骤(4)的具体实现方式如下，设相机内参为(x0，y0，f)，(x0，y0)代表像主点坐标，f表示相机焦距，其值通过相机内参标定获得；相机在控制场中两个外参为(RCW，TCW)，RCW表示绕3个坐标轴旋转形成的3×3的旋转矩阵，TCW表示相机坐标系原点相对于控制场坐标系原点的3×1的平移向量；
相机原点S＝(Xs Ys Zs)T，控制点A＝(X Y Z)T和相片上对应的像点a＝(x，y)三点共线，共线条件方程表示为：将上式中未知参数用l1-l11表示，得到只和像点坐标和控制点坐标相关的方程形式，表现形式如下：进一步化简上式得到共线方程的线性化形式：
当控制场中存在n个控制点时，即列出2n个方程式，写为矩阵的形式如下：将等式左边系数矩阵记为A，未知数记为X，等式右边的值记为L；根据最小二乘法，可求得未知数解为：X＝(ATA)-1ATL
解上述方程得到了系数l1-l11，相机外参的平移向量满足以下关系：联立方程求解平移向量的三个未知数，确定了相机在控制场坐标系中的位置；
同理，系数li(i＝1，2…11)和构成旋转矩阵的三个旋转角之间的关系如下：由上式求得组成旋转矩阵的三个角度，确定了相机在控制场坐标系中的姿态；
最终得到相机在控制场坐标系中的外参值(RCW，TCW)。
3.根据权利要求2所述的单线激光器和光学面阵相机的快速标定方法，其特征在于：步骤(6)的具体实现方式如下，对于空间中任意一点P，在控制场坐标系下的坐标为Pw＝(Xw，Yw，Zw)T，在相机坐标系下的坐标为Pc＝(Xc，Yc，Zc)T，在激光坐标系下的坐标为Pl＝(Xl，Yl，Zl)T，则三者满足以下关系：Pc＝RCWPw+TCW
结合两式，激光坐标系到相机坐标系的外参(RCL，TCL)可以表示为：
一种高精度的单线激光器和光学相机的快速标定方法
技术领域
[0001] 本发明涉及计算机视觉和移动测量技术领域，具体涉及的是一种单线激光器和光学相机之间的标定系统和方法。
背景技术
[0002] 在测绘、移动测量和即时定位与地图构建等领域，高分辨率相机和单线激光器这两种传感器经常被组合使用。相机能够提供周围场景的色彩和纹理信息，但是相机的成像过程无法得到准确的物距信息。和相机相比，单线激光器可以获得相机视场内的高精度物距信息，但是其分辨率较低，纹理信息缺失或不丰富。为了充分利用两种传感器的优点，同时获得场景的三维几何和色彩纹理信息，需要将两种传感器的数据融合。由于安装位置、采集方式和参照坐标表达的不同，要将两种传感器数据一一对应起来，首要问题就是标定激光器和光学相机之间的刚体变换参数，即激光坐标系和相机坐标系之间的旋转矩阵和平移向量等外参。
[0003] 现在的标定方法普遍存在以下两个问题，一是理论上需要至少采集多组的不同姿态下的观测数据来求解方程，实际中为了得到更高的精度甚至需要进行20组以上的观测，标定过程繁琐耗时。二是，标定过程中用LM优化的方法优化对外参初值优化，初值如果不准确可能使得最后的解陷入局部极小值，无法得到稳定的外参解。
[0004] 本发明提出了一种基于摄影测量控制场的单线激光器和光学相机的快速标定方法，该方法不仅能简化标定流程，显著减少观测次数，而且能够取得足够高的精度和稳定性。
发明内容
[0005] 本发明提出了基于控制场的单线激光和相机的外参标定方法。以高精度的控制场为共同参照基准，分别用直接线性变换(DLT)和三点透视(P3P)方法求解相机和激光器相对于控制场参照坐标系的位置关系。
[0006] 本发明采用的技术方案是：一种高精度的单线激光器和光学相机的快速标定方法，包括：首先安装激光器和相机，将相机和单线激光器固定在同一个测量装置上，然后在房间中布设摄影测量控制场，得到控制点的准确坐标值，之后将测量装置放置在摄影测量控制场中采集一组有效数据，包括相机影像数据和激光距离数据，接下来根据相机影像数据标定相机的位置和姿态，紧接着根据激光距离数据标定激光的位置和姿态，最后将控制场坐标系作为中介得到单线激光器和相机的外参值。
[0007] 具体包括如下步骤：
[0008] (1)安装激光器和相机，将相机和单线激光器固定在同一个测量装置上，并保证他们之间的位置不发生变化，定义激光坐标系和相机坐标系。依据线激光器的工作原理，设置激光坐标系是以激光扫描中心为原点，扫描平面为x-o-z平面建立右手系。相机坐标系按照小孔成像模型建立，以相机光心为原点，与成像平面平行建立x-o-y平面，光心前方为z轴建立右手系。
[0009] (2)在房间中布设摄影测量控制场，在三维空间中放置大量靶标和标志物，用全站仪测量控制点在控制场坐标系下的精确坐标。其中，控制场坐标系是以控制场墙角顶点为原点，墙角三条棱边分x轴、y轴和z轴建立的右手系。
[0010] (3)将测量装置放置在摄影测量控制场中，打开激光采集设备和相机设备，调整设备位置，使得保证激光能够同时发射到控制场墙角的三个面上，同时相机能够拍摄到足够多的靶标点。当设备稳定工作后，采集一组观测数据，包括相机影像数据和激光距离数据。
[0011] (4)根据相机影像数据标定相机的位置和姿态。在影像中选取出控制点的像素坐标，根据相机光心、控制点像点和三维空间中控制点共线的条件可以建立共线条件方程。将共线方程中的相机内参和外参元素线性化，得到共线方程的线性化形式，解该线性方程最终得到相机在控制场坐标系中的位置和姿态。
[0012] (5)根据激光距离数据标定激光的位置和姿态。从激光距离数据中选取出打到墙角三个平面上的激光点，分别拟合这些激光点，得到三条激光直线方程，求三条直线两两之间的交点，得到三交点在激光坐标系下的坐标。计算三个交点两两之间距离，因为三个激光交点和墙角的顶点构成了一个垂直三棱锥，利用勾股定理可以求得激光点到墙角顶点的距离，即得到了三个交点在控制场坐标系下的坐标。最后，利用三个点在两个坐标系下的不同坐标值求解激光器在控制场坐标系中的位置和姿态。
[0013] (6)激光和相机的联合标定，通过上述步骤分别建立了相机坐标系和激光坐标系相对于控制场坐标系的联系，接下来以控制场坐标系为中介，经过坐标变换得到激光坐标系和相机坐标系之间的转换关系，也就是我们需要的标定的激光器和相机之间的外参。
[0014] 进一步的，步骤(4)的具体实现方式如下，
[0015] 设相机内参为(x0，y0，f)，(x0，y0)代表像主点坐标，f表示相机焦距，其值可以通过相机内参标定获得。相机在控制场中两个外参为(RCW，TCW)，RCW表示绕3个坐标轴旋转形成的3×3的旋转矩阵，TCW表示相机坐标系原点相对于控制场坐标系原点的3×1的平移向量；
[0016]
[0017] TCW＝(Xs Ys Zs)T
[0018] 相机原点S＝(Xs Ys Zs)T，控制点A＝(X Y Z)T和相片上对应的像点a＝(x，y)三点共线，共线条件方程可表示为：
[0019]
[0020] 将上式中未知参数用l1-l11表示，得到只和像点坐标和控制点坐标相关的方程形式，表现形式如下：
[0021]
[0022] 进一步化简上式得到共线方程的线性化形式：
[0023]
[0024] 当控制场中存在n个控制点时，即可列出2n个方程式，写为矩阵的形式如下：
[0025]
[0026] 将等式左边系数矩阵记为A，未知数记为X，等式右边的值记为L；根据最小二乘法，可求得未知数解为：
[0027] X＝(ATA)-1ATL
[0028] 解上述方程得到了系数l1-l11，相机外参的平移向量满足以下关系：
[0029]
[0030] 联立方程求解平移向量的三个未知数，确定了相机在控制场坐标系中的位置；
[0031] 同理，系数li(i＝1，2…11)和构成旋转矩阵的三个旋转角之间的关系如下：
[0032]
[0033] 由上式可求得组成旋转矩阵的三个角度，确定了相机在控制场坐标系中的姿态；
[0034] 通过上述流程，最终得到相机在控制场坐标系中的外参值(RCW，TCW)。
[0035] 进一步的，步骤(5)的具体实现方式如下，
[0036] 设墙角的三个平面分别表示为Π1、Π2和Π3，单线激光器的扫描面与墙角的三条棱边分别相交于P1、P2和P3三个点，其中P1和P2将激光点分为三段，分别记为L1、L2和L3；
[0037] 拟合打到三个平面上的激光点得到三条激光线段的直线方程L1、L2和L3，两两做叉乘计算直线交点，得到三个交点在激光坐标系下的坐标为：
[0038]
[0039] 三个交点和控制场坐标系原点O构成了一个垂直三棱锥，记三棱锥三条底边的长度为d1，d2，d3，计算公式如下所示：
[0040]
[0041] 设OP1、OP2和OP3长度分别为λ1、λ2和λ3，由于三条棱边两两垂直，依据勾股定理得出：
[0042]
[0043] 上式存在八组可能的解，但是在控制场中三条棱边的长度都是大于零的，推导出此式的唯一实数解：
[0044]
[0045] 在控制场坐标系中，三个交点位于三条坐标轴上，三条棱边的长度已知，故三个交点在控制场坐标系中的坐标可以表示为：
[0046]
[0047] (P1，P2，P3)和(Q1，Q2，Q3)分别是三个交点在激光坐标系下和控制场坐标系下的坐标，因此求解两个坐标系之间的旋转矩阵RLW和平移向量TLW构成了一个三点对应问题，这个问题的求解如下：
[0048] 1)设置P1为原点，x轴设置为P2P1的单位向量：
[0049]
[0050] 2)y轴设置为三个点构成的平面上垂直于x轴的单位向量：
[0051]
[0052] 3)z轴设置同时垂直于为x轴和y轴的单位向量：
[0053]
[0054] 4)故激光坐标系下的旋转矩阵表示为：
[0055] R1＝[vx vy vz]
[0056] 5)对Q1、Q2、Q3重复同样的过程计算控制场坐标系下的旋转矩阵R2；
[0057] 6)所以，两个坐标轴之间的旋转矩阵可以表示为：
[0058] RLW＝R1R2-1
[0059] 7)两个坐标系之间的平移向量可以表示为：
[0060]
[0061] 至此，得到了激光坐标系在控制场坐标系下的外参(RLW，TLW)。
[0062] 进一步的，步骤(6)的具体实现方式如下，
[0063] 对于空间中任意一点P，在控制场坐标系下的坐标为Pw＝(Xw，Yw，Zw)T，在相机坐标系下的坐标为Pc＝(Xc，Yc，Zc)T，在激光坐标系下的坐标为Pl＝(Xl，Yl，Zl)T，则三者满足以下关系：
[0064] Pc＝RCWPw+TCW
[0065] Pl＝RLWPw+TLW
[0066] 结合两式，激光坐标系到相机坐标系的外参(RCL，TCL)可以表示为：
[0067]
[0068] 本发明相对于现有技术，可以快速的完成单线激光和相机的标定，而且可以保证较高精度，在工程实践中有较高的实用性。
附图说明
[0069] 图1是本发明测量装置示意图；
[0070] 图2是本发明方法流程图；
[0071] 图3是本发明设备坐标系示意图；
[0072] 图4是本发明摄影测量控制场布设图；
[0073] 图5是本发明光学相机标定过程示意图；
[0074] 图6是本发明单线激光器标定过程示意图。
具体实施方式
[0075] 现结合附图和具体实施例对本发明做进一步详细的说明。
[0076] 图1是本发明标定的设备，该设备包含一个全景相机和3个单线激光器。本发明所涉及的单线激光器和相机的外参标定方法，首先安装激光器和相机，将相机和单线激光器固定在同一个测量装置上，然后在房间中布设摄影测量控制场，得到控制点的准确坐标值，之后将测量装置放置在摄影测量控制场中采集一组有效数据，接下来根据相机影像数据标定相机的位置和姿态，紧接着根据激光距离数据标定激光的位置和姿态，最后将控制场坐标系作为中介得到单线激光器和相机的外参值。激光和相机标定的方法总体流程见图2。接下来对每一部分的实施方式进行具体说明。
[0077] 步骤一：安装激光器和相机，将相机和单线激光器固定在同一个测量装置上，并保证他们之间的位置不发生变化。接下来按照图3所示定义激光坐标系和相机坐标系。依据线激光的工作原理，设置激光坐标系OL-XLYLZL是以激光扫描中心为原点，扫描平面为x-o-z平面建立右手系。相机坐标系Oc-XcYcZc按照小孔成像模型建立，以相机光心为原点，与成像平面平行建立x-o-y平面，光心前方为z轴建立右手系。
[0078] 步骤二：如图4所示，在房间中布设摄影测量控制场，在三维空间中放置大量靶标和标志物，用全站仪测量控制点在控制场坐标系下的精确坐标。其中，控制场坐标系Ow-XwYwZw是以控制场墙角顶点为原点，墙角三条棱边分别为x轴、y轴和z轴建立的右手系。
[0079] 步骤三：将测量装置放置在摄影测量控制场中，打开激光采集设备和相机设备，调整设备位置，使得保证激光能够同时发射到控制场墙角的三个面上，同时相机能够拍摄到足够多的靶标点。当设备稳定工作后，采集一组观测数据，包括相机影像数据和激光距离数据。
[0080] 步骤四：设相机内参为(x0，y0，f)，(x0，y0)代表像主点坐标，f表示相机焦距，内参值可以通过相机内参标定获得。相机在控制场中两个外参为(RCW，TCW)，，RCW表示绕3个坐标轴旋转形成的3×3的旋转矩阵，TCW表示相机坐标系原点相对于控制场坐标系原点的3×1的平移向量。
[0081]
[0082] TCW＝(Xs Ys Zs)T
[0083] 如图5所示，在控制场坐标系中，记相机原点坐标为S＝(Xs Ys Zs)T，控制点坐标为A＝(X Y Z)T，相片上对应的像点为a＝(x，y)。成像过程中满足三点共线，共线条件方程可表示为：
[0084]
[0085] 将上式中未知参数用l1-l11表示，得到只和像点坐标和控制点坐标相关的方程形式，表现形式如下：
[0086]
[0087] 进一步化简上式得到共线方程的线性化形式：
[0088]
[0089] 当控制场中存在n个控制点时，即可列出2n个方程式，写为矩阵的形式如下：
[0090]
[0091] 将等式左边系数矩阵记为A，未知数记为X，等式右边的值记为L。根据最小二乘法，可求得未知数解为：
[0092] X＝(ATA)-1ATL
[0093] 解上述方程得到了系数l1-l11，相机外参的平移向量满足以下关系：
[0094]
[0095] 联立方程求解平移向量的三个未知数，确定了相机在控制场坐标系中的位置。
[0096] 同理，系数li(i＝1，2…11)和构成旋转矩阵的三个旋转角之间的关系如下：
[0097]
[0098] 由上式可求得组成旋转矩阵的三个角度，确定了相机在控制场坐标系中的姿态。
[0099] 通过上述流程，最终得到相机在控制场坐标系中的外参值(RCW，TCW)。
[0100] 步骤五：如图6所示，设墙角的三个平面分别表示为∏1、∏2和∏3。单线激光器的扫描面与墙角的三条棱边分别相交于P1、P2和P3三个点，其中P1和P2将激光点分为三段，分别记为L1、L2和L3。
[0101] 拟合打到三个平面上的激光点得到三条激光线段的直线方程L1、L2和L3。两两做叉乘计算直线交点，得到三个交点在激光坐标系下的坐标为：
[0102]
[0103] 三个交点和控制场坐标系原点O构成了一个垂直三棱锥，记三棱锥三条底边的长度为d1，d2，d3，计算公式如下所示：
[0104]
[0105] 设OP1、OP2和OP3长度分别为λ1、λ2和λ3。由于三条棱边两两垂直，依据勾股定理可以得出：
[0106]
[0107] 上式存在八组可能的解，但是在控制场中三条棱边的长度都是大于零的，可以推导出此式的唯一实数解：
[0108]
[0109] 在控制场坐标系中，三个交点位于三条坐标轴上，三条棱边的长度已知，故三个交点在控制场坐标系中的坐标可以表示为：
[0110]
[0111] (P1，P2，P3)和(Q1，Q2，Q3)分别是三个交点在激光坐标系下和控制场坐标系下的坐标，因此求解两个坐标系之间的旋转矩阵RLW和平移向量TLW构成了一个三点对应问题，这个问题的求解如下：
[0112] 1)设置P1为原点，x轴设置为P2P1的单位向量：
[0113]
[0114] 2)y轴设置为三个点构成的平面上垂直于x轴的单位向量：
[0115]
[0116] 3)z轴设置同时垂直于为x轴和y轴的单位向量：
[0117]
[0118] 4)故激光坐标系下的旋转矩阵表示为：
[0119] R1＝[vx vy vz]
[0120] 5)对Q1、Q2、Q3重复同样的过程计算控制场坐标系下的旋转矩阵R2。
[0121] 6)所以，两个坐标轴之间的旋转矩阵可以表示为：
[0122] RLW＝R1R2-1
[0123] 7)两个坐标系之间的平移向量可以表示为：
[0124]
[0125] 至此，我们得到了激光坐标系在控制场坐标系下的外参(RLW，TLW)。
[0126] 步骤六：通过步骤四和五分别建立了相机坐标系和激光坐标系相对于控制场坐标系的联系，接下来以控制场坐标系为中介，经过坐标变换得到激光坐标系和相机坐标系之T间的转换关系。对于空间中任意一点P，在控制场坐标系下的坐标为Pw＝(Xw，Yw，Zw) ，在相机坐标系下的坐标为Pc＝(Xc，Yc，Zc)T，在激光坐标系下的坐标为Pl＝(Xl，Yl，Zl)T。则三者满足以下关系：
[0127] Pc＝RCWPw+TCW
[0128] Pl＝RLWPw+TLW
[0129] 结合两式，激光坐标系到相机坐标系的外参(RCL，TCL)可以表示为：
[0130]
[0131] 借助控制场坐标系我们可以很方便的建立激光和相机之间的联系。确定相机坐标系和激光坐标系到控制场坐标系的转换后，依据上式即可计算出激光和相机之间的外参。
[0132] 以上仅为本发明的典型实施例而已，并非用于限定本发明的保护范围，因此，凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
