OpenCV  4.1.2
Open Source Computer Vision
samples/cpp/tutorial_code/features2D/Homography/pose_from_homography.cpp

An example program about pose estimation from coplanar points

Check the corresponding tutorial for more details

#include <iostream>
#include <opencv2/core.hpp>
using namespace std;
using namespace cv;
namespace
{
enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners, Pattern patternType = CHESSBOARD)
{
corners.resize(0);
switch (patternType)
{
case CHESSBOARD:
case CIRCLES_GRID:
for( int i = 0; i < boardSize.height; i++ )
for( int j = 0; j < boardSize.width; j++ )
corners.push_back(Point3f(float(j*squareSize),
float(i*squareSize), 0));
break;
case ASYMMETRIC_CIRCLES_GRID:
for( int i = 0; i < boardSize.height; i++ )
for( int j = 0; j < boardSize.width; j++ )
corners.push_back(Point3f(float((2*j + i % 2)*squareSize),
float(i*squareSize), 0));
break;
default:
CV_Error(Error::StsBadArg, "Unknown pattern type\n");
}
}
void poseEstimationFromCoplanarPoints(const string &imgPath, const string &intrinsicsPath, const Size &patternSize,
const float squareSize)
{
Mat img = imread( samples::findFile( imgPath) );
Mat img_corners = img.clone(), img_pose = img.clone();
vector<Point2f> corners;
bool found = findChessboardCorners(img, patternSize, corners);
if (!found)
{
cout << "Cannot find chessboard corners." << endl;
return;
}
drawChessboardCorners(img_corners, patternSize, corners, found);
imshow("Chessboard corners detection", img_corners);
vector<Point3f> objectPoints;
calcChessboardCorners(patternSize, squareSize, objectPoints);
vector<Point2f> objectPointsPlanar;
for (size_t i = 0; i < objectPoints.size(); i++)
{
objectPointsPlanar.push_back(Point2f(objectPoints[i].x, objectPoints[i].y));
}
FileStorage fs( samples::findFile( intrinsicsPath ), FileStorage::READ);
Mat cameraMatrix, distCoeffs;
fs["camera_matrix"] >> cameraMatrix;
fs["distortion_coefficients"] >> distCoeffs;
vector<Point2f> imagePoints;
undistortPoints(corners, imagePoints, cameraMatrix, distCoeffs);
Mat H = findHomography(objectPointsPlanar, imagePoints);
cout << "H:\n" << H << endl;
// Normalization to ensure that ||c1|| = 1
double norm = sqrt(H.at<double>(0,0)*H.at<double>(0,0) +
H.at<double>(1,0)*H.at<double>(1,0) +
H.at<double>(2,0)*H.at<double>(2,0));
H /= norm;
Mat c1 = H.col(0);
Mat c2 = H.col(1);
Mat c3 = c1.cross(c2);
Mat tvec = H.col(2);
Mat R(3, 3, CV_64F);
for (int i = 0; i < 3; i++)
{
R.at<double>(i,0) = c1.at<double>(i,0);
R.at<double>(i,1) = c2.at<double>(i,0);
R.at<double>(i,2) = c3.at<double>(i,0);
}
cout << "R (before polar decomposition):\n" << R << "\ndet(R): " << determinant(R) << endl;
Mat W, U, Vt;
SVDecomp(R, W, U, Vt);
R = U*Vt;
cout << "R (after polar decomposition):\n" << R << "\ndet(R): " << determinant(R) << endl;
Mat rvec;
Rodrigues(R, rvec);
drawFrameAxes(img_pose, cameraMatrix, distCoeffs, rvec, tvec, 2*squareSize);
imshow("Pose from coplanar points", img_pose);
}
const char* params
= "{ help h | | print usage }"
"{ image | left04.jpg | path to a chessboard image }"
"{ intrinsics | left_intrinsics.yml | path to camera intrinsics }"
"{ width bw | 9 | chessboard width }"
"{ height bh | 6 | chessboard height }"
"{ square_size | 0.025 | chessboard square size }";
}
int main(int argc, char *argv[])
{
CommandLineParser parser(argc, argv, params);
if (parser.has("help"))
{
parser.about("Code for homography tutorial.\n"
"Example 1: pose from homography with coplanar points.\n");
parser.printMessage();
return 0;
}
Size patternSize(parser.get<int>("width"), parser.get<int>("height"));
float squareSize = (float) parser.get<double>("square_size");
poseEstimationFromCoplanarPoints(parser.get<String>("image"),
parser.get<String>("intrinsics"),
patternSize, squareSize);
return 0;
}
cv::String
std::string String
Definition: cvstd.hpp:150
calib3d.hpp
cv::Mat::clone
Mat clone() const CV_NODISCARD
Creates a full copy of the array and the underlying data.
cv::Point3_
Template class for 3D points specified by its coordinates x, y and z.
Definition: types.hpp:239
cv::samples::findFile
cv::String findFile(const cv::String &relative_path, bool required=true, bool silentMode=false)
Try to find requested data file.
cv::Mat::at
_Tp & at(int i0=0)
Returns a reference to the specified array element.
cv::Point2f
Point_< float > Point2f
Definition: types.hpp:192
cv::waitKey
int waitKey(int delay=0)
Waits for a pressed key.
cv::FileStorage
XML/YAML/JSON file storage class that encapsulates all the information necessary for writing or readi...
Definition: persistence.hpp:303
cv::sqrt
softfloat sqrt(const softfloat &a)
Square root.
cv::Size_
Template class for specifying the size of an image or rectangle.
Definition: types.hpp:315
highgui.hpp
cv::Error::StsBadArg
function arg/param is bad
Definition: base.hpp:74
core.hpp
cv::norm
static double norm(const Matx< _Tp, m, n > &M)
cv::Rodrigues
void Rodrigues(InputArray src, OutputArray dst, OutputArray jacobian=noArray())
Converts a rotation matrix to a rotation vector or vice versa.
cv::findHomography
Mat findHomography(InputArray srcPoints, InputArray dstPoints, int method=0, double ransacReprojThreshold=3, OutputArray mask=noArray(), const int maxIters=2000, const double confidence=0.995)
Finds a perspective transformation between two planes.
cv::imread
Mat imread(const String &filename, int flags=IMREAD_COLOR)
Loads an image from a file.
cv::Size_::width
_Tp width
the width
Definition: types.hpp:339
cv::SVDecomp
void SVDecomp(InputArray src, OutputArray w, OutputArray u, OutputArray vt, int flags=0)
cv::drawFrameAxes
void drawFrameAxes(InputOutputArray image, InputArray cameraMatrix, InputArray distCoeffs, InputArray rvec, InputArray tvec, float length, int thickness=3)
Draw axes of the world/object coordinate system from pose estimation.
CV_Error
#define CV_Error(code, msg)
Call the error handler.
Definition: base.hpp:320
cv::Mat::col
Mat col(int x) const
Creates a matrix header for the specified matrix column.
CV_64F
#define CV_64F
Definition: interface.h:79
cv::imshow
void imshow(const String &winname, InputArray mat)
Displays an image in the specified window.
cv::drawChessboardCorners
void drawChessboardCorners(InputOutputArray image, Size patternSize, InputArray corners, bool patternWasFound)
Renders the detected chessboard corners.
cv::Mat::cross
Mat cross(InputArray m) const
Computes a cross-product of two 3-element vectors.
cv::Mat
n-dimensional dense array class
Definition: mat.hpp:791
cv::CommandLineParser
Designed for command line parsing.
Definition: utility.hpp:831
cv::Size_::height
_Tp height
the height
Definition: types.hpp:340
cv::determinant
static double determinant(const Matx< _Tp, m, m > &a)
cv
"black box" representation of the file storage associated with a file on disk.
Definition: affine.hpp:51
imgproc.hpp
cv::findChessboardCorners
bool findChessboardCorners(InputArray image, Size patternSize, OutputArray corners, int flags=CALIB_CB_ADAPTIVE_THRESH+CALIB_CB_NORMALIZE_IMAGE)
Finds the positions of internal corners of the chessboard.
cv::undistortPoints
void undistortPoints(InputArray src, OutputArray dst, InputArray cameraMatrix, InputArray distCoeffs, InputArray R=noArray(), InputArray P=noArray())
Computes the ideal point coordinates from the observed point coordinates.