Merge pull request #23150 from savuor:port5_stereo_calib_per_obj

### Changes

* Port of #22519 to 5.x
* Distortion coefficients were not copied properly, fixed
* Minor coding style chages

### Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [x] The PR is proposed to the proper branch
- [x] There is a reference to the original bug report and related work
- [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
      Patch to opencv_extra has the same branch name.
- [x] The feature is well documented and sample code can be built with the project CMake
This commit is contained in:
Rostislav Vasilikhin 2023-02-02 14:44:28 +01:00 committed by GitHub
parent 29dc07b1f3
commit 23dec329b4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 271 additions and 118 deletions

View File

@ -994,6 +994,15 @@ second camera coordinate system.
@param T Output translation vector, see description above.
@param E Output essential matrix.
@param F Output fundamental matrix.
@param rvecs Output vector of rotation vectors ( @ref Rodrigues ) estimated for each pattern view in the
coordinate system of the first camera of the stereo pair (e.g. std::vector<cv::Mat>). More in detail, each
i-th rotation vector together with the corresponding i-th translation vector (see the next output parameter
description) brings the calibration pattern from the object coordinate space (in which object points are
specified) to the camera coordinate space of the first camera of the stereo pair. In more technical terms,
the tuple of the i-th rotation and translation vector performs a change of basis from object coordinate space
to camera coordinate space of the first camera of the stereo pair.
@param tvecs Output vector of translation vectors estimated for each pattern view, see parameter description
of previous output parameter ( rvecs ).
@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:
- @ref CALIB_FIX_INTRINSIC Fix cameraMatrix? and distCoeffs? so that only R, T, E, and F
@ -1091,6 +1100,7 @@ CV_EXPORTS_AS(stereoCalibrateExtended) double stereoCalibrate( InputArrayOfArray
InputOutputArray cameraMatrix1, InputOutputArray distCoeffs1,
InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2,
Size imageSize, InputOutputArray R, InputOutputArray T, OutputArray E, OutputArray F,
OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
OutputArray perViewErrors, int flags = CALIB_FIX_INTRINSIC,
TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6) );
@ -1103,6 +1113,14 @@ CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints,
int flags = CALIB_FIX_INTRINSIC,
TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6) );
/// @overload
CV_EXPORTS_W 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) );
/** @brief Computes Hand-Eye calibration: \f$_{}^{g}\textrm{T}_c\f$
@ -1635,6 +1653,15 @@ similar to D1 .
@param imageSize Size of the image used only to initialize camera intrinsic matrix.
@param R Output rotation matrix between the 1st and the 2nd camera coordinate systems.
@param T Output translation vector between the coordinate systems of the cameras.
@param rvecs Output vector of rotation vectors ( @ref Rodrigues ) estimated for each pattern view in the
coordinate system of the first camera of the stereo pair (e.g. std::vector<cv::Mat>). More in detail, each
i-th rotation vector together with the corresponding i-th translation vector (see the next output parameter
description) brings the calibration pattern from the object coordinate space (in which object points are
specified) to the camera coordinate space of the first camera of the stereo pair. In more technical terms,
the tuple of the i-th rotation and translation vector performs a change of basis from object coordinate space
to camera coordinate space of the first camera of the stereo pair.
@param tvecs Output vector of translation vectors estimated for each pattern view, see parameter description
of previous output parameter ( rvecs ).
@param flags Different flags that may be zero or a combination of the following values:
- @ref fisheye::CALIB_FIX_INTRINSIC Fix K1, K2? and D1, D2? so that only R, T matrices
are estimated.
@ -1650,9 +1677,15 @@ zero.
@param criteria Termination criteria for the iterative optimization algorithm.
*/
CV_EXPORTS_W double stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize,
OutputArray R, OutputArray T, int flags = fisheye::CALIB_FIX_INTRINSIC,
TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON));
InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize,
OutputArray R, OutputArray T, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags = fisheye::CALIB_FIX_INTRINSIC,
TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON));
/// @overload
CV_EXPORTS_W double stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize,
OutputArray R, OutputArray T, int flags = fisheye::CALIB_FIX_INTRINSIC,
TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON));
//! @} calib3d_fisheye
} //end namespace fisheye

View File

@ -563,7 +563,8 @@ static double calibrateCameraInternal( const Mat& objectPoints,
//std::cout << "single camera calib. param after LM: " << param0.t() << "\n";
// If solver failed, then the last calculated perViewErr can be wrong & should be recalculated
// If solver failed or last LM iteration was not successful,
// then the last calculated perViewErr can be wrong & should be recalculated
Mat JtErr, JtJ, JtJinv, JtJN;
double reprojErr = 0;
if (!stdDevs.empty())
@ -651,13 +652,14 @@ static double stereoCalibrateImpl(
Mat& _cameraMatrix2, Mat& _distCoeffs2,
Size imageSize, Mat matR, Mat matT,
Mat matE, Mat matF,
Mat rvecs, Mat tvecs,
Mat perViewErr, int flags,
TermCriteria termCrit )
{
int NINTRINSIC = CALIB_NINTRINSIC;
double dk[2][14]={{0}};
Mat Dist[2];
// initial camera intrinsicss
Vec<double, 14> distInitial[2];
Matx33d A[2];
int pointsTotal = 0, maxPoints = 0, nparams;
bool recomputeIntrinsics = false;
@ -667,7 +669,7 @@ static double stereoCalibrateImpl(
_imagePoints1.depth() == _objectPoints.depth() );
CV_Assert( (_npoints.cols == 1 || _npoints.rows == 1) &&
_npoints.type() == CV_32S );
_npoints.type() == CV_32S );
int nimages = (int)_npoints.total();
for(int i = 0; i < nimages; i++ )
@ -682,6 +684,27 @@ static double stereoCalibrateImpl(
_objectPoints.convertTo(objectPoints, CV_64F);
objectPoints = objectPoints.reshape(3, 1);
if( !rvecs.empty() )
{
int cn = rvecs.channels();
int depth = rvecs.depth();
if( (depth != CV_32F && depth != CV_64F) ||
((rvecs.rows != nimages || (rvecs.cols*cn != 3 && rvecs.cols*cn != 9)) &&
(rvecs.rows != 1 || rvecs.cols != nimages || cn != 3)) )
CV_Error( CV_StsBadArg, "the output array of rotation vectors must be 3-channel "
"1xn or nx1 array or 1-channel nx3 or nx9 array, where n is the number of views" );
}
if( !tvecs.empty() )
{
int cn = tvecs.channels();
int depth = tvecs.depth();
if( (depth != CV_32F && depth != CV_64F) ||
((tvecs.rows != nimages || tvecs.cols*cn != 3) &&
(tvecs.rows != 1 || tvecs.cols != nimages || cn != 3)) )
CV_Error( CV_StsBadArg, "the output array of translation vectors must be 3-channel "
"1xn or nx1 array or 1-channel nx3 array, where n is the number of views" );
}
for(int k = 0; k < 2; k++ )
{
const Mat& points = k == 0 ? _imagePoints1 : _imagePoints2;
@ -694,29 +717,29 @@ static double stereoCalibrateImpl(
((points.rows == pointsTotal && points.cols*cn == 2) ||
(points.rows == 1 && points.cols == pointsTotal && cn == 2)));
A[k] = Matx33d(1, 0, 0, 0, 1, 0, 0, 0, 1);
Dist[k] = Mat(1,14,CV_64F,dk[k]);
A[k] = Matx33d::eye();
points.convertTo(imagePoints[k], CV_64F);
imagePoints[k] = imagePoints[k].reshape(2, 1);
if( flags & (CALIB_FIX_INTRINSIC|CALIB_USE_INTRINSIC_GUESS|
CALIB_FIX_ASPECT_RATIO|CALIB_FIX_FOCAL_LENGTH) )
if( flags & ( CALIB_FIX_INTRINSIC | CALIB_USE_INTRINSIC_GUESS |
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) )
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) )
{
Mat tdist( distCoeffs.size(), CV_MAKETYPE(CV_64F, distCoeffs.channels()), dk[k] );
Mat tdist( distCoeffs.size(), CV_MAKETYPE(CV_64F, distCoeffs.channels()), distInitial[k].val);
distCoeffs.convertTo(tdist, CV_64F);
}
if( !(flags & (CALIB_FIX_INTRINSIC|CALIB_USE_INTRINSIC_GUESS)))
if( !(flags & (CALIB_FIX_INTRINSIC | CALIB_USE_INTRINSIC_GUESS)))
{
Mat matA(A[k], false);
Mat mIntr(A[k], /* copyData = */ false);
Mat mDist(distInitial[k], /* copyData = */ false);
calibrateCameraInternal(objectPoints, imagePoints[k],
_npoints, imageSize, 0, matA, Dist[k],
_npoints, imageSize, 0, mIntr, mDist,
Mat(), Mat(), Mat(), Mat(), Mat(), flags, termCrit);
}
}
@ -732,7 +755,7 @@ static double stereoCalibrateImpl(
if( flags & CALIB_FIX_ASPECT_RATIO )
{
for(int k = 0; k < 2; k++ )
aspectRatio[k] = A[k](0, 0)/A[k](1, 1);
aspectRatio[k] = A[k](0, 0) / A[k](1, 1);
}
recomputeIntrinsics = (flags & CALIB_FIX_INTRINSIC) == 0;
@ -817,7 +840,7 @@ static double stereoCalibrateImpl(
for(int k = 0; k < 2; k++ )
{
Mat imgpt_ik = imagePoints[k].colRange(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], distInitial[k], rv, T[k], false, SOLVEPNP_ITERATIVE );
Rodrigues(rv, R[k]);
if( k == 0 )
@ -883,11 +906,11 @@ static double stereoCalibrateImpl(
{
size_t idx = (nimages+1)*6 + k*NINTRINSIC;
if( flags & CALIB_ZERO_TANGENT_DIST )
dk[k][2] = dk[k][3] = 0;
distInitial[k][2] = distInitial[k][3] = 0;
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];
param[idx + 4 + i] = distInitial[k][i];
}
}
}
@ -942,7 +965,7 @@ static double stereoCalibrateImpl(
{
intrin[k] = A[k];
for(int j = 0; j < 14; j++)
distCoeffs[k][j] = dk[k][j];
distCoeffs[k][j] = distInitial[k][j];
}
}
@ -1105,7 +1128,12 @@ static double stereoCalibrateImpl(
Mat& cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2;
Mat& distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2;
A[k].convertTo(cameraMatrix, cameraMatrix.depth());
Mat tdist( distCoeffs.size(), CV_MAKETYPE(CV_64F,distCoeffs.channels()), Dist[k].data );
std::vector<double> vdist(14);
for(int j = 0; j < 14; j++)
vdist[j] = param[idx + 4 + j];
Mat tdist( distCoeffs.size(), CV_MAKETYPE(CV_64F, distCoeffs.channels()), vdist.data());
tdist.convertTo(distCoeffs, distCoeffs.depth());
}
}
@ -1126,6 +1154,33 @@ static double stereoCalibrateImpl(
}
}
Mat r1d = rvecs.empty() ? Mat() : rvecs.reshape(1, nimages);
Mat t1d = tvecs.empty() ? Mat() : tvecs.reshape(1, nimages);
for(int i = 0; i < nimages; i++ )
{
int idx = (i + 1) * 6;
if( !rvecs.empty() )
{
Vec3d srcR(param[idx + 0], param[idx + 1], param[idx + 2]);
if( rvecs.rows * rvecs.cols * rvecs.channels() == nimages * 9 )
{
Matx33d rod;
Rodrigues(srcR, rod);
rod.convertTo(r1d.row(i).reshape(1, 3), rvecs.depth());
}
else if (rvecs.rows * rvecs.cols * rvecs.channels() == nimages * 3 )
{
Mat(Mat(srcR).t()).convertTo(r1d.row(i), rvecs.depth());
}
}
if( !tvecs.empty() )
{
Vec3d srcT(param[idx + 3], param[idx + 4], param[idx + 5]);
Mat(Mat(srcT).t()).convertTo(t1d.row(i), tvecs.depth());
}
}
return std::sqrt(reprojErr/(pointsTotal*2));
}
@ -1265,23 +1320,17 @@ Mat initCameraMatrix2D( InputArrayOfArrays objectPoints,
return cameraMatrix;
}
static Mat prepareDistCoeffs(Mat& distCoeffs0, int rtype, int outputSize = 14)
static Mat prepareDistCoeffs(Mat& distCoeffs0, int rtype, int outputSize)
{
Size sz = distCoeffs0.size();
int n = sz.area();
if (n > 0)
CV_Assert(sz.width == 1 || sz.height == 1);
CV_Assert((int)distCoeffs0.total() <= outputSize);
Mat distCoeffs = Mat::zeros(distCoeffs0.cols == 1 ? Size(1, outputSize) : Size(outputSize, 1), rtype);
if( distCoeffs0.size() == Size(1, 4) ||
distCoeffs0.size() == Size(1, 5) ||
distCoeffs0.size() == Size(1, 8) ||
distCoeffs0.size() == Size(1, 12) ||
distCoeffs0.size() == Size(1, 14) ||
distCoeffs0.size() == Size(4, 1) ||
distCoeffs0.size() == Size(5, 1) ||
distCoeffs0.size() == Size(8, 1) ||
distCoeffs0.size() == Size(12, 1) ||
distCoeffs0.size() == Size(14, 1) )
Mat distCoeffs = Mat::zeros(sz.width == 1 ? Size(1, outputSize) : Size(outputSize, 1), rtype);
if( n == 4 || n == 5 || n == 8 || n == 12 || n == 14 )
{
Mat dstCoeffs(distCoeffs, Rect(0, 0, distCoeffs0.cols, distCoeffs0.rows));
distCoeffs0.convertTo(dstCoeffs, rtype);
distCoeffs0.convertTo(distCoeffs(Rect(Point(), sz)), rtype);
}
return distCoeffs;
}
@ -1352,7 +1401,7 @@ double calibrateCameraRO(InputArrayOfArrays _objectPoints,
(flags & CALIB_THIN_PRISM_MODEL) &&
!(flags & CALIB_TILTED_MODEL) ?
prepareDistCoeffs(distCoeffs, rtype, 12) :
prepareDistCoeffs(distCoeffs, rtype);
prepareDistCoeffs(distCoeffs, rtype, 14);
if( !(flags & CALIB_RATIONAL_MODEL) &&
(!(flags & CALIB_THIN_PRISM_MODEL)) &&
(!(flags & CALIB_TILTED_MODEL)))
@ -1534,7 +1583,23 @@ double stereoCalibrate( InputArrayOfArrays _objectPoints,
InputOutputArray _cameraMatrix2, InputOutputArray _distCoeffs2,
Size imageSize, InputOutputArray _Rmat, InputOutputArray _Tmat,
OutputArray _Emat, OutputArray _Fmat,
OutputArray _perViewErrors, int flags ,
OutputArray _perViewErrors, int flags,
TermCriteria criteria)
{
return stereoCalibrate(_objectPoints, _imagePoints1, _imagePoints2, _cameraMatrix1, _distCoeffs1,
_cameraMatrix2, _distCoeffs2, imageSize, _Rmat, _Tmat, _Emat, _Fmat,
noArray(), noArray(), _perViewErrors, flags, criteria);
}
double 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,
OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs,
OutputArray _perViewErrors, int flags,
TermCriteria criteria)
{
int rtype = CV_64F;
@ -1544,8 +1609,8 @@ double stereoCalibrate( InputArrayOfArrays _objectPoints,
Mat distCoeffs2 = _distCoeffs2.getMat();
cameraMatrix1 = prepareCameraMatrix(cameraMatrix1, rtype, flags);
cameraMatrix2 = prepareCameraMatrix(cameraMatrix2, rtype, flags);
distCoeffs1 = prepareDistCoeffs(distCoeffs1, rtype);
distCoeffs2 = prepareDistCoeffs(distCoeffs2, rtype);
distCoeffs1 = prepareDistCoeffs(distCoeffs1, rtype, 14);
distCoeffs2 = prepareDistCoeffs(distCoeffs2, rtype, 14);
if( !(flags & CALIB_RATIONAL_MODEL) &&
(!(flags & CALIB_THIN_PRISM_MODEL)) &&
@ -1561,13 +1626,18 @@ double stereoCalibrate( InputArrayOfArrays _objectPoints,
_Tmat.create(3, 1, rtype);
}
Mat objPt, imgPt, imgPt2, npoints;
int nimages = int(_objectPoints.total());
CV_Assert( nimages > 0 );
Mat objPt, imgPt, imgPt2, npoints, rvecLM, tvecLM;
collectCalibrationData( _objectPoints, _imagePoints1, _imagePoints2,
objPt, imgPt, imgPt2, npoints );
Mat matR = _Rmat.getMat(), matT = _Tmat.getMat();
bool E_needed = _Emat.needed(), F_needed = _Fmat.needed(), errors_needed = _perViewErrors.needed();
bool E_needed = _Emat.needed(), F_needed = _Fmat.needed();
bool rvecs_needed = _rvecs.needed(), tvecs_needed = _tvecs.needed();
bool errors_needed = _perViewErrors.needed();
Mat matE, matF, matErr;
if( E_needed )
@ -1581,22 +1651,59 @@ double stereoCalibrate( InputArrayOfArrays _objectPoints,
matF = _Fmat.getMat();
}
bool rvecs_mat_vec = _rvecs.isMatVector();
bool tvecs_mat_vec = _tvecs.isMatVector();
if( rvecs_needed )
{
_rvecs.create(nimages, 1, CV_64FC3);
if( rvecs_mat_vec )
rvecLM.create(nimages, 3, CV_64F);
else
rvecLM = _rvecs.getMat();
}
if( tvecs_needed )
{
_tvecs.create(nimages, 1, CV_64FC3);
if( tvecs_mat_vec )
tvecLM.create(nimages, 3, CV_64F);
else
tvecLM = _tvecs.getMat();
}
if( errors_needed )
{
int nimages = int(_objectPoints.total());
_perViewErrors.create(nimages, 2, CV_64F);
matErr = _perViewErrors.getMat();
}
double err = stereoCalibrateImpl(objPt, imgPt, imgPt2, npoints, cameraMatrix1,
distCoeffs1, cameraMatrix2, distCoeffs2, imageSize,
matR, matT, matE, matF,
matR, matT, matE, matF, rvecLM, tvecLM,
matErr, flags, criteria);
cameraMatrix1.copyTo(_cameraMatrix1);
cameraMatrix2.copyTo(_cameraMatrix2);
distCoeffs1.copyTo(_distCoeffs1);
distCoeffs2.copyTo(_distCoeffs2);
for(int i = 0; i < nimages; i++ )
{
if( rvecs_needed && rvecs_mat_vec )
{
_rvecs.create(3, 1, CV_64F, i, true);
Mat rv = _rvecs.getMat(i);
Mat(rvecLM.row(i).t()).copyTo(rv);
}
if( tvecs_needed && tvecs_mat_vec )
{
_tvecs.create(3, 1, CV_64F, i, true);
Mat tv = _tvecs.getMat(i);
Mat(tvecLM.row(i).t()).copyTo(tv);
}
}
return err;
}

View File

@ -887,6 +887,15 @@ double cv::fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArray
double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize,
OutputArray R, OutputArray T, int flags, TermCriteria criteria)
{
return cv::fisheye::stereoCalibrate(objectPoints, imagePoints1, imagePoints2, K1, D1, K2, D2, imageSize, R, T,
noArray(), noArray(), flags, criteria);
}
double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize,
OutputArray R, OutputArray T, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
int flags, TermCriteria criteria)
{
CV_INSTRUMENT_REGION();
@ -1102,12 +1111,12 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
rms = sqrt(rms);
_K1 = Matx33d(intrinsicLeft.f[0], intrinsicLeft.f[0] * intrinsicLeft.alpha, intrinsicLeft.c[0],
0, intrinsicLeft.f[1], intrinsicLeft.c[1],
0, 0, 1);
0, intrinsicLeft.f[1], intrinsicLeft.c[1],
0, 0, 1);
_K2 = Matx33d(intrinsicRight.f[0], intrinsicRight.f[0] * intrinsicRight.alpha, intrinsicRight.c[0],
0, intrinsicRight.f[1], intrinsicRight.c[1],
0, 0, 1);
0, intrinsicRight.f[1], intrinsicRight.c[1],
0, 0, 1);
Mat _R;
Rodrigues(omcur, _R);
@ -1118,6 +1127,27 @@ double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
if (D2.needed()) Mat(intrinsicRight.k).convertTo(D2, D2.empty() ? CV_64FC1 : D2.type());
if (R.needed()) _R.convertTo(R, R.empty() ? CV_64FC1 : R.type());
if (T.needed()) Mat(Tcur).convertTo(T, T.empty() ? CV_64FC1 : T.type());
if (rvecs.isMatVector())
{
if(rvecs.empty())
rvecs.create(n_images, 1, CV_64FC3);
if(tvecs.empty())
tvecs.create(n_images, 1, CV_64FC3);
for(int i = 0; i < n_images; i++ )
{
rvecs.create(3, 1, CV_64F, i, true);
tvecs.create(3, 1, CV_64F, i, true);
rvecs1[i].copyTo(rvecs.getMat(i));
tvecs1[i].copyTo(tvecs.getMat(i));
}
}
else
{
if (rvecs.needed()) cv::Mat(rvecs1).convertTo(rvecs, rvecs.empty() ? CV_64FC3 : rvecs.type());
if (tvecs.needed()) cv::Mat(tvecs1).convertTo(tvecs, tvecs.empty() ? CV_64FC3 : tvecs.type());
}
return rms;
}

View File

@ -1256,10 +1256,8 @@ protected:
virtual void correct( const Mat& F,
const Mat &points1, const Mat &points2,
Mat &newPoints1, Mat &newPoints2 ) = 0;
#if 0 // not ported: #22519
int compare(double* val, double* refVal, int len,
double eps, const char* paramName);
#endif
void run(int);
};
@ -1326,13 +1324,11 @@ bool CV_StereoCalibrationTest::checkPandROI( int test_case_idx, const Mat& M, co
return true;
}
#if 0 // not ported: #22519
int CV_StereoCalibrationTest::compare(double* val, double* ref_val, int len,
double eps, const char* param_name )
{
return cvtest::cmpEps2_64f( ts, val, ref_val, len, eps, param_name );
}
#endif
void CV_StereoCalibrationTest::run( int )
{
@ -1340,9 +1336,7 @@ void CV_StereoCalibrationTest::run( int )
const double maxReprojErr = 2;
const double maxScanlineDistErr_c = 3;
const double maxScanlineDistErr_uc = 4;
#if 0 // not ported: #22519
const double maxDiffBtwRmsErrors = 1e-4;
#endif
FILE* f = 0;
for(int testcase = 1; testcase <= ntests; testcase++)
@ -1399,7 +1393,7 @@ void CV_StereoCalibrationTest::run( int )
if(left.empty() || right.empty())
{
ts->printf( cvtest::TS::LOG, "Can not load images %s and %s, testcase %d\n",
imglist[i*2].c_str(), imglist[i*2+1].c_str(), testcase );
imglist[i*2].c_str(), imglist[i*2+1].c_str(), testcase );
ts->set_failed_test_info( cvtest::TS::FAIL_MISSING_TEST_DATA );
return;
}
@ -1409,8 +1403,8 @@ void CV_StereoCalibrationTest::run( int )
if(!found1 || !found2)
{
ts->printf( cvtest::TS::LOG, "The function could not detect boards (%d x %d) on the images %s and %s, testcase %d\n",
patternSize.width, patternSize.height,
imglist[i*2].c_str(), imglist[i*2+1].c_str(), testcase );
patternSize.width, patternSize.height,
imglist[i*2].c_str(), imglist[i*2+1].c_str(), testcase );
ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT );
return;
}
@ -1444,17 +1438,16 @@ void CV_StereoCalibrationTest::run( int )
+ CALIB_FIX_K3
+ CALIB_FIX_K4 + CALIB_FIX_K5 //+ CV_CALIB_FIX_K6
);
#if 1 // not ported: #22519
rmsErrorFromStereoCalib /= nframes*npoints;
#endif
/* rmsErrorFromStereoCalib /= nframes*npoints; */
if (rmsErrorFromStereoCalib > maxReprojErr)
{
ts->printf(cvtest::TS::LOG, "The average reprojection error is too big (=%g), testcase %d\n",
rmsErrorFromStereoCalib, testcase);
rmsErrorFromStereoCalib, testcase);
ts->set_failed_test_info(cvtest::TS::FAIL_INVALID_OUTPUT);
return;
}
#if 0 // not ported: #22519
double rmsErrorFromReprojectedImgPts = 0.0f;
if (rotMats1.empty() || transVecs1.empty())
{
@ -1464,9 +1457,8 @@ void CV_StereoCalibrationTest::run( int )
}
else
{
vector<Point2f > reprojectedImgPts[2] = {vector<Point2f>(nframes), vector<Point2f>(nframes)};
size_t totalPoints = 0;
double totalErr[2] = { 0, 0 }, viewErr[2];
double totalErr[2] = { 0, 0 };
for (size_t i = 0; i < objpt.size(); ++i) {
RotMat r1 = rotMats1[i];
Vec3d t1 = transVecs1[i];
@ -1475,9 +1467,12 @@ void CV_StereoCalibrationTest::run( int )
Mat T2t = R * t1;
Vec3d t2 = Mat(T2t + T);
vector<Point2f> reprojectedImgPts[2] = { vector<Point2f>(nframes),
vector<Point2f>(nframes) };
projectPoints(objpt[i], r1, t1, M1, D1, reprojectedImgPts[0]);
projectPoints(objpt[i], r2, t2, M2, D2, reprojectedImgPts[1]);
double viewErr[2];
viewErr[0] = cv::norm(imgpt1[i], reprojectedImgPts[0], cv::NORM_L2SQR);
viewErr[1] = cv::norm(imgpt2[i], reprojectedImgPts[1], cv::NORM_L2SQR);
@ -1490,16 +1485,15 @@ void CV_StereoCalibrationTest::run( int )
rmsErrorPerViewFromReprojectedImgPts2[i] = sqrt(viewErr[1] / n);
}
rmsErrorFromReprojectedImgPts = std::sqrt((totalErr[0] + totalErr[1]) / (2 * totalPoints));
}
if (abs(rmsErrorFromStereoCalib - rmsErrorFromReprojectedImgPts) > maxDiffBtwRmsErrors)
{
ts->printf(cvtest::TS::LOG,
"The difference of the average reprojection error from the calibration function and from the "
"reprojected image points is too big (|%g - %g| = %g), testcase %d\n",
rmsErrorFromStereoCalib, rmsErrorFromReprojectedImgPts,
(rmsErrorFromStereoCalib - rmsErrorFromReprojectedImgPts), testcase);
"The difference of the average reprojection error from the calibration function and from the "
"reprojected image points is too big (|%g - %g| = %g), testcase %d\n",
rmsErrorFromStereoCalib, rmsErrorFromReprojectedImgPts,
(rmsErrorFromStereoCalib - rmsErrorFromReprojectedImgPts), testcase);
ts->set_failed_test_info(cvtest::TS::FAIL_INVALID_OUTPUT);
return;
}
@ -1510,28 +1504,27 @@ void CV_StereoCalibrationTest::run( int )
CV_Assert(rmsErrorPerView2.size() == (size_t)nframes);
CV_Assert(rmsErrorPerViewFromReprojectedImgPts2.size() == (size_t)nframes);
int code1 = compare(&rmsErrorPerView1[0], &rmsErrorPerViewFromReprojectedImgPts1[0], nframes,
maxDiffBtwRmsErrors, "per view errors vector");
maxDiffBtwRmsErrors, "per view errors vector");
int code2 = compare(&rmsErrorPerView2[0], &rmsErrorPerViewFromReprojectedImgPts2[0], nframes,
maxDiffBtwRmsErrors, "per view errors vector");
maxDiffBtwRmsErrors, "per view errors vector");
if (code1 < 0)
{
ts->printf(cvtest::TS::LOG,
"Some of the per view rms reprojection errors differ between calibration function and reprojected "
"points, for the first camera, testcase %d\n",
testcase);
"Some of the per view rms reprojection errors differ between calibration function and reprojected "
"points, for the first camera, testcase %d\n",
testcase);
ts->set_failed_test_info(code1);
return;
}
if (code2 < 0)
{
ts->printf(cvtest::TS::LOG,
"Some of the per view rms reprojection errors differ between calibration function and reprojected "
"points, for the second camera, testcase %d\n",
testcase);
"Some of the per view rms reprojection errors differ between calibration function and reprojected "
"points, for the second camera, testcase %d\n",
testcase);
ts->set_failed_test_info(code2);
return;
}
#endif
Mat R1, R2, P1, P2, Q;
Rect roi1, roi2;
@ -1771,33 +1764,23 @@ protected:
};
double CV_StereoCalibrationTest_CPP::calibrateStereoCamera( const vector<vector<Point3f> >& objectPoints,
const vector<vector<Point2f> >& imagePoints1,
const vector<vector<Point2f> >& imagePoints2,
Mat& cameraMatrix1, Mat& distCoeffs1,
Mat& cameraMatrix2, Mat& distCoeffs2,
Size imageSize, Mat& R, Mat& T,
Mat& E, Mat& F,
std::vector<RotMat>& rotationMatrices, std::vector<Vec3d>& translationVectors,
vector<double>& perViewErrors1, vector<double>& perViewErrors2,
TermCriteria criteria, int flags )
const vector<vector<Point2f> >& imagePoints1,
const vector<vector<Point2f> >& imagePoints2,
Mat& cameraMatrix1, Mat& distCoeffs1,
Mat& cameraMatrix2, Mat& distCoeffs2,
Size imageSize, Mat& R, Mat& T,
Mat& E, Mat& F,
std::vector<RotMat>& rotationMatrices, std::vector<Vec3d>& translationVectors,
vector<double>& perViewErrors1, vector<double>& perViewErrors2,
TermCriteria criteria, int flags )
{
#if 1 // not ported: #22519
CV_UNUSED(rotationMatrices);
CV_UNUSED(translationVectors);
CV_UNUSED(perViewErrors1);
CV_UNUSED(perViewErrors2);
return stereoCalibrate( objectPoints, imagePoints1, imagePoints2,
cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2,
imageSize, R, T, E, F, flags, criteria );
#else
vector<Mat> rvecs, tvecs;
Mat perViewErrorsMat;
double avgErr = stereoCalibrate( objectPoints, imagePoints1, imagePoints2,
cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2,
imageSize, R, T, E, F,
rvecs, tvecs, perViewErrorsMat,
flags, criteria );
cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2,
imageSize, R, T, E, F,
rvecs, tvecs, perViewErrorsMat,
flags, criteria );
size_t numImgs = imagePoints1.size();
@ -1810,10 +1793,10 @@ double CV_StereoCalibrationTest_CPP::calibrateStereoCamera( const vector<vector<
perViewErrors2.resize(numImgs);
}
for (size_t i = 0; i < numImgs; i++)
for (int i = 0; i < (int)numImgs; i++)
{
perViewErrors1[i] = perViewErrorsMat.at<double>((int)i, 0);
perViewErrors2[i] = perViewErrorsMat.at<double>((int)i, 1);
perViewErrors1[i] = perViewErrorsMat.at<double>(i, 0);
perViewErrors2[i] = perViewErrorsMat.at<double>(i, 1);
}
if (rotationMatrices.size() != numImgs)
@ -1825,7 +1808,7 @@ double CV_StereoCalibrationTest_CPP::calibrateStereoCamera( const vector<vector<
translationVectors.resize(numImgs);
}
for (size_t i = 0; i < numImgs; i++)
for( size_t i = 0; i < numImgs; i++ )
{
Mat r9;
cv::Rodrigues( rvecs[i], r9 );
@ -1833,7 +1816,6 @@ double CV_StereoCalibrationTest_CPP::calibrateStereoCamera( const vector<vector<
tvecs[i].convertTo(translationVectors[i], CV_64F);
}
return avgErr;
#endif
}
void CV_StereoCalibrationTest_CPP::rectify( const Mat& cameraMatrix1, const Mat& distCoeffs1,

View File

@ -883,7 +883,7 @@ TEST_F(fisheyeTest, CalibrationWithDifferentPointsNumber)
cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));
}
#if 0 // not ported: #22519
TEST_F(fisheyeTest, stereoCalibrateWithPerViewTransformations)
{
const int n_images = 34;
@ -924,10 +924,11 @@ TEST_F(fisheyeTest, stereoCalibrateWithPerViewTransformations)
flag |= cv::fisheye::CALIB_FIX_SKEW;
double rmsErrorStereoCalib = cv::fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints,
K1, D1, K2, D2, imageSize, theR, theT, rvecs, tvecs, flag,
cv::TermCriteria(3, 12, 0));
K1, D1, K2, D2, imageSize, theR, theT, rvecs, tvecs, flag,
cv::TermCriteria(3, 12, 0));
std::vector<cv::Point2d> reprojectedImgPts[2] = {std::vector<cv::Point2d>(n_images), std::vector<cv::Point2d>(n_images)};
std::vector<cv::Point2d> reprojectedImgPts[2] = { std::vector<cv::Point2d>(n_images),
std::vector<cv::Point2d>(n_images) };
size_t totalPoints = 0;
double totalMSError[2] = { 0, 0 };
for( size_t i = 0; i < n_images; i++ )
@ -969,12 +970,12 @@ TEST_F(fisheyeTest, stereoCalibrateWithPerViewTransformations)
-0.006071257768382089, -0.006271040135405457, 0.9999619062167968);
cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699);
cv::Matx33d K1_correct (561.195925927249, 0, 621.282400272412,
0, 562.849402029712, 380.555455380889,
0, 0, 1);
0, 562.849402029712, 380.555455380889,
0, 0, 1);
cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359,
0, 561.90171021422, 380.401340535339,
0, 0, 1);
cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359,
0, 561.90171021422, 380.401340535339,
0, 0, 1);
cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771);
cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);
@ -990,7 +991,7 @@ TEST_F(fisheyeTest, stereoCalibrateWithPerViewTransformations)
EXPECT_NEAR(rmsErrorStereoCalib, rmsErrorFromReprojectedImgPts, 1e-4);
}
#endif
TEST_F(fisheyeTest, estimateNewCameraMatrixForUndistortRectify)
{