#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:
}
}
void poseEstimationFromCoplanarPoints(
const string &imgPath,
const string &intrinsicsPath,
const Size &patternSize,
const float squareSize)
{
vector<Point2f> corners;
if (!found)
{
cout << "Cannot find chessboard corners." << endl;
return;
}
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));
}
Mat cameraMatrix, distCoeffs;
fs["camera_matrix"] >> cameraMatrix;
fs["distortion_coefficients"] >> distCoeffs;
vector<Point2f> imagePoints;
cout << "H:\n" << H << endl;
H.
at<
double>(1,0)*H.
at<
double>(1,0) +
H.
at<
double>(2,0)*H.
at<
double>(2,0));
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;
R = U*Vt;
cout <<
"R (after polar decomposition):\n" << R <<
"\ndet(R): " <<
determinant(R) << endl;
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[])
{
{
parser.
about(
"Code for homography tutorial.\n" "Example 1: pose from homography with coplanar points.\n");
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"),
patternSize, squareSize);
return 0;
}