Merge pull request opencv#19539 from asmorkalov:as/calib_fix_focal_length

* Added CALIB_FIX_FOCAL_LENGTH to fisheye calibration opencv#13450

Sometimes you want to calibrate just the principal point of a camera, or just the distortion coefficients. In this case, you can pass the CALIB_FIX_FOCAL_LENGTH flag to keep Fx and Fy

* Added test for CALIB_FIX_FOCAL_LENGTH option in fisheye callinration.

Author: asmorkalov

modules/calib3d/include/opencv2/calib3d.hpp

@@ -3773,7 +3773,8 @@ namespace fisheye
         CALIB_FIX_K4                = 1 << 7,
         CALIB_FIX_INTRINSIC         = 1 << 8,
         CALIB_FIX_PRINCIPAL_POINT   = 1 << 9,
-        CALIB_ZERO_DISPARITY        = 1 << 10
+        CALIB_ZERO_DISPARITY        = 1 << 10,
+        CALIB_FIX_FOCAL_LENGTH      = 1 << 11
     };
 
     /** @brief Projects points using fisheye model
@@ -3927,6 +3928,8 @@ namespace fisheye
     are set to zeros and stay zero.
     -    @ref fisheye::CALIB_FIX_PRINCIPAL_POINT  The principal point is not changed during the global
 optimization. It stays at the center or at a different location specified when @ref fisheye::CALIB_USE_INTRINSIC_GUESS is set too.
+    -    @ref fisheye::CALIB_FIX_FOCAL_LENGTH The focal length is not changed during the global
+optimization. It is the \f$max(width,height)/\pi\f$ or the provided \f$f_x\f$, \f$f_y\f$ when @ref fisheye::CALIB_USE_INTRINSIC_GUESS is set too.
     @param criteria Termination criteria for the iterative optimization algorithm.
      */
     CV_EXPORTS_W double calibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, const Size& image_size,

modules/calib3d/src/fisheye.cpp

@@ -754,8 +754,8 @@ double cv::fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArray
     IntrinsicParams currentParam;
     IntrinsicParams errors;
 
-    finalParam.isEstimate[0] = 1;
-    finalParam.isEstimate[1] = 1;
+    finalParam.isEstimate[0] = flags & CALIB_FIX_FOCAL_LENGTH ? 0 : 1;
+    finalParam.isEstimate[1] = flags & CALIB_FIX_FOCAL_LENGTH ? 0 : 1;
     finalParam.isEstimate[2] = flags & CALIB_FIX_PRINCIPAL_POINT ? 0 : 1;
     finalParam.isEstimate[3] = flags & CALIB_FIX_PRINCIPAL_POINT ? 0 : 1;
     finalParam.isEstimate[4] = flags & CALIB_FIX_SKEW ? 0 : 1;

modules/calib3d/test/test_fisheye.cpp

@@ -345,7 +345,7 @@ TEST_F(fisheyeTest, Calibration)
     std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
     std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
 
-    const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
+    const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
     cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
     CV_Assert(fs_left.isOpened());
     for(int i = 0; i < n_images; ++i)
@@ -373,14 +373,61 @@ TEST_F(fisheyeTest, Calibration)
     EXPECT_MAT_NEAR(theD, this->D, 1e-10);
 }
 
+TEST_F(fisheyeTest, CalibrationWithFixedFocalLength)
+{
+    const int n_images = 34;
+
+    std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
+    std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
+
+    const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
+    cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
+    CV_Assert(fs_left.isOpened());
+    for(int i = 0; i < n_images; ++i)
+        fs_left[cv::format("image_%d", i )] >> imagePoints[i];
+    fs_left.release();
+
+    cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
+    CV_Assert(fs_object.isOpened());
+    for(int i = 0; i < n_images; ++i)
+        fs_object[cv::format("image_%d", i )] >> objectPoints[i];
+    fs_object.release();
+
+    int flag = 0;
+    flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
+    flag |= cv::fisheye::CALIB_CHECK_COND;
+    flag |= cv::fisheye::CALIB_FIX_SKEW;
+    flag |= cv::fisheye::CALIB_FIX_FOCAL_LENGTH;
+    flag |= cv::fisheye::CALIB_USE_INTRINSIC_GUESS;
+
+    cv::Matx33d theK = this->K;
+    const cv::Matx33d newK(
+        558.478088, 0.000000, 620.458461,
+        0.000000, 560.506767, 381.939362,
+        0.000000, 0.000000, 1.000000);
+
+    cv::Vec4d theD;
+    const cv::Vec4d newD(-0.001461, -0.003298, 0.006057, -0.003742);
+
+    cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, theK, theD,
+                           cv::noArray(), cv::noArray(), flag, cv::TermCriteria(3, 20, 1e-6));
+
+    // ensure that CALIB_FIX_FOCAL_LENGTH works and focal lenght has not changed
+    EXPECT_EQ(theK(0,0), K(0,0));
+    EXPECT_EQ(theK(1,1), K(1,1));
+
+    EXPECT_MAT_NEAR(theK, newK, 1e-6);
+    EXPECT_MAT_NEAR(theD, newD, 1e-6);
+}
+
 TEST_F(fisheyeTest, Homography)
 {
     const int n_images = 1;
 
     std::vector<std::vector<cv::Point2d> > imagePoints(n_images);
     std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
 
-    const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
+    const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
     cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
     CV_Assert(fs_left.isOpened());
     for(int i = 0; i < n_images; ++i)
@@ -498,7 +545,7 @@ TEST_F(fisheyeTest, stereoRectify)
         "For the purpose of continuity the following should be true: cv::CALIB_ZERO_DISPARITY == cv::fisheye::CALIB_ZERO_DISPARITY"
     );
 
-    const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
+    const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
 
     cv::Size calibration_size = this->imageSize, requested_size = calibration_size;
     cv::Matx33d K1 = this->K, K2 = K1;
@@ -599,7 +646,7 @@ TEST_F(fisheyeTest, stereoCalibrate)
 {
     const int n_images = 34;
 
-    const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
+    const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
 
     std::vector<std::vector<cv::Point2d> > leftPoints(n_images);
     std::vector<std::vector<cv::Point2d> > rightPoints(n_images);
@@ -666,7 +713,7 @@ TEST_F(fisheyeTest, stereoCalibrateFixIntrinsic)
 {
     const int n_images = 34;
 
-    const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
+    const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
 
     std::vector<std::vector<cv::Point2d> > leftPoints(n_images);
     std::vector<std::vector<cv::Point2d> > rightPoints(n_images);
@@ -822,6 +869,7 @@ const cv::Matx33d fisheyeTest::K(558.478087865323,               0, 620.45851536
 
 const cv::Vec4d fisheyeTest::D(-0.0014613319981768, -0.00329861110580401, 0.00605760088590183, -0.00374209380722371);
 
+
 const cv::Matx33d fisheyeTest::R ( 9.9756700084424932e-01, 6.9698277640183867e-02, 1.4929569991321144e-03,
                             -6.9711825162322980e-02, 9.9748249845531767e-01, 1.2997180766418455e-02,
                             -5.8331736398316541e-04,-1.3069635393884985e-02, 9.9991441852366736e-01);