diff --git a/CMakeLists.txt b/CMakeLists.txt index 62d12ea..832b434 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,7 @@ add_executable( example_18-01 example_18-01.cpp ) add_executable( example_18-01_from_disk example_18-01_from_disk.cpp ) add_executable( example_19-01 example_19-01.cpp ) add_executable( example_19-02 example_19-02.cpp ) - +add_executable( example_19-03 example_19-03.cpp ) add_executable( example_19-04 example_19-04.cpp ) add_executable( example_20-01 example_20-01.cpp ) add_executable( example_20-02 example_20-02.cpp ) @@ -130,7 +130,7 @@ target_link_libraries( example_18-01 ${OpenCV_LIBS} ) target_link_libraries( example_18-01_from_disk ${OpenCV_LIBS} ) target_link_libraries( example_19-01 ${OpenCV_LIBS} ) target_link_libraries( example_19-02 ${OpenCV_LIBS} ) - +target_link_libraries( example_19-03 ${OpenCV_LIBS} ) target_link_libraries( example_19-04 ${OpenCV_LIBS} ) target_link_libraries( example_20-01 ${OpenCV_LIBS} ) target_link_libraries( example_20-02 ${OpenCV_LIBS} ) diff --git a/example_19-03.cpp b/example_19-03.cpp new file mode 100644 index 0000000..eca492d --- /dev/null +++ b/example_19-03.cpp @@ -0,0 +1,305 @@ +// Example 19-3. Stereo calibration, rectification, and correspondence +#pragma warning(disable : 4996) +#include +#include +#include +#include +#include +#include + +using namespace std; + +void help(char *argv[]) { + cout + << "\n\nExample 19-3. Stereo calibration, rectification, and " + "correspondence" + << "\n Reads in list of locations of a sequence of checkerboard " + "calibration" + << "\n objects from a left,right stereo camera pair. Calibrates, " + "rectifies and then" + << "\n does stereo correspondence." + << "\n" + << "\n This program will run on default parameters assuming you " + "created a build directory" + << "\n directly below the Learning-OpenCV-3 directory and are " + "running programs there. NOTE: the list_of_stereo_pairs> must" + << "\n give the full path name to the left right images, in " + "alternating" + << "\n lines: left image, right image, one path/filename per line, see" + << "\n stereoData/example_19-03_list.txt file, you can comment out " + "lines" + << "\n there by starting them with #." + << "\n" + << "\nDefault Call (with parameters: board_w = 9, board_h = 6, list = " + "../stereoData_19-03_list.txt):" + << "\n" << argv[0] << "\n" + << "\nManual call:" + << "\n" << argv[0] << " [ ]" + << "\n\n PRESS ANY KEY TO STEP THROUGH RESULTS AT EACH STAGE." + << "\n" << endl; +} + +static void StereoCalib(const char *imageList, int nx, int ny, + bool useUncalibrated) { + bool displayCorners = true; + bool showUndistorted = true; + bool isVerticalStereo = false; // horiz or vert cams + const int maxScale = 1; + const float squareSize = 1.f; + + // actual square size + FILE *f = fopen(imageList, "rt"); + int i, j, lr; + int N = nx * ny; + cv::Size board_sz = cv::Size(nx, ny); + vector imageNames[2]; + vector boardModel; + vector > objectPoints; + vector > points[2]; + vector corners[2]; + bool found[2] = {false, false}; + cv::Size imageSize; + + // READ IN THE LIST OF CIRCLE GRIDS: + // + if (!f) { + cout << "Cannot open file " << imageList << endl; + return; + } + for (i = 0; i < ny; i++) + for (j = 0; j < nx; j++) + boardModel.push_back( + cv::Point3f((float)(i * squareSize), (float)(j * squareSize), 0.f)); + i = 0; + for (;;) { + char buf[1024]; + lr = i % 2; + if (lr == 0) + found[0] = found[1] = false; + if (!fgets(buf, sizeof(buf) - 3, f)) + break; + size_t len = strlen(buf); + while (len > 0 && isspace(buf[len - 1])) + buf[--len] = '\0'; + if (buf[0] == '#') + continue; + cv::Mat img = cv::imread(buf, 0); + if (img.empty()) + break; + imageSize = img.size(); + imageNames[lr].push_back(buf); + i++; + + // If we did not find board on the left image, + // it does not make sense to find it on the right. + // + if (lr == 1 && !found[0]) + continue; + + // Find circle grids and centers therein: + for (int s = 1; s <= maxScale; s++) { + cv::Mat timg = img; + if (s > 1) + resize(img, timg, cv::Size(), s, s, cv::INTER_CUBIC); + // Just as example, this would be the call if you had circle calibration + // boards ... + // found[lr] = cv::findCirclesGrid(timg, cv::Size(nx, ny), + // corners[lr], + // cv::CALIB_CB_ASYMMETRIC_GRID | + // cv::CALIB_CB_CLUSTERING); + //...but we have chessboards in our images + found[lr] = cv::findChessboardCorners(timg, board_sz, corners[lr]); + + if (found[lr] || s == maxScale) { + cv::Mat mcorners(corners[lr]); + mcorners *= (1. / s); + } + if (found[lr]) + break; + } + if (displayCorners) { + cout << buf << endl; + cv::Mat cimg; + cv::cvtColor(img, cimg, cv::COLOR_GRAY2BGR); + + // draw chessboard corners works for circle grids too + cv::drawChessboardCorners(cimg, cv::Size(nx, ny), corners[lr], found[lr]); + cv::imshow("Corners", cimg); + if ((cv::waitKey(0) & 255) == 27) // Allow ESC to quit + exit(-1); + } else + cout << '.'; + if (lr == 1 && found[0] && found[1]) { + objectPoints.push_back(boardModel); + points[0].push_back(corners[0]); + points[1].push_back(corners[1]); + } + } + fclose(f); + + // CALIBRATE THE STEREO CAMERAS + cv::Mat M1 = cv::Mat::eye(3, 3, CV_64F); + cv::Mat M2 = cv::Mat::eye(3, 3, CV_64F); + cv::Mat D1, D2, R, T, E, F; + cout << "\nRunning stereo calibration ...\n"; + cv::stereoCalibrate( + objectPoints, points[0], points[1], M1, D1, M2, D2, imageSize, R, T, E, F, + cv::CALIB_FIX_ASPECT_RATIO | cv::CALIB_ZERO_TANGENT_DIST | + cv::CALIB_SAME_FOCAL_LENGTH, + cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 100, + 1e-5)); + cout << "Done! Press any key to step through images, ESC to exit\n\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 lines[2]; + double avgErr = 0; + int nframes = (int)objectPoints.size(); + for (i = 0; i < nframes; i++) { + vector &pt0 = points[0][i]; + vector &pt1 = points[1][i]; + cv::undistortPoints(pt0, pt0, M1, D1, cv::Mat(), M1); + cv::undistortPoints(pt1, pt1, M2, D2, cv::Mat(), M2); + cv::computeCorrespondEpilines(pt0, 1, F, lines[0]); + cv::computeCorrespondEpilines(pt1, 2, F, lines[1]); + + for (j = 0; j < N; j++) { + double err = fabs(pt0[j].x * lines[1][j].x + pt0[j].y * lines[1][j].y + + lines[1][j].z) + + fabs(pt1[j].x * lines[0][j].x + pt1[j].y * lines[0][j].y + + lines[0][j].z); + avgErr += err; + } + } + cout << "avg err = " << avgErr / (nframes * N) << endl; + + // COMPUTE AND DISPLAY RECTIFICATION + // + if (showUndistorted) { + cv::Mat R1, R2, P1, P2, map11, map12, map21, map22; + + // IF BY CALIBRATED (BOUGUET'S METHOD) + // + if (!useUncalibrated) { + stereoRectify(M1, D1, M2, D2, imageSize, R, T, R1, R2, P1, P2, + cv::noArray(), 0); + isVerticalStereo = fabs(P2.at(1, 3)) > fabs(P2.at(0, 3)); + // Precompute maps for cvRemap() + initUndistortRectifyMap(M1, D1, R1, P1, imageSize, CV_16SC2, map11, + map12); + initUndistortRectifyMap(M2, D2, R2, P2, imageSize, CV_16SC2, map21, + map22); + } + + // OR ELSE HARTLEY'S METHOD + // + else { + + // use intrinsic parameters of each camera, but + // compute the rectification transformation directly + // from the fundamental matrix + vector allpoints[2]; + for (i = 0; i < nframes; i++) { + copy(points[0][i].begin(), points[0][i].end(), + back_inserter(allpoints[0])); + copy(points[1][i].begin(), points[1][i].end(), + back_inserter(allpoints[1])); + } + cv::Mat F = findFundamentalMat(allpoints[0], allpoints[1], cv::FM_8POINT); + cv::Mat H1, H2; + cv::stereoRectifyUncalibrated(allpoints[0], allpoints[1], F, imageSize, + H1, H2, 3); + R1 = M1.inv() * H1 * M1; + R2 = M2.inv() * H2 * M2; + + // Precompute map for cvRemap() + // + cv::initUndistortRectifyMap(M1, D1, R1, P1, imageSize, CV_16SC2, map11, + map12); + cv::initUndistortRectifyMap(M2, D2, R2, P2, imageSize, CV_16SC2, map21, + map22); + } + + // RECTIFY THE IMAGES AND FIND DISPARITY MAPS + // + cv::Mat pair; + if (!isVerticalStereo) + pair.create(imageSize.height, imageSize.width * 2, CV_8UC3); + else + pair.create(imageSize.height * 2, imageSize.width, CV_8UC3); + + // Setup for finding stereo corrrespondences + // + cv::Ptr stereo = cv::StereoSGBM::create( + -64, 128, 11, 100, 1000, 32, 0, 15, 1000, 16, cv::StereoSGBM::MODE_HH); + + for (i = 0; i < nframes; i++) { + cv::Mat img1 = cv::imread(imageNames[0][i].c_str(), 0); + cv::Mat img2 = cv::imread(imageNames[1][i].c_str(), 0); + cv::Mat img1r, img2r, disp, vdisp; + if (img1.empty() || img2.empty()) + continue; + cv::remap(img1, img1r, map11, map12, cv::INTER_LINEAR); + cv::remap(img2, img2r, map21, map22, cv::INTER_LINEAR); + if (!isVerticalStereo || !useUncalibrated) { + + // When the stereo camera is oriented vertically, + // Hartley method does not transpose the + // image, so the epipolar lines in the rectified + // images are vertical. Stereo correspondence + // function does not support such a case. + stereo->compute(img1r, img2r, disp); + cv::normalize(disp, vdisp, 0, 256, cv::NORM_MINMAX, CV_8U); + cv::imshow("disparity", vdisp); + } + if (!isVerticalStereo) { + cv::Mat part = pair.colRange(0, imageSize.width); + cvtColor(img1r, part, cv::COLOR_GRAY2BGR); + part = pair.colRange(imageSize.width, imageSize.width * 2); + cvtColor(img2r, part, cv::COLOR_GRAY2BGR); + for (j = 0; j < imageSize.height; j += 16) + cv::line(pair, cv::Point(0, j), cv::Point(imageSize.width * 2, j), + cv::Scalar(0, 255, 0)); + } else { + cv::Mat part = pair.rowRange(0, imageSize.height); + cv::cvtColor(img1r, part, cv::COLOR_GRAY2BGR); + part = pair.rowRange(imageSize.height, imageSize.height * 2); + cv::cvtColor(img2r, part, cv::COLOR_GRAY2BGR); + for (j = 0; j < imageSize.width; j += 16) + line(pair, cv::Point(j, 0), cv::Point(j, imageSize.height * 2), + cv::Scalar(0, 255, 0)); + } + cv::imshow("rectified", pair); + if ((cv::waitKey() & 255) == 27) + break; + } + } +} + +// +//Default Call (with parameters: board_w = 9, board_h = 6, list = +// ../stereoData_19-03_list.txt): +//./example_19-03 +// +//Manual call: +//./example_19-03 [ ] +// +// Press any key to step through results, ESC to exit +// + + +int main(int argc, char **argv) { + help(argv); + int board_w = 9, board_h = 6; + const char *board_list = "../stereoData/example_19-03_list.txt"; + if (argc == 4) { + board_list = argv[1]; + board_w = atoi(argv[2]); + board_h = atoi(argv[3]); + } + StereoCalib(board_list, board_w, board_h, true); + return 0; +} diff --git a/stereoData/example_19-03_list.txt b/stereoData/example_19-03_list.txt new file mode 100644 index 0000000..d067eda --- /dev/null +++ b/stereoData/example_19-03_list.txt @@ -0,0 +1,28 @@ +../stereoData/left01.jpg +../stereoData/right01.jpg +../stereoData/left02.jpg +../stereoData/right02.jpg +../stereoData/left03.jpg +../stereoData/right03.jpg +../stereoData/left04.jpg +../stereoData/right04.jpg +../stereoData/left05.jpg +../stereoData/right05.jpg +../stereoData/left06.jpg +../stereoData/right06.jpg +../stereoData/left07.jpg +../stereoData/right07.jpg +../stereoData/left08.jpg +../stereoData/right08.jpg +../stereoData/left09.jpg +../stereoData/right09.jpg +#../stereoData/left10.jpg +#../stereoData/right10.jpg +../stereoData/left11.jpg +../stereoData/right11.jpg +../stereoData/left12.jpg +../stereoData/right12.jpg +../stereoData/left13.jpg +../stereoData/right13.jpg +../stereoData/left14.jpg +../stereoData/right14.jpg