Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues 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 2 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
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 261
      modules/calib/src/calibration.cpp
  2. 57
      modules/calib/test/test_cameracalibration.cpp

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

@ -661,22 +661,14 @@ static double calibrateCameraInternal( const Mat& objectPoints,
//////////////////////////////// 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(
const Mat& _objectPoints, const Mat& _imagePoints1,
const Mat& _imagePoints2, const Mat& _npoints,
Mat& _cameraMatrix1, Mat& _distCoeffs1,
Mat& _cameraMatrix2, Mat& _distCoeffs2,
Size imageSize, Mat* matR, Mat* matT,
Mat* matE, Mat* matF,
Mat* perViewErr, int flags,
Size imageSize, Mat matR, Mat matT,
Mat matE, Mat matF,
Mat perViewErr, int flags,
TermCriteria termCrit )
{
const int NINTRINSIC = 18;
@ -725,11 +717,12 @@ static double stereoCalibrateImpl(
imagePoints[k] = imagePoints[k].reshape(2, 1);
if( flags & (CALIB_FIX_INTRINSIC|CALIB_USE_INTRINSIC_GUESS|
CALIB_FIX_ASPECT_RATIO|CALIB_FIX_FOCAL_LENGTH) )
CALIB_FIX_ASPECT_RATIO|CALIB_FIX_FOCAL_LENGTH) )
cameraMatrix.convertTo(A[k], CV_64F);
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] );
distCoeffs.convertTo(tdist, CV_64F);
@ -741,8 +734,6 @@ static double stereoCalibrateImpl(
calibrateCameraInternal(objectPoints, imagePoints[k],
_npoints, imageSize, 0, matA, Dist[k],
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;
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,
// 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);
std::vector<uchar> mask(nparams, (uchar)1);
@ -777,7 +767,7 @@ static double stereoCalibrateImpl(
if( recomputeIntrinsics )
{
uchar* imask = &mask[0] + nparams - NINTRINSIC*2;
size_t idx = nparams - NINTRINSIC*2;
if( !(flags & CALIB_RATIONAL_MODEL) )
flags |= CALIB_FIX_K4 | CALIB_FIX_K5 | CALIB_FIX_K6;
if( !(flags & CALIB_THIN_PRISM_MODEL) )
@ -785,42 +775,43 @@ static double stereoCalibrateImpl(
if( !(flags & CALIB_TILTED_MODEL) )
flags |= CALIB_FIX_TAUX_TAUY;
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 )
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 )
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) )
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 )
imask[4] = imask[NINTRINSIC+4] = 0;
mask[idx + 4] = mask[idx + NINTRINSIC+4] = 0;
if( flags & CALIB_FIX_K2 )
imask[5] = imask[NINTRINSIC+5] = 0;
mask[idx + 5] = mask[idx + NINTRINSIC+5] = 0;
if( flags & CALIB_FIX_K3 )
imask[8] = imask[NINTRINSIC+8] = 0;
mask[idx + 8] = mask[idx + NINTRINSIC+8] = 0;
if( flags & CALIB_FIX_K4 )
imask[9] = imask[NINTRINSIC+9] = 0;
mask[idx + 9] = mask[idx + NINTRINSIC+9] = 0;
if( flags & CALIB_FIX_K5 )
imask[10] = imask[NINTRINSIC+10] = 0;
mask[idx + 10] = mask[idx + NINTRINSIC+10] = 0;
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 )
{
imask[12] = imask[NINTRINSIC+12] = 0;
imask[13] = imask[NINTRINSIC+13] = 0;
imask[14] = imask[NINTRINSIC+14] = 0;
imask[15] = imask[NINTRINSIC+15] = 0;
mask[idx + 12] = mask[idx + NINTRINSIC+12] = 0;
mask[idx + 13] = mask[idx + NINTRINSIC+13] = 0;
mask[idx + 14] = mask[idx + NINTRINSIC+14] = 0;
mask[idx + 15] = mask[idx + NINTRINSIC+15] = 0;
}
if( flags & CALIB_FIX_TAUX_TAUY )
{
imask[16] = imask[NINTRINSIC+16] = 0;
imask[17] = imask[NINTRINSIC+17] = 0;
mask[idx + 16] = mask[idx + NINTRINSIC+16] = 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)
Mat RT0(6, nimages, CV_64F);
double* RT0data = RT0.ptr<double>();
std::vector<double> rtsort(nimages*6);
/*
Compute initial estimate of pose
For each image, compute:
@ -836,19 +827,19 @@ static double stereoCalibrateImpl(
for(int i = 0; i < nimages; 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];
Vec3d rv, T[2];
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 );
Rodrigues(rv, R[k]);
if( k == 0 )
{
// 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 + 2] = rv[2];
param[(i+1)*6 + 3] = T[0][0];
@ -861,12 +852,12 @@ static double stereoCalibrateImpl(
Rodrigues(R[0], rv);
RT0data[i] = rv[0];
RT0data[i + nimages] = rv[1];
RT0data[i + nimages*2] = rv[2];
RT0data[i + nimages*3] = T[1][0];
RT0data[i + nimages*4] = T[1][1];
RT0data[i + nimages*5] = T[1][2];
rtsort[i + nimages*0] = rv[0];
rtsort[i + nimages*1] = rv[1];
rtsort[i + nimages*2] = rv[2];
rtsort[i + nimages*3] = T[1][0];
rtsort[i + nimages*4] = T[1][1];
rtsort[i + nimages*5] = T[1][2];
pos += ni;
}
@ -874,12 +865,12 @@ static double stereoCalibrateImpl(
if(flags & CALIB_USE_EXTRINSIC_GUESS)
{
Vec3d R, T;
matT->convertTo(T, CV_64F);
matT.convertTo(T, CV_64F);
if( matR->rows == 3 && matR->cols == 3 )
Rodrigues(*matR, R);
if( matR.rows == 3 && matR.cols == 3 )
Rodrigues(matR, R);
else
matR->convertTo(R, CV_64F);
matR.convertTo(R, CV_64F);
param[0] = R[0];
param[1] = R[1];
@ -893,39 +884,41 @@ static double stereoCalibrateImpl(
// find the medians and save the first 6 parameters
for(int i = 0; i < 6; i++ )
{
double* rti = RT0data + i*nimages;
qsort( rti, nimages, sizeof(*rti), dbCmp );
param[i] = nimages % 2 != 0 ? rti[nimages/2] : (rti[nimages/2 - 1] + rti[nimages/2])*0.5;
size_t idx = i*nimages;
std::nth_element(rtsort.begin() + idx,
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 )
{
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 )
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);
iparam[4] = dk[k][0]; iparam[5] = dk[k][1]; iparam[6] = dk[k][2];
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];
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];
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);
for (int i = 0; i < 14; i++)
{
param[idx + 4 + i] = dk[k][i];
}
}
}
A[0] = A[1] = Matx33d(1, 0, 0, 0, 1, 0, 0, 0, 1);
//std::cout << "param before LM: " << Mat(param, false).t() << "\n";
// Preallocated place for callback calculations
Mat errBuf( maxPoints*2, 1, CV_64F );
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)
{
double* param_p = _param.getMat().ptr<double>();
Vec3d om_LR(param_p[0], param_p[1], param_p[2]);
Vec3d T_LR(param_p[3], param_p[4], param_p[5]);
Mat_<double> param_m = _param.getMat();
Vec3d om_LR(param_m(0), param_m(1), param_m(2));
Vec3d T_LR(param_m(3), param_m(4), param_m(5));
Vec3d om[2], T[2];
Matx33d dr3dr1, dr3dr2, dt3dr2, dt3dt1, dt3dt2;
@ -933,30 +926,27 @@ static double stereoCalibrateImpl(
if( recomputeIntrinsics )
{
double* iparam = param_p + (nimages+1)*6;
//double* ipparam = solver.prevParam->data.db + (nimages+1)*6;
size_t idx = (nimages+1)*6;
if( flags & CALIB_SAME_FOCAL_LENGTH )
{
iparam[NINTRINSIC] = iparam[0];
iparam[NINTRINSIC+1] = iparam[1];
//ipparam[NINTRINSIC] = ipparam[0];
//ipparam[NINTRINSIC+1] = ipparam[1];
param_m(idx + NINTRINSIC ) = param_m(idx + 0);
param_m(idx + NINTRINSIC+1) = param_m(idx + 1);
}
if( flags & CALIB_FIX_ASPECT_RATIO )
{
iparam[0] = iparam[1]*aspectRatio[0];
iparam[NINTRINSIC] = iparam[NINTRINSIC+1]*aspectRatio[1];
//ipparam[0] = ipparam[1]*aspectRatio[0];
//ipparam[NINTRINSIC] = ipparam[NINTRINSIC+1]*aspectRatio[1];
param_m(idx + 0) = aspectRatio[0]*param_m(idx + 1 );
param_m(idx + NINTRINSIC) = aspectRatio[1]*param_m(idx + 1 + NINTRINSIC);
}
for(int k = 0; k < 2; k++ )
{
A[k] = Matx33d(iparam[k*NINTRINSIC+0], 0, iparam[k*NINTRINSIC+2],
0, iparam[k*NINTRINSIC+1], iparam[k*NINTRINSIC+3],
0, 0, 1);
double fx = param_m(idx + k*NINTRINSIC+0), fy = param_m(idx + k*NINTRINSIC+1);
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);
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);
double* pi = param_p + (i+1)*6;
om[0] = Vec3d(pi[0], pi[1], pi[2]);
T[0] = Vec3d(pi[3], pi[4], pi[5]);
size_t idx = (i+1)*6;
om[0] = Vec3d(param_m(idx + 0), param_m(idx + 1), param_m(idx + 2));
T[0] = Vec3d(param_m(idx + 3), param_m(idx + 4), param_m(idx + 5));
if( JtJ_.needed() || JtErr_.needed() )
composeRT( om[0], T[0], om_LR, T_LR, om[1], T[1], dr3dr1, noArray(),
@ -975,8 +965,11 @@ static double stereoCalibrateImpl(
else
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);
err.resize(ni*2); Je.resize(ni*2); J_LR.resize(ni*2); Ji.resize(ni*2);
Mat objpt_i = objectPoints(Range::all(), Range(ptPos, ptPos + ni));
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 dpdf = Ji.colRange(0, 2);
@ -987,7 +980,7 @@ static double stereoCalibrateImpl(
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() )
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}
for(int p = 0; p < ni*2; p++ )
{
Mat de3dr3( 1, 3, CV_64F, Je.ptr(p));
Mat de3dt3( 1, 3, CV_64F, de3dr3.ptr<double>() + 3 );
Mat de3dr2( 1, 3, CV_64F, J_LR.ptr(p) );
Mat de3dt2( 1, 3, CV_64F, de3dr2.ptr<double>() + 3 );
double _de3dr1[3], _de3dt1[3];
Mat de3dr1( 1, 3, CV_64F, _de3dr1 );
Mat de3dt1( 1, 3, CV_64F, _de3dt1 );
gemm(de3dr3, dr3dr1, 1, noArray(), 0, de3dr1);
gemm(de3dt3, dt3dt1, 1, noArray(), 0, de3dt1);
gemm(de3dr3, dr3dr2, 1, noArray(), 0, de3dr2);
gemm(de3dt3, dt3dr2, 1, de3dr2, 1, de3dr2);
gemm(de3dt3, dt3dt2, 1, noArray(), 0, de3dt2);
de3dr1.copyTo(de3dr3);
de3dt1.copyTo(de3dt3);
Matx13d de3dr3, de3dt3, de3dr2, de3dt2, de3dr1, de3dt1;
for(int j = 0; j < 3; j++)
de3dr3(j) = Je.at<double>(p, j);
for(int j = 0; j < 3; j++)
de3dt3(j) = Je.at<double>(p, 3+j);
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;
for(int j = 0; j < 3; j++)
Je.at<double>(p, j) = de3dr1(j);
for(int j = 0; j < 3; j++)
Je.at<double>(p, 3+j) = de3dt1(j);
for(int j = 0; j < 3; j++)
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;
@ -1050,8 +1055,8 @@ static double stereoCalibrateImpl(
}
double viewErr = norm(err, NORM_L2SQR);
if(perViewErr)
perViewErr->at<double>(i, k) = std::sqrt(viewErr/ni);
if(!perViewErr.empty())
perViewErr.at<double>(i, k) = std::sqrt(viewErr/ni);
reprojErr += viewErr;
}
@ -1080,21 +1085,19 @@ static double stereoCalibrateImpl(
Vec3d T_LR(param[3], param[4], param[5]);
Matx33d R_LR;
Rodrigues( om_LR, R_LR );
if( matR->rows == 1 || matR->cols == 1 )
om_LR.convertTo(*matR, matR->depth());
if( matR.rows == 1 || matR.cols == 1 )
om_LR.convertTo(matR, matR.depth());
else
R_LR.convertTo(*matR, matR->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);
}
R_LR.convertTo(matR, matR.depth());
T_LR.convertTo(matT, matT.depth());
if( recomputeIntrinsics )
{
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& distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2;
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],
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;
if (matE)
E.convertTo(*matE, matE->depth());
if( matF )
if( !matE.empty() )
E.convertTo(matE, matE.depth());
if( !matF.empty())
{
Matx33d iA0 = A[0].inv(), iA1 = A[1].inv();
Matx33d F = iA1.t()*E*iA0;
F.convertTo(*matF, matF->depth(), fabs(F(2,2)) > 0 ? 1./F(2,2) : 1.);
Matx33d F = iA1.t() * E * iA0;
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,
distCoeffs1, cameraMatrix2, distCoeffs2, imageSize,
&matR, &matT, E_needed ? &matE : NULL, F_needed ? &matF : NULL,
errors_needed ? &matErr : NULL, flags, criteria);
matR, matT, matE, matF,
matErr, flags, criteria);
cameraMatrix1.copyTo(_cameraMatrix1);
cameraMatrix2.copyTo(_cameraMatrix2);
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;
}
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)
{
// the testcase from http://code.opencv.org/issues/4334

Write Preview
Loading…
Cancel
Save