potok/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2026-07-30 15:53:03 +04:00
Code Packages Releases Wiki Activity

Merge pull request #8585 from tonyke1993:ap3p

Browse Source 
Enable p3p and ap3p in solvePnPRansac (#8585)

* add paper info

* allow p3p and ap3p being RANSAC kernel

* keep previous code

* apply catrees comment

* fix getMat

* add comment

* add solvep3p test

* test return value

* fix warnings
This commit is contained in:
Tong Ke
2017-06-28 05:27:30 -07:00
committed by Alexander Alekhin
parent dcf3d988d5
commit 8088d6785a
7 changed files with 260 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/calib3d/src/ap3p.cpp
+33 -1
View File
@@ -313,6 +313,38 @@ bool ap3p::solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Ma
return result;
}
int ap3p::solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, const cv::Mat &opoints, const cv::Mat &ipoints) {
CV_INSTRUMENT_REGION()
double rotation_matrix[4][3][3], translation[4][3];
std::vector<double> points;
if (opoints.depth() == ipoints.depth()) {
if (opoints.depth() == CV_32F)
extract_points<cv::Point3f, cv::Point2f>(opoints, ipoints, points);
else
extract_points<cv::Point3d, cv::Point2d>(opoints, ipoints, points);
} else if (opoints.depth() == CV_32F)
extract_points<cv::Point3f, cv::Point2d>(opoints, ipoints, points);
else
extract_points<cv::Point3d, cv::Point2f>(opoints, ipoints, points);
int solutions = solve(rotation_matrix, translation,
points[0], points[1], points[2], points[3], points[4],
points[5], points[6], points[7], points[8], points[9],
points[10], points[11], points[12], points[13], points[14]);
for (int i = 0; i < solutions; i++) {
cv::Mat R, tvec;
cv::Mat(3, 1, CV_64F, translation[i]).copyTo(tvec);
cv::Mat(3, 3, CV_64F, rotation_matrix[i]).copyTo(R);
Rs.push_back(R);
tvecs.push_back(tvec);
}
return solutions;
}
bool
ap3p::solve(double R[3][3], double t[3], double mu0, double mv0, double X0, double Y0, double Z0, double mu1,
double mv1,
@@ -383,4 +415,4 @@ int ap3p::solve(double R[4][3][3], double t[4][3], double mu0, double mv0, doubl
return computePoses(featureVectors, worldPoints, R, t);
}
}
}
modules/calib3d/src/ap3p.h
+4 -2
View File
@@ -17,8 +17,9 @@ private:
template<typename OpointType, typename IpointType>
void extract_points(const cv::Mat &opoints, const cv::Mat &ipoints, std::vector<double> &points) {
points.clear();
points.resize(20);
for (int i = 0; i < 4; i++) {
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
points.resize(5*npoints);
for (int i = 0; i < npoints; i++) {
points[i * 5] = ipoints.at<IpointType>(i).x * fx + cx;
points[i * 5 + 1] = ipoints.at<IpointType>(i).y * fy + cy;
points[i * 5 + 2] = opoints.at<OpointType>(i).x;
@@ -39,6 +40,7 @@ public:
ap3p(cv::Mat cameraMatrix);
bool solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Mat &ipoints);
int solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, const cv::Mat &opoints, const cv::Mat &ipoints);
int solve(double R[4][3][3], double t[4][3],
double mu0, double mv0, double X0, double Y0, double Z0,
modules/calib3d/src/p3p.cpp
+35
View File
@@ -57,6 +57,41 @@ bool p3p::solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat
return result;
}
int p3p::solve(std::vector<cv::Mat>& Rs, std::vector<cv::Mat>& tvecs, const cv::Mat& opoints, const cv::Mat& ipoints)
{
CV_INSTRUMENT_REGION()
double rotation_matrix[4][3][3], translation[4][3];
std::vector<double> points;
if (opoints.depth() == ipoints.depth())
{
if (opoints.depth() == CV_32F)
extract_points<cv::Point3f,cv::Point2f>(opoints, ipoints, points);
else
extract_points<cv::Point3d,cv::Point2d>(opoints, ipoints, points);
}
else if (opoints.depth() == CV_32F)
extract_points<cv::Point3f,cv::Point2d>(opoints, ipoints, points);
else
extract_points<cv::Point3d,cv::Point2f>(opoints, ipoints, points);
int solutions = solve(rotation_matrix, translation,
points[0], points[1], points[2], points[3], points[4],
points[5], points[6], points[7], points[8], points[9],
points[10], points[11], points[12], points[13], points[14]);
for (int i = 0; i < solutions; i++) {
cv::Mat R, tvec;
cv::Mat(3, 1, CV_64F, translation[i]).copyTo(tvec);
cv::Mat(3, 3, CV_64F, rotation_matrix[i]).copyTo(R);
Rs.push_back(R);
tvecs.push_back(tvec);
}
return solutions;
}
bool p3p::solve(double R[3][3], double t[3],
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,
modules/calib3d/src/p3p.h
+4 -2
View File
@@ -11,6 +11,7 @@ class p3p
p3p(cv::Mat cameraMatrix);
bool solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat& ipoints);
int solve(std::vector<cv::Mat>& Rs, std::vector<cv::Mat>& tvecs, const cv::Mat& opoints, const cv::Mat& ipoints);
int solve(double R[4][3][3], double t[4][3],
double mu0, double mv0, double X0, double Y0, double Z0,
double mu1, double mv1, double X1, double Y1, double Z1,
@@ -34,8 +35,9 @@ class p3p
void extract_points(const cv::Mat& opoints, const cv::Mat& ipoints, std::vector<double>& points)
{
points.clear();
points.resize(20);
for(int i = 0; i < 4; i++)
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
points.resize(5*npoints);
for(int i = 0; i < npoints; i++)
{
points[i*5] = ipoints.at<IpointType>(i).x*fx + cx;
points[i*5+1] = ipoints.at<IpointType>(i).y*fy + cy;
modules/calib3d/src/solvepnp.cpp
+59 -1
View File
@@ -268,7 +268,12 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
int model_points = 5;
int ransac_kernel_method = SOLVEPNP_EPNP;
if( npoints == 4 )
if( flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P)
{
model_points = 4;
ransac_kernel_method = flags;
}
else if( npoints == 4 )
{
model_points = 4;
ransac_kernel_method = SOLVEPNP_P3P;
@@ -337,4 +342,57 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
return true;
}
int solveP3P( InputArray _opoints, InputArray _ipoints,
InputArray _cameraMatrix, InputArray _distCoeffs,
OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, int flags) {
CV_INSTRUMENT_REGION()
Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
CV_Assert( npoints == 3 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
CV_Assert( flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P );
Mat cameraMatrix0 = _cameraMatrix.getMat();
Mat distCoeffs0 = _distCoeffs.getMat();
Mat cameraMatrix = Mat_<double>(cameraMatrix0);
Mat distCoeffs = Mat_<double>(distCoeffs0);
Mat undistortedPoints;
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
std::vector<Mat> Rs, ts;
int solutions = 0;
if (flags == SOLVEPNP_P3P)
{
p3p P3Psolver(cameraMatrix);
solutions = P3Psolver.solve(Rs, ts, opoints, undistortedPoints);
}
else if (flags == SOLVEPNP_AP3P)
{
ap3p P3Psolver(cameraMatrix);
solutions = P3Psolver.solve(Rs, ts, opoints, undistortedPoints);
}
if (solutions == 0) {
return 0;
}
if (_rvecs.needed()) {
_rvecs.create(solutions, 1, CV_64F);
}
if (_tvecs.needed()) {
_tvecs.create(solutions, 1, CV_64F);
}
for (int i = 0; i < solutions; i++) {
Mat rvec;
Rodrigues(Rs[i], rvec);
_tvecs.getMatRef(i) = ts[i];
_rvecs.getMatRef(i) = rvec;
}
return solutions;
}
}