Chiebot-Mirror
/
opencv
8
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Merge pull request #22460 from savuor:levmarq_stereocalib_fix

Browse Source 
Stereo calibration fixed when intrinsic guess is given
pull/22491/head
Alexander Smorkalov 3 years ago committed by GitHub
parent 335d7bd16e a8b4ffac50
commit 8bfb419b0f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 189 additions and 129 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 247
      modules/calib/src/calibration.cpp
  2. 57
      modules/calib/test/test_cameracalibration.cpp

247
modules/calib/src/calibration.cpp
Unescape View File

@ -661,22 +661,14 @@ static double calibrateCameraInternal( const Mat& objectPoints,
//////////////////////////////// Stereo Calibration /////////////////////////////////// //////////////////////////////// Stereo Calibration ///////////////////////////////////
static int dbCmp( const void* _a, const void* _b )
{
double a = *(const double*)_a;
double b = *(const double*)_b;
return (a > b) - (a < b);
}
static double stereoCalibrateImpl( static double stereoCalibrateImpl(
const Mat& _objectPoints, const Mat& _imagePoints1, const Mat& _objectPoints, const Mat& _imagePoints1,
const Mat& _imagePoints2, const Mat& _npoints, const Mat& _imagePoints2, const Mat& _npoints,
Mat& _cameraMatrix1, Mat& _distCoeffs1, Mat& _cameraMatrix1, Mat& _distCoeffs1,
Mat& _cameraMatrix2, Mat& _distCoeffs2, Mat& _cameraMatrix2, Mat& _distCoeffs2,
Size imageSize, Mat* matR, Mat* matT, Size imageSize, Mat matR, Mat matT,
Mat* matE, Mat* matF, Mat matE, Mat matF,
Mat* perViewErr, int flags, Mat perViewErr, int flags,
TermCriteria termCrit ) TermCriteria termCrit )
{ {
const int NINTRINSIC = 18; const int NINTRINSIC = 18;
@ -729,7 +721,8 @@ static double stereoCalibrateImpl(
cameraMatrix.convertTo(A[k], CV_64F); cameraMatrix.convertTo(A[k], CV_64F);
if( flags & (CALIB_FIX_INTRINSIC|CALIB_USE_INTRINSIC_GUESS| if( flags & (CALIB_FIX_INTRINSIC|CALIB_USE_INTRINSIC_GUESS|
CALIB_FIX_K1|CALIB_FIX_K2|CALIB_FIX_K3|CALIB_FIX_K4|CALIB_FIX_K5|CALIB_FIX_K6|CALIB_FIX_TANGENT_DIST) ) CALIB_FIX_K1|CALIB_FIX_K2|CALIB_FIX_K3|CALIB_FIX_K4|CALIB_FIX_K5|CALIB_FIX_K6|
CALIB_FIX_TANGENT_DIST) )
{ {
Mat tdist( distCoeffs.size(), CV_MAKETYPE(CV_64F, distCoeffs.channels()), dk[k] ); Mat tdist( distCoeffs.size(), CV_MAKETYPE(CV_64F, distCoeffs.channels()), dk[k] );
distCoeffs.convertTo(tdist, CV_64F); distCoeffs.convertTo(tdist, CV_64F);
@ -741,8 +734,6 @@ static double stereoCalibrateImpl(
calibrateCameraInternal(objectPoints, imagePoints[k], calibrateCameraInternal(objectPoints, imagePoints[k],
_npoints, imageSize, 0, matA, Dist[k], _npoints, imageSize, 0, matA, Dist[k],
0, 0, 0, 0, 0, flags, termCrit); 0, 0, 0, 0, 0, flags, termCrit);
//std::cout << "K(" << k << "): " << A[k] << "\n";
//std::cout << "Dist(" << k << "): " << Dist[k] << "\n";
} }
} }
@ -762,13 +753,12 @@ static double stereoCalibrateImpl(
recomputeIntrinsics = (flags & CALIB_FIX_INTRINSIC) == 0; recomputeIntrinsics = (flags & CALIB_FIX_INTRINSIC) == 0;
Mat err( maxPoints*2, 1, CV_64F );
Mat Je( maxPoints*2, 6, CV_64F );
Mat J_LR( maxPoints*2, 6, CV_64F );
Mat Ji( maxPoints*2, NINTRINSIC, CV_64F, Scalar(0) );
// we optimize for the inter-camera R(3),t(3), then, optionally, // we optimize for the inter-camera R(3),t(3), then, optionally,
// for intrinisic parameters of each camera ((fx,fy,cx,cy,k1,k2,p1,p2) ~ 8 parameters). // for intrinisic parameters of each camera ((fx,fy,cx,cy,k1,k2,p1,p2) ~ 8 parameters).
// Param mapping is:
// - from 0 next 6: stereo pair Rt, from 6+i*6 next 6: Rt for each ith camera of nimages,
// - from 6*(nimages+1) next NINTRINSICS: intrinsics for 1st camera: fx, fy, cx, cy, 14 x dist
// - next NINTRINSICS: the same for for 2nd camera
nparams = 6*(nimages+1) + (recomputeIntrinsics ? NINTRINSIC*2 : 0); nparams = 6*(nimages+1) + (recomputeIntrinsics ? NINTRINSIC*2 : 0);
std::vector<uchar> mask(nparams, (uchar)1); std::vector<uchar> mask(nparams, (uchar)1);
@ -777,7 +767,7 @@ static double stereoCalibrateImpl(
if( recomputeIntrinsics ) if( recomputeIntrinsics )
{ {
uchar* imask = &mask[0] + nparams - NINTRINSIC*2; size_t idx = nparams - NINTRINSIC*2;
if( !(flags & CALIB_RATIONAL_MODEL) ) if( !(flags & CALIB_RATIONAL_MODEL) )
flags |= CALIB_FIX_K4 | CALIB_FIX_K5 | CALIB_FIX_K6; flags |= CALIB_FIX_K4 | CALIB_FIX_K5 | CALIB_FIX_K6;
if( !(flags & CALIB_THIN_PRISM_MODEL) ) if( !(flags & CALIB_THIN_PRISM_MODEL) )
@ -785,42 +775,43 @@ static double stereoCalibrateImpl(
if( !(flags & CALIB_TILTED_MODEL) ) if( !(flags & CALIB_TILTED_MODEL) )
flags |= CALIB_FIX_TAUX_TAUY; flags |= CALIB_FIX_TAUX_TAUY;
if( flags & CALIB_FIX_ASPECT_RATIO ) if( flags & CALIB_FIX_ASPECT_RATIO )
imask[0] = imask[NINTRINSIC] = 0; mask[idx + 0] = mask[idx + NINTRINSIC] = 0;
if ( flags & CALIB_SAME_FOCAL_LENGTH)
mask[idx + NINTRINSIC] = mask[idx + NINTRINSIC + 1] = 0;
if( flags & CALIB_FIX_FOCAL_LENGTH ) if( flags & CALIB_FIX_FOCAL_LENGTH )
imask[0] = imask[1] = imask[NINTRINSIC] = imask[NINTRINSIC+1] = 0; mask[idx + 0] = mask[idx + 1] = mask[idx + NINTRINSIC] = mask[idx + NINTRINSIC+1] = 0;
if( flags & CALIB_FIX_PRINCIPAL_POINT ) if( flags & CALIB_FIX_PRINCIPAL_POINT )
imask[2] = imask[3] = imask[NINTRINSIC+2] = imask[NINTRINSIC+3] = 0; mask[idx + 2] = mask[idx + 3] = mask[idx + NINTRINSIC+2] = mask[idx + NINTRINSIC+3] = 0;
if( flags & (CALIB_ZERO_TANGENT_DIST|CALIB_FIX_TANGENT_DIST) ) if( flags & (CALIB_ZERO_TANGENT_DIST|CALIB_FIX_TANGENT_DIST) )
imask[6] = imask[7] = imask[NINTRINSIC+6] = imask[NINTRINSIC+7] = 0; mask[idx + 6] = mask[idx + 7] = mask[idx + NINTRINSIC+6] = mask[idx + NINTRINSIC+7] = 0;
if( flags & CALIB_FIX_K1 ) if( flags & CALIB_FIX_K1 )
imask[4] = imask[NINTRINSIC+4] = 0; mask[idx + 4] = mask[idx + NINTRINSIC+4] = 0;
if( flags & CALIB_FIX_K2 ) if( flags & CALIB_FIX_K2 )
imask[5] = imask[NINTRINSIC+5] = 0; mask[idx + 5] = mask[idx + NINTRINSIC+5] = 0;
if( flags & CALIB_FIX_K3 ) if( flags & CALIB_FIX_K3 )
imask[8] = imask[NINTRINSIC+8] = 0; mask[idx + 8] = mask[idx + NINTRINSIC+8] = 0;
if( flags & CALIB_FIX_K4 ) if( flags & CALIB_FIX_K4 )
imask[9] = imask[NINTRINSIC+9] = 0; mask[idx + 9] = mask[idx + NINTRINSIC+9] = 0;
if( flags & CALIB_FIX_K5 ) if( flags & CALIB_FIX_K5 )
imask[10] = imask[NINTRINSIC+10] = 0; mask[idx + 10] = mask[idx + NINTRINSIC+10] = 0;
if( flags & CALIB_FIX_K6 ) if( flags & CALIB_FIX_K6 )
imask[11] = imask[NINTRINSIC+11] = 0; mask[idx + 11] = mask[idx + NINTRINSIC+11] = 0;
if( flags & CALIB_FIX_S1_S2_S3_S4 ) if( flags & CALIB_FIX_S1_S2_S3_S4 )
{ {
imask[12] = imask[NINTRINSIC+12] = 0; mask[idx + 12] = mask[idx + NINTRINSIC+12] = 0;
imask[13] = imask[NINTRINSIC+13] = 0; mask[idx + 13] = mask[idx + NINTRINSIC+13] = 0;
imask[14] = imask[NINTRINSIC+14] = 0; mask[idx + 14] = mask[idx + NINTRINSIC+14] = 0;
imask[15] = imask[NINTRINSIC+15] = 0; mask[idx + 15] = mask[idx + NINTRINSIC+15] = 0;
} }
if( flags & CALIB_FIX_TAUX_TAUY ) if( flags & CALIB_FIX_TAUX_TAUY )
{ {
imask[16] = imask[NINTRINSIC+16] = 0; mask[idx + 16] = mask[idx + NINTRINSIC+16] = 0;
imask[17] = imask[NINTRINSIC+17] = 0; mask[idx + 17] = mask[idx + NINTRINSIC+17] = 0;
} }
} }
// storage for initial [om(R){i}|t{i}] (in order to compute the median for each component) // storage for initial [om(R){i}|t{i}] (in order to compute the median for each component)
Mat RT0(6, nimages, CV_64F); std::vector<double> rtsort(nimages*6);
double* RT0data = RT0.ptr<double>();
/* /*
Compute initial estimate of pose Compute initial estimate of pose
For each image, compute: For each image, compute:
@ -836,19 +827,19 @@ static double stereoCalibrateImpl(
for(int i = 0; i < nimages; i++ ) for(int i = 0; i < nimages; i++ )
{ {
int ni = _npoints.at<int>(i); int ni = _npoints.at<int>(i);
Mat objpt_i(1, ni, CV_64FC3, objectPoints.ptr<double>() + pos*3); Mat objpt_i = objectPoints(Range::all(), Range(pos, pos + ni));
Matx33d R[2]; Matx33d R[2];
Vec3d rv, T[2]; Vec3d rv, T[2];
for(int k = 0; k < 2; k++ ) for(int k = 0; k < 2; k++ )
{ {
Mat imgpt_ik = Mat(1, ni, CV_64FC2, imagePoints[k].ptr<double>() + pos*2); Mat imgpt_ik = imagePoints[k](Range::all(), Range(pos, pos + ni));
solvePnP(objpt_i, imgpt_ik, A[k], Dist[k], rv, T[k], false, SOLVEPNP_ITERATIVE ); solvePnP(objpt_i, imgpt_ik, A[k], Dist[k], rv, T[k], false, SOLVEPNP_ITERATIVE );
Rodrigues(rv, R[k]); Rodrigues(rv, R[k]);
if( k == 0 ) if( k == 0 )
{ {
// save initial om_left and T_left // save initial om_left and T_left
param[(i+1)*6] = rv[0]; param[(i+1)*6 + 0] = rv[0];
param[(i+1)*6 + 1] = rv[1]; param[(i+1)*6 + 1] = rv[1];
param[(i+1)*6 + 2] = rv[2]; param[(i+1)*6 + 2] = rv[2];
param[(i+1)*6 + 3] = T[0][0]; param[(i+1)*6 + 3] = T[0][0];
@ -861,12 +852,12 @@ static double stereoCalibrateImpl(
Rodrigues(R[0], rv); Rodrigues(R[0], rv);
RT0data[i] = rv[0]; rtsort[i + nimages*0] = rv[0];
RT0data[i + nimages] = rv[1]; rtsort[i + nimages*1] = rv[1];
RT0data[i + nimages*2] = rv[2]; rtsort[i + nimages*2] = rv[2];
RT0data[i + nimages*3] = T[1][0]; rtsort[i + nimages*3] = T[1][0];
RT0data[i + nimages*4] = T[1][1]; rtsort[i + nimages*4] = T[1][1];
RT0data[i + nimages*5] = T[1][2]; rtsort[i + nimages*5] = T[1][2];
pos += ni; pos += ni;
} }
@ -874,12 +865,12 @@ static double stereoCalibrateImpl(
if(flags & CALIB_USE_EXTRINSIC_GUESS) if(flags & CALIB_USE_EXTRINSIC_GUESS)
{ {
Vec3d R, T; Vec3d R, T;
matT->convertTo(T, CV_64F); matT.convertTo(T, CV_64F);
if( matR->rows == 3 && matR->cols == 3 ) if( matR.rows == 3 && matR.cols == 3 )
Rodrigues(*matR, R); Rodrigues(matR, R);
else else
matR->convertTo(R, CV_64F); matR.convertTo(R, CV_64F);
param[0] = R[0]; param[0] = R[0];
param[1] = R[1]; param[1] = R[1];
@ -893,39 +884,41 @@ static double stereoCalibrateImpl(
// find the medians and save the first 6 parameters // find the medians and save the first 6 parameters
for(int i = 0; i < 6; i++ ) for(int i = 0; i < 6; i++ )
{ {
double* rti = RT0data + i*nimages; size_t idx = i*nimages;
qsort( rti, nimages, sizeof(*rti), dbCmp ); std::nth_element(rtsort.begin() + idx,
param[i] = nimages % 2 != 0 ? rti[nimages/2] : (rti[nimages/2 - 1] + rti[nimages/2])*0.5; rtsort.begin() + idx + nimages/2,
rtsort.begin() + idx + nimages);
double h = rtsort[idx + nimages/2];
param[i] = (nimages % 2 == 0) ? (h + rtsort[idx + nimages/2 - 1]) * 0.5 : h;
} }
} }
if( recomputeIntrinsics ) if( recomputeIntrinsics )
{
for(int k = 0; k < 2; k++ ) for(int k = 0; k < 2; k++ )
{ {
double* iparam = &param[(nimages+1)*6 + k*NINTRINSIC]; size_t idx = (nimages+1)*6 + k*NINTRINSIC;
if( flags & CALIB_ZERO_TANGENT_DIST ) if( flags & CALIB_ZERO_TANGENT_DIST )
dk[k][2] = dk[k][3] = 0; dk[k][2] = dk[k][3] = 0;
iparam[0] = A[k](0, 0); iparam[1] = A[k](1, 1); iparam[2] = A[k](0, 2); iparam[3] = A[k](1, 2); param[idx + 0] = A[k](0, 0); param[idx + 1] = A[k](1, 1); param[idx + 2] = A[k](0, 2); param[idx + 3] = A[k](1, 2);
iparam[4] = dk[k][0]; iparam[5] = dk[k][1]; iparam[6] = dk[k][2]; for (int i = 0; i < 14; i++)
iparam[7] = dk[k][3]; iparam[8] = dk[k][4]; iparam[9] = dk[k][5]; {
iparam[10] = dk[k][6]; iparam[11] = dk[k][7]; param[idx + 4 + i] = dk[k][i];
iparam[12] = dk[k][8]; }
iparam[13] = dk[k][9]; }
iparam[14] = dk[k][10];
iparam[15] = dk[k][11];
iparam[16] = dk[k][12];
iparam[17] = dk[k][13];
} }
A[0] = A[1] = Matx33d(1, 0, 0, 0, 1, 0, 0, 0, 1); // Preallocated place for callback calculations
Mat errBuf( maxPoints*2, 1, CV_64F );
//std::cout << "param before LM: " << Mat(param, false).t() << "\n"; Mat JeBuf( maxPoints*2, 6, CV_64F );
Mat J_LRBuf( maxPoints*2, 6, CV_64F );
Mat JiBuf( maxPoints*2, NINTRINSIC, CV_64F, Scalar(0) );
auto lmcallback = [&](InputOutputArray _param, OutputArray JtErr_, OutputArray JtJ_, double& errnorm) auto lmcallback = [&](InputOutputArray _param, OutputArray JtErr_, OutputArray JtJ_, double& errnorm)
{ {
double* param_p = _param.getMat().ptr<double>(); Mat_<double> param_m = _param.getMat();
Vec3d om_LR(param_p[0], param_p[1], param_p[2]); Vec3d om_LR(param_m(0), param_m(1), param_m(2));
Vec3d T_LR(param_p[3], param_p[4], param_p[5]); Vec3d T_LR(param_m(3), param_m(4), param_m(5));
Vec3d om[2], T[2]; Vec3d om[2], T[2];
Matx33d dr3dr1, dr3dr2, dt3dr2, dt3dt1, dt3dt2; Matx33d dr3dr1, dr3dr2, dt3dr2, dt3dt1, dt3dt2;
@ -933,30 +926,27 @@ static double stereoCalibrateImpl(
if( recomputeIntrinsics ) if( recomputeIntrinsics )
{ {
double* iparam = param_p + (nimages+1)*6; size_t idx = (nimages+1)*6;
//double* ipparam = solver.prevParam->data.db + (nimages+1)*6;
if( flags & CALIB_SAME_FOCAL_LENGTH ) if( flags & CALIB_SAME_FOCAL_LENGTH )
{ {
iparam[NINTRINSIC] = iparam[0]; param_m(idx + NINTRINSIC ) = param_m(idx + 0);
iparam[NINTRINSIC+1] = iparam[1]; param_m(idx + NINTRINSIC+1) = param_m(idx + 1);
//ipparam[NINTRINSIC] = ipparam[0];
//ipparam[NINTRINSIC+1] = ipparam[1];
} }
if( flags & CALIB_FIX_ASPECT_RATIO ) if( flags & CALIB_FIX_ASPECT_RATIO )
{ {
iparam[0] = iparam[1]*aspectRatio[0]; param_m(idx + 0) = aspectRatio[0]*param_m(idx + 1 );
iparam[NINTRINSIC] = iparam[NINTRINSIC+1]*aspectRatio[1]; param_m(idx + NINTRINSIC) = aspectRatio[1]*param_m(idx + 1 + NINTRINSIC);
//ipparam[0] = ipparam[1]*aspectRatio[0];
//ipparam[NINTRINSIC] = ipparam[NINTRINSIC+1]*aspectRatio[1];
} }
for(int k = 0; k < 2; k++ ) for(int k = 0; k < 2; k++ )
{ {
A[k] = Matx33d(iparam[k*NINTRINSIC+0], 0, iparam[k*NINTRINSIC+2], double fx = param_m(idx + k*NINTRINSIC+0), fy = param_m(idx + k*NINTRINSIC+1);
0, iparam[k*NINTRINSIC+1], iparam[k*NINTRINSIC+3], double cx = param_m(idx + k*NINTRINSIC+2), cy = param_m(idx + k*NINTRINSIC+3);
A[k] = Matx33d(fx, 0, cx,
0, fy, cy,
0, 0, 1); 0, 0, 1);
for(int j = 0; j < 14; j++) for(int j = 0; j < 14; j++)
dk[k][j] = iparam[k*NINTRINSIC+4+j]; dk[k][j] = param_m(idx + k*NINTRINSIC+4+j);
} }
} }
@ -965,9 +955,9 @@ static double stereoCalibrateImpl(
{ {
int ni = _npoints.at<int>(i); int ni = _npoints.at<int>(i);
double* pi = param_p + (i+1)*6; size_t idx = (i+1)*6;
om[0] = Vec3d(pi[0], pi[1], pi[2]); om[0] = Vec3d(param_m(idx + 0), param_m(idx + 1), param_m(idx + 2));
T[0] = Vec3d(pi[3], pi[4], pi[5]); T[0] = Vec3d(param_m(idx + 3), param_m(idx + 4), param_m(idx + 5));
if( JtJ_.needed() || JtErr_.needed() ) if( JtJ_.needed() || JtErr_.needed() )
composeRT( om[0], T[0], om_LR, T_LR, om[1], T[1], dr3dr1, noArray(), composeRT( om[0], T[0], om_LR, T_LR, om[1], T[1], dr3dr1, noArray(),
@ -975,8 +965,11 @@ static double stereoCalibrateImpl(
else else
composeRT( om[0], T[0], om_LR, T_LR, om[1], T[1] ); composeRT( om[0], T[0], om_LR, T_LR, om[1], T[1] );
Mat objpt_i(1, ni, CV_64FC3, objectPoints.ptr<double>() + ptPos*3); Mat objpt_i = objectPoints(Range::all(), Range(ptPos, ptPos + ni));
err.resize(ni*2); Je.resize(ni*2); J_LR.resize(ni*2); Ji.resize(ni*2); Mat err = errBuf (Range(0, ni*2), Range::all());
Mat Je = JeBuf (Range(0, ni*2), Range::all());
Mat J_LR = J_LRBuf(Range(0, ni*2), Range::all());
Mat Ji = JiBuf (Range(0, ni*2), Range::all());
Mat tmpImagePoints = err.reshape(2, 1); Mat tmpImagePoints = err.reshape(2, 1);
Mat dpdf = Ji.colRange(0, 2); Mat dpdf = Ji.colRange(0, 2);
@ -987,7 +980,7 @@ static double stereoCalibrateImpl(
for(int k = 0; k < 2; k++ ) for(int k = 0; k < 2; k++ )
{ {
Mat imgpt_ik(1, ni, CV_64FC2, imagePoints[k].ptr<double>() + ptPos*2); Mat imgpt_ik = imagePoints[k](Range::all(), Range(ptPos, ptPos + ni));
if( JtJ_.needed() || JtErr_.needed() ) if( JtJ_.needed() || JtErr_.needed() )
projectPoints(objpt_i, om[k], T[k], A[k], Dist[k], projectPoints(objpt_i, om[k], T[k], A[k], Dist[k],
@ -1010,23 +1003,35 @@ static double stereoCalibrateImpl(
// convert de3/{dr3,dt3} => de3{dr1,dt1} & de3{dr2,dt2} // convert de3/{dr3,dt3} => de3{dr1,dt1} & de3{dr2,dt2}
for(int p = 0; p < ni*2; p++ ) for(int p = 0; p < ni*2; p++ )
{ {
Mat de3dr3( 1, 3, CV_64F, Je.ptr(p)); Matx13d de3dr3, de3dt3, de3dr2, de3dt2, de3dr1, de3dt1;
Mat de3dt3( 1, 3, CV_64F, de3dr3.ptr<double>() + 3 ); for(int j = 0; j < 3; j++)
Mat de3dr2( 1, 3, CV_64F, J_LR.ptr(p) ); de3dr3(j) = Je.at<double>(p, j);
Mat de3dt2( 1, 3, CV_64F, de3dr2.ptr<double>() + 3 );
double _de3dr1[3], _de3dt1[3]; for(int j = 0; j < 3; j++)
Mat de3dr1( 1, 3, CV_64F, _de3dr1 ); de3dt3(j) = Je.at<double>(p, 3+j);
Mat de3dt1( 1, 3, CV_64F, _de3dt1 );
for(int j = 0; j < 3; j++)
de3dr2(j) = J_LR.at<double>(p, j);
for(int j = 0; j < 3; j++)
de3dt2(j) = J_LR.at<double>(p, 3+j);
de3dr1 = de3dr3 * dr3dr1;
de3dt1 = de3dt3 * dt3dt1;
de3dr2 = de3dr3 * dr3dr2 + de3dt3 * dt3dr2;
de3dt2 = de3dt3 * dt3dt2;
gemm(de3dr3, dr3dr1, 1, noArray(), 0, de3dr1); for(int j = 0; j < 3; j++)
gemm(de3dt3, dt3dt1, 1, noArray(), 0, de3dt1); Je.at<double>(p, j) = de3dr1(j);
gemm(de3dr3, dr3dr2, 1, noArray(), 0, de3dr2); for(int j = 0; j < 3; j++)
gemm(de3dt3, dt3dr2, 1, de3dr2, 1, de3dr2); Je.at<double>(p, 3+j) = de3dt1(j);
gemm(de3dt3, dt3dt2, 1, noArray(), 0, de3dt2);
de3dr1.copyTo(de3dr3); for(int j = 0; j < 3; j++)
de3dt1.copyTo(de3dt3); J_LR.at<double>(p, j) = de3dr2(j);
for(int j = 0; j < 3; j++)
J_LR.at<double>(p, 3+j) = de3dt2(j);
} }
JtJ(Rect(0, 0, 6, 6)) += J_LR.t()*J_LR; JtJ(Rect(0, 0, 6, 6)) += J_LR.t()*J_LR;
@ -1050,8 +1055,8 @@ static double stereoCalibrateImpl(
} }
double viewErr = norm(err, NORM_L2SQR); double viewErr = norm(err, NORM_L2SQR);
if(perViewErr) if(!perViewErr.empty())
perViewErr->at<double>(i, k) = std::sqrt(viewErr/ni); perViewErr.at<double>(i, k) = std::sqrt(viewErr/ni);
reprojErr += viewErr; reprojErr += viewErr;
} }
@ -1080,21 +1085,19 @@ static double stereoCalibrateImpl(
Vec3d T_LR(param[3], param[4], param[5]); Vec3d T_LR(param[3], param[4], param[5]);
Matx33d R_LR; Matx33d R_LR;
Rodrigues( om_LR, R_LR ); Rodrigues( om_LR, R_LR );
if( matR->rows == 1 || matR->cols == 1 ) if( matR.rows == 1 || matR.cols == 1 )
om_LR.convertTo(*matR, matR->depth()); om_LR.convertTo(matR, matR.depth());
else else
R_LR.convertTo(*matR, matR->depth()); R_LR.convertTo(matR, matR.depth());
T_LR.convertTo(*matT, matT->depth()); T_LR.convertTo(matT, matT.depth());
for(int k = 0; k < 2; k++ )
{
double* iparam = &param[(nimages+1)*6 + k*NINTRINSIC];
A[k] = Matx33d(iparam[0], 0, iparam[2], 0, iparam[1], iparam[3], 0, 0, 1);
}
if( recomputeIntrinsics ) if( recomputeIntrinsics )
{ {
for(int k = 0; k < 2; k++ ) for(int k = 0; k < 2; k++ )
{ {
size_t idx = (nimages+1)*6 + k*NINTRINSIC;
A[k] = Matx33d(param[idx + 0], 0, param[idx + 2], 0, param[idx + 1], param[idx + 3], 0, 0, 1);
Mat& cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2; Mat& cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2;
Mat& distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2; Mat& distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2;
A[k].convertTo(cameraMatrix, cameraMatrix.depth()); A[k].convertTo(cameraMatrix, cameraMatrix.depth());
@ -1103,19 +1106,19 @@ static double stereoCalibrateImpl(
} }
} }
if( matE || matF ) if( !matE.empty() || !matF.empty() )
{ {
Matx33d Tx(0, -T_LR[2], T_LR[1], Matx33d Tx(0, -T_LR[2], T_LR[1],
T_LR[2], 0, -T_LR[0], T_LR[2], 0, -T_LR[0],
-T_LR[1], T_LR[0], 0); -T_LR[1], T_LR[0], 0);
Matx33d E = Tx*R_LR; Matx33d E = Tx*R_LR;
if (matE) if( !matE.empty() )
E.convertTo(*matE, matE->depth()); E.convertTo(matE, matE.depth());
if( matF ) if( !matF.empty())
{ {
Matx33d iA0 = A[0].inv(), iA1 = A[1].inv(); Matx33d iA0 = A[0].inv(), iA1 = A[1].inv();
Matx33d F = iA1.t() * E * iA0; Matx33d F = iA1.t() * E * iA0;
F.convertTo(*matF, matF->depth(), fabs(F(2,2)) > 0 ? 1./F(2,2) : 1.); F.convertTo(matF, matF.depth(), fabs(F(2,2)) > 0 ? 1./F(2,2) : 1.);
} }
} }
@ -1592,8 +1595,8 @@ double stereoCalibrate( InputArrayOfArrays _objectPoints,
double err = stereoCalibrateImpl(objPt, imgPt, imgPt2, npoints, cameraMatrix1, double err = stereoCalibrateImpl(objPt, imgPt, imgPt2, npoints, cameraMatrix1,
distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize,
&matR, &matT, E_needed ? &matE : NULL, F_needed ? &matF : NULL, matR, matT, matE, matF,
errors_needed ? &matErr : NULL, flags, criteria); matErr, flags, criteria);
cameraMatrix1.copyTo(_cameraMatrix1); cameraMatrix1.copyTo(_cameraMatrix1);
cameraMatrix2.copyTo(_cameraMatrix2); cameraMatrix2.copyTo(_cameraMatrix2);
distCoeffs1.copyTo(_distCoeffs1); distCoeffs1.copyTo(_distCoeffs1);

57
modules/calib/test/test_cameracalibration.cpp
Unescape View File

@ -2007,6 +2007,63 @@ TEST(Calib3d_StereoCalibrate, regression_11131)
EXPECT_GE(roi2.area(), 400*300) << roi2; EXPECT_GE(roi2.area(), 400*300) << roi2;
} }
TEST(Calib_StereoCalibrate, regression_22421)
{
cv::Mat K1, K2, dist1, dist2;
std::vector<Mat> image_points1, image_points2;
Mat desiredR, desiredT;
std::string fname = cvtest::TS::ptr()->get_data_path() + std::string("/cv/cameracalibration/regression22421.yaml.gz");
FileStorage fs(fname, FileStorage::Mode::READ);
ASSERT_TRUE(fs.isOpened());
fs["imagePoints1"] >> image_points1;
fs["imagePoints2"] >> image_points2;
fs["K1"] >> K1;
fs["K2"] >> K2;
fs["dist1"] >> dist1;
fs["dist2"] >> dist2;
fs["desiredR"] >> desiredR;
fs["desiredT"] >> desiredT;
std::vector<float> obj_points = {0, 0, 0,
0.5, 0, 0,
1, 0, 0,
1.5000001, 0, 0,
2, 0, 0,
0, 0.5, 0,
0.5, 0.5, 0,
1, 0.5, 0,
1.5000001, 0.5, 0,
2, 0.5, 0,
0, 1, 0,
0.5, 1, 0,
1, 1, 0,
1.5000001, 1, 0,
2, 1, 0,
0, 1.5000001, 0,
0.5, 1.5000001, 0,
1, 1.5000001, 0,
1.5000001, 1.5000001, 0,
2, 1.5000001, 0};
cv::Mat obj_points_mat(obj_points, true);
obj_points_mat = obj_points_mat.reshape(1, obj_points.size() / 3);
std::vector<cv::Mat> grid_points(image_points1.size(), obj_points_mat);
cv::Mat R, T;
double error = cv::stereoCalibrate(grid_points, image_points1, image_points2,
K1, dist1, K2, dist2, cv::Size(), R, T,
cv::noArray(), cv::noArray(), cv::noArray(), cv::CALIB_FIX_INTRINSIC);
EXPECT_LE(error, 0.071544);
double rerr = cv::norm(R, desiredR, NORM_INF);
double terr = cv::norm(T, desiredT, NORM_INF);
EXPECT_LE(rerr, 0.0000001);
EXPECT_LE(terr, 0.0000001);
}
TEST(Calib3d_Triangulate, accuracy) TEST(Calib3d_Triangulate, accuracy)
{ {
// the testcase from http://code.opencv.org/issues/4334 // the testcase from http://code.opencv.org/issues/4334

Write Preview
Loading…
Cancel
Save