samples/cpp/tutorial_code/features/Homography/pose_from_homography.cpp#

An example program about pose estimation from coplanar points Check the corresponding tutorial for more details

  1#include <iostream>
  2#include <opencv2/core.hpp>
  3#include <opencv2/imgproc.hpp>
  4#include <opencv2/geometry.hpp>
  5#include <opencv2/calib.hpp>
  6#include <opencv2/objdetect.hpp>
  7#include <opencv2/highgui.hpp>
  8
  9using namespace std;
 10using namespace cv;
 11
 12namespace
 13{
 14enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
 15
 16void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners, Pattern patternType = CHESSBOARD)
 17{
 18    corners.resize(0);
 19
 20    switch (patternType)
 21    {
 22    case CHESSBOARD:
 23    case CIRCLES_GRID:
 24        //! [compute-chessboard-object-points]
 25        for( int i = 0; i < boardSize.height; i++ )
 26            for( int j = 0; j < boardSize.width; j++ )
 27                corners.push_back(Point3f(float(j*squareSize),
 28                                          float(i*squareSize), 0));
 29        //! [compute-chessboard-object-points]
 30        break;
 31
 32    case ASYMMETRIC_CIRCLES_GRID:
 33        for( int i = 0; i < boardSize.height; i++ )
 34            for( int j = 0; j < boardSize.width; j++ )
 35                corners.push_back(Point3f(float((2*j + i % 2)*squareSize),
 36                                          float(i*squareSize), 0));
 37        break;
 38
 39    default:
 40        CV_Error(Error::StsBadArg, "Unknown pattern type\n");
 41    }
 42}
 43
 44void poseEstimationFromCoplanarPoints(const string &imgPath, const string &intrinsicsPath, const Size &patternSize,
 45                                             const float squareSize)
 46{
 47    Mat img = imread( samples::findFile( imgPath) );
 48    Mat img_corners = img.clone(), img_pose = img.clone();
 49
 50    //! [find-chessboard-corners]
 51    vector<Point2f> corners;
 52    bool found = findChessboardCorners(img, patternSize, corners);
 53    //! [find-chessboard-corners]
 54
 55    if (!found)
 56    {
 57        cout << "Cannot find chessboard corners." << endl;
 58        return;
 59    }
 60    drawChessboardCorners(img_corners, patternSize, corners, found);
 61    imshow("Chessboard corners detection", img_corners);
 62
 63    //! [compute-object-points]
 64    vector<Point3f> objectPoints;
 65    calcChessboardCorners(patternSize, squareSize, objectPoints);
 66    vector<Point2f> objectPointsPlanar;
 67    for (size_t i = 0; i < objectPoints.size(); i++)
 68    {
 69        objectPointsPlanar.push_back(Point2f(objectPoints[i].x, objectPoints[i].y));
 70    }
 71    //! [compute-object-points]
 72
 73    //! [load-intrinsics]
 74    FileStorage fs( samples::findFile( intrinsicsPath ), FileStorage::READ);
 75    Mat cameraMatrix, distCoeffs;
 76    fs["camera_matrix"] >> cameraMatrix;
 77    fs["distortion_coefficients"] >> distCoeffs;
 78    //! [load-intrinsics]
 79
 80    //! [compute-image-points]
 81    vector<Point2f> imagePoints;
 82    undistortPoints(corners, imagePoints, cameraMatrix, distCoeffs);
 83    //! [compute-image-points]
 84
 85    //! [estimate-homography]
 86    Mat H = findHomography(objectPointsPlanar, imagePoints);
 87    cout << "H:\n" << H << endl;
 88    //! [estimate-homography]
 89
 90    //! [pose-from-homography]
 91    // Normalization to ensure that ||c1|| = 1
 92    double norm = sqrt(H.at<double>(0,0)*H.at<double>(0,0) +
 93                       H.at<double>(1,0)*H.at<double>(1,0) +
 94                       H.at<double>(2,0)*H.at<double>(2,0));
 95
 96    H /= norm;
 97    Mat c1  = H.col(0);
 98    Mat c2  = H.col(1);
 99    Mat c3 = c1.cross(c2);
100
101    Mat tvec = H.col(2);
102    Mat R(3, 3, CV_64F);
103
104    for (int i = 0; i < 3; i++)
105    {
106        R.at<double>(i,0) = c1.at<double>(i,0);
107        R.at<double>(i,1) = c2.at<double>(i,0);
108        R.at<double>(i,2) = c3.at<double>(i,0);
109    }
110    //! [pose-from-homography]
111
112    //! [polar-decomposition-of-the-rotation-matrix]
113    cout << "R (before polar decomposition):\n" << R << "\ndet(R): " << determinant(R) << endl;
114    Mat_<double> W, U, Vt;
115    SVDecomp(R, W, U, Vt);
116    R = U*Vt;
117    double det = determinant(R);
118    if (det < 0)
119    {
120        Vt.at<double>(2,0) *= -1;
121        Vt.at<double>(2,1) *= -1;
122        Vt.at<double>(2,2) *= -1;
123
124        R = U*Vt;
125    }
126    cout << "R (after polar decomposition):\n" << R << "\ndet(R): " << determinant(R) << endl;
127    //! [polar-decomposition-of-the-rotation-matrix]
128
129    //! [display-pose]
130    Mat rvec;
131    Rodrigues(R, rvec);
132    drawFrameAxes(img_pose, cameraMatrix, distCoeffs, rvec, tvec, 2*squareSize);
133    imshow("Pose from coplanar points", img_pose);
134    waitKey();
135    //! [display-pose]
136}
137
138const char* params
139    = "{ help h         |       | print usage }"
140      "{ image          | left04.jpg | path to a chessboard image }"
141      "{ intrinsics     | left_intrinsics.yml | path to camera intrinsics }"
142      "{ width bw       | 9     | chessboard width }"
143      "{ height bh      | 6     | chessboard height }"
144      "{ square_size    | 0.025 | chessboard square size }";
145}
146
147int main(int argc, char *argv[])
148{
149    CommandLineParser parser(argc, argv, params);
150
151    if (parser.has("help"))
152    {
153        parser.about("Code for homography tutorial.\n"
154            "Example 1: pose from homography with coplanar points.\n");
155        parser.printMessage();
156        return 0;
157    }
158
159    Size patternSize(parser.get<int>("width"), parser.get<int>("height"));
160    float squareSize = (float) parser.get<double>("square_size");
161    poseEstimationFromCoplanarPoints(parser.get<String>("image"),
162                                     parser.get<String>("intrinsics"),
163                                     patternSize, squareSize);
164
165    return 0;
166}