Skip to content

O
opencv
Commit 203dc3bb authored by Pavel Rojtberg's avatar Pavel Rojtberg Committed by Alexander Alekhin
Browse files
Options

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
parent 7b8ab4e5
Hide whitespace changes
Inline Side-by-side
Showing with 152 additions and 33 deletions
modules/calib3d/include/opencv2/calib3d.hpp
View file @ 203dc3bb
......@@ -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.
......
modules/calib3d/src/calibration.cpp
View file @ 203dc3bb
......@@ -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;
bool E_needed = _Emat.needed(), F_needed = _Fmat.needed(), errors_needed = _perViewErrors.needed();
if( _Emat.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();
}
if( errors_needed )
{
int nimages = int(_objectPoints.total());
_perViewErrors.create(nimages, 2, CV_64F);
c_matErr = _perViewErrors.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 );
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);
......
modules/calib3d/test/test_cameracalibration.cpp
View file @ 203dc3bb
......@@ -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
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment