相机标定原理——opencv内参解析解推导
封闭解估计五个内在参数 cvInitIntrinsicParams2D()
源代码
/* 2.1 封闭解估计参数 */
CV_IMPL void cvInitIntrinsicParams2D( const CvMat* objectPoints,
const CvMat* imagePoints, const CvMat* npoints,
CvSize imageSize, CvMat* cameraMatrix,
double aspectRatio )
{
Ptr<CvMat> matA, _b, _allH;
int i, j, pos, nimages, ni = 0;
// 相机内参a[9]
double a[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 };
// 相机外参H[9],焦距f[2]
double H[9] = {0}, f[2] = {0};
CvMat _a = cvMat( 3, 3, CV_64F, a );
CvMat matH = cvMat( 3, 3, CV_64F, H );
CvMat _f = cvMat( 2, 1, CV_64F, f );
// 图像数量
nimages = npoints->rows + npoints->cols - 1;
matA.reset(cvCreateMat( 2*nimages, 2, CV_64F ));
_b.reset(cvCreateMat( 2*nimages, 1, CV_64F ));
// a[2], a[5]:初始化光心坐标
a[2] = (!imageSize.width) ? 0.5 : (imageSize.width - 1)*0.5;
a[5] = (!imageSize.height) ? 0.5 : (imageSize.height - 1)*0.5;
// 所有图像的 单应性矩阵H
_allH.reset(cvCreateMat( nimages, 9, CV_64F ));
// 获得焦距的初始值
for( i = 0, pos = 0; i < nimages; i++, pos += ni )
{
double* Ap = matA->data.db + i*4;
double* bp = _b->data.db + i*2;
ni = npoints->data.i[i];
double h[3], v[3], d1[3], d2[3];
double n[4] = {0,0,0,0};
// 提取每张图像的图像点和对象点
CvMat _m, matM;
cvGetCols( objectPoints, &matM, pos, pos + ni );
cvGetCols( imagePoints, &_m, pos, pos + ni );
// 图像点与对象点的 单应性矩阵H
/* cvFindHomography: 计算 图像点与对象点的 单应性矩阵H(投影变换矩阵)*/
cvFindHomography( &matM, &_m, &matH );
memcpy( _allH->data.db + i*9, H, sizeof(H) );
/* 根据H求解相机两个焦距内参 */
H[0] -= H[6]*a[2]; H[1] -= H[7]*a[2]; H[2] -= H[8]*a[2];
H[3] -= H[6]*a[5]; H[4] -= H[7]*a[5]; H[5] -= H[8]*a[5];
for( j = 0; j < 3; j++ )
{
double t0 = H[j*3], t1 = H[j*3+1];
h[j] = t0; v[j] = t1;
d1[j] = (t0 + t1)*0.5;
d2[j] = (t0 - t1)*0.5;
n[0] += t0*t0; n[1] += t1*t1;
n[2] += d1[j]*d1[j]; n[3] += d2[j]*d2[j];
}
for( j = 0; j < 4; j++ )
n[j] = 1./std::sqrt(n[j]);
for( j = 0; j < 3; j++ )
{
h[j] *= n[0]; v[j] *= n[1];
d1[j] *= n[2]; d2[j] *= n[3];
}
Ap[0] = h[0]*v[0]; Ap[1] = h[1]*v[1];
Ap[2] = d1[0]*d2[0]; Ap[3] = d1[1]*d2[1];
bp[0] = -h[2]*v[2]; bp[1] = -d1[2]*d2[2];
}
/*
cvSolve : 求解线性系统或者最小二乘法问题
求解两个焦距参数
*/
cvSolve( matA, _b, &_f, CV_NORMAL + CV_SVD );
a[0] = std::sqrt(fabs(1./f[0]));
a[4] = std::sqrt(fabs(1./f[1]));
if( aspectRatio != 0 )
{
double tf = (a[0] + a[4])/(aspectRatio + 1.);
a[0] = aspectRatio*tf;
a[4] = tf;
}
cvConvert( &_a, cameraMatrix );
}
opencv中封闭解 计算原理及步骤:
(1) 通过对象点和图像点由cvFindHomography()函数计算单应性矩阵H;
(2) 有:
H=A[r1,r2,t]=[h11h12h13h21h22h23h31h32h33]
\mathbf{H}=\mathbf{A}[\mathbf{r} 1, \mathbf{r} \mathbf{2}, \mathbf{t}]=\left[\begin{array}{lll}
h 11 & h 12 & h 13 \\
h 21 & h 22 & h 23 \\
h 31 & h 32 & h 33
\end{array}\right]
H=A[r1,r2,t]=h11h21h31h12h22h32h13h23h33
此时,A中设定倾斜系数c=0,主点u0, v0由图像大小得到,因此现在的相机内参只有两个焦距未知量:
A=[α0u00βv0001]
A=\left[\begin{array}{ccc}
\alpha & 0 & u_0 \\
0 & \beta & v_0 \\
0 & 0 & 1
\end{array}\right]
A=α000β0u0v01
由上面两式有:
[α0u00βv0001][r11r12t1r21r22t2r31r32t3]=[H0H1H2H3H4H5H6H7H8]
\left[\begin{array}{ccc}
\alpha & 0 & u_0 \\
0 & \beta & v_0 \\
0 & 0 & 1
\end{array}\right]\left[\begin{array}{lll}
r_{11} & r_{12} & t_1 \\
r_{21} & r_{22} & t_2 \\
r_{31} & r_{32} & t_3
\end{array}\right]=\left[\begin{array}{ccc}
H_0 & H_1 & H_2 \\
H_3 & H_4 & H_5 \\
H_6 & H_7 & H_8
\end{array}\right]
α000β0u0v01r11r21r31r12r22r32t1t2t3=H0H3H6H1H4H7H2H5H8
展开有得到以下9项(求解焦距只需要这几项):
αr11+u0r31=H0αr12+u0r32=H1αt1+u0t3=H2βr21+v0r31=H3βr22+v0r32=H4βt2+v0t3=H5r31=H6r32=H7t3=H8
\begin{aligned}
\alpha r_{11}+u_0 r_{31} & =H_0 \\
\alpha r_{12}+u_0 r_{32} & =H_1 \\
\alpha t_1+u_0 t_3 & =H_2 \\
\beta r_{21}+v_0 r_{31} & =H_3 \\
\beta r_{22}+v_0 r_{32} & =H_4 \\
\beta t_2+v_0 t_3 & =H_5 \\
r_{31} & =H_6 \\
r_{32} & =H_7 \\
t_3 & =H_8
\end{aligned}
αr11+u0r31αr12+u0r32αt1+u0t3βr21+v0r31βr22+v0r32βt2+v0t3r31r32t3=H0=H1=H2=H3=H4=H5=H6=H7=H8
将下面三项带入上面四项有:
αr11=H0−u0H6αr12=H1−u0H7αt1=H2−u0H8βr21=H3−v0H6βr22=H4−v0H7βt2=H5−v0H8r31=H6r32=H7t3=H8(a)
\begin{aligned}
\alpha r_{11} & =H_0-u_0 H_6 \\
\alpha r_{12} & =H_1-u_0 H_7 \\
\alpha t_1 & =H_2-u_0 H_8 \\
\beta r_{21} & =H_3-v_0 H_6 \\
\beta r_{22} & =H_4-v_0 H_7 \\
\beta t_2 & =H_5-v_0 H_8 \\
r_{31} & =H_6 \\
r_{32} & =H_7 \\
t_3 & =H_8
\end{aligned}\tag{a}
αr11αr12αt1βr21βr22βt2r31r32t3=H0−u0H6=H1−u0H7=H2−u0H8=H3−v0H6=H4−v0H7=H5−v0H8=H6=H7=H8(a)
上式就是以下代码:
H[0] -= H[6]*a[2]; H[1] -= H[7]*a[2]; H[2] -= H[8]*a[2];
H[3] -= H[6]*a[5]; H[4] -= H[7]*a[5]; H[5] -= H[8]*a[5];
此时H经过更新(根据代码)为:
αr11=H0αr12=H1αt1=H2βr21=H3βr22=H4βt2=H5r31=H6r32=H7t3=H8(b)
\begin{aligned}
\alpha r_{11} & =H_0 \\
\alpha r_{12} & =H_1 \\
\alpha t_1 & =H_2 \\
\beta r_{21} & =H_3 \\
\beta r_{22} & =H_4 \\
\beta t_2 & =H_5 \\
r_{31} & =H_6 \\
r_{32} & =H_7 \\
t_3 & =H_8
\end{aligned}\tag{b}
αr11αr12αt1βr21βr22βt2r31r32t3=H0=H1=H2=H3=H4=H5=H6=H7=H8(b)
即
r11=H0/αr12=H1/αt1=H2/αr21=H3/βr22=H4/βt2=H5/βr31=H6r32=H7t3=H8
\begin{aligned}
r_{11} & =H_0/\alpha \\
r_{12} & =H_1/\alpha \\
t_1 & =H_2/\alpha \\
r_{21} & =H_3/\beta \\
r_{22} & =H_4/\beta \\
t_2 & =H_5/\beta \\
r_{31} & =H_6 \\
r_{32} & =H_7 \\
t_3 & =H_8
\end{aligned}
r11r12t1r21r22t2r31r32t3=H0/α=H1/α=H2/α=H3/β=H4/β=H5/β=H6=H7=H8
(3) 在(a)中,我们发现6个方程有8个未知量,要想解出焦距还需要两个方程,这两个方程就是前面的两个约束:
r1Tr2=0r1Tr1=r2Tr2=1
\begin{gathered}
r_1^T r_2=0 \\
r_1^T r_1=r_2^T r_2=1
\end{gathered}
r1Tr2=0r1Tr1=r2Tr2=1
结合式(b):
r1:r11=H0/αr21=H3/βr31=H6r2:r12=H1/αr22=H4/βr32=H7
\begin{aligned}
r_1: & \\
r_{11} & =H_0 / \alpha \\
r_{21} & =H_3 / \beta \\
r_{31} & =H_6 \\
r_2: & \\
r_{12} & =H_1 / \alpha \\
r_{22} & =H_4 / \beta \\
r_{32} & =H_7
\end{aligned}
r1:r11r21r31r2:r12r22r32=H0/α=H3/β=H6=H1/α=H4/β=H7
则有两个新约束方程:
H0H1α2+H3H4β2+H6H7=0H02α2+H32β2+H62=H12α2+H42β2+H72⇒H02−H12α2+H32−H42β2+(H62−H72)=0(C)
\begin{aligned}
& \frac{H_0 H_1}{\alpha^2}+\frac{H_3 H_4}{\beta^2}+H_6 H_7=0 \\
& \frac{H_0^2}{\alpha^2}+\frac{H_3^2}{\beta^2}+H_6^2=\frac{H_1^2}{\alpha^2}+\frac{H_4^2}{\beta^2}+H_7^2 \\
& \Rightarrow \frac{H_0^2-H_1^2}{\alpha^2}+\frac{H_3^2-H_4^2}{\beta^2}+\left(H_6^2-H_7^2\right)=0
\end{aligned}\tag{C}
α2H0H1+β2H3H4+H6H7=0α2H02+β2H32+H62=α2H12+β2H42+H72⇒α2H02−H12+β2H32−H42+(H62−H72)=0(C)
根据(C)结合所有图像则可以得到方程组求解出两个焦距。代码为(代码中n是为了单位化向量r):
for( j = 0; j < 3; j++ )
{
double t0 = H[j*3], t1 = H[j*3+1];
h[j] = t0; v[j] = t1;
d1[j] = (t0 + t1)*0.5;
d2[j] = (t0 - t1)*0.5;
n[0] += t0*t0; n[1] += t1*t1;
n[2] += d1[j]*d1[j]; n[3] += d2[j]*d2[j];
}
for( j = 0; j < 4; j++ )
n[j] = 1./std::sqrt(n[j]);
for( j = 0; j < 3; j++ )
{
h[j] *= n[0]; v[j] *= n[1];
d1[j] *= n[2]; d2[j] *= n[3];
}
Ap[0] = h[0]*v[0]; Ap[1] = h[1]*v[1];
Ap[2] = d1[0]*d2[0]; Ap[3] = d1[1]*d2[1];
bp[0] = -h[2]*v[2]; bp[1] = -d1[2]*d2[2];
}
/*
cvSolve : 求解线性系统或者最小二乘法问题
求解两个焦距参数
*/
cvSolve( matA, _b, &_f, CV_NORMAL + CV_SVD );
a[0] = std::sqrt(fabs(1./f[0]));
a[4] = std::sqrt(fabs(1./f[1]));
将代码中的公式罗列出:
j=0t0=H0,t1=H1;h[0]=t0=H0;v[0]=t1=H1;d1[0]=(t0+t1)∗0.5=H0+H12;d2[0]=(t0−t1)∗0.5=H0−H12;n[0]+=t0∗t0=H02;n[1]+=t1∗t1=H12;n[2]+=d1[0]∗d1[0]=(H0+H12)2;n[3]+=d2[0]∗d2[0]=(H0−H12)2;
\begin{aligned}
& j=0 \\
& t 0=H_0, t 1=H_1 ; \\
& h[0]=t 0=H_0 ; v[0]=t 1=H_1 ; \\
& d 1[0]=(t 0+t 1) * 0.5=\frac{H_0+H_1}{2} ; \\
& d 2[0]=(t 0-t 1) * 0.5=\frac{H_0-H_1}{2} ; \\
& n[0]+=t 0 * t 0=H_0^2 ; \\
& n[1]+=t 1 * t 1=H_1^2 ; \\
& n[2]+=d 1[0] * d 1[0]=\left(\frac{H_0+H_1}{2}\right)^2 ; \\
& n[3]+=d 2[0] * d 2[0]=\left(\frac{H_0-H_1}{2}\right)^2 ;
\end{aligned}
j=0t0=H0,t1=H1;h[0]=t0=H0;v[0]=t1=H1;d1[0]=(t0+t1)∗0.5=2H0+H1;d2[0]=(t0−t1)∗0.5=2H0−H1;n[0]+=t0∗t0=H02;n[1]+=t1∗t1=H12;n[2]+=d1[0]∗d1[0]=(2H0+H1)2;n[3]+=d2[0]∗d2[0]=(2H0−H1)2;
j=1t0=H3,t1=H4;h[1]=t0=H3;v[1]=t1=H4;d1[1]=(t0+t1)∗0.5=H3+H42;d2[1]=(t0−t1)∗0.5=H3−H42;n[0]+=t0∗t0=H32+H02;n[1]+=t1∗t1=H42+H12;n[2]+=d1[1]∗d1[1]=(H3+H42)2+(H0+H12)2;n[3]+=d2[1]∗d2[1]=(H3−H42)2+(H0−H12)2; \begin{aligned} & j=1 \\ & t 0=H_3, t 1=H_4 ; \\ & h[1]=t 0=H_3 ; v[1]=t 1=H_4 ; \\ & d 1[1]=(t 0+t 1) * 0.5=\frac{H_3+H_4}{2} ; \\ & d 2[1]=(t 0-t 1) * 0.5=\frac{H_3-H_4}{2} ; \\ & n[0]+=t 0 * t 0=H_3^2 +H_0^2; \\ & n[1]+=t 1 * t 1=H_4^2 +H_1^2; \\ & n[2]+=d 1[1] * d 1[1]=\left(\frac{H_3+H_4}{2}\right)^2+\left(\frac{H_0+H_1}{2}\right)^2 ; \\ & n[3]+=d 2[1] * d 2[1]=\left(\frac{H_3-H_4}{2}\right)^2+\left(\frac{H_0-H_1}{2}\right)^2 ; \end{aligned} j=1t0=H3,t1=H4;h[1]=t0=H3;v[1]=t1=H4;d1[1]=(t0+t1)∗0.5=2H3+H4;d2[1]=(t0−t1)∗0.5=2H3−H4;n[0]+=t0∗t0=H32+H02;n[1]+=t1∗t1=H42+H12;n[2]+=d1[1]∗d1[1]=(2H3+H4)2+(2H0+H1)2;n[3]+=d2[1]∗d2[1]=(2H3−H4)2+(2H0−H1)2;
j=2t0=H6,t1=H7;h[1]=t0=H6;v[1]=t1=H7;d1[1]=(t0+t1)∗0.5=H6+H72;d2[1]=(t0−t1)∗0.5=H6−H72;n[0]+=t0∗t0=H62+H32+H02;n[1]+=t1∗t1=H72+H42+H12;n[2]+=d1[1]∗d1[1]=(H6+H72)2+(H3+H42)2+(H0+H12)2;n[3]+=d2[1]∗d2[1]=(H6−H72)2+(H3−H42)2+(H0−H12)2; \begin{aligned} & j=2 \\ & t 0=H_6, t 1=H_7 ; \\ & h[1]=t 0=H_6 ; v[1]=t 1=H_7 ; \\ & d 1[1]=(t 0+t 1) * 0.5=\frac{H_6+H_7}{2} ; \\ & d 2[1]=(t 0-t 1) * 0.5=\frac{H_6-H_7}{2} ; \\ & n[0]+=t 0 * t 0=H_6^2+ H_3^2+H_0^2; \\ & n[1]+=t 1 * t 1=H_7^2+ H_4^2+H_1^2 ; \\ & n[2]+=d 1[1] * d 1[1]=\left(\frac{H_6+H_7}{2}\right)^2+\left(\frac{H_3+H_4}{2}\right)^2+\left(\frac{H_0+H_1}{2}\right)^2 ; \\ & n[3]+=d 2[1] * d 2[1]=\left(\frac{H_6-H_7}{2}\right)^2+\left(\frac{H_3-H_4}{2}\right)^2+\left(\frac{H_0-H_1}{2}\right)^2 ; \end{aligned} j=2t0=H6,t1=H7;h[1]=t0=H6;v[1]=t1=H7;d1[1]=(t0+t1)∗0.5=2H6+H7;d2[1]=(t0−t1)∗0.5=2H6−H7;n[0]+=t0∗t0=H62+H32+H02;n[1]+=t1∗t1=H72+H42+H12;n[2]+=d1[1]∗d1[1]=(2H6+H7)2+(2H3+H4)2+(2H0+H1)2;n[3]+=d2[1]∗d2[1]=(2H6−H7)2+(2H3−H4)2+(2H0−H1)2;
上述式子就是以下代码:
for( j = 0; j < 3; j++ )
{
double t0 = H[j*3], t1 = H[j*3+1];
h[j] = t0; v[j] = t1;
d1[j] = (t0 + t1)*0.5;
d2[j] = (t0 - t1)*0.5;
n[0] += t0*t0; n[1] += t1*t1;
n[2] += d1[j]*d1[j]; n[3] += d2[j]*d2[j];
}
变量n开根号求倒数
n[0]=1n[0]=1H62+H32+H02;n[1]=1n[1]=1H72+H42+H12;n[2]=1n[2]=1(H6+H72)2+(H3+H42)2+(H0+H12)2;n[3]=1n[3]=1(H6−H72)2+(H3−H42)2+(H0−H12)2;
\begin{aligned}
& n[0]=\frac{1}{\sqrt{n[0]}}=\frac{1}{\sqrt{H_6^2+H_3^2+H_0^2}} ; \\
& n[1]=\frac{1}{\sqrt{n[1]}}=\frac{1}{\sqrt{H_7^2+H_4^2+H_1^2}} ; \\
& n[2]=\frac{1}{\sqrt{n[2]}}=\frac{1}{\sqrt{\left(\frac{H_6+H_7}{2}\right)^2+\left(\frac{H_3+H_4}{2}\right)^2+\left(\frac{H_0+H_1}{2}\right)^2}} ; \\
& n[3]=\frac{1}{\sqrt{n[3]}}=\frac{1}{\sqrt{\left(\frac{H_6-H_7}{2}\right)^2+\left(\frac{H_3-H_4}{2}\right)^2+\left(\frac{H_0-H_1}{2}\right)^2}} ;
\end{aligned}
n[0]=n[0]1=H62+H32+H021;n[1]=n[1]1=H72+H42+H121;n[2]=n[2]1=(2H6+H7)2+(2H3+H4)2+(2H0+H1)21;n[3]=n[3]1=(2H6−H7)2+(2H3−H4)2+(2H0−H1)21;
代码如下:
for( j = 0; j < 4; j++ )
n[j] = 1./std::sqrt(n[j]);
求h,v,d1,d2
j=0h[0]=n[0]h[0]=1H62+H32+H02∗H0v[0]=n[1]v[0]=1H72+H42+H12∗H1d1[0]=n[2]d1[0]=1(H6+H72)2+(H3+H42)2+(H0+H12)2∗H0+H12d2[0]=n[3]d2[0]=1(H6−H72)2+(H3−H42)2+(H0−H12)2∗H0−H12
\begin{aligned}
& j=0 \\
& h[0]=n[0] h[0]=\frac{1}{\sqrt{H_6^2+H_3^2+H_0^2}} * H_0 \\
& v[0]=n[1] v[0]=\frac{1}{\sqrt{H_7^2+H_4^2+H_1^2}} * H_1 \\
& d 1[0]=n[2] d 1[0]=\frac{1}{\sqrt{\left(\frac{H_6+H_7}{2}\right)^2+\left(\frac{H_3+H_4}{2}\right)^2+\left(\frac{H_0+H_1}{2}\right)^2}} * \frac{H_0+H_1}{2} \\
& d 2[0]=n[3] d 2[0]=\frac{1}{\sqrt{\left(\frac{H_6-H_7}{2}\right)^2+\left(\frac{H_3-H_4}{2}\right)^2+\left(\frac{H_0-H_1}{2}\right)^2}} * \frac{H_0-H_1}{2}
\end{aligned}
j=0h[0]=n[0]h[0]=H62+H32+H021∗H0v[0]=n[1]v[0]=H72+H42+H121∗H1d1[0]=n[2]d1[0]=(2H6+H7)2+(2H3+H4)2+(2H0+H1)21∗2H0+H1d2[0]=n[3]d2[0]=(2H6−H7)2+(2H3−H4)2+(2H0−H1)21∗2H0−H1
j=1h[1]=n[0]h[1]=1H62+H32+H02∗H3v[1]=n[1]v[1]=1H72+H42+H12∗H4d1[1]=n[2]d1[1]=1(H6+H72)2+(H3+H42)2+(H0+H12)2∗H3+H42d2[1]=n[3]d2[1]=1(H6−H72)2+(H3−H42)2+(H0−H12)2∗H3−H42 \begin{aligned} & j=1 \\ & h[1]=n[0] h[1]=\frac{1}{\sqrt{H_6^2+H_3^2+H_0^2}} * H_3 \\ & v[1]=n[1] v[1]=\frac{1}{\sqrt{H_7^2+H_4^2+H_1^2}} * H_4 \\ & d 1[1]=n[2] d 1[1]=\frac{1}{\sqrt{\left(\frac{H_6+H_7}{2}\right)^2+\left(\frac{H_3+H_4}{2}\right)^2+\left(\frac{H_0+H_1}{2}\right)^2}} * \frac{H_3+H_4}{2} \\ & d 2[1]=n[3] d 2[1]=\frac{1}{\sqrt{\left(\frac{H_6-H_7}{2}\right)^2+\left(\frac{H_3-H_4}{2}\right)^2+\left(\frac{H_0-H_1}{2}\right)^2}} * \frac{H_3-H_4}{2} \end{aligned} j=1h[1]=n[0]h[1]=H62+H32+H021∗H3v[1]=n[1]v[1]=H72+H42+H121∗H4d1[1]=n[2]d1[1]=(2H6+H7)2+(2H3+H4)2+(2H0+H1)21∗2H3+H4d2[1]=n[3]d2[1]=(2H6−H7)2+(2H3−H4)2+(2H0−H1)21∗2H3−H4
j=2h[2]=n[0]h[2]=1H62+H32+H02∗H6v[2]=n[1]v[2]=1H72+H42+H12∗H7d1[2]=n[2]d1[2]=1(H6+H72)2+(H3+H42)2+(H0+H12)2∗H6+H72d2[2]=n[3]d2[2]=1(H6−H72)2+(H3−H42)2+(H0−H12)2∗H6−H72 \begin{aligned} & j=2 \\ & h[2]=n[0] h[2]=\frac{1}{\sqrt{H_6^2+H_3^2+H_0^2}} * H_6 \\ & v[2]=n[1] v[2]=\frac{1}{\sqrt{H_7^2+H_4^2+H_1^2}} * H_7 \\ & d 1[2]=n[2] d 1[2]=\frac{1}{\sqrt{\left(\frac{H_6+H_7}{2}\right)^2+\left(\frac{H_3+H_4}{2}\right)^2+\left(\frac{H_0+H_1}{2}\right)^2}} * \frac{H_6+H_7}{2} \\ & d 2[2]=n[3] d 2[2]=\frac{1}{\sqrt{\left(\frac{H_6-H_7}{2}\right)^2+\left(\frac{H_3-H_4}{2}\right)^2+\left(\frac{H_0-H_1}{2}\right)^2}} * \frac{H_6-H_7}{2} \end{aligned} j=2h[2]=n[0]h[2]=H62+H32+H021∗H6v[2]=n[1]v[2]=H72+H42+H121∗H7d1[2]=n[2]d1[2]=(2H6+H7)2+(2H3+H4)2+(2H0+H1)21∗2H6+H7d2[2]=n[3]d2[2]=(2H6−H7)2+(2H3−H4)2+(2H0−H1)21∗2H6−H7
上述式子代码如下:
for( j = 0; j < 3; j++ )
{
h[j] *= n[0]; v[j] *= n[1];
d1[j] *= n[2]; d2[j] *= n[3];
}
计算Ap,Bp
Ap[0]=h[0]∗v[0]=H0H1H62+H32+H02H72+H42+H12Ap[1]=h[1]∗v[1]=H3H4H62+H32+H02H72+H42+H12Ap[2]=d1[0]∗d2[0]=(H0+H1)(H0−H1)4(H6+H72)2+(H3+H42)2+(H0+H12)2(H6−H72)2+(H3−H42)2+(H0−H12)2Ap[3]=d1[1]∗d2[1]=(H3+H4)(H3−H4)4(H6+H72)2+(H3+H42)2+(H0+H12)2(H6−H72)2+(H3−H42)2+(H0−H12)2bp[0]=−h[2]∗v[2]=−H6H7H62+H32+H02H72+H42+H12bp[1]=−d1[2]∗d2[2]=−(H6+H7)(H6−H7)4(H6+H72)2+(H3+H42)2+(H0+H12)2(H6−H72)2+(H3−H42)2+(H0−H12)2
\begin{aligned}
& A p[0]=h[0] * v[0]=\frac{H_0 H_1}{\sqrt{H_6^2+H_3^2+H_0^2} \sqrt{H_7^2+H_4^2+H_1^2}} \\
& A p[1]=h[1] * v[1]=\frac{H_3 H_4}{\sqrt{H_6^2+H_3^2+H_0^2} \sqrt{H_7^2+H_4^2+H_1^2}} \\
& A p[2]=d 1[0] * d 2[0]=\frac{\left(H_0+H_1\right)\left(H_0-H_1\right)}{4 \sqrt{\left(\frac{H_6+H_7}{2}\right)^2+\left(\frac{H_3+H_4}{2}\right)^2+\left(\frac{H_0+H_1}{2}\right)^2} \sqrt{\left(\frac{H_6-H_7}{2}\right)^2+\left(\frac{H_3-H_4}{2}\right)^2+\left(\frac{H_0-H_1}{2}\right)^2}} \\
& A p[3]=d 1[1] * d 2[1]=\frac{\left(H_3+H_4\right)\left(H_3-H_4\right)}{4 \sqrt{\left(\frac{H_6+H_7}{2}\right)^2+\left(\frac{H_3+H_4}{2}\right)^2+\left(\frac{H_0+H_1}{2}\right)^2} \sqrt{\left(\frac{H_6-H_7}{2}\right)^2+\left(\frac{H_3-H_4}{2}\right)^2+\left(\frac{H_0-H_1}{2}\right)^2}} \\
& b p[0]=-h[2] * v[2]=-\frac{H_6 H_7}{\sqrt{H_6^2+H_3^2+H_0^2} \sqrt{H_7^2+H_4^2+H_1^2}} \\
& b p[1]=-d 1[2] * d 2[2]=-\frac{\left(H_6+H_7\right)\left(H_6-H_7\right)}{4 \sqrt{\left(\frac{H_6+H_7}{2}\right)^2+\left(\frac{H_3+H_4}{2}\right)^2+\left(\frac{H_0+H_1}{2}\right)^2} \sqrt{\left(\frac{H_6-H_7}{2}\right)^2+\left(\frac{H_3-H_4}{2}\right)^2+\left(\frac{H_0-H_1}{2}\right)^2}}
\end{aligned}
Ap[0]=h[0]∗v[0]=H62+H32+H02H72+H42+H12H0H1Ap[1]=h[1]∗v[1]=H62+H32+H02H72+H42+H12H3H4Ap[2]=d1[0]∗d2[0]=4(2H6+H7)2+(2H3+H4)2+(2H0+H1)2(2H6−H7)2+(2H3−H4)2+(2H0−H1)2(H0+H1)(H0−H1)Ap[3]=d1[1]∗d2[1]=4(2H6+H7)2+(2H3+H4)2+(2H0+H1)2(2H6−H7)2+(2H3−H4)2+(2H0−H1)2(H3+H4)(H3−H4)bp[0]=−h[2]∗v[2]=−H62+H32+H02H72+H42+H12H6H7bp[1]=−d1[2]∗d2[2]=−4(2H6+H7)2+(2H3+H4)2+(2H0+H1)2(2H6−H7)2+(2H3−H4)2+(2H0−H1)2(H6+H7)(H6−H7)
代码如下:
for( j = 0; j < 3; j++ )
{
h[j] *= n[0]; v[j] *= n[1];
d1[j] *= n[2]; d2[j] *= n[3];
}
结合式(C),相当于用最小二乘法求解系数a,b
aH0H1+bH3H4=−H6H7a(H02−H12)+b(H32−H42)=−(H62−H72)
\begin{aligned}
& a H_0 H_1+b H_3 H_4=-H_6 H_7 \\
& a\left(H_0^2-H_1^2\right)+b\left(H_3^2-H_4^2\right)=-\left(H_6^2-H_7^2\right)
\end{aligned}
aH0H1+bH3H4=−H6H7a(H02−H12)+b(H32−H42)=−(H62−H72)
即
a[0]=std::sqrt(fabs(1./f[0]))=∣1a∣a[4]=std::sqrt(fabs(1./f[1]))=∣1b∣
\begin{aligned}
& a[0]=\operatorname{std}:: \operatorname{sqrt}(f a b s(1 . / f[0]))=\sqrt{\left|\frac{1}{a}\right|} \\
& a[4]=\operatorname{std}:: \operatorname{sqrt}(f a b s(1 . / f[1]))=\sqrt{\left|\frac{1}{b}\right|}
\end{aligned}
a[0]=std::sqrt(fabs(1./f[0]))=a1a[4]=std::sqrt(fabs(1./f[1]))=b1
对应代码如下:
/*
cvSolve : 求解线性系统或者最小二乘法问题
求解两个焦距参数
*/
cvSolve( matA, _b, &_f, CV_NORMAL + CV_SVD );
a[0] = std::sqrt(fabs(1./f[0]));
a[4] = std::sqrt(fabs(1./f[1]));