mirror of
https://github.com/zebrajr/opencv.git
synced 2026-01-15 12:15:17 +00:00
Add tutorial and codes for the homography tutorial.
This commit is contained in:
catree
@@ -0,0 +1,23 @@
|
||||
%YAML:1.0
|
||||
---
|
||||
image_width: 640
|
||||
image_height: 480
|
||||
board_width: 9
|
||||
board_height: 6
|
||||
square_size: 1.
|
||||
aspectRatio: 1.
|
||||
flags: 2
|
||||
camera_matrix: !!opencv-matrix
|
||||
rows: 3
|
||||
cols: 3
|
||||
dt: d
|
||||
data: [ 5.3591575307485539e+02, 0., 3.4228314953752817e+02, 0.,
|
||||
5.3591575307485539e+02, 2.3557082321320789e+02, 0., 0., 1. ]
|
||||
distortion_coefficients: !!opencv-matrix
|
||||
rows: 5
|
||||
cols: 1
|
||||
dt: d
|
||||
data: [ -2.6637290673868386e-01, -3.8586722644459073e-02,
|
||||
1.7831841406179300e-03, -2.8122035403651473e-04,
|
||||
2.3838760574917545e-01 ]
|
||||
avg_reprojection_error: 3.9259109564815858e-01
|
||||
@@ -0,0 +1,156 @@
|
||||
#include <iostream>
|
||||
#include <opencv2/opencv_modules.hpp>
|
||||
#ifdef HAVE_OPENCV_ARUCO
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/aruco.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:
|
||||
//! [compute-chessboard-object-points]
|
||||
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));
|
||||
//! [compute-chessboard-object-points]
|
||||
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(imgPath);
|
||||
Mat img_corners = img.clone(), img_pose = img.clone();
|
||||
|
||||
//! [find-chessboard-corners]
|
||||
vector<Point2f> corners;
|
||||
bool found = findChessboardCorners(img, patternSize, corners);
|
||||
//! [find-chessboard-corners]
|
||||
|
||||
if (!found)
|
||||
{
|
||||
cout << "Cannot find chessboard corners." << endl;
|
||||
return;
|
||||
}
|
||||
drawChessboardCorners(img_corners, patternSize, corners, found);
|
||||
imshow("Chessboard corners detection", img_corners);
|
||||
|
||||
//! [compute-object-points]
|
||||
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));
|
||||
}
|
||||
//! [compute-object-points]
|
||||
|
||||
//! [load-intrinsics]
|
||||
FileStorage fs(intrinsicsPath, FileStorage::READ);
|
||||
Mat cameraMatrix, distCoeffs;
|
||||
fs["camera_matrix"] >> cameraMatrix;
|
||||
fs["distortion_coefficients"] >> distCoeffs;
|
||||
//! [load-intrinsics]
|
||||
|
||||
//! [compute-image-points]
|
||||
vector<Point2f> imagePoints;
|
||||
undistortPoints(corners, imagePoints, cameraMatrix, distCoeffs);
|
||||
//! [compute-image-points]
|
||||
|
||||
//! [estimate-homography]
|
||||
Mat H = findHomography(objectPointsPlanar, imagePoints);
|
||||
cout << "H:\n" << H << endl;
|
||||
//! [estimate-homography]
|
||||
|
||||
//! [pose-from-homography]
|
||||
// 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);
|
||||
}
|
||||
//! [pose-from-homography]
|
||||
|
||||
//! [display-pose]
|
||||
Mat rvec;
|
||||
Rodrigues(R, rvec);
|
||||
aruco::drawAxis(img_pose, cameraMatrix, distCoeffs, rvec, tvec, 2*squareSize);
|
||||
imshow("Pose from coplanar points", img_pose);
|
||||
waitKey();
|
||||
//! [display-pose]
|
||||
}
|
||||
|
||||
const char* about = "Code for homography tutorial.\n"
|
||||
"Example 1: pose from homography with coplanar points.\n";
|
||||
|
||||
const char* params
|
||||
= "{ h help | false | print usage }"
|
||||
"{ image | | path to a chessboard image (left04.jpg) }"
|
||||
"{ intrinsics | | path to camera intrinsics (left_intrinsics.yml) }"
|
||||
"{ width w | 9 | chessboard width }"
|
||||
"{ height h | 6 | chessboard height }"
|
||||
"{ square_size | 0.025 | chessboard square size }";
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
CommandLineParser parser(argc, argv, params);
|
||||
|
||||
if (parser.get<bool>("help"))
|
||||
{
|
||||
cout << about << endl;
|
||||
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;
|
||||
}
|
||||
#else
|
||||
int main()
|
||||
{
|
||||
std::cerr << "FATAL ERROR: This sample requires opencv_aruco module (from opencv_contrib)" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,131 @@
|
||||
#include <iostream>
|
||||
#include <opencv2/opencv_modules.hpp>
|
||||
#ifdef HAVE_OPENCV_ARUCO
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/aruco.hpp>
|
||||
|
||||
using namespace std;
|
||||
using namespace cv;
|
||||
|
||||
namespace
|
||||
{
|
||||
enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
|
||||
|
||||
Scalar randomColor( RNG& rng )
|
||||
{
|
||||
int icolor = (unsigned int) rng;
|
||||
return Scalar( icolor & 255, (icolor >> 8) & 255, (icolor >> 16) & 255 );
|
||||
}
|
||||
|
||||
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 perspectiveCorrection(const string &img1Path, const string &img2Path, const Size &patternSize, RNG &rng)
|
||||
{
|
||||
Mat img1 = imread(img1Path);
|
||||
Mat img2 = imread(img2Path);
|
||||
|
||||
//! [find-corners]
|
||||
vector<Point2f> corners1, corners2;
|
||||
bool found1 = findChessboardCorners(img1, patternSize, corners1);
|
||||
bool found2 = findChessboardCorners(img2, patternSize, corners2);
|
||||
//! [find-corners]
|
||||
|
||||
if (!found1 || !found2)
|
||||
{
|
||||
cout << "Error, cannot find the chessboard corners in both images." << endl;
|
||||
return;
|
||||
}
|
||||
|
||||
//! [estimate-homography]
|
||||
Mat H = findHomography(corners1, corners2);
|
||||
cout << "H:\n" << H << endl;
|
||||
//! [estimate-homography]
|
||||
|
||||
//! [warp-chessboard]
|
||||
Mat img1_warp;
|
||||
warpPerspective(img1, img1_warp, H, img1.size());
|
||||
//! [warp-chessboard]
|
||||
|
||||
Mat img_draw_warp;
|
||||
hconcat(img2, img1_warp, img_draw_warp);
|
||||
imshow("Desired chessboard view / Warped source chessboard view", img_draw_warp);
|
||||
|
||||
//! [compute-transformed-corners]
|
||||
Mat img_draw_matches;
|
||||
hconcat(img1, img2, img_draw_matches);
|
||||
for (size_t i = 0; i < corners1.size(); i++)
|
||||
{
|
||||
Mat pt1 = (Mat_<double>(3,1) << corners1[i].x, corners1[i].y, 1);
|
||||
Mat pt2 = H * pt1;
|
||||
pt2 /= pt2.at<double>(2);
|
||||
|
||||
Point end( (int) (img1.cols + pt2.at<double>(0)), (int) pt2.at<double>(1) );
|
||||
line(img_draw_matches, corners1[i], end, randomColor(rng), 2);
|
||||
}
|
||||
|
||||
imshow("Draw matches", img_draw_matches);
|
||||
waitKey();
|
||||
//! [compute-transformed-corners]
|
||||
}
|
||||
|
||||
const char* about = "Code for homography tutorial.\n"
|
||||
"Example 2: perspective correction.\n";
|
||||
|
||||
const char* params
|
||||
= "{ h help | false | print usage }"
|
||||
"{ image1 | | path to the source chessboard image (left02.jpg) }"
|
||||
"{ image2 | | path to the desired chessboard image (left01.jpg) }"
|
||||
"{ width w | 9 | chessboard width }"
|
||||
"{ height h | 6 | chessboard height }";
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
cv::RNG rng( 0xFFFFFFFF );
|
||||
CommandLineParser parser(argc, argv, params);
|
||||
|
||||
if (parser.get<bool>("help"))
|
||||
{
|
||||
cout << about << endl;
|
||||
parser.printMessage();
|
||||
return 0;
|
||||
}
|
||||
|
||||
Size patternSize(parser.get<int>("width"), parser.get<int>("height"));
|
||||
perspectiveCorrection(parser.get<string>("image1"),
|
||||
parser.get<string>("image2"),
|
||||
patternSize, rng);
|
||||
|
||||
return 0;
|
||||
}
|
||||
#else
|
||||
int main()
|
||||
{
|
||||
std::cerr << "FATAL ERROR: This sample requires opencv_aruco module (from opencv_contrib)" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,210 @@
|
||||
#include <iostream>
|
||||
#include <opencv2/opencv_modules.hpp>
|
||||
#ifdef HAVE_OPENCV_ARUCO
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/aruco.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");
|
||||
}
|
||||
}
|
||||
|
||||
//! [compute-homography]
|
||||
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;
|
||||
}
|
||||
//! [compute-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;
|
||||
}
|
||||
|
||||
//! [compute-c2Mc1]
|
||||
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;
|
||||
}
|
||||
//! [compute-c2Mc1]
|
||||
|
||||
void homographyFromCameraDisplacement(const string &img1Path, const string &img2Path, const Size &patternSize,
|
||||
const float squareSize, const string &intrinsicsPath)
|
||||
{
|
||||
Mat img1 = imread(img1Path);
|
||||
Mat img2 = imread(img2Path);
|
||||
|
||||
//! [compute-poses]
|
||||
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(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);
|
||||
//! [compute-poses]
|
||||
|
||||
Mat img1_copy_pose = img1.clone(), img2_copy_pose = img2.clone();
|
||||
Mat img_draw_poses;
|
||||
aruco::drawAxis(img1_copy_pose, cameraMatrix, distCoeffs, rvec1, tvec1, 2*squareSize);
|
||||
aruco::drawAxis(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);
|
||||
|
||||
//! [compute-camera-displacement]
|
||||
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);
|
||||
//! [compute-camera-displacement]
|
||||
|
||||
//! [compute-plane-normal-at-camera-pose-1]
|
||||
Mat normal = (Mat_<double>(3,1) << 0, 0, 1);
|
||||
Mat normal1 = R1*normal;
|
||||
//! [compute-plane-normal-at-camera-pose-1]
|
||||
|
||||
//! [compute-plane-distance-to-the-camera-frame-1]
|
||||
Mat origin(3, 1, CV_64F, Scalar(0));
|
||||
Mat origin1 = R1*origin + tvec1;
|
||||
double d_inv1 = 1.0 / normal1.dot(origin1);
|
||||
//! [compute-plane-distance-to-the-camera-frame-1]
|
||||
|
||||
//! [compute-homography-from-camera-displacement]
|
||||
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);
|
||||
//! [compute-homography-from-camera-displacement]
|
||||
|
||||
//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;
|
||||
|
||||
//! [estimate-homography]
|
||||
Mat H = findHomography(corners1, corners2);
|
||||
cout << "\nfindHomography H:\n" << H << endl;
|
||||
//! [estimate-homography]
|
||||
|
||||
cout << "homography from camera displacement:\n" << homography << endl;
|
||||
cout << "homography from absolute camera poses:\n" << homography2 << endl << endl;
|
||||
|
||||
//! [warp-chessboard]
|
||||
Mat img1_warp;
|
||||
warpPerspective(img1, img1_warp, H, img1.size());
|
||||
//! [warp-chessboard]
|
||||
|
||||
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);
|
||||
|
||||
waitKey();
|
||||
}
|
||||
|
||||
const char* about = "Code for homography tutorial.\n"
|
||||
"Example 3: homography from the camera displacement.\n";
|
||||
|
||||
const char* params
|
||||
= "{ h help | false | print usage }"
|
||||
"{ image1 | | path to the source chessboard image (left02.jpg) }"
|
||||
"{ image2 | | path to the desired chessboard image (left01.jpg) }"
|
||||
"{ intrinsics | | path to camera intrinsics (left_intrinsics.yml) }"
|
||||
"{ width w | 9 | chessboard width }"
|
||||
"{ height h | 6 | chessboard height }"
|
||||
"{ square_size | 0.025 | chessboard square size }";
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
CommandLineParser parser(argc, argv, params);
|
||||
|
||||
if (parser.get<bool>("help"))
|
||||
{
|
||||
cout << about << endl;
|
||||
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;
|
||||
}
|
||||
#else
|
||||
int main()
|
||||
{
|
||||
std::cerr << "FATAL ERROR: This sample requires opencv_aruco module (from opencv_contrib)" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,195 @@
|
||||
#include <iostream>
|
||||
#include <opencv2/opencv_modules.hpp>
|
||||
#ifdef HAVE_OPENCV_ARUCO
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/aruco.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;
|
||||
}
|
||||
|
||||
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 decomposeHomography(const string &img1Path, const string &img2Path, const Size &patternSize,
|
||||
const float squareSize, const string &intrinsicsPath)
|
||||
{
|
||||
Mat img1 = imread(img1Path);
|
||||
Mat img2 = imread(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;
|
||||
}
|
||||
|
||||
//! [compute-poses]
|
||||
vector<Point3f> objectPoints;
|
||||
calcChessboardCorners(patternSize, squareSize, objectPoints);
|
||||
|
||||
FileStorage fs(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);
|
||||
//! [compute-poses]
|
||||
|
||||
//! [compute-camera-displacement]
|
||||
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);
|
||||
//! [compute-camera-displacement]
|
||||
|
||||
//! [compute-plane-normal-at-camera-pose-1]
|
||||
Mat normal = (Mat_<double>(3,1) << 0, 0, 1);
|
||||
Mat normal1 = R1*normal;
|
||||
//! [compute-plane-normal-at-camera-pose-1]
|
||||
|
||||
//! [compute-plane-distance-to-the-camera-frame-1]
|
||||
Mat origin(3, 1, CV_64F, Scalar(0));
|
||||
Mat origin1 = R1*origin + tvec1;
|
||||
double d_inv1 = 1.0 / normal1.dot(origin1);
|
||||
//! [compute-plane-distance-to-the-camera-frame-1]
|
||||
|
||||
//! [compute-homography-from-camera-displacement]
|
||||
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);
|
||||
//! [compute-homography-from-camera-displacement]
|
||||
|
||||
//! [decompose-homography-from-camera-displacement]
|
||||
vector<Mat> Rs_decomp, ts_decomp, normals_decomp;
|
||||
int solutions = decomposeHomographyMat(homography, cameraMatrix, 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;
|
||||
Mat rvec_decomp;
|
||||
Rodrigues(Rs_decomp[i], rvec_decomp);
|
||||
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;
|
||||
}
|
||||
//! [decompose-homography-from-camera-displacement]
|
||||
|
||||
//! [estimate homography]
|
||||
Mat H = findHomography(corners1, corners2);
|
||||
//! [estimate homography]
|
||||
|
||||
//! [decompose-homography-estimated-by-findHomography]
|
||||
solutions = decomposeHomographyMat(H, cameraMatrix, Rs_decomp, ts_decomp, normals_decomp);
|
||||
cout << "Decompose homography matrix estimated by findHomography():" << endl << endl;
|
||||
for (int i = 0; i < solutions; i++)
|
||||
{
|
||||
double factor_d1 = 1.0 / d_inv1;
|
||||
Mat rvec_decomp;
|
||||
Rodrigues(Rs_decomp[i], rvec_decomp);
|
||||
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;
|
||||
}
|
||||
//! [decompose-homography-estimated-by-findHomography]
|
||||
}
|
||||
|
||||
const char* about = "Code for homography tutorial.\n"
|
||||
"Example 4: decompose the homography matrix.\n";
|
||||
|
||||
const char* params
|
||||
= "{ h help | false | print usage }"
|
||||
"{ image1 | | path to the source chessboard image (left02.jpg) }"
|
||||
"{ image2 | | path to the desired chessboard image (left01.jpg) }"
|
||||
"{ intrinsics | | path to camera intrinsics (left_intrinsics.yml) }"
|
||||
"{ width w | 9 | chessboard width }"
|
||||
"{ height h | 6 | chessboard height }"
|
||||
"{ square_size | 0.025 | chessboard square size }";
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
CommandLineParser parser(argc, argv, params);
|
||||
|
||||
if (parser.get<bool>("help"))
|
||||
{
|
||||
cout << about << endl;
|
||||
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"),
|
||||
parser.get<string>("image2"),
|
||||
patternSize, squareSize,
|
||||
parser.get<string>("intrinsics"));
|
||||
|
||||
return 0;
|
||||
}
|
||||
#else
|
||||
int main()
|
||||
{
|
||||
std::cerr << "FATAL ERROR: This sample requires opencv_aruco module (from opencv_contrib)" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
Reference in New Issue
Block a user