added 3-camera rectification and 8-coeff distortion model

modules/calib3d/include/opencv2/calib3d/calib3d.hpp

@@ -220,6 +220,9 @@ CVAPI(void) cvDrawChessboardCorners( CvArr* image, CvSize pattern_size,
 #define CV_CALIB_FIX_K1  32
 #define CV_CALIB_FIX_K2  64
 #define CV_CALIB_FIX_K3  128
+#define CV_CALIB_FIX_K4  2048
+#define CV_CALIB_FIX_K5  4096
+#define CV_CALIB_FIX_K6  8192
 
 /* Finds intrinsic and extrinsic camera parameters
    from a few views of known calibration pattern */
@@ -544,6 +547,9 @@ enum
     CALIB_FIX_K1 = 32,
     CALIB_FIX_K2 = 64,
     CALIB_FIX_K3 = 128,
+    CALIB_FIX_K4 = 2048,
+    CALIB_FIX_K5 = 4096,
+    CALIB_FIX_K6 = 8192,
     // only for stereo
     CALIB_FIX_INTRINSIC = 256,
     CALIB_SAME_FOCAL_LENGTH = 512,
@@ -605,6 +611,17 @@ CV_EXPORTS bool stereoRectifyUncalibrated( const Mat& points1,
                                            Mat& H1, Mat& H2,
                                            double threshold=5 );
 
+//! computes the rectification transformations for 3-head camera, where the heads are on the same line.
+CV_EXPORTS float rectify3( const Mat& cameraMatrix1, const Mat& distCoeffs1,
+                   const Mat& cameraMatrix2, const Mat& distCoeffs2,
+                   const Mat& cameraMatrix3, const Mat& distCoeffs3,
+                   const vector<vector<Point2f> >& imgpt1,
+                   const vector<vector<Point2f> >& imgpt3,
+                   Size imageSize, const Mat& R12, const Mat& T12, const Mat& R13, const Mat& T13,
+                   Mat& R1, Mat& R2, Mat& R3, Mat& P1, Mat& P2, Mat& P3, Mat& Q,
+                   double alpha, Size newImgSize,
+                   Rect* roi1, Rect* roi2, int flags );
+    
 //! returns the optimal new camera matrix
 CV_EXPORTS Mat getOptimalNewCameraMatrix( const Mat& cameraMatrix, const Mat& distCoeffs,
                                           Size imageSize, double alpha, Size newImgSize=Size(),

modules/imgproc/src/undistort.cpp

@@ -89,7 +89,7 @@ void initUndistortRectifyMap( const Mat& _cameraMatrix, const Mat& _distCoeffs,
         distCoeffs = Mat_<double>(_distCoeffs);
     else
     {
-        distCoeffs.create(5, 1);
+        distCoeffs.create(8, 1);
         distCoeffs = 0.;
     }
 
@@ -101,8 +101,9 @@ void initUndistortRectifyMap( const Mat& _cameraMatrix, const Mat& _distCoeffs,
     double u0 = A(0, 2),  v0 = A(1, 2);
     double fx = A(0, 0),  fy = A(1, 1);
 
-    CV_Assert( distCoeffs.size() == Size(1, 4) || distCoeffs.size() == Size(1, 5) ||
-               distCoeffs.size() == Size(4, 1) || distCoeffs.size() == Size(5, 1));
+    CV_Assert( distCoeffs.size() == Size(1, 4) || distCoeffs.size() == Size(4, 1) || 
+               distCoeffs.size() == Size(1, 5) || distCoeffs.size() == Size(5, 1) ||
+               distCoeffs.size() == Size(1, 8) || distCoeffs.size() == Size(8, 1));
 
     if( distCoeffs.rows != 1 && !distCoeffs.isContinuous() )
         distCoeffs = distCoeffs.t();
@@ -111,7 +112,10 @@ void initUndistortRectifyMap( const Mat& _cameraMatrix, const Mat& _distCoeffs,
     double k2 = ((double*)distCoeffs.data)[1];
     double p1 = ((double*)distCoeffs.data)[2];
     double p2 = ((double*)distCoeffs.data)[3];
-    double k3 = distCoeffs.cols + distCoeffs.rows - 1 == 5 ? ((double*)distCoeffs.data)[4] : 0.;
+    double k3 = distCoeffs.cols + distCoeffs.rows - 1 >= 5 ? ((double*)distCoeffs.data)[4] : 0.;
+    double k4 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? ((double*)distCoeffs.data)[5] : 0.;
+    double k5 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? ((double*)distCoeffs.data)[6] : 0.;
+    double k6 = distCoeffs.cols + distCoeffs.rows - 1 >= 8 ? ((double*)distCoeffs.data)[7] : 0.;
 
     for( int i = 0; i < size.height; i++ )
     {
@@ -126,7 +130,7 @@ void initUndistortRectifyMap( const Mat& _cameraMatrix, const Mat& _distCoeffs,
             double w = 1./_w, x = _x*w, y = _y*w;
             double x2 = x*x, y2 = y*y;
             double r2 = x2 + y2, _2xy = 2*x*y;
-            double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2;
+            double kr = (1 + ((k3*r2 + k2)*r2 + k1)*r2)/(1 + ((k6*r2 + k5)*r2 + k4)*r2);
             double u = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + u0;
             double v = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + v0;
             if( m1type == CV_16SC2 )
@@ -248,7 +252,7 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
                    const CvMat* _distCoeffs,
                    const CvMat* matR, const CvMat* matP )
 {
-    double A[3][3], RR[3][3], k[5]={0,0,0,0,0}, fx, fy, ifx, ify, cx, cy;
+    double A[3][3], RR[3][3], k[8]={0,0,0,0,0,0,0,0}, fx, fy, ifx, ify, cx, cy;
     CvMat matA=cvMat(3, 3, CV_64F, A), _Dk;
     CvMat _RR=cvMat(3, 3, CV_64F, RR);
     const CvPoint2D32f* srcf;
@@ -276,7 +280,8 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
         CV_Assert( CV_IS_MAT(_distCoeffs) &&
             (_distCoeffs->rows == 1 || _distCoeffs->cols == 1) &&
             (_distCoeffs->rows*_distCoeffs->cols == 4 ||
-            _distCoeffs->rows*_distCoeffs->cols == 5) );
+             _distCoeffs->rows*_distCoeffs->cols == 5 ||
+             _distCoeffs->rows*_distCoeffs->cols == 8)); 
 
         _Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols,
             CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k);
@@ -341,7 +346,7 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
         for( j = 0; j < iters; j++ )
         {
             double r2 = x*x + y*y;
-            double icdist = 1./(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
+            double icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
             double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
             double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
             x = (x0 - deltaX)*icdist;
@@ -488,10 +493,13 @@ static Point2f invMapPointSpherical(Point2f _p, float alpha, int projType)
 }
 
     
-float initWideAngleProjMap( const Mat& cameraMatrix, const Mat& distCoeffs,
+float initWideAngleProjMap( const Mat& cameraMatrix0, const Mat& distCoeffs0,
                             Size imageSize, int destImageWidth, int m1type,
                             Mat& map1, Mat& map2, int projType, double _alpha )
 {
+    double k[8] = {0,0,0,0,0,0,0,0}, M[9]={0,0,0,0,0,0,0,0,0};
+    Mat distCoeffs(distCoeffs0.rows, distCoeffs0.cols, CV_MAKETYPE(CV_64F,distCoeffs0.channels()), k);
+    Mat cameraMatrix(3,3,CV_64F,M);
     Point2f scenter((float)cameraMatrix.at<double>(0,2), (float)cameraMatrix.at<double>(1,2));
     Point2f dcenter((destImageWidth-1)*0.5f, 0.f);
     float xmin = FLT_MAX, xmax = -FLT_MAX, ymin = FLT_MAX, ymax = -FLT_MAX;
@@ -500,6 +508,13 @@ float initWideAngleProjMap( const Mat& cameraMatrix, const Mat& distCoeffs,
     Mat _u(u), I = Mat::eye(3,3,CV_64F);
     float alpha = (float)_alpha;
     
+    int ndcoeffs = distCoeffs0.cols*distCoeffs0.rows*distCoeffs0.channels();
+    CV_Assert((distCoeffs0.cols == 1 || distCoeffs0.rows == 1) &&
+              (ndcoeffs == 4 || ndcoeffs == 5 || ndcoeffs == 8));
+    CV_Assert(cameraMatrix0.size() == Size(3,3));
+    distCoeffs0.convertTo(distCoeffs,CV_64F);
+    cameraMatrix0.convertTo(cameraMatrix,CV_64F);
+    
     alpha = std::min(alpha, 0.999f);
     
     for( int i = 0; i < N; i++ )
@@ -520,14 +535,8 @@ float initWideAngleProjMap( const Mat& cameraMatrix, const Mat& distCoeffs,
     dcenter.y = (dsize.height - 1)*0.5f;
     
     Mat mapxy(dsize, CV_32FC2);
-    double k1 = distCoeffs.at<double>(0,0),
-    k2 = distCoeffs.at<double>(1,0),
-    k3 = distCoeffs.at<double>(4,0),
-    p1 = distCoeffs.at<double>(2,0),
-    p2 = distCoeffs.at<double>(3,0);
-    double fx = cameraMatrix.at<double>(0,0),
-    fy = cameraMatrix.at<double>(1,1),
-    cx = scenter.x, cy = scenter.y;
+    double k1 = k[0], k2 = k[1], k3 = k[2], p1 = k[3], p2 = k[4], k4 = k[5], k5 = k[6], k6 = k[7];
+    double fx = cameraMatrix.at<double>(0,0), fy = cameraMatrix.at<double>(1,1), cx = scenter.x, cy = scenter.y;
     
     for( int y = 0; y < dsize.height; y++ )
     {
@@ -543,7 +552,7 @@ float initWideAngleProjMap( const Mat& cameraMatrix, const Mat& distCoeffs,
             }
             double x2 = q.x*q.x, y2 = q.y*q.y;
             double r2 = x2 + y2, _2xy = 2*q.x*q.y;
-            double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2;
+            double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2/(1 + ((k6*r2 + k5)*r2 + k4)*r2);
             double u = fx*(q.x*kr + p1*_2xy + p2*(r2 + 2*x2)) + cx;
             double v = fy*(q.y*kr + p1*(r2 + 2*y2) + p2*_2xy) + cy;
             

samples/cpp/calibration.cpp

@@ -82,7 +82,7 @@ static bool runCalibration( vector<vector<Point2f> > imagePoints,
     if( flags & CV_CALIB_FIX_ASPECT_RATIO )
         cameraMatrix.at<double>(0,0) = aspectRatio;
     
-    distCoeffs = Mat::zeros(5, 1, CV_64F);
+    distCoeffs = Mat::zeros(8, 1, CV_64F);
     
     vector<vector<Point3f> > objectPoints(1);
     calcChessboardCorners(boardSize, squareSize, objectPoints[0]);
@@ -90,7 +90,7 @@ static bool runCalibration( vector<vector<Point2f> > imagePoints,
 	objectPoints.resize(imagePoints.size(),objectPoints[0]);
     
     calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix,
-                    distCoeffs, rvecs, tvecs, flags);
+                    distCoeffs, rvecs, tvecs, flags|CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);///*|CV_CALIB_FIX_K3*/|CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);
     
     bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);
     
@@ -237,6 +237,7 @@ int main( int argc, char** argv )
     int flags = 0;
     VideoCapture capture;
     bool flipVertical = false;
+    bool showUndistorted = false;
     int delay = 1000;
     clock_t prevTimestamp = 0;
     int mode = DETECTION;
@@ -269,6 +270,7 @@ int main( int argc, char** argv )
             "     [-a <aspectRatio>]      # fix aspect ratio (fx/fy)\n"
             "     [-p]                     # fix the principal point at the center\n"
             "     [-v]                     # flip the captured images around the horizontal axis\n"
+            "     [-su]                    # show undistorted images after calibration\n"
             "     [input_data]             # input data, one of the following:\n"
             "                              #  - text file with a list of the images of the board\n"
             "                              #  - name of video file with a video of the board\n"
@@ -336,6 +338,10 @@ int main( int argc, char** argv )
         {
             outputFilename = argv[++i];
         }
+        else if( strcmp( s, "-su" ) == 0 )
+        {
+            showUndistorted = true;
+        }
         else if( s[0] != '-' )
         {
 			if( isdigit(s[0]) )
@@ -469,5 +475,27 @@ int main( int argc, char** argv )
                 break;
         }
     }
+    
+    if( !capture.isOpened() && showUndistorted )
+    {
+        Mat view, rview, map1, map2;
+        initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
+                                getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0),
+                                imageSize, CV_16SC2, map1, map2);
+        
+        for( i = 0; i < (int)imageList.size(); i++ )
+        {
+            view = imread(imageList[i], 1);
+            if(!view.data)
+                continue;
+            //undistort( view, rview, cameraMatrix, distCoeffs, cameraMatrix );
+            remap(view, rview, map1, map2, INTER_LINEAR);
+            imshow("Image View", rview);
+            int c = waitKey();
+            if( (c & 255) == 27 || c == 'q' || c == 'Q' )
+                break;
+        }
+    }
+                                                
     return 0;
 }