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

modules/calib/include/opencv2/calib.hpp

@@ -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

modules/calib/src/calibration.cpp

@@ -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;
 }
 

modules/calib/src/fisheye.cpp

@@ -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;
 }

modules/calib/test/test_cameracalibration.cpp

@@ -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,

modules/calib/test/test_fisheye.cpp

@@ -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)
 {