An example program with homography decomposition.
#include <iostream>
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:
}
}
Mat computeHomography(
const Mat &R_1to2,
const Mat &tvec_1to2,
const double d_inv,
const Mat &normal)
{
Mat homography = R_1to2 + d_inv * tvec_1to2*normal.
t();
return homography;
}
void computeC2MC1(
const Mat &R1,
const Mat &tvec1,
const Mat &R2,
const Mat &tvec2,
{
tvec_1to2 = R2 * (-R1.
t()*tvec1) + tvec2;
}
void decomposeHomography(
const string &img1Path,
const string &img2Path,
const Size &patternSize,
const float squareSize, const string &intrinsicsPath)
{
vector<Point2f> corners1, corners2;
if (!found1 || !found2)
{
cout << "Error, cannot find the chessboard corners in both images." << endl;
return;
}
vector<Point3f> objectPoints;
calcChessboardCorners(patternSize, squareSize, objectPoints);
Mat cameraMatrix, distCoeffs;
fs["camera_matrix"] >> cameraMatrix;
fs["distortion_coefficients"] >> distCoeffs;
solvePnP(objectPoints, corners1, cameraMatrix, distCoeffs, rvec1, tvec1);
solvePnP(objectPoints, corners2, cameraMatrix, distCoeffs, rvec2, tvec2);
computeC2MC1(R1, tvec1, R2, tvec2, R_1to2, t_1to2);
Mat origin1 = R1*origin + tvec1;
double d_inv1 = 1.0 / normal1.
dot(origin1);
Mat homography_euclidean = computeHomography(R_1to2, t_1to2, d_inv1, normal1);
Mat homography = cameraMatrix * homography_euclidean * cameraMatrix.
inv();
homography /= homography.
at<
double>(2,2);
homography_euclidean /= homography_euclidean.
at<
double>(2,2);
vector<Mat> Rs_decomp, ts_decomp, normals_decomp;
cout << "Decompose homography matrix computed from the camera displacement:" << endl << endl;
for (int i = 0; i < solutions; i++)
{
double factor_d1 = 1.0 / d_inv1;
cout << "Solution " << i << ":" << endl;
cout <<
"rvec from homography decomposition: " << rvec_decomp.
t() << endl;
cout <<
"rvec from camera displacement: " << rvec_1to2.
t() << endl;
cout <<
"tvec from homography decomposition: " << ts_decomp[i].
t() <<
" and scaled by d: " << factor_d1 * ts_decomp[i].
t() << endl;
cout <<
"tvec from camera displacement: " << t_1to2.
t() << endl;
cout <<
"plane normal from homography decomposition: " << normals_decomp[i].
t() << endl;
cout <<
"plane normal at camera 1 pose: " << normal1.
t() << endl << endl;
}
cout << "Decompose homography matrix estimated by findHomography():" << endl << endl;
for (int i = 0; i < solutions; i++)
{
double factor_d1 = 1.0 / d_inv1;
cout << "Solution " << i << ":" << endl;
cout <<
"rvec from homography decomposition: " << rvec_decomp.
t() << endl;
cout <<
"rvec from camera displacement: " << rvec_1to2.
t() << endl;
cout <<
"tvec from homography decomposition: " << ts_decomp[i].
t() <<
" and scaled by d: " << factor_d1 * ts_decomp[i].
t() << endl;
cout <<
"tvec from camera displacement: " << t_1to2.
t() << endl;
cout <<
"plane normal from homography decomposition: " << normals_decomp[i].
t() << endl;
cout <<
"plane normal at camera 1 pose: " << normal1.
t() << endl << endl;
}
}
= "{ help h | | print usage }"
"{ image1 | left02.jpg | path to the source chessboard image }"
"{ image2 | left01.jpg | path to the desired 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[])
{
if ( parser.has("help") )
{
parser.about( "Code for homography tutorial.\n"
"Example 4: decompose the homography matrix.\n" );
parser.printMessage();
return 0;
}
Size patternSize(parser.get<
int>(
"width"), parser.get<
int>(
"height"));
float squareSize = (float) parser.get<double>("square_size");
decomposeHomography(parser.get<
String>(
"image1"),
patternSize, squareSize,
parser.get<
String>(
"intrinsics"));
return 0;
}
Designed for command line parsing.
Definition: utility.hpp:818
XML/YAML/JSON file storage class that encapsulates all the information necessary for writing or readi...
Definition: persistence.hpp:304
n-dimensional dense array class
Definition: mat.hpp:811
MatExpr inv(int method=DECOMP_LU) const
Inverses a matrix.
double dot(InputArray m) const
Computes a dot-product of two vectors.
_Tp & at(int i0=0)
Returns a reference to the specified array element.
MatExpr t() const
Transposes a matrix.
Template class for specifying the size of an image or rectangle.
Definition: types.hpp:330
_Tp height
the height
Definition: types.hpp:358
_Tp width
the width
Definition: types.hpp:357
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.
bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=SOLVEPNP_ITERATIVE)
Finds an object pose from 3D-2D point correspondences.
void Rodrigues(InputArray src, OutputArray dst, OutputArray jacobian=noArray())
Converts a rotation matrix to a rotation vector or vice versa.
int decomposeHomographyMat(InputArray H, InputArray K, OutputArrayOfArrays rotations, OutputArrayOfArrays translations, OutputArrayOfArrays normals)
Decompose a homography matrix to rotation(s), translation(s) and plane normal(s).
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.
std::string String
Definition: cvstd.hpp:152
Scalar_< double > Scalar
Definition: types.hpp:691
#define CV_64F
Definition: interface.h:79
cv::String findFile(const cv::String &relative_path, bool required=true, bool silentMode=false)
Try to find requested data file.
#define CV_Error(code, msg)
Call the error handler.
Definition: base.hpp:320
CV_EXPORTS_W Mat imread(const String &filename, int flags=IMREAD_COLOR)
Loads an image from a file.
@ StsBadArg
function arg/param is bad
Definition: base.hpp:74
PyParams params(const std::string &tag, const std::string &model, const std::string &weights, const std::string &device)
"black box" representation of the file storage associated with a file on disk.
Definition: core.hpp:106