mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2025-06-07 17:44:04 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Extended stereoCalibrate function for pinhole model to return per view rotation and translation vectors between the calibration object coordinate space and the coordinate space of the first camera of the stereo pair. Added overloaded versions of the function for downward compatibility.

Browse Source
This commit is contained in:
Stefan Spiss 2022-09-07 17:03:51 +02:00
parent 8af4fe9ed3
commit 8ae03fcd6e
2 changed files with 140 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
modules/calib3d/include/opencv2/calib3d.hpp
View File

@ -1748,6 +1748,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
@ -1845,7 +1854,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,
OutputArray perViewErrors, int flags = CALIB_FIX_INTRINSIC,
OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, OutputArray perViewErrors, int flags = CALIB_FIX_INTRINSIC,
TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) );
/// @overload
@ -1857,6 +1866,15 @@ CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints,
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,
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 rectification transforms for each head of a calibrated stereo camera.
@param cameraMatrix1 First camera intrinsic matrix.

122
modules/calib3d/src/calibration.cpp
View File

@ -1973,7 +1973,7 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i
CvMat* _cameraMatrix2, CvMat* _distCoeffs2,
CvSize imageSize, CvMat* matR, CvMat* matT,
CvMat* matE, CvMat* matF,
CvMat* perViewErr, int flags,
CvMat* rvecs, CvMat* tvecs, CvMat* perViewErr, int flags,
CvTermCriteria termCrit )
{
const int NINTRINSIC = 18;
@ -2013,6 +2013,28 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i
cvConvert( _objectPoints, objectPoints );
cvReshape( objectPoints, objectPoints, 3, 1 );
if( rvecs )
{
int cn = CV_MAT_CN(rvecs->type);
if( !CV_IS_MAT(rvecs) ||
(CV_MAT_DEPTH(rvecs->type) != CV_32F && CV_MAT_DEPTH(rvecs->type) != 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 )
{
int cn = CV_MAT_CN(tvecs->type);
if( !CV_IS_MAT(tvecs) ||
(CV_MAT_DEPTH(tvecs->type) != CV_32F && CV_MAT_DEPTH(tvecs->type) != 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( k = 0; k < 2; k++ )
{
const CvMat* points = k == 0 ? _imagePoints1 : _imagePoints2;
@ -2440,6 +2462,39 @@ static double cvStereoCalibrateImpl( const CvMat* _objectPoints, const CvMat* _i
}
}
CvMat tmp = cvMat(3, 3, CV_64F);
for( i = 0; i < nimages; i++ )
{
CvMat src, dst;
if( rvecs )
{
src = cvMat(3, 1, CV_64F, solver.param->data.db+(i+1)*6);
if( rvecs->rows == nimages && rvecs->cols*CV_MAT_CN(rvecs->type) == 9 )
{
dst = cvMat(3, 3, CV_MAT_DEPTH(rvecs->type),
rvecs->data.ptr + rvecs->step*i);
cvRodrigues2( &src, &tmp );
cvConvert( &tmp, &dst );
}
else
{
dst = cvMat(3, 1, CV_MAT_DEPTH(rvecs->type), rvecs->rows == 1 ?
rvecs->data.ptr + i*CV_ELEM_SIZE(rvecs->type) :
rvecs->data.ptr + rvecs->step*i);
cvConvert( &src, &dst );
}
}
if( tvecs )
{
src = cvMat(3, 1,CV_64F,solver.param->data.db+(i+1)*6+3);
dst = cvMat(3, 1, CV_MAT_DEPTH(tvecs->type), tvecs->rows == 1 ?
tvecs->data.ptr + i*CV_ELEM_SIZE(tvecs->type) :
tvecs->data.ptr + tvecs->step*i);
cvConvert( &src, &dst );
}
}
return std::sqrt(reprojErr/(pointsTotal*2));
}
double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1,
@ -2453,7 +2508,7 @@ double cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1
{
return cvStereoCalibrateImpl(_objectPoints, _imagePoints1, _imagePoints2, _npoints, _cameraMatrix1,
_distCoeffs1, _cameraMatrix2, _distCoeffs2, imageSize, matR, matT, matE,
matF, NULL, flags, termCrit);
matF, NULL, NULL, NULL, flags, termCrit);
}
static void
@ -3888,6 +3943,21 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints,
OutputArray _Emat, OutputArray _Fmat,
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 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,
OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, OutputArray _perViewErrors, int flags,
TermCriteria criteria)
{
int rtype = CV_64F;
Mat cameraMatrix1 = _cameraMatrix1.getMat();
@ -3913,7 +3983,10 @@ double cv::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 );
@ -3923,7 +3996,7 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints,
Mat matR_ = _Rmat.getMat(), matT_ = _Tmat.getMat();
CvMat c_matR = cvMat(matR_), c_matT = cvMat(matT_), c_matE, c_matF, c_matErr;
bool E_needed = _Emat.needed(), F_needed = _Fmat.needed(), errors_needed = _perViewErrors.needed();
bool E_needed = _Emat.needed(), F_needed = _Fmat.needed(), rvecs_needed = _rvecs.needed(), tvecs_needed = _tvecs.needed(), errors_needed = _perViewErrors.needed();
Mat matE_, matF_, matErr_;
if( E_needed )
@ -3939,9 +4012,31 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints,
c_matF = cvMat(matF_);
}
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();
}
CvMat c_rvecLM = cvMat(rvecLM), c_tvecLM = cvMat(tvecLM);
if( errors_needed )
{
int nimages = int(_objectPoints.total());
_perViewErrors.create(nimages, 2, CV_64F);
matErr_ = _perViewErrors.getMat();
c_matErr = cvMat(matErr_);
@ -3950,6 +4045,7 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints,
double err = cvStereoCalibrateImpl(&c_objPt, &c_imgPt, &c_imgPt2, &c_npoints, &c_cameraMatrix1,
&c_distCoeffs1, &c_cameraMatrix2, &c_distCoeffs2, cvSize(imageSize), &c_matR,
&c_matT, E_needed ? &c_matE : NULL, F_needed ? &c_matF : NULL,
rvecs_needed ? &c_rvecLM : NULL, tvecs_needed ? &c_tvecLM : NULL,
errors_needed ? &c_matErr : NULL, flags, cvTermCriteria(criteria));
cameraMatrix1.copyTo(_cameraMatrix1);
@ -3957,6 +4053,22 @@ double cv::stereoCalibrate( InputArrayOfArrays _objectPoints,
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);
memcpy(rv.ptr(), rvecLM.ptr(i), 3*sizeof(double));
}
if( tvecs_needed && tvecs_mat_vec )
{
_tvecs.create(3, 1, CV_64F, i, true);
Mat tv = _tvecs.getMat(i);
memcpy(tv.ptr(), tvecLM.ptr(i), 3*sizeof(double));
}
}
return err;
}