cvcalibration.cpp.svn-base
来自「非结构化路识别」· SVN-BASE 代码 · 共 2,039 行 · 第 1/5 页
SVN-BASE
2,039 行
haveFound = 1;
/* Copy vector */
icvCopyVector_64d( V + num * t, num, resV );
break;
}
}
if( haveFound == 0 )
{
icvDeleteMatrix( V );
icvDeleteVector( E );
return CV_NO_ERR; /* Error have no found */
}
icvDeleteMatrix( V );
icvDeleteVector( E );
return CV_NO_ERR;
}
/*======================================================================================*/
CvStatus
icvInitIntrinsicParams( int numImages,
int *numPoints,
CvSize imageSize,
CvPoint2D64d * imagePoints,
CvPoint2D64d * objectPoints,
CvVect64d focalLength,
CvPoint2D64d * principalPoint,
CvVect64d distortion, CvMatr64d cameraMatrix )
{
CvMatr64d allHomographies;
CvMatr64d A;
CvMatr64d Atran;
CvVect64d b;
int base;
int t;
int currImage;
double norm;
double a1, b1, c1;
double a2, b2, c2;
double a3, b3, c3;
double a4, b4, c4;
double tmpH[3 * 3];
double Htran[3 * 3];
double SubMatr[3 * 3];
double AtA[2 * 2];
double invAtA[2 * 2];
double tmpb[2];
double V_hori_pix[3];
double V_vert_pix[3];
double V_diag1_pix[3];
double V_diag2_pix[3];
/* Compute all homographies */
allHomographies = icvCreateMatrix_64d( 9, numImages );
A = icvCreateMatrix_64d( 2, numImages * 2 );
Atran = icvCreateMatrix_64d( numImages * 2, 2 );
b = icvCreateVector_64d( numImages * 2 );
base = 0;
for( t = 0; t < numImages; t++ )
{
icvFindHomography( numPoints[t],
imageSize,
imagePoints + base, objectPoints + base,
allHomographies + t * 9 );
base += numPoints[t];
}
/* Init principal point */
principalPoint->x = imageSize.width / 2.0f - 0.5f;
principalPoint->y = imageSize.height / 2.0f - 0.5f;
/* Init distortion coeficients */
distortion[0] = 0.0f;
distortion[1] = 0.0f;
distortion[2] = 0.0f;
distortion[3] = 0.0f;
/* compute subtract matrix */
SubMatr[0] = 1.0;
SubMatr[1] = 0.0;
SubMatr[2] = -principalPoint->x;
SubMatr[3] = 0.0;
SubMatr[4] = 1.0;
SubMatr[5] = -principalPoint->y;
SubMatr[6] = 0.0;
SubMatr[7] = 0.0;
SubMatr[8] = 1.0;
/* Subtract principal point shift from all homographies */
for( t = 0; t < numImages; t++ )
{
icvMulMatrix_64d( SubMatr, 3, 3, allHomographies + t * 9, 3, 3, tmpH );
icvCopyMatrix_64d( tmpH, 3, 3, allHomographies + t * 9 );
}
/* Create */
for( currImage = 0; currImage < numImages; currImage++ )
{
icvTransposeMatrix_64d( allHomographies + currImage * 9, 3, 3, Htran );
/* Create vanishing points */
icvCopyVector_64d( Htran + 0, 3, V_hori_pix );
icvCopyVector_64d( Htran + 3, 3, V_vert_pix );
icvAddVector_64d( Htran + 0, Htran + 3, V_diag1_pix, 3 );
icvScaleVector_64d( V_diag1_pix, V_diag1_pix, 3, 0.5f );
icvSubVector_64d( Htran + 0, Htran + 3, V_diag2_pix, 3 );
icvScaleVector_64d( V_diag2_pix, V_diag2_pix, 3, 0.5f );
norm = icvNormVector_64d( V_hori_pix, 3 );
icvScaleVector_64d( V_hori_pix, V_hori_pix, 3, 1.0f / norm );
norm = icvNormVector_64d( V_vert_pix, 3 );
icvScaleVector_64d( V_vert_pix, V_vert_pix, 3, 1.0f / norm );
norm = icvNormVector_64d( V_diag1_pix, 3 );
icvScaleVector_64d( V_diag1_pix, V_diag1_pix, 3, 1.0f / norm );
norm = icvNormVector_64d( V_diag2_pix, 3 );
icvScaleVector_64d( V_diag2_pix, V_diag2_pix, 3, 1.0f / norm );
a1 = V_hori_pix[0];
b1 = V_hori_pix[1];
c1 = V_hori_pix[2];
a2 = V_vert_pix[0];
b2 = V_vert_pix[1];
c2 = V_vert_pix[2];
a3 = V_diag1_pix[0];
b3 = V_diag1_pix[1];
c3 = V_diag1_pix[2];
a4 = V_diag2_pix[0];
b4 = V_diag2_pix[1];
c4 = V_diag2_pix[2];
/* Combine matrix A */
A[currImage * 4 + 0] = a1 * a2;
A[currImage * 4 + 1] = b1 * b2;
A[currImage * 4 + 2] = a3 * a4;
A[currImage * 4 + 3] = b3 * b4;
/* Combine vector b */
b[currImage * 2 + 0] = -c1 * c2;
b[currImage * 2 + 1] = -c3 * c4;
}
/* Compute AtA */
icvMulTransMatrixR_64d( A, 2, 2 * numImages, AtA );
/* Compute inverse of matr AtA */
icvInvertMatrix_64d( AtA, 2, invAtA );
/* Compute trans A */
icvTransposeMatrix_64d( A, 2, 2 * numImages, Atran );
/* multiplicate matrix At and vector b */
icvTransformVector_64d( Atran, b, tmpb, 2 * numImages, 2 );
icvTransformVector_64d( invAtA, tmpb, focalLength, 2, 2 );
focalLength[0] = sqrt( fabs( 1.0 / focalLength[0] ));
focalLength[1] = sqrt( fabs( 1.0 / focalLength[1] ));
cameraMatrix[0] = focalLength[0];
cameraMatrix[1] = 0.0f;
cameraMatrix[2] = principalPoint->x;
cameraMatrix[3] = 0.0f;
cameraMatrix[4] = focalLength[1];
cameraMatrix[5] = principalPoint->y;
cameraMatrix[6] = 0.0f;
cameraMatrix[7] = 0.0f;
cameraMatrix[8] = 1.0f;
icvDeleteMatrix( allHomographies );
icvDeleteMatrix( A );
icvDeleteMatrix( Atran );
icvDeleteVector( b );
return CV_NO_ERR;
}
/*======================================================================================*/
IPCVAPI_IMPL( CvStatus, icvRodrigues_64d, (CvMatr64d rotMatr,
CvVect64d rotVect,
CvMatr64d Jacobian, CvRodriguesType convType) )
{
double eps = DBL_EPSILON;
double bigeps = 10e+20*eps;
int t;
double dm3din[3*4];
double dm2dm3[4*4];
if( convType == CV_RODRIGUES_V2M )
{/* Convert Vector to Matrix Jac is w=3 x h=9 */
double theta;
double invTheta;
double alpha,betta,gamma;
double omega[3];
double omegav[3*3];
double A[3*3];
double tmp33[3*3];
double tmp49[4*9];
double tmp34[3*4];
double dRdm1[21*9];
double dm1dm2[4*21];
double w1,w2,w3;
theta = icvNormVector_64d( rotVect, 3 );
if( theta < eps )
{
/* Set Rotate matrix to ones */
icvSetIdentity_64d(rotMatr, 3, 3);
if( Jacobian != 0 )
{
//icvSetIdentity_64d(Jacobian,9,3);
Jacobian[0] = 0;
Jacobian[1] = 0;
Jacobian[2] = 0;
Jacobian[3] = 0;
Jacobian[4] = 0;
Jacobian[5] = 1;
Jacobian[6] = 0;
Jacobian[7] = -1;
Jacobian[8] = 0;
Jacobian[9] = 0;
Jacobian[10] = 0;
Jacobian[11] = -1;
Jacobian[12] = 0;
Jacobian[13] = 0;
Jacobian[14] = 0;
Jacobian[15] = 1;
Jacobian[16] = 0;
Jacobian[17] = 0;
Jacobian[18] = 0;
Jacobian[19] = 1;
Jacobian[20] = 0;
Jacobian[21] = -1;
Jacobian[22] = 0;
Jacobian[23] = 0;
Jacobian[24] = 0;
Jacobian[25] = 0;
Jacobian[26] = 0;
}
}
else
{
invTheta = 1.0 / theta;
icvScaleVector_64d(rotVect, omega, 3, 1.0f / theta);
icvSetIdentity_64d(dm3din,3,4);
icvCopyVector_64d(omega,3,dm3din+9);
icvSetIdentity_64d(dm2dm3,4,4);
icvScaleMatrix_64d(dm2dm3,dm2dm3,4,3,invTheta);
for( t = 0; t < 3; t++ )
{
dm2dm3[t * 4 + 3] = - omega[t]/theta;
}
alpha = (double)cos(theta);
betta = (double)sin(theta);
gamma = 1 - alpha;
omegav[0] = 0;
omegav[1] = - omega[2];
omegav[2] = omega[1];
omegav[3] = omega[2];
omegav[4] = 0;
omegav[5] = - omega[0];
omegav[6] = - omega[1];
omegav[7] = omega[0];
omegav[8] = 0;
/* Compute OMEGA*OMEGAt */
icvMulMatrix_64d(omega,1,3,omega,3,1,A);
icvSetZero_64d(dm1dm2,4,21);
dm1dm2[0 * 4 + 3] = - betta;
dm1dm2[1 * 4 + 3] = alpha;
dm1dm2[2 * 4 + 3] = betta;
dm1dm2[4 * 4 + 2] = 1;
dm1dm2[5 * 4 + 1] = - 1;
dm1dm2[6 * 4 + 2] = - 1;
dm1dm2[8 * 4 + 0] = 1;
dm1dm2[9 * 4 + 1] = 1;
dm1dm2[10* 4 + 0] = - 1;
w1 = omega[0];
w2 = omega[1];
w3 = omega[2];
dm1dm2[12 * 4 + 0] = 2 * w1;
dm1dm2[13 * 4 + 0] = w2;
dm1dm2[14 * 4 + 0] = w3;
dm1dm2[15 * 4 + 0] = w2;
dm1dm2[16 * 4 + 0] = 0;
dm1dm2[17 * 4 + 0] = 0;
dm1dm2[18 * 4 + 0] = w3;
dm1dm2[19 * 4 + 0] = 0;
dm1dm2[20 * 4 + 0] = 0;
dm1dm2[12 * 4 + 1] = 0;
dm1dm2[13 * 4 + 1] = w1;
dm1dm2[14 * 4 + 1] = 0;
dm1dm2[15 * 4 + 1] = w1;
dm1dm2[16 * 4 + 1] = 2 * w2;
dm1dm2[17 * 4 + 1] = w3;
dm1dm2[18 * 4 + 1] = 0;
dm1dm2[19 * 4 + 1] = w3;
dm1dm2[20 * 4 + 1] = 0;
dm1dm2[12 * 4 + 2] = 0;
dm1dm2[13 * 4 + 2] = 0;
dm1dm2[14 * 4 + 2] = w1;
dm1dm2[15 * 4 + 2] = 0;
dm1dm2[16 * 4 + 2] = 0;
dm1dm2[17 * 4 + 2] = w2;
dm1dm2[18 * 4 + 2] = w1;
dm1dm2[19 * 4 + 2] = w2;
dm1dm2[20 * 4 + 2] = 2 * w3;
icvSetIdentity_64d(rotMatr, 3, 3);
icvScaleMatrix_64d(rotMatr, rotMatr, 3, 3, alpha);
icvScaleMatrix_64d(omegav, tmp33, 3, 3, betta);
icvAddMatrix_64d(rotMatr, tmp33, rotMatr, 3, 3);
icvScaleMatrix_64d(A, tmp33, 3, 3, gamma);
icvAddMatrix_64d(rotMatr, tmp33, rotMatr, 3, 3);
if( Jacobian != 0 )
{
icvSetZero_64d(dRdm1,21,9);
dRdm1[0 * 21 + 0] = 1;
dRdm1[4 * 21 + 0] = 1;
dRdm1[8 * 21 + 0] = 1;
for( t = 0; t < 9; t++ )
{
dRdm1[t * 21 + 1] = omegav[(t%3)*3 + t/3];
dRdm1[t * 21 + t + 3] = betta;
dRdm1[t * 21 + 2 ] = A[(t%3)*3 + t/3];
dRdm1[t * 21 + t +12] = gamma;
}
/* Calculate Jacobian = dRdm1 * dm1dm2 * dm2dm3 * dm3din */
icvMulMatrix_64d(dRdm1,21,9,dm1dm2,4,21,tmp49);
icvMulMatrix_64d(dm2dm3,4,4,dm3din,3,4,tmp34);
icvMulMatrix_64d(tmp49,4,9,tmp34,3,4,Jacobian);
}
}
}
else
{/* convType==CV_M2V Convert Matrix to Vector Jac is w=9 x h=3 */
double trace;
double theta;
double dthetadtr;
double vth;
double dtrdR[9*1];
double dvar1dtheta[2];
double om1[3];
double dthetadR[9];
double dvardR[9*5];
double dvar2dvar[5*4];
double tmp53[5*3];
double om[3];
double domegadvar2[4*3];
double tmp3[3];
double invRotMatr[9];
double tmpEye3[9];
double tmpmat3[9];
double detRotMatr;
double matrS[3];
CvMat tmpMat, matS;
/* Need to test corrected input data */
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?