mirror of
https://github.com/opencv/opencv.git
synced 2025-06-07 17:44:04 +08:00
Merge pull request #10667 from paroj:stereo_calib_ex
calib3d: add stereoCalibrateExtended (#10667) * cvCalibrateCamera2Internal: simplify per view error computation * calib3d: add stereoCalibrateExtended - allow CALIB_USE_EXTRINSIC_GUESS - returns per view errors * calib3d: add stereoCalibrateExtended test
This commit is contained in:
parent
7b8ab4e5c6
commit
203dc3bb48
@ -276,6 +276,7 @@ enum { CALIB_USE_INTRINSIC_GUESS = 0x00001,
|
||||
// for stereo rectification
|
||||
CALIB_ZERO_DISPARITY = 0x00400,
|
||||
CALIB_USE_LU = (1 << 17), //!< use LU instead of SVD decomposition for solving. much faster but potentially less precise
|
||||
CALIB_USE_EXTRINSIC_GUESS = (1 << 22), //!< for stereoCalibrate
|
||||
};
|
||||
|
||||
//! the algorithm for finding fundamental matrix
|
||||
@ -1125,11 +1126,14 @@ is similar to distCoeffs1 .
|
||||
@param T Output translation vector between the coordinate systems of the cameras.
|
||||
@param E Output essential matrix.
|
||||
@param F Output fundamental matrix.
|
||||
@param perViewErrors Output vector of the RMS re-projection error estimated for each pattern view.
|
||||
@param flags Different flags that may be zero or a combination of the following values:
|
||||
- **CALIB_FIX_INTRINSIC** Fix cameraMatrix? and distCoeffs? so that only R, T, E , and F
|
||||
matrices are estimated.
|
||||
- **CALIB_USE_INTRINSIC_GUESS** Optimize some or all of the intrinsic parameters
|
||||
according to the specified flags. Initial values are provided by the user.
|
||||
- **CALIB_USE_EXTRINSIC_GUESS** R, T contain valid initial values that are optimized further.
|
||||
Otherwise R, T are initialized to the median value of the pattern views (each dimension separately).
|
||||
- **CALIB_FIX_PRINCIPAL_POINT** Fix the principal points during the optimization.
|
||||
- **CALIB_FIX_FOCAL_LENGTH** Fix \f$f^{(j)}_x\f$ and \f$f^{(j)}_y\f$ .
|
||||
- **CALIB_FIX_ASPECT_RATIO** Optimize \f$f^{(j)}_y\f$ . Fix the ratio \f$f^{(j)}_x/f^{(j)}_y\f$
|
||||
@ -1194,6 +1198,15 @@ Similarly to calibrateCamera , the function minimizes the total re-projection er
|
||||
points in all the available views from both cameras. The function returns the final value of the
|
||||
re-projection error.
|
||||
*/
|
||||
CV_EXPORTS_AS(stereoCalibrateExtended) double stereoCalibrate( InputArrayOfArrays objectPoints,
|
||||
InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
|
||||
InputOutputArray cameraMatrix1, InputOutputArray distCoeffs1,
|
||||
InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2,
|
||||
Size imageSize, InputOutputArray R,InputOutputArray T, OutputArray E, OutputArray F,
|
||||
OutputArray perViewErrors, int flags = CALIB_FIX_INTRINSIC,
|
||||
TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) );
|
||||
|
||||
/// @overload
|
||||
CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints,
|
||||
InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
|
||||
InputOutputArray cameraMatrix1, InputOutputArray distCoeffs1,
|
||||
@ -1202,7 +1215,6 @@ CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints,
|
||||
int flags = CALIB_FIX_INTRINSIC,
|
||||
TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) );
|
||||
|
||||
|
||||
/** @brief Computes rectification transforms for each head of a calibrated stereo camera.
|
||||
|
||||
@param cameraMatrix1 First camera matrix.
|
||||
|
@ -1629,7 +1629,12 @@ static double cvCalibrateCamera2Internal( const CvMat* objectPoints,
|
||||
JtErr.rowRange(NINTRINSIC + i * 6, NINTRINSIC + (i + 1) * 6) = _Je.t() * _err;
|
||||
}
|
||||
|
||||
reprojErr += norm(_err, NORM_L2SQR);
|
||||
double viewErr = norm(_err, NORM_L2SQR);
|
||||
|
||||
if( perViewErrors )
|
||||
perViewErrors->data.db[i] = std::sqrt(viewErr / ni);
|
||||
|
||||
reprojErr += viewErr;
|
||||
}
|
||||
if( _errNorm )
|
||||
*_errNorm = reprojErr;
|
||||
@ -1671,13 +1676,6 @@ static double cvCalibrateCamera2Internal( const CvMat* objectPoints,
|
||||
for( i = 0, pos = 0; i < nimages; i++ )
|
||||
{
|
||||
CvMat src, dst;
|
||||
if( perViewErrors )
|
||||
{
|
||||
ni = npoints->data.i[i*npstep];
|
||||
perViewErrors->data.db[i] = std::sqrt(cv::norm(allErrors.colRange(pos, pos + ni),
|
||||
NORM_L2SQR) / ni);
|
||||
pos+=ni;
|
||||
}
|
||||
|
||||
if( rvecs )
|
||||
{
|
||||
@ -1757,14 +1755,13 @@ static int dbCmp( const void* _a, const void* _b )
|
||||
return (a > b) - (a < b);
|
||||
}
|
||||
|
||||
|
||||
double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1,
|
||||
static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _imagePoints1,
|
||||
const CvMat* _imagePoints2, const CvMat* _npoints,
|
||||
CvMat* _cameraMatrix1, CvMat* _distCoeffs1,
|
||||
CvMat* _cameraMatrix2, CvMat* _distCoeffs2,
|
||||
CvSize imageSize, CvMat* matR, CvMat* matT,
|
||||
CvMat* matE, CvMat* matF,
|
||||
int flags,
|
||||
CvMat* perViewErr, int flags,
|
||||
CvTermCriteria termCrit )
|
||||
{
|
||||
const int NINTRINSIC = 18;
|
||||
@ -1866,9 +1863,6 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
|
||||
// for intrinisic parameters of each camera ((fx,fy,cx,cy,k1,k2,p1,p2) ~ 8 parameters).
|
||||
nparams = 6*(nimages+1) + (recomputeIntrinsics ? NINTRINSIC*2 : 0);
|
||||
|
||||
// storage for initial [om(R){i}|t{i}] (in order to compute the median for each component)
|
||||
RT0.reset(cvCreateMat( 6, nimages, CV_64F ));
|
||||
|
||||
CvLevMarq solver( nparams, 0, termCrit );
|
||||
|
||||
if(flags & CALIB_USE_LU) {
|
||||
@ -1918,6 +1912,8 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
|
||||
}
|
||||
}
|
||||
|
||||
// storage for initial [om(R){i}|t{i}] (in order to compute the median for each component)
|
||||
RT0.reset(cvCreateMat( 6, nimages, CV_64F ));
|
||||
/*
|
||||
Compute initial estimate of pose
|
||||
For each image, compute:
|
||||
@ -1968,12 +1964,32 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
|
||||
RT0->data.db[i + nimages*5] = t[1][2];
|
||||
}
|
||||
|
||||
// find the medians and save the first 6 parameters
|
||||
for( i = 0; i < 6; i++ )
|
||||
if(flags & CALIB_USE_EXTRINSIC_GUESS)
|
||||
{
|
||||
qsort( RT0->data.db + i*nimages, nimages, CV_ELEM_SIZE(RT0->type), dbCmp );
|
||||
solver.param->data.db[i] = nimages % 2 != 0 ? RT0->data.db[i*nimages + nimages/2] :
|
||||
(RT0->data.db[i*nimages + nimages/2 - 1] + RT0->data.db[i*nimages + nimages/2])*0.5;
|
||||
Vec3d R, T;
|
||||
cvarrToMat(matT).convertTo(T, CV_64F);
|
||||
|
||||
if( matR->rows == 3 && matR->cols == 3 )
|
||||
Rodrigues(cvarrToMat(matR), R);
|
||||
else
|
||||
cvarrToMat(matR).convertTo(R, CV_64F);
|
||||
|
||||
solver.param->data.db[0] = R[0];
|
||||
solver.param->data.db[1] = R[1];
|
||||
solver.param->data.db[2] = R[2];
|
||||
solver.param->data.db[3] = T[0];
|
||||
solver.param->data.db[4] = T[1];
|
||||
solver.param->data.db[5] = T[2];
|
||||
}
|
||||
else
|
||||
{
|
||||
// find the medians and save the first 6 parameters
|
||||
for( i = 0; i < 6; i++ )
|
||||
{
|
||||
qsort( RT0->data.db + i*nimages, nimages, CV_ELEM_SIZE(RT0->type), dbCmp );
|
||||
solver.param->data.db[i] = nimages % 2 != 0 ? RT0->data.db[i*nimages + nimages/2] :
|
||||
(RT0->data.db[i*nimages + nimages/2 - 1] + RT0->data.db[i*nimages + nimages/2])*0.5;
|
||||
}
|
||||
}
|
||||
|
||||
if( recomputeIntrinsics )
|
||||
@ -2151,7 +2167,12 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
|
||||
}
|
||||
}
|
||||
|
||||
reprojErr += norm(err, NORM_L2SQR);
|
||||
double viewErr = norm(err, NORM_L2SQR);
|
||||
|
||||
if(perViewErr)
|
||||
perViewErr->data.db[i*2 + k] = std::sqrt(viewErr/ni);
|
||||
|
||||
reprojErr += viewErr;
|
||||
}
|
||||
}
|
||||
if(_errNorm)
|
||||
@ -2209,7 +2230,19 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
|
||||
|
||||
return std::sqrt(reprojErr/(pointsTotal*2));
|
||||
}
|
||||
|
||||
double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1,
|
||||
const CvMat* _imagePoints2, const CvMat* _npoints,
|
||||
CvMat* _cameraMatrix1, CvMat* _distCoeffs1,
|
||||
CvMat* _cameraMatrix2, CvMat* _distCoeffs2,
|
||||
CvSize imageSize, CvMat* matR, CvMat* matT,
|
||||
CvMat* matE, CvMat* matF,
|
||||
int flags,
|
||||
CvTermCriteria termCrit )
|
||||
{
|
||||
return cvStereoCalibrateImpl(_objectPoints, _imagePoints1, _imagePoints2, _npoints, _cameraMatrix1,
|
||||
_distCoeffs1, _cameraMatrix2, _distCoeffs2, imageSize, matR, matT, matE,
|
||||
matF, NULL, flags, termCrit);
|
||||
}
|
||||
|
||||
static void
|
||||
icvGetRectangles( const CvMat* cameraMatrix, const CvMat* distCoeffs,
|
||||
@ -3474,7 +3507,26 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints,
|
||||
InputOutputArray _cameraMatrix1, InputOutputArray _distCoeffs1,
|
||||
InputOutputArray _cameraMatrix2, InputOutputArray _distCoeffs2,
|
||||
Size imageSize, OutputArray _Rmat, OutputArray _Tmat,
|
||||
OutputArray _Emat, OutputArray _Fmat, int flags ,
|
||||
OutputArray _Emat, OutputArray _Fmat, int flags,
|
||||
TermCriteria criteria)
|
||||
{
|
||||
Mat Rmat, Tmat;
|
||||
double ret = stereoCalibrate(_objectPoints, _imagePoints1, _imagePoints2, _cameraMatrix1, _distCoeffs1,
|
||||
_cameraMatrix2, _distCoeffs2, imageSize, Rmat, Tmat, _Emat, _Fmat,
|
||||
noArray(), flags, criteria);
|
||||
Rmat.copyTo(_Rmat);
|
||||
Tmat.copyTo(_Tmat);
|
||||
return ret;
|
||||
}
|
||||
|
||||
double cv::stereoCalibrate( InputArrayOfArrays _objectPoints,
|
||||
InputArrayOfArrays _imagePoints1,
|
||||
InputArrayOfArrays _imagePoints2,
|
||||
InputOutputArray _cameraMatrix1, InputOutputArray _distCoeffs1,
|
||||
InputOutputArray _cameraMatrix2, InputOutputArray _distCoeffs2,
|
||||
Size imageSize, InputOutputArray _Rmat, InputOutputArray _Tmat,
|
||||
OutputArray _Emat, OutputArray _Fmat,
|
||||
OutputArray _perViewErrors, int flags ,
|
||||
TermCriteria criteria)
|
||||
{
|
||||
int rtype = CV_64F;
|
||||
@ -3495,8 +3547,11 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints,
|
||||
distCoeffs2 = distCoeffs2.rows == 1 ? distCoeffs2.colRange(0, 5) : distCoeffs2.rowRange(0, 5);
|
||||
}
|
||||
|
||||
_Rmat.create(3, 3, rtype);
|
||||
_Tmat.create(3, 1, rtype);
|
||||
if((flags & CALIB_USE_EXTRINSIC_GUESS) == 0)
|
||||
{
|
||||
_Rmat.create(3, 3, rtype);
|
||||
_Tmat.create(3, 1, rtype);
|
||||
}
|
||||
|
||||
Mat objPt, imgPt, imgPt2, npoints;
|
||||
|
||||
@ -3505,22 +3560,32 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints,
|
||||
CvMat c_objPt = objPt, c_imgPt = imgPt, c_imgPt2 = imgPt2, c_npoints = npoints;
|
||||
CvMat c_cameraMatrix1 = cameraMatrix1, c_distCoeffs1 = distCoeffs1;
|
||||
CvMat c_cameraMatrix2 = cameraMatrix2, c_distCoeffs2 = distCoeffs2;
|
||||
CvMat c_matR = _Rmat.getMat(), c_matT = _Tmat.getMat(), c_matE, c_matF, *p_matE = 0, *p_matF = 0;
|
||||
CvMat c_matR = _Rmat.getMat(), c_matT = _Tmat.getMat(), c_matE, c_matF, c_matErr;
|
||||
|
||||
if( _Emat.needed() )
|
||||
bool E_needed = _Emat.needed(), F_needed = _Fmat.needed(), errors_needed = _perViewErrors.needed();
|
||||
|
||||
if( E_needed )
|
||||
{
|
||||
_Emat.create(3, 3, rtype);
|
||||
p_matE = &(c_matE = _Emat.getMat());
|
||||
c_matE = _Emat.getMat();
|
||||
}
|
||||
if( _Fmat.needed() )
|
||||
if( F_needed )
|
||||
{
|
||||
_Fmat.create(3, 3, rtype);
|
||||
p_matF = &(c_matF = _Fmat.getMat());
|
||||
c_matF = _Fmat.getMat();
|
||||
}
|
||||
|
||||
double err = cvStereoCalibrate(&c_objPt, &c_imgPt, &c_imgPt2, &c_npoints, &c_cameraMatrix1,
|
||||
&c_distCoeffs1, &c_cameraMatrix2, &c_distCoeffs2, imageSize,
|
||||
&c_matR, &c_matT, p_matE, p_matF, flags, criteria );
|
||||
if( errors_needed )
|
||||
{
|
||||
int nimages = int(_objectPoints.total());
|
||||
_perViewErrors.create(nimages, 2, CV_64F);
|
||||
c_matErr = _perViewErrors.getMat();
|
||||
}
|
||||
|
||||
double err = cvStereoCalibrateImpl(&c_objPt, &c_imgPt, &c_imgPt2, &c_npoints, &c_cameraMatrix1,
|
||||
&c_distCoeffs1, &c_cameraMatrix2, &c_distCoeffs2, imageSize, &c_matR,
|
||||
&c_matT, E_needed ? &c_matE : NULL, F_needed ? &c_matF : NULL,
|
||||
errors_needed ? &c_matErr : NULL, flags, criteria);
|
||||
|
||||
cameraMatrix1.copyTo(_cameraMatrix1);
|
||||
cameraMatrix2.copyTo(_cameraMatrix2);
|
||||
|
@ -2076,6 +2076,48 @@ TEST(Calib3d_StereoCalibrate_C, regression) { CV_StereoCalibrationTest_C test; t
|
||||
TEST(Calib3d_StereoCalibrate_CPP, regression) { CV_StereoCalibrationTest_CPP test; test.safe_run(); }
|
||||
TEST(Calib3d_StereoCalibrateCorner, regression) { CV_StereoCalibrationCornerTest test; test.safe_run(); }
|
||||
|
||||
TEST(Calib3d_StereoCalibrate_CPP, extended)
|
||||
{
|
||||
cvtest::TS* ts = cvtest::TS::ptr();
|
||||
String filepath = cv::format("%scv/stereo/case%d/", ts->get_data_path().c_str(), 1 );
|
||||
|
||||
Mat left = imread(filepath+"left01.png");
|
||||
Mat right = imread(filepath+"right01.png");
|
||||
if(left.empty() || right.empty())
|
||||
{
|
||||
ts->set_failed_test_info( cvtest::TS::FAIL_MISSING_TEST_DATA );
|
||||
return;
|
||||
}
|
||||
|
||||
vector<vector<Point2f> > imgpt1(1), imgpt2(1);
|
||||
vector<vector<Point3f> > objpt(1);
|
||||
Size patternSize(9, 6), imageSize(640, 480);
|
||||
bool found1 = findChessboardCorners(left, patternSize, imgpt1[0]);
|
||||
bool found2 = findChessboardCorners(right, patternSize, imgpt2[0]);
|
||||
|
||||
if(!found1 || !found2)
|
||||
{
|
||||
ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
|
||||
return;
|
||||
}
|
||||
|
||||
for( int j = 0; j < patternSize.width*patternSize.height; j++ )
|
||||
objpt[0].push_back(Point3f((float)(j%patternSize.width), (float)(j/patternSize.width), 0.f));
|
||||
|
||||
Mat K1, K2, c1, c2, R, T, E, F, err;
|
||||
int flags = 0;
|
||||
double res0 = stereoCalibrate( objpt, imgpt1, imgpt2,
|
||||
K1, c1, K2, c2,
|
||||
imageSize, R, T, E, F, err, flags);
|
||||
|
||||
flags = CALIB_USE_EXTRINSIC_GUESS;
|
||||
double res1 = stereoCalibrate( objpt, imgpt1, imgpt2,
|
||||
K1, c1, K2, c2,
|
||||
imageSize, R, T, E, F, err, flags);
|
||||
EXPECT_LE(res1, res0);
|
||||
EXPECT_TRUE(err.total() == 2);
|
||||
}
|
||||
|
||||
TEST(Calib3d_Triangulate, accuracy)
|
||||
{
|
||||
// the testcase from http://code.opencv.org/issues/4334
|
||||
|
Loading…
Reference in New Issue
Block a user