#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;
}
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,
{
tvec_1to2 = R2 * (-R1.
t()*tvec1) + tvec2;
}
void homographyFromCameraDisplacement(
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);
Mat img1_copy_pose = img1.
clone(), img2_copy_pose = img2.
clone();
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);
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);
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;
cout << "\nfindHomography H:\n" << H << endl;
cout << "homography from camera displacement:\n" << homography << endl;
cout << "homography from absolute camera poses:\n" << homography2 << endl << endl;
imshow(
"Warped image using homography computed from camera displacement", img1_warp_custom);
hconcat(img1_warp, img1_warp_custom, img_draw_compare);
imshow(
"Warped images comparison", img_draw_compare);
imshow(
"Warped image using homography computed from absolute camera poses", img1_warp_custom2);
}
const char* params
= "{ help h | | print usage }"
"{ image1 | ../data/left02.jpg | path to the source chessboard image }"
"{ image2 | ../data/left01.jpg | path to the desired chessboard image }"
"{ intrinsics | ../data/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[])
{
{
parser.
about(
"Code for homography tutorial.\n" "Example 3: homography from the camera displacement.\n");
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"),
patternSize, squareSize,
return 0;
}