Merge pull request #22443 from catree:feat_calibrate_camera_exe_initial_guess

samples/cpp/calibration.cpp

@@ -10,6 +10,7 @@
 #include <stdio.h>
 #include <string.h>
 #include <time.h>
+#include <iostream>
 
 using namespace cv;
 using namespace std;
@@ -37,9 +38,6 @@ const char * usage =
 "</images>\n"
 "</opencv_storage>\n";
 
-
-
-
 const char* liveCaptureHelp =
     "When the live video from camera is used as input, the following hot-keys may be used:\n"
         "  <ESC>, 'q' - quit the program\n"
@@ -69,6 +67,13 @@ static void help(char** argv)
         "     [-V]                     # use a video file, and not an image list, uses\n"
         "                              # [input_data] string for the video file name\n"
         "     [-su]                    # show undistorted images after calibration\n"
+        "     [-ws=<number_of_pixel>]  # half of search window for cornerSubPix (11 by default)\n"
+        "     [-fx=<X focal length>]   # focal length in X-dir as an initial intrinsic guess (if this flag is used, fx, fy, cx, cy must be set)\n"
+        "     [-fy=<Y focal length>]   # focal length in Y-dir as an initial intrinsic guess (if this flag is used, fx, fy, cx, cy must be set)\n"
+        "     [-cx=<X center point>]   # camera center point in X-dir as an initial intrinsic guess (if this flag is used, fx, fy, cx, cy must be set)\n"
+        "     [-cy=<Y center point>]   # camera center point in Y-dir as an initial intrinsic guess (if this flag is used, fx, fy, cx, cy must be set)\n"
+        "     [-imshow-scale           # image resize scaling factor when displaying the results (must be >= 1)\n"
+        "     [-enable-k3=<0/1>        # to enable (1) or disable (0) K3 coefficient for the distortion model\n"
         "     [input_data]             # input data, one of the following:\n"
         "                              #  - text file with a list of the images of the board\n"
         "                              #    the text file can be generated with imagelist_creator\n"
@@ -142,7 +147,6 @@ static bool runCalibration( vector<vector<Point2f> > imagePoints,
                     vector<float>& reprojErrs,
                     double& totalAvgErr)
 {
-    cameraMatrix = Mat::eye(3, 3, CV_64F);
     if( flags & CALIB_FIX_ASPECT_RATIO )
         cameraMatrix.at<double>(0,0) = aspectRatio;
 
@@ -154,8 +158,7 @@ static bool runCalibration( vector<vector<Point2f> > imagePoints,
     objectPoints.resize(imagePoints.size(),objectPoints[0]);
 
     double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix,
-                    distCoeffs, rvecs, tvecs, flags|CALIB_FIX_K4|CALIB_FIX_K5);
+                    distCoeffs, rvecs, tvecs, flags | CALIB_USE_LU);
-                    ///*|CALIB_FIX_K3*/|CALIB_FIX_K4|CALIB_FIX_K5);
     printf("RMS error reported by calibrateCamera: %g\n", rms);
 
     bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);
@@ -166,7 +169,6 @@ static bool runCalibration( vector<vector<Point2f> > imagePoints,
     return ok;
 }
 
-
 static void saveCameraParams( const string& filename,
                        Size imageSize, Size boardSize,
                        float squareSize, float aspectRatio, int flags,
@@ -199,7 +201,7 @@ static void saveCameraParams( const string& filename,
 
     if( flags != 0 )
     {
-        sprintf( buf, "flags: %s%s%s%s",
+        snprintf( buf, sizeof(buf), "flags: %s%s%s%s",
             flags & CALIB_USE_INTRINSIC_GUESS ? "+use_intrinsic_guess" : "",
             flags & CALIB_FIX_ASPECT_RATIO ? "+fix_aspectRatio" : "",
             flags & CALIB_FIX_PRINCIPAL_POINT ? "+fix_principal_point" : "",
@@ -296,7 +298,7 @@ static bool runAndSave(const string& outputFilename,
     bool ok = runCalibration(imagePoints, imageSize, boardSize, patternType, squareSize,
                    aspectRatio, flags, cameraMatrix, distCoeffs,
                    rvecs, tvecs, reprojErrs, totalAvgErr);
-    printf("%s. avg reprojection error = %.2f\n",
+    printf("%s. avg reprojection error = %.7f\n",
            ok ? "Calibration succeeded" : "Calibration failed",
            totalAvgErr);
 
@@ -312,11 +314,10 @@ static bool runAndSave(const string& outputFilename,
     return ok;
 }
 
-
 int main( int argc, char** argv )
 {
     Size boardSize, imageSize;
-    float squareSize, aspectRatio;
+    float squareSize, aspectRatio = 1;
     Mat cameraMatrix, distCoeffs;
     string outputFilename;
     string inputFilename = "";
@@ -339,7 +340,10 @@ int main( int argc, char** argv )
 
     cv::CommandLineParser parser(argc, argv,
         "{help ||}{w||}{h||}{pt|chessboard|}{n|10|}{d|1000|}{s|1|}{o|out_camera_data.yml|}"
-        "{op||}{oe||}{zt||}{a|1|}{p||}{v||}{V||}{su||}"
+        "{op||}{oe||}{zt||}{a||}{p||}{v||}{V||}{su||}"
+        "{ws|11|}"
+        "{fx||}{fy||}{cx||}{cy||}"
+        "{imshow-scale|1|}{enable-k3|1|}"
         "{@input_data|0|}");
     if (parser.has("help"))
     {
@@ -362,12 +366,13 @@ int main( int argc, char** argv )
     }
     squareSize = parser.get<float>("s");
     nframes = parser.get<int>("n");
-    aspectRatio = parser.get<float>("a");
     delay = parser.get<int>("d");
     writePoints = parser.has("op");
     writeExtrinsics = parser.has("oe");
-    if (parser.has("a"))
+    if (parser.has("a")) {
         flags |= CALIB_FIX_ASPECT_RATIO;
+        aspectRatio = parser.get<float>("a");
+    }
     if ( parser.has("zt") )
         flags |= CALIB_ZERO_TANGENT_DIST;
     if ( parser.has("p") )
@@ -381,6 +386,24 @@ int main( int argc, char** argv )
         cameraId = parser.get<int>("@input_data");
     else
         inputFilename = parser.get<string>("@input_data");
+    int winSize = parser.get<int>("ws");
+    cameraMatrix = Mat::eye(3, 3, CV_64F);
+    if (parser.has("fx") && parser.has("fy") && parser.has("cx") && parser.has("cy"))
+    {
+        cameraMatrix.at<double>(0,0) = parser.get<double>("fx");
+        cameraMatrix.at<double>(0,2) = parser.get<double>("cx");
+        cameraMatrix.at<double>(1,1) = parser.get<double>("fy");
+        cameraMatrix.at<double>(1,2) = parser.get<double>("cy");
+        flags |= CALIB_USE_INTRINSIC_GUESS;
+        std::cout << "Use the following camera matrix as an initial guess:\n" << cameraMatrix << std::endl;
+    }
+    int viewScaleFactor = parser.get<int>("imshow-scale");
+    bool useK3 = parser.get<bool>("enable-k3");
+    std::cout << "Use K3 distortion coefficient? " << useK3 << std::endl;
+    if (!useK3)
+    {
+        flags |= CALIB_FIX_K3;
+    }
     if (!parser.check())
     {
         help(argv);
@@ -471,8 +494,8 @@ int main( int argc, char** argv )
         }
 
        // improve the found corners' coordinate accuracy
-        if( pattern == CHESSBOARD && found) cornerSubPix( viewGray, pointbuf, Size(11,11),
+        if( pattern == CHESSBOARD && found) cornerSubPix( viewGray, pointbuf, Size(winSize,winSize),
-            Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 ));
+            Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.0001 ));
 
         if( mode == CAPTURING && found &&
            (!capture.isOpened() || clock() - prevTimestamp > delay*1e-3*CLOCKS_PER_SEC) )
@@ -494,9 +517,9 @@ int main( int argc, char** argv )
         if( mode == CAPTURING )
         {
             if(undistortImage)
-                msg = format( "%d/%d Undist", (int)imagePoints.size(), nframes );
+                msg = cv::format( "%d/%d Undist", (int)imagePoints.size(), nframes );
             else
-                msg = format( "%d/%d", (int)imagePoints.size(), nframes );
+                msg = cv::format( "%d/%d", (int)imagePoints.size(), nframes );
         }
 
         putText( view, msg, textOrigin, 1, 1,
@@ -510,8 +533,17 @@ int main( int argc, char** argv )
             Mat temp = view.clone();
             undistort(temp, view, cameraMatrix, distCoeffs);
         }
-
+        if (viewScaleFactor > 1)
+        {
+            Mat viewScale;
+            resize(view, viewScale, Size(), 1.0/viewScaleFactor, 1.0/viewScaleFactor, INTER_AREA);
+            imshow("Image View", viewScale);
+        }
+        else
+        {
             imshow("Image View", view);
+        }
+
         char key = (char)waitKey(capture.isOpened() ? 50 : 500);
 
         if( key == 27 )
@@ -552,9 +584,17 @@ int main( int argc, char** argv )
             view = imread(imageList[i], 1);
             if(view.empty())
                 continue;
-            //undistort( view, rview, cameraMatrix, distCoeffs, cameraMatrix );
             remap(view, rview, map1, map2, INTER_LINEAR);
+            if (viewScaleFactor > 1)
+            {
+                Mat rviewScale;
+                resize(rview, rviewScale, Size(), 1.0/viewScaleFactor, 1.0/viewScaleFactor, INTER_AREA);
+                imshow("Image View", rviewScale);
+            }
+            else
+            {
                 imshow("Image View", rview);
+            }
             char c = (char)waitKey();
             if( c == 27 || c == 'q' || c == 'Q' )
                 break;