|
| 1 | +//Example 19 - 1. Bird’s - eye view |
| 2 | +#include <opencv2/opencv.hpp> |
| 3 | +#include <iostream> |
| 4 | + using namespace std; |
| 5 | + |
| 6 | +void help(char *argv[]) { |
| 7 | + cout << "\nExample 19-01, using homography to get a bird's eye view." |
| 8 | + << "\nThis file relies on you having created an intrinsic file via example_18-01_from_disk" |
| 9 | + << "\n but here, that file is already stored in ../birdseye/intrinsics.xml" |
| 10 | + << "\nCall:" |
| 11 | + << "\n./example_19-01 <chessboard_width> <chessboard_height> <path/camera_calib_filename> <path/chessboard_image>" |
| 12 | + << "\n\nExample:" |
| 13 | + << "\n./example_19-01 0 0 ../birdseye/intrinsics.xml ../birdseye\n" |
| 14 | + << "\nPress 'd' for lower birdseye view, and 'u' for higher (it adjusts the apparent 'Z' height), Esc to exit\n" |
| 15 | + << endl; |
| 16 | +} |
| 17 | + |
| 18 | +// args: [board_w] [board_h] [intrinsics.xml] [checker_image] |
| 19 | +// |
| 20 | +int main(int argc, char *argv[]) { |
| 21 | + if (argc != 5) { |
| 22 | + cout << "\nERROR: too few parameters\n"; |
| 23 | + help(argv); |
| 24 | + return -1; |
| 25 | + } |
| 26 | + // Input Parameters: |
| 27 | + // |
| 28 | + int board_w = atoi(argv[1]); |
| 29 | + int board_h = atoi(argv[2]); |
| 30 | + int board_n = board_w * board_h; |
| 31 | + cv::Size board_sz(board_w, board_h); |
| 32 | + cv::FileStorage fs(argv[3], cv::FileStorage::READ); |
| 33 | + cv::Mat intrinsic, distortion; |
| 34 | + |
| 35 | + fs["camera_matrix"] >> intrinsic; |
| 36 | + fs["distortion_coefficients"] >> distortion; |
| 37 | + |
| 38 | + if (!fs.isOpened() || intrinsic.empty() || distortion.empty()) { |
| 39 | + cout << "Error: Couldn't load intrinsic parameters from " << argv[3] |
| 40 | + << endl; |
| 41 | + return -1; |
| 42 | + } |
| 43 | + fs.release(); |
| 44 | + |
| 45 | + cv::Mat gray_image, image, image0 = cv::imread(argv[4], 1); |
| 46 | + if (image0.empty()) { |
| 47 | + cout << "Error: Couldn't load image " << argv[4] << endl; |
| 48 | + return -1; |
| 49 | + } |
| 50 | + |
| 51 | + // UNDISTORT OUR IMAGE |
| 52 | + // |
| 53 | + cv::undistort(image0, image, intrinsic, distortion, intrinsic); |
| 54 | + cv::cvtColor(image, gray_image, cv::COLOR_BGRA2GRAY); |
| 55 | + |
| 56 | + // GET THE CHECKERBOARD ON THE PLANE |
| 57 | + // |
| 58 | + vector<cv::Point2f> corners; |
| 59 | + bool found = cv::findChessboardCorners( // True if found |
| 60 | + image, // Input image |
| 61 | + board_sz, // Pattern size |
| 62 | + corners, // Results |
| 63 | + cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FILTER_QUADS); |
| 64 | + if (!found) { |
| 65 | + cout << "Couldn't acquire checkerboard on " << argv[4] << ", only found " |
| 66 | + << corners.size() << " of " << board_n << " corners\n"; |
| 67 | + return -1; |
| 68 | + } |
| 69 | + |
| 70 | + // Get Subpixel accuracy on those corners |
| 71 | + // |
| 72 | + cv::cornerSubPix( |
| 73 | + gray_image, // Input image |
| 74 | + corners, // Initial guesses, also output |
| 75 | + cv::Size(11, 11), // Search window size |
| 76 | + cv::Size(-1, -1), // Zero zone (in this case, don't use) |
| 77 | + cv::TermCriteria(cv::TermCriteria::EPS | cv::TermCriteria::COUNT, 30, |
| 78 | + 0.1)); |
| 79 | + |
| 80 | + // GET THE IMAGE AND OBJECT POINTS: |
| 81 | + // Object points are at (r,c): |
| 82 | + // (0,0), (board_w-1,0), (0,board_h-1), (board_w-1,board_h-1) |
| 83 | + // That means corners are at: corners[r*board_w + c] |
| 84 | + // |
| 85 | + cv::Point2f objPts[4], imgPts[4]; |
| 86 | + objPts[0].x = 0; |
| 87 | + objPts[0].y = 0; |
| 88 | + objPts[1].x = board_w - 1; |
| 89 | + objPts[1].y = 0; |
| 90 | + objPts[2].x = 0; |
| 91 | + objPts[2].y = board_h - 1; |
| 92 | + objPts[3].x = board_w - 1; |
| 93 | + objPts[3].y = board_h - 1; |
| 94 | + imgPts[0] = corners[0]; |
| 95 | + imgPts[1] = corners[board_w - 1]; |
| 96 | + imgPts[2] = corners[(board_h - 1) * board_w]; |
| 97 | + imgPts[3] = corners[(board_h - 1) * board_w + board_w - 1]; |
| 98 | + |
| 99 | + // DRAW THE POINTS in order: B,G,R,YELLOW |
| 100 | + // |
| 101 | + cv::circle(image, imgPts[0], 9, cv::Scalar(255, 0, 0), 3); |
| 102 | + cv::circle(image, imgPts[1], 9, cv::Scalar(0, 255, 0), 3); |
| 103 | + cv::circle(image, imgPts[2], 9, cv::Scalar(0, 0, 255), 3); |
| 104 | + cv::circle(image, imgPts[3], 9, cv::Scalar(0, 255, 255), 3); |
| 105 | + |
| 106 | + // DRAW THE FOUND CHECKERBOARD |
| 107 | + // |
| 108 | + cv::drawChessboardCorners(image, board_sz, corners, found); |
| 109 | + cv::imshow("Checkers", image); |
| 110 | + |
| 111 | + // FIND THE HOMOGRAPHY |
| 112 | + // |
| 113 | + cv::Mat H = cv::getPerspectiveTransform(objPts, imgPts); |
| 114 | + |
| 115 | + // LET THE USER ADJUST THE Z HEIGHT OF THE VIEW |
| 116 | + // |
| 117 | + cout << "\nPress 'd' for lower birdseye view, and 'u' for higher (it adjusts the apparent 'Z' height), Esc to exit" << endl; |
| 118 | + double Z = 25; |
| 119 | + cv::Mat birds_image; |
| 120 | + for (;;) { |
| 121 | + // escape key stops |
| 122 | + H.at<double>(2, 2) = Z; |
| 123 | + // USE HOMOGRAPHY TO REMAP THE VIEW |
| 124 | + // |
| 125 | + cv::warpPerspective(image, // Source image |
| 126 | + birds_image, // Output image |
| 127 | + H, // Transformation matrix |
| 128 | + image.size(), // Size for output image |
| 129 | + cv::WARP_INVERSE_MAP | cv::INTER_LINEAR, |
| 130 | + cv::BORDER_CONSTANT, cv::Scalar::all(0) // Fill border with black |
| 131 | + ); |
| 132 | + cv::imshow("Birds_Eye", birds_image); |
| 133 | + int key = cv::waitKey() & 255; |
| 134 | + if (key == 'u') |
| 135 | + Z += 0.5; |
| 136 | + if (key == 'd') |
| 137 | + Z -= 0.5; |
| 138 | + if (key == 27) |
| 139 | + break; |
| 140 | + } |
| 141 | + |
| 142 | + // SHOW ROTATION AND TRANSLATION VECTORS |
| 143 | + // |
| 144 | + vector<cv::Point2f> image_points; |
| 145 | + vector<cv::Point3f> object_points; |
| 146 | + for (int i = 0; i < 4; ++i) { |
| 147 | + image_points.push_back(imgPts[i]); |
| 148 | + object_points.push_back(cv::Point3f(objPts[i].x, objPts[i].y, 0)); |
| 149 | + } |
| 150 | + cv::Mat rvec, tvec, rmat; |
| 151 | + cv::solvePnP(object_points, // 3-d points in object coordinate |
| 152 | + image_points, // 2-d points in image coordinates |
| 153 | + intrinsic, // Our camera matrix |
| 154 | + cv::Mat(), // Since we corrected distortion in the |
| 155 | + // beginning,now we have zero distortion |
| 156 | + // coefficients |
| 157 | + rvec, // Output rotation *vector*. |
| 158 | + tvec // Output translation vector. |
| 159 | + ); |
| 160 | + cv::Rodrigues(rvec, rmat); |
| 161 | + |
| 162 | + // PRINT AND EXIT |
| 163 | + cout << "rotation matrix: " << rmat << endl; |
| 164 | + cout << "translation vector: " << tvec << endl; |
| 165 | + cout << "homography matrix: " << H << endl; |
| 166 | + cout << "inverted homography matrix: " << H.inv() << endl; |
| 167 | + |
| 168 | + return 1; |
| 169 | +} |
0 commit comments