OpenCV  4.5.2
Open Source Computer Vision
samples/cpp/tutorial_code/features2D/Homography/homography_from_camera_displacement.cpp

An example program about homography from the camera displacement

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");
}
}
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;
}
Mat computeHomography(const Mat &R1, const Mat &tvec1, const Mat &R2, const Mat &tvec2,
const double d_inv, const Mat &normal)
{
Mat homography = R2 * R1.t() + d_inv * (-R2 * R1.t() * tvec1 + tvec2) * normal.t();
return homography;
}
void computeC2MC1(const Mat &R1, const Mat &tvec1, const Mat &R2, const Mat &tvec2,
Mat &R_1to2, Mat &tvec_1to2)
{
//c2Mc1 = c2Mo * oMc1 = c2Mo * c1Mo.inv()
R_1to2 = R2 * R1.t();
tvec_1to2 = R2 * (-R1.t()*tvec1) + tvec2;
}
void homographyFromCameraDisplacement(const string &img1Path, const string &img2Path, const Size &patternSize,
const float squareSize, const string &intrinsicsPath)
{
Mat img1 = imread( samples::findFile( img1Path ) );
Mat img2 = imread( samples::findFile( img2Path ) );
vector<Point2f> corners1, corners2;
bool found1 = findChessboardCorners(img1, patternSize, corners1);
bool found2 = findChessboardCorners(img2, patternSize, corners2);
if (!found1 || !found2)
{
cout << "Error, cannot find the chessboard corners in both images." << endl;
return;
}
vector<Point3f> objectPoints;
calcChessboardCorners(patternSize, squareSize, objectPoints);
FileStorage fs( samples::findFile( intrinsicsPath ), FileStorage::READ);
Mat cameraMatrix, distCoeffs;
fs["camera_matrix"] >> cameraMatrix;
fs["distortion_coefficients"] >> distCoeffs;
Mat rvec1, tvec1;
solvePnP(objectPoints, corners1, cameraMatrix, distCoeffs, rvec1, tvec1);
Mat rvec2, tvec2;
solvePnP(objectPoints, corners2, cameraMatrix, distCoeffs, rvec2, tvec2);
Mat img1_copy_pose = img1.clone(), img2_copy_pose = img2.clone();
Mat img_draw_poses;
drawFrameAxes(img1_copy_pose, cameraMatrix, distCoeffs, rvec1, tvec1, 2*squareSize);
drawFrameAxes(img2_copy_pose, cameraMatrix, distCoeffs, rvec2, tvec2, 2*squareSize);
hconcat(img1_copy_pose, img2_copy_pose, img_draw_poses);
imshow("Chessboard poses", img_draw_poses);
Mat R1, R2;
Rodrigues(rvec1, R1);
Rodrigues(rvec2, R2);
Mat R_1to2, t_1to2;
computeC2MC1(R1, tvec1, R2, tvec2, R_1to2, t_1to2);
Mat rvec_1to2;
Rodrigues(R_1to2, rvec_1to2);
Mat normal = (Mat_<double>(3,1) << 0, 0, 1);
Mat normal1 = R1*normal;
Mat origin(3, 1, CV_64F, Scalar(0));
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);
//Same but using absolute camera poses instead of camera displacement, just for check
Mat homography_euclidean2 = computeHomography(R1, tvec1, R2, tvec2, d_inv1, normal1);
Mat homography2 = cameraMatrix * homography_euclidean2 * cameraMatrix.inv();
homography_euclidean2 /= homography_euclidean2.at<double>(2,2);
homography2 /= homography2.at<double>(2,2);
cout << "\nEuclidean Homography:\n" << homography_euclidean << endl;
cout << "Euclidean Homography 2:\n" << homography_euclidean2 << endl << endl;
Mat H = findHomography(corners1, corners2);
cout << "\nfindHomography H:\n" << H << endl;
cout << "homography from camera displacement:\n" << homography << endl;
cout << "homography from absolute camera poses:\n" << homography2 << endl << endl;
Mat img1_warp;
warpPerspective(img1, img1_warp, H, img1.size());
Mat img1_warp_custom;
warpPerspective(img1, img1_warp_custom, homography, img1.size());
imshow("Warped image using homography computed from camera displacement", img1_warp_custom);
Mat img_draw_compare;
hconcat(img1_warp, img1_warp_custom, img_draw_compare);
imshow("Warped images comparison", img_draw_compare);
Mat img1_warp_custom2;
warpPerspective(img1, img1_warp_custom2, homography2, img1.size());
imshow("Warped image using homography computed from absolute camera poses", img1_warp_custom2);
}
const char* params
= "{ 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[])
{
CommandLineParser parser(argc, argv, params);
if (parser.has("help"))
{
parser.about("Code for homography tutorial.\n"
"Example 3: homography from the camera displacement.\n");
parser.printMessage();
return 0;
}
Size patternSize(parser.get<int>("width"), parser.get<int>("height"));
float squareSize = (float) parser.get<double>("square_size");
homographyFromCameraDisplacement(parser.get<String>("image1"),
parser.get<String>("image2"),
patternSize, squareSize,
parser.get<String>("intrinsics"));
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::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::hconcat
void hconcat(const Mat *src, size_t nsrc, OutputArray dst)
Applies horizontal concatenation to given matrices.
cv::Size_
Template class for specifying the size of an image or rectangle.
Definition: types.hpp:315
highgui.hpp
cv::Error::StsBadArg
@ StsBadArg
function arg/param is bad
Definition: base.hpp:74
core.hpp
cv::Rodrigues
void Rodrigues(InputArray src, OutputArray dst, OutputArray jacobian=noArray())
Converts a rotation matrix to a rotation vector or vice versa.
cv::Mat::dot
double dot(InputArray m) const
Computes a dot-product of two vectors.
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::solvePnP
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. This function returns the rotation and the tra...
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::t
MatExpr t() const
Transposes a matrix.
cv::Mat::size
MatSize size
Definition: mat.hpp:2118
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::Scalar
Scalar_< double > Scalar
Definition: types.hpp:669
cv::Mat
n-dimensional dense array class
Definition: mat.hpp:801
cv::Mat::inv
MatExpr inv(int method=DECOMP_LU) const
Inverses a matrix.
cv::CommandLineParser
Designed for command line parsing.
Definition: utility.hpp:799
cv::Size_::height
_Tp height
the height
Definition: types.hpp:340
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::Mat_< double >
cv::hal::warpPerspective
void warpPerspective(int src_type, const uchar *src_data, size_t src_step, int src_width, int src_height, uchar *dst_data, size_t dst_step, int dst_width, int dst_height, const double M[9], int interpolation, int borderType, const double borderValue[4])
cv::gapi::ie::params
PyParams params(const std::string &tag, const std::string &model, const std::string &weights, const std::string &device)