Finds an object pose from 3D-2D point correspondences

C++: bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=ITERATIVE )

Parameters:

objectPoints – Array of object points in the object coordinate space, 3xN/Nx3 1-channel or 1xN/Nx1 3-channel, where N is the number of points. vector<Point3f> can be also passed here. imagePoints – Array of corresponding image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, where N is the number of points. vector<Point2f> can be also passed here. cameraMatrix – Input camera matrix  . distCoeffs – Input vector of distortion coefficients  of 4, 5, or 8 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed. rvec – Output rotation vector (see Rodrigues() ) that, together with tvec , brings points from the model coordinate system to the camera coordinate system. tvec – Output translation vector. useExtrinsicGuess – If true (1), the function uses the provided rvec and tvec values as initial approximations of the rotation and translation vectors, respectively, and further optimizes them. flags – Method for solving a PnP problem: CV_ITERATIVE Iterative method is based on Levenberg-Marquardt optimization. In this case the function finds such a pose that minimizes reprojection error, that is the sum of squared distances between the observed projections imagePoints and the projected (using projectPoints() ) objectPoints . CV_P3P Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang “Complete Solution Classification for the Perspective-Three-Point Problem”. In this case the function requires exactly four object and image points. CV_EPNP Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the paper “EPnP: Efficient Perspective-n-Point Camera Pose Estimation”.

The function estimates the object pose given a set of object points, their corresponding image projections, as well as the camera matrix and the distortion coefficients.

	#include "opencv.hpp"
	#include <iostream>
	#include <string>
	using namespace std;
	using namespace cv;
	vector<cv::Point2f> Generate2DPoints();
	vector<cv::Point3f> Generate3DPoints();
	int main()
	{
	vector<cv::Point2f> imagePoints = Generate2DPoints();
	vector<cv::Point3f> objectPoints = Generate3DPoints();
	cout << "there are " << imagePoints.size() << " Image points and " << objectPoints.size() << " Object points" << endl;
	Mat cameraMatrix(3,3, DataType<double>::type);
	setIdentity(cameraMatrix);
	cout<< " Camera matrix " <<endl<< cameraMatrix << endl;
	Mat distCoeffs(4,1,DataType<double>::type);
	distCoeffs.at<double>(0) = 0;
	distCoeffs.at<double>(1) = 0;
	distCoeffs.at<double>(2) = 0;
	distCoeffs.at<double>(3) = 0;
	cout<< " Camera distorsion matrix " <<endl<< distCoeffs << endl;
	Mat rvec(3,1,DataType<double>::type);
	Mat tvec(3,1,DataType<double>::type);
	cv::solvePnP(objectPoints, imagePoints,cameraMatrix, distCoeffs, rvec, tvec);
	cout << " rvec: " << rvec << endl;
	cout << " tvec: " << tvec << endl;
	std::vector<cv::Point2f> projectedPoints;
	cv::projectPoints(objectPoints, rvec, tvec, cameraMatrix,distCoeffs, projectedPoints);
	for(unsigned int i =0; i < projectedPoints.size(); i++)
	{
	cout << "Image Points : " << imagePoints[i] << "projected to " << projectedPoints[i] <<endl;
	}
	waitKey();
	return 0;
	}
	std::vector<cv::Point2f> Generate2DPoints()
	{
	std::vector<cv::Point2f> points;
	float x,y;
	x=282;y=274;
	points.push_back(cv::Point2f(x,y));
	x=397;y=227;
	points.push_back(cv::Point2f(x,y));
	x=577;y=271;
	points.push_back(cv::Point2f(x,y));
	x=462;y=318;
	points.push_back(cv::Point2f(x,y));
	x=270;y=479;
	points.push_back(cv::Point2f(x,y));
	x=450;y=523;
	points.push_back(cv::Point2f(x,y));
	x=566;y=475;
	points.push_back(cv::Point2f(x,y));
	for(unsigned int i = 0; i < points.size(); ++i)
	{
	std::cout << points[i] << std::endl;
	}
	return points;
	}
	std::vector<cv::Point3f> Generate3DPoints()
	{
	std::vector<cv::Point3f> points;
	float x,y,z;
	x=.5;y=.5;z=-.5;
	points.push_back(cv::Point3f(x,y,z));
	x=.5;y=.5;z=.5;
	points.push_back(cv::Point3f(x,y,z));
	x=-.5;y=.5;z=.5;
	points.push_back(cv::Point3f(x,y,z));
	x=-.5;y=.5;z=-.5;
	points.push_back(cv::Point3f(x,y,z));
	x=.5;y=-.5;z=-.5;
	points.push_back(cv::Point3f(x,y,z));
	x=-.5;y=-.5;z=-.5;
	points.push_back(cv::Point3f(x,y,z));
	x=-.5;y=-.5;z=.5;
	points.push_back(cv::Point3f(x,y,z));
	for(unsigned int i = 0; i < points.size(); ++i)
	{
	std::cout << points[i] << std::endl;
	}
	return points;
	}

view raw gistfile1.cpp hosted with ❤ by GitHub