OpenCV Stereoscopic Vision detailed analysis (iii)---three-dimensional calibration and correction of source code

#pragma warning (disable:4996)/* *************** license:************************** Oct. 3, right to use thi

   s code in any the want without warrenty, support or any guarentee of it working. Book:it would is nice if you cited It:learning Opencv:computer Vision with the OpenCV Library by Gary Bradski A nd Adrian kaehler Published by O ' Reilly Media, October 3, AVAILABLE at:http://www.amazon.com/learning- opencv-computer-vision-library/dp/0596516134 or:http://oreilly.com/catalog/9780596516130/isbn-10:0596516134 o r:isbn-13:978-0596516130 other OPENCV SITES: * The source code was on SourceForge at:http://sourceforge.net/ projects/opencvlibrary/* The OpenCV wiki page (as of the OCT 1, the This is to changing over servers, but should co Me back): http://opencvlibrary.sourceforge.net/* A active user group is AT:HTTP://TECH.GROUPS.YAHOO.COM/GR oup/opencv/* The Minutes of weekly OpenCV development Meetings is AT:HTTP://PR.WILLOWGARAGE.COM/WIKI/OPENCV ************************************************** * * #i Nclude "opencv2/opencv.hpp" #include <iostream> #include <stdio.h> #include <stdarg.h> #include <

opencv/cv.h> #include <opencv/cvaux.h> #include <opencv/cxcore.h> #include <opencv/highgui.h>

using namespace Std; Given A list of chessboard images, the number of corners (NX, NY)//On the chessboards, and a flag:usecalibrated f or calibrated (0) or//uncalibrated (1:use cvstereocalibrate (), 2:compute fundamental//matrix separately) stereo.
Calibrate the cameras and display the//rectified results along with the computed disparity images.
    static void Stereocalib (const char* imageList, int nx, int ny, int useuncalibrated) {int displaycorners = 0;
    int showundistorted = 1; BOOL Isverticalstereo = FALSE;//OPENCV can handle left-right//or Up-down camera arr Angements const int maxscale = 1; const float squaresize = 1.F;
    Set this to your actual square size file* f = fopen (ImageList, "RT");
    int I, J, LR, nframes, n = nx*ny, n = 0;
    Vector<string> imagenames[2];
    Vector<cvpoint3d32f> objectpoints;
    Vector<cvpoint2d32f> points[2];
    Vector<int> npoints;
    Vector<uchar> active[2];
    Vector<cvpoint2d32f> temp (n);
    Cvsize imageSize = {0,0};
    ARRAY and VECTOR storage:double m1[3][3], m2[3][3], d1[5], d2[5];
    Double r[3][3], t[3], e[3][3], f[3][3];
    Cvmat _m1 = Cvmat (3, 3, cv_64f, M1);
    Cvmat _m2 = Cvmat (3, 3, cv_64f, M2);
    Cvmat _d1 = Cvmat (1, 5, cv_64f, D1);
    Cvmat _d2 = Cvmat (1, 5, cv_64f, D2);
    Cvmat _r = Cvmat (3, 3, cv_64f, R);
    Cvmat _t = Cvmat (3, 1, cv_64f, T);
    Cvmat _e = Cvmat (3, 3, cv_64f, E);
    Cvmat _f = Cvmat (3, 3, cv_64f, F);
if (displaycorners) Cvnamedwindow ("Corners", 1);
  READ in the LIST of chessboards:  if (!f) {fprintf (stderr, "Can not open file%s\n", imageList);
    Return } for (i=0;;
        i++) {char buf[1024];
        int count = 0, result=0;
        LR = i% 2;
        vector<cvpoint2d32f>& pts = POINTS[LR];
        if (!fgets (buf, sizeof (BUF) -3, F)) break;
        size_t len = strlen (BUF);
        while (len > 0 && isspace (buf[len-1])) Buf[--len] = ' + ';
        if (buf[0] = = ' # ') continue;
        iplimage* img = cvloadimage (buf, 0);
        if (!img) break;
        imageSize = Cvgetsize (IMG);
    Imagenames[lr].push_back (BUF); FIND chessboards and Corners therein:for (int s = 1; s <= maxscale; s++) {iplimage* ti
            MG = img;
                    if (S > 1) {timg = Cvcreateimage (Cvsize (img->width*s,img->height*s),
                Img->depth, Img->nchannels); Cvresize (IMG,Timg, cv_inter_cubic); }//Find corner result = Cvfindchessboardcorners (timg, Cvsize (NX, NY), &temp[0], &am
                P;count, Cv_calib_cb_adaptive_thresh |
            Cv_calib_cb_normalize_image);
            if (timg! = img) cvreleaseimage (&timg); if (Result | | s = = Maxscale) for (j = 0; J < Count; J + +) {temp[j].x/= s
                ;
            Temp[j].y/= S;
        } if (result) break;
            } if (displaycorners) {printf ("%s\n", buf);
            iplimage* cimg = Cvcreateimage (ImageSize, 8, 3);
            Cvcvtcolor (IMG, cimg, CV_GRAY2BGR);
            Cvdrawchessboardcorners (Cimg, Cvsize (NX, NY), &temp[0], count, result);
            Cvshowimage ("Corners", cimg);
            Cvreleaseimage (&AMP;CIMG); if (cvwaitkey (0) = =)//allow ESC tO Quit exit (-1);
        } else Putchar ('. ');
        N = Pts.size ();
        Pts.resize (n + N, cvpoint2d32f (0,0));
    Active[lr].push_back ((UCHAR) result);
        ASSERT (Result! = 0); if (result) {//calibration'll suffer without subpixel interpolation Cvfindcornersubpix (i MG, &temp[0], Count, Cvsize (one, one), Cvsize ( -1,-1), Cvtermcriteria (CV_TERMCRIT_ITER+CV
            _termcrit_eps, 30, 0.01));
        Copy (Temp.begin (), Temp.end (), Pts.begin () + N);
    } cvreleaseimage (&AMP;IMG);
    } fclose (f);
printf ("\ n"); HARVEST chessboard 3D OBJECT Point list:nframes = Active[0].size ();//number of good chessboads found Objectpoi
    Nts.resize (Nframes*n); for (i = 0; i < NY; i++) for (j = 0; J < NX; J + +) Objectpoints[i*nx + j] = cvpoint3d32f (i*
    Squaresize, j*squaresize, 0); for (i = 1; i < NframeS
    i++) Copy (Objectpoints.begin (), Objectpoints.begin () + N, objectpoints.begin () + i*n);
    Npoints.resize (Nframes,n);
    N = Nframes*n;
    Cvmat _objectpoints = Cvmat (1, N, CV_32FC3, &objectpoints[0]);
    Cvmat _imagepoints1 = Cvmat (1, N, CV_32FC2, &points[0][0]);
    Cvmat _imagepoints2 = Cvmat (1, N, CV_32FC2, &points[1][0]);
    Cvmat _npoints = Cvmat (1, Npoints.size (), cv_32s, &npoints[0]);
    Cvsetidentity (&AMP;_M1);
    Cvsetidentity (&AMP;_M2);
    Cvzero (&AMP;_D1);

Cvzero (&AMP;_D2);
    Calibrate the STEREO cameras printf ("Running STEREO calibration ...");
    Fflush (stdout); Three-dimensional calibration, return, internal parameters, external parameters, stereo parameters//parameter 1:n*3 matrix, which contains three-dimensional objects in the M image of each image of the physical coordinates of the K points, N=KXM//Parameters 2, 3: are Nx2 matrix, Storing the reference points of all objects provided by objectpoints in the left and right coordinate systems,//when using a chessboard to calibrate two cameras, the two parameters are called cvfindcheessboardconers () to find the return value of the left and right views//
    Parameter 4: The number of points on each image, the Mx1 matrix//parameter 5,7:3*3 the internal parameter of the camera matrix, output parameters.
    Parameter 6, 8: Distortion matrix for cameras 1 and 2. Parameter 9: Is the image size in pixels. It is only used to refine or calculate internal parameters//parameter 10: Output contact left and rightCamera rotation matrix//Parameter 11: Contact the left and right camera translation vector//parameter 12:e, output parameters, Eigen matrix//parameter 13:f, output parameters, base matrix//Parameter 14: Set the internal optimization criteria, the typical call is Cvtermcriteria (CV _termcrit_iter+ cv_termcrit_eps, 1e-5)//Parameter 15://Once you have the rotation or translation value (r,t) or the base matrix F of the function output, we can use these results to correct two stereoscopic images so that the polar line follows the image line
    	The same is true for the sweep lines of the two images that are aligned//and are transmitted in ancient. Cvstereocalibrate (&_objectpoints, &_imagepoints1, &_imagepoints2, &_npoints, &_M1, &A Mp;_d1, &_m2, &_d2, ImageSize, &_r, &_t, &_e, &_f, Cvtermcriteria (cv_termcrit_iter + Cv_termcrit_eps, 1e-5), Cv_calib_fix_aspect_ratio + cv_calib_zero_tangent_dist + CV
    _calib_same_focal_length);
printf ("done\n"); Calibration quality CHECK//Because the output fundamental matrix implicitly//Includes all the output information,/ /We can check the quality of calibration using the//epipolar geometry constraint:m2^t*f*m1=0 vector<cvpoint3d32
    F> lines[2];
    Points[0].resize (N); Points[1].resize (N);
    _imagepoints1 = Cvmat (1, N, CV_32FC2, &points[0][0]);
    _imagepoints2 = Cvmat (1, N, CV_32FC2, &points[1][0]);
    Lines[0].resize (N);
    Lines[1].resize (N);
    Polar line: Nx3 's floating-point matrix, polar-line expression: ax+by+c=0 cvmat _l1 = Cvmat (1, N, CV_32FC3, &lines[0][0]);
Cvmat _l2 = Cvmat (1, N, CV_32FC3, &lines[1][0]);
    Always working in undistorted space//To the original point to do the distortion processing//input parameters for a series of 2D points.
    Cvundistortpoints (&_imagepoints1, &_imagepoints1, &_m1, &_d1, 0, &AMP;_M1);
    Cvundistortpoints (&_imagepoints2, &_imagepoints2, &_m2, &_d2, 0, &_m2); Calculate Polar Line//According to the Point column in an image, calculate its corresponding polar line in another image, each point corresponds to a polar line cvcomputecorrespondepilines (&_imagepoints1, 1, &_f, &
    _L1);
    Cvcomputecorrespondepilines (&_imagepoints2, 2, &_f, &AMP;_L2);
    Double avgerr = 0; for (i = 0; i < N; i++) {Double err = fabs (points[0][i].x*lines[1][i].x + points[0][i].y*line S[1][i].y + lines[1][i].Z) + fabs (points[1][i].x*lines[0][i].x + points[1][i].y*lines[0][i].y + lines[0][i].z);
    Avgerr + = err;
} printf ("Avg err =%g\n", avgerr/(Nframes*n)); COMPUTE and DISPLAY rectification//int showundistorted = 1;  Displays the image after distortion if (showundistorted) {cvmat* mx1 = Cvcreatemat (Imagesize.height, ImageSize.Width,
        CV_32F);
        cvmat* my1 = Cvcreatemat (Imagesize.height, ImageSize.Width, cv_32f);
        cvmat* MX2 = Cvcreatemat (Imagesize.height, ImageSize.Width, cv_32f);
        cvmat* my2 = Cvcreatemat (Imagesize.height, ImageSize.Width, cv_32f);
        cvmat* img1r = Cvcreatemat (Imagesize.height, ImageSize.Width, cv_8u);
        cvmat* IMG2R = Cvcreatemat (Imagesize.height, ImageSize.Width, cv_8u);
        Cvmat* disp = Cvcreatemat (Imagesize.height, ImageSize.Width, cv_16s);
cvmat* Vdisp = Cvcreatemat (Imagesize.height,            ImageSize.Width, cv_8u);
        cvmat* pair;
        Double r1[3][3], r2[3][3], p1[3][4], p2[3][4];
        3*3 matrix: Corrected rotation matrix Cvmat _r1 = Cvmat (3, 3, cv_64f, R1) of the alignment between the left and right camera planes returned by Cvsterocalibrate;
        Cvmat _r2 = Cvmat (3, 3, cv_64f, R2);  If calibration stereo correction is used: Bouguet algorithm stereocalib ("Ch12_list.txt", 9, 6, 1); The last parameter//if by calibrated (Bouguet ' S METHOD) IF (useuncalibrated = = 0) {Cvmat _p1 = Cvmat (3
            , 4, cv_64f, P1);
            Cvmat _p2 = Cvmat (3, 4, cv_64f, P2); Stereo correction//Cvstereorectify (&AMP;_M1, &_m2, &_d1, &_d2, ImageSize, &amp
            ; _r, &_t, &_r1, &AMP;_R2, &AMP;_P1, &AMP;_P2, 0, 0/*cv_calib_zero_disparity*/);
    Isverticalstereo = Fabs (p2[1][3]) > Fabs (p2[0][3]); Precompute maps for Cvremap ()//Calculate Correction Lookup mapping table for left/Right view//Cvinitundistortrectifymap (&AMP;_M1 , &AMP;_D1,&AMP;_R1,&AMP;_P1,MX1,MY1);
        Cvinitundistortrectifymap (&AMP;_M2,&AMP;_D2,&AMP;_R2,&AMP;_P2,MX2,MY2);  }//or Else HARTLEY ' S method//If using non-calibrated stereo correction algorithm: HARTLEY algorithm else if (useuncalibrated = = 1 | | useuncalibrated = 2)//Use intrinsic parameters of all camera, but//compute the rectification transformation directly/
            /From the fundamental matrix {double h1[3][3], h2[3][3], im[3][3];
            Cvmat _h1 = Cvmat (3, 3, cv_64f, H1);
            Cvmat _h2 = Cvmat (3, 3, cv_64f, H2);
    Cvmat _im = Cvmat (3, 3, cv_64f, IM);
                Just to show could has independently used F if (useuncalibrated = = 2)//COMPUTE base matrix
            Cvfindfundamentalmat (&_imagepoints1, &_imagepoints2, &_f);
                Non-calibrated stereo correction cvstereorectifyuncalibrated (&_imagepoints1, &_imagepoints2, &_f,
     ImageSize, &_h1, &AMP;_H2, 3);       Cvinvert (&AMP;_M1, &_im);
            Cvmatmul (&AMP;_H1, &AMP;_M1, &AMP;_R1);
            Cvmatmul (&_im, &AMP;_R1, &AMP;_R1);
            Cvinvert (&_m2, &_im);
            Cvmatmul (&AMP;_H2, &_m2, &AMP;_R2);
    Cvmatmul (&_im, &AMP;_R2, &AMP;_R2); Precompute map for Cvremap ()//calculate correction for left and right view lookup mapping table Cvinitundistortrectifymap (&_m1,&_d1,&_

            R1,&AMP;_M1,MX1,MY1);
        Cvinitundistortrectifymap (&AMP;_M2,&AMP;_D1,&AMP;_R2,&AMP;_M2,MX2,MY2);
        } else assert (0);
Cvnamedwindow ("rectified", 1); Rectify the IMAGES and FIND disparity MAPS if (!isverticalstereo) pair = Cvcreatemat (imagesize.he
        ight, imagesize.width*2, CV_8UC3);
else pair = Cvcreatemat (imagesize.height*2, ImageSize.Width, CV_8UC3); Setup for finding stereo corrrespondences//Initialize block match status (internal allocations and parameters) Cvstereobmstate *bmstate = CvcreatestEreobmstate ();
        ASSERT (Bmstate! = 0);
        bmstate->prefiltersize=41;
        bmstate->prefiltercap=31;
        bmstate->sadwindowsize=41;
        bmstate->mindisparity=-64;
        bmstate->numberofdisparities=128;
        bmstate->texturethreshold=10;
        bmstate->uniquenessratio=15;
            for (i = 0; i < nframes; i++) {iplimage* img1=cvloadimage (imagenames[0][i].c_str (), 0);
            iplimage* Img2=cvloadimage (imagenames[1][i].c_str (), 0);
                if (img1 && img2) {Cvmat part;
                Cvremap (IMG1, img1r, MX1, my1);
                Cvremap (Img2, IMG2R, MX2, my2); if (!isverticalstereo | | useuncalibrated! = 0) {//When the stereo camera is oriented vert ically,//useuncalibrated==0 does not transpose the//image, so the epipolar lines in the RE Ctified//images is vertical. SteREO Correspondence//function does not support such a case.
                	Calculate parallax//parameter 1: Corrected left image//Parameter 2: Corrected right image//Parameter 3: Output parameter, parallax mapping, and input image size of single channel image.
                	Parameter 4:cvstereobmstate *bmstate = Cvcreatestereobmstate ();
                    CVFINDSTEREOCORRESPONDENCEBM (IMG1R, IMG2R, disp, bmstate);
                    Cvnormalize (disp, vdisp, 0, Cv_minmax);
                    Cvnamedwindow ("disparity");
                Cvshowimage ("disparity", vdisp); } if (!isverticalstereo) {cvgetcols (pair, &part, 0, IMAGESIZE.W
                    Idth);
                    Cvcvtcolor (IMG1R, &part, CV_GRAY2BGR);
                    Cvgetcols (pair, &part, ImageSize.Width, imagesize.width*2);
                    Cvcvtcolor (IMG2R, &part, CV_GRAY2BGR); for (j = 0; J < Imagesize.height;
                        J + = +) Cvline (pair, Cvpoint (0,j), Cvpoint (IMAGESIZE.WIDTH*2,J),
                Cv_rgb (0,255,0));
                    } else {cvgetrows (pair, &part, 0, imagesize.height);
                    Cvcvtcolor (IMG1R, &part, CV_GRAY2BGR);
                    Cvgetrows (pair, &part, Imagesize.height, imagesize.height*2);
                    Cvcvtcolor (IMG2R, &part, CV_GRAY2BGR);
                        for (j = 0; J < imagesize.width; j + =) Cvline (pair, Cvpoint (j,0),
                Cvpoint (j,imagesize.height*2), Cv_rgb (0,255,0));
                } cvshowimage ("Rectified", pair);
            if (cvwaitkey () = =) break;
            } cvreleaseimage (&AMP;IMG1);
Cvreleaseimage (&AMP;IMG2);        } cvreleasestereobmstate (&bmstate);
        Cvreleasemat (&AMP;MX1);
        Cvreleasemat (&AMP;MY1);
        Cvreleasemat (&AMP;MX2);
        Cvreleasemat (&AMP;MY2);
        Cvreleasemat (&AMP;IMG1R);
        Cvreleasemat (&AMP;IMG2R);
    Cvreleasemat (&AMP;DISP);
    }} int main (void) {Stereocalib ("Ch12_list.txt", 9, 6, 1);
return 0;
 }