一种基于相机位置的焦距、径向畸变及姿态标定的方法与流程

1.本发明涉及机器视觉、摄影测量、slam等技术领域，具体涉及一种基于相机位置的焦距、径向畸变及姿态标定方法。


背景技术：

2.在机器视觉、摄影测量、slam等相关领域，为进行测量和估计，需对相机的畸变、焦距、位置和姿态等内外参数进行标定。目前内外参数标定方法有很多，如针对相机内参数已知或者可采用理想内参数的外参数标定方法[an accurate o(n)solution to the pnp problem]；针对部分相机内参数已知的内外参数标定方法[camera pose and calibration from 4or 5known 3d points]；以及针对相机部分姿态已知的内外参数标定方法[closed-form solutions to minimal absolute pose problems with known vertical direction]。随着定位技术的发展，定位设备逐渐小型化、廉价化且精度满足工程需求，其开始大量应用于相机定位中，以获取相机位置。另外，在机器视觉中，为扩大相机视场，常采用短焦或者鱼眼镜头，但该类型镜头具有严重畸变等缺点，若不考虑畸变影响，将对测量和估计带来很大误差。本发明针对相机位置已知，焦距未知且镜头具有严重畸变的情况，对相机进行标定，包括焦距、径向畸变以及姿态等参数。
[0003]
当前，在无先验知识的情况下，对相机焦距、径向畸变以及姿态等参数的标定需要的标志点数量需大于等于4个。文献[pose estimation with radial distortion and unknown focal length]提出了一种基于4个标志点的焦距、径向畸变以及姿态标定方法，为简化标定过程，采用了除法径向畸变模型，且径向畸变系数只能估计一个；文献[real-time solution to the absolute pose problem with unknown radial distortion and focal length]提出了一种基于5个标志点的焦距、径向畸变以及姿态标定方法，为简化标定过程，采用了除法径向畸变模型，径向畸变系数估计数量可达3个；上述方法具有多解现象。可以看到，为实现焦距、径向畸变以及姿态标定，需不低于4个标志点，且为简化标定过程，常采用除法径向畸变模型，且具有多解现象，需要额外的约束条件去得到唯一解。精确标志点的获取与维护需要耗费大量的人力物力，比如野外大风环境下，对飞行器姿态进行测量时，对相机需要提前标定就必须设置多个标志点，而野外大风环境对标志点的设立带来严峻挑战。


技术实现要素：

[0004]
本发明的目的是提供一种基于相机位置的焦距、径向畸变及姿态标定方法，解决现有标定方法中仅采用某一种畸变模型，在同时求解相机焦距、畸变和姿态时需要≥4个标志点，且部分方法具有多解的技术问题。
[0005]
为达到上述目的，解决上述技术问题，本发明的而技术方案如下：
[0006]
一种基于相机位置的焦距、径向畸变及姿态标定的方法，其特征在于，包括如下步骤：
[0007]
设相机位置点c(x
0 y
0 z0)已知，三个标志点p1(x
1 y
1 z1)、p2(x
2 y
2 z2)、p3(x
3 y
3 z3)已知，无畸变条件下外标志点与相机成像位置具有如下关系：p1、p2、p3为无畸变条件下p1、p2、p3在相机中的成像位置；pc为相机主点，位于图像中心；oc为相机位置；
[0008]
步骤1、建立非线性方程组迭代求解中间变量
[0009]
选取除法畸变模型和传统畸变模型；
[0010]
除法畸变模型如下
[0011][0012]
传统畸变模型如下
[0013][0014]
其中，ki为径向畸变系数，径向畸变下的几何结构如下：∠p1ocp2,∠p2ocp3,∠p3ocp1分别表示为α1,β1,γ1，由于pi和oc位置已知，则这三个角的角度大小通过三角形δp1ocp2,δp2ocp3,δp3ocp1计算得到；同时，∠p1pcp2,∠p2pcp3,∠p3pcp1分别表示为α2,β2,γ2；主点pc和畸变点已知，α2,β2,γ2通过三角形获取；
[0015]
设pcpi＝xi,ocpi＝yi，三角形δp1pcp2,δp1ocp2具有公共边p1p2，通过余弦定理
[0016][0017]
其中，ocpc＝f，为焦距，其垂直于平面p1p2p3，因此，根据三角形δocpcpi得到
[0018][0019]
将式(4)带入式(3)，进而可以得到
[0020][0021]
同理，能够获取另外两个方程
[0022][0023]
设然后得到具有三个未知数的方程组
[0024][0025]
采用迭代方法求解该非线性方程组的解fi；
[0026]
步骤2、计算焦距和畸变系数
[0027]
2.1对于除法畸变模型
[0028]
两参数模型如下：
[0029][0030]
其中，kj(j＝1,2)为径向畸变系数，为畸变点到主点的距离，进而获得
[0031][0032]
其中，为已知量；则得到一个多项式方程组：
[0033][0034]
将f,k1f,k2f视为上述方程组的三个未知量，然后一个线性方程组可以得到如下：
[0035]
a1x1＝y1ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0036]
其中
[0037][0038]
则该方程组的解为则焦距和畸变系数计算如下：
[0039][0040]
2.2对于传统畸变模型
[0041]
两参数模型如下：
[0042][0043]
同理，得到一个线性方程组如下：
[0044]
a2x2＝y2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
[0045]
其中
[0046][0047]
焦距和畸变系数通过公式(16)线性求解获取：
[0048][0049]
步骤3、计算相机姿态
[0050]
根据步骤2得到的畸变系数以及对应的畸变模型，能够得到畸变矫正后成像点的位置，畸变矫正后的2d-3d点对应关系如下：设o
c-xcyczc为相机坐标系，o
w-x
wywzw
为世界坐标系；采用任意两个标志点和相机位置来计算相机姿态，也就是旋转矩阵r
w_c
以及平移向量
t
w_c
；
[0051]
定义一个新的相机坐标系o
c-x
c2yc2zc2
，一个新的世界坐标系o
c-x
w2yw2zw2
；新的相机坐标系定义如下：
[0052][0053]
其中，在第一步求解后为已知量；根据定义，在新的相机坐标系下，x
c2
轴为向量z
c2
轴垂直于平面ocp1p2，y
c2
轴根据右手坐标系进行定义；原始相机坐标系o
c-xcyczc下的点pc的坐标通过下式转换到新的相机坐标系o
c-x
w2yw2zw2
下的坐标p
c2
：
[0054][0055]
新的世界坐标系定义如下：
[0056][0057]
根据定义，新的世界坐标系下，oc为坐标系原点，x
w2
轴为向量z
w2
轴垂直于平面ocp1p2，y
w2
轴根据右手坐标系进行定义；原始世界坐标系o
c-x
wywzw
下的点pw的坐标可以通过下式转换到新的世界坐标系o
c-x
w2yw2zw2
下的坐标p
w2
：
[0058][0059]
新的相机坐标系和新的世界坐标系，假设原始相机坐标系下的点pc和原始世界坐标系下的点pw为同一个点，根据新的相机坐标系和新的世界坐标系的定义，获得到各坐标系下的转换关系；
[0060]
旋转矩阵r
w_c
以及平移向量t
w_c
计算如下：
[0061][0062]
至此，相机姿态计算完毕。
[0063]
进一步的，步骤1中fi采用lm法进行求解；选择不考虑畸变条件下的fi的值作为迭代计算的初始值，该初始值通过传统的pnp算法给出，由于该初始值接近真值，因此能够通过少量的迭代即能求解该非线性方程的解fi。
[0064]
相对于现有技术，本发明的有效收益如下：
[0065]
1、本发明提出的方法解决了现有标定方法中，在同时求解相机焦距、畸变和姿态
时需要≥4个标志点，且部分方法具有多解的技术问题以及只采用某一种畸变模型的问题。
[0066]
2、本发明提出的方法仅需利用3个标志点和已知的相机位置，即可同时求解得到相机的焦距、畸变和姿态，且具有唯一解且本发明能够适用于多种畸变模型。
[0067]
3、本发明适用于可提前获取相机位置，畸变严重且径向畸变模型未知，可利用的标志点数量少的应用场景。
附图说明
[0068]
图1为无畸变下的3d点和相机成像点的对应关系示意图；
[0069]
图2为径向畸变下的相机成像几何结构示意图；
[0070]
图3畸变矫正后的2d-3d点对应关系示意图；
[0071]
图4不同坐标系转换关系示意图。
具体实施方式
[0072]
下面结合附图和实施例对本发明进行详细的解释和说明。
[0073]
本发明提出了一种基于相机位置和三个标志点的相机焦距、畸变及姿态标定方法，只需要布设三个标志点，测量相机位置和三个标志点位置，拍摄标志点图像，得到相机与标志点的成像对应关系，利用径向畸变后与相机主点间的方位不发生变化这一重要几何现象，建立了一种迭代求解焦距和4阶畸变的方法，进而对图像进行畸变矫正，最后利用无畸变的图像对应的标志点的成像位置对相机姿态进行标定。与现有标定方法相比，该方法减少了标志点的数量，缩短了标定时间，具有唯一解，且可采用多种径向畸变模型等优点。
[0074]
当相机位置点c(x
0 y
0 z0)已知，三个标志点p1(x
1 y
1 z1)、p2(x
2 y
2 z2)、p3(x
3 y
3 z3)已知，无畸变条件下外标志点与相机成像位置具体关系如图1所示。
[0075]
其中，p1、p2、p3为无畸变条件下p1、p2、p3在相机中的成像位置。pc为相机主点，位于图像中心；oc为相机位置。在实际运用中，由于畸变的存在，只能获取得到畸变的2d成像点而无畸变的2d成像点pi是无法获取的。本发明的目的就是利用3d标志点pi(i＝1,2,3)和畸变的2d成像点来标定相机的焦距、畸变以及姿态。
[0076]
为了简化解决技术问题的过程，本发明将该标定问题分解为两个子问题进行求解，使得求解更加高效：第一个问题是求解焦距和畸变参数，第二个问题是求解相机外参数。
[0077]
对于在第一个问题，利用径向畸变后与相机主点间的方位不发生变化这一重要几何现象，建立了一种迭代求解焦距和4阶畸变的方法，在迭代过程中，利用不考虑畸变时的中间变量作为初始值，且该初始值比较好地接近真值，这样即使方法需要迭代，但初始值很好，也可快速得到全局最优解；此外，这种方法能够适应多种畸变模型，扩大了方法的适用性。在求解得到畸变和焦距后，获取矫正图像，并利用几何约束线性求解相机的外参数。
[0078]
对于第二个问题中，利用第一个问题得到的畸变参数，先对图像进行畸变矫正，得到无畸变图像；然后利用相机位置，已知的标志点和对应的成像点即可对相机姿态进行标定。
[0079]
本发明的具体实现步骤如下：
[0080]
步骤1、建立非线性方程组迭代求解中间变量
[0081]
目前，存在很多畸变模型，并且对于大部分数码相机，主要的畸变为径向畸变，使用最为广泛的为除法畸变模型和传统畸变模型。
[0082]
除法畸变模型如下
[0083][0084]
传统畸变模型如下
[0085][0086]
其中，ki为径向畸变系数，径向畸变主要受前两项的影响，因此本发明中分析前两项。当然，随着标志点数量的增加，分析的畸变系数数量也会增加。可以看到，不论哪一种畸变模型，在畸变下，成像点和畸变中心(即相机主点)间的方向不发生变化，仅仅距离发生变化。本发明利用径向畸变下成像点和畸变中心间的方向不发生变化这一重要且关键的几何特征，对相机的焦距和畸变进行计算。根据这一特征，径向畸变下的几何结构如图2所示。
[0087]
图2中，∠p1ocp2,∠p2ocp3,∠p3ocp1分别表示为α1,β1,γ1，由于pi和oc位置已知，则这三个角的角度大小可以通过图1中的三角形δp1ocp2,δp2ocp3,δp3ocp1计算得到。同时，∠p1pcp2,∠p2pcp3,∠p3pcp1分别表示为α2,β2,γ2。这里，主点pc和畸变点已知，但未畸变点pi未知。但是，由于畸变前后，成像点与主点间的方向不发生变化，因此，α2,β2,γ2可以通过三角形过三角形计算得到。在接下来的计算过程中，仅仅涉及未畸变点和畸变中心的距离，不涉及畸变系数，因此的方法不直接计算畸变系数。
[0088]
设pcpi＝xi,ocpi＝yi，因为三角形δp1pcp2,δp1ocp2具有公共边p1p2，因此通过余弦定理可以得到
[0089][0090]
在图2中，ocpc＝f，为焦距，其垂直于平面p1p2p3，因此，根据三角形δocpcpi可以得到
[0091][0092]
将式(4)带入式(3)，进而可以得到
[0093][0094]
同理，能够获取另外两个方程
[0095][0096]
设然后得到具有三个未知数的方程组
[0097]
[0098]
接下来采用迭代方法求解该非线性方程组，如lm(levenberg-marquardt)方法。对于非线性方程组，差的初始值迭代到全局最优解的速度慢，甚至只能求解得到局部最优解，无法得到全局最优解，而好的初始值能够提高计算速度并能得到全局最优解，因此该方法需要好的初始值。因此，为了获得全局最优解，重要一步就是选择该非线性方程组的初始值。这里采用不考虑畸变条件下的fi的初始值，该初始值可以通过传统的pnp算法给出，由于该初始值接近真值，因此可通过少量的迭代即能求解该非线性方程的解fi。
[0099]
步骤2、计算焦距和畸变系数
[0100]
获取fi的值后，根据不同的径向畸变模型可以获取得到焦距和径向畸变系数。由于常用的径向畸变模型为除法畸变模型和传统畸变模型，本发明对两种模型下的计算方法分别展开说明。
[0101]
2.1对于除法畸变模型
[0102]
两参数模型如下：
[0103][0104]
其中，kj(j＝1,2)为径向畸变系数，为畸变点到主点的距离，进而得到
[0105][0106]
这里，为已知量；因此，可以得到一个多项式方程组，如下所示。
[0107][0108]
为实现线性求解，将f,k1f,k2f视为上述方程组的三个未知量。然后一个线性方程组可以得到如下：
[0109]
a1x1＝y1ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(31)
[0110]
其中
[0111][0112]
则该方程组的解为然后，焦距和畸变系数计算如下：
[0113]
[0114]
2.2传统畸变模型。两参数模型如下：
[0115][0116]
同理，可以得到一个线性方程组如下：
[0117]
a2x2＝y2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(35)
[0118]
其中
[0119][0120]
最后，焦距和畸变系数可以直接线性求解：
[0121][0122]
至此，在两种畸变模型下均通过三个标志点和相机位置获取得到了焦距和两个畸变系数。
[0123]
步骤3、计算相机姿态
[0124]
根据步骤1得到的畸变系数以及对应的畸变模型，可以得到畸变矫正后成像点的位置。畸变矫正后的2d-3d点对应关系如图3所示。
[0125]
图3中，o
c-xcyczc为相机坐标系，o
w-x
wywzw
为世界坐标系。采用任意两个标志点和相机位置来计算相机姿态，也就是旋转矩阵r
w_c
以及平移向量t
w_c
。
[0126]
为计算相机姿态，定义一个新的相机坐标系o
c-x
c2yc2zc2
，一个新的世界坐标系o
c-x
w2yw2zw2
；新的相机坐标系定义如下：
[0127][0128]
其中，在第一步求解后为已知量。根据定义，在新的相机坐标系下，x
c2
轴为向量z
c2
轴垂直于平面ocp1p2，y
c2
轴根据右手坐标系进行定义。原始相机坐标系o
c-xcyczc下的点pc的坐标可以通过下式转换到新的相机坐标系o
c-x
w2yw2zw2
下的坐标p
c2
：
[0129][0130]
新的世界坐标系定义如下：
[0131][0132]
根据定义，新的世界坐标系下，oc为坐标系原点，x
w2
轴为向量z
w2
轴垂直于平面ocp1p2，y
w2
轴根据右手坐标系进行定义。原始世界坐标系o
c-x
wywzw
下的点pw的坐标可以通过下式转换到新的世界坐标系o
c-x
w2yw2zw2
下的坐标p
w2
：
[0133][0134]
显然，新的相机坐标系和新的世界坐标系重合。假设原始相机坐标系下的点pc和原始世界坐标系下的点pw为同一个点，根据新的相机坐标系和新的世界坐标系的定义，可以得到各坐标系下的转换关系如图4所示。
[0135]
因此，旋转矩阵r
w_c
以及平移向量t
w_c
计算如下：
[0136][0137]
至此，相机姿态计算完毕。
[0138]
实施例1
[0139]
下面给定三个标志点坐标p1(-9.31，13.50，200.00)，p2(22.86，0.06，200.00)，p3(22.58，6.38，230.00)，相机位置oc(0，0，0)。设置相机分辨率为1280
×
800，像元尺寸为14μm，采用除法畸变模型，两个径向畸变系数为分别为-0.01，-0.02，焦距50mm；其余相机内参数已知，设置相机理论外参数旋转矩阵为如下：
[0140][0141]
为逼近真实情况，在各3d标志点的2d成像点加入0.2个像素的误差。采用本发明方法，计算得到焦距为50.0385mm，畸变系数为-0.0101和0.0201，外参数旋转矩阵测量结果为：
[0142][0143]
由计算结果可以看到焦距、畸变系数和外参数的误差均很小。为更好的分析本发明方法的计算精度，对各点的重投影误差进行计算，得到平均重投影误差为0.12个像素，表明本发明方法计算精度高。