generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

fixed compilation errors in epnp with gcc

Browse Source
This commit is contained in:
Alexander Shishkov 2011-12-20 12:10:12 +00:00
parent c5d8ec4ac0
commit 6fa936472e
1 changed files with 27 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
modules/calib3d/src/epnp.cpp
View File

@ -2,37 +2,41 @@
using namespace std; using namespace std;
#include "precomp.hpp" #include "precomp.hpp"
#include "epnp.h" #include "epnp.h"
namespace cv namespace cv
{ {
double ePnP( InputArray _opoints, InputArray _ipoints, double ePnP( InputArray _opoints, InputArray _ipoints,
InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _cameraMatrix, InputArray _distCoeffs,
OutputArray _rvec, OutputArray _tvec) OutputArray _rvec, OutputArray _tvec)
{ {
Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) ); CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
Mat undistortedPoints; Mat undistortedPoints;
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
epnp PnP; epnp PnP;
PnP.set_internal_parameters(cameraMatrix.at<double> (0, 2), cameraMatrix.at<double> (1, 2), cameraMatrix.at<double> (0, 0), cameraMatrix.at<double> (1, 1)); PnP.set_internal_parameters(cameraMatrix.at<double> (0, 2), cameraMatrix.at<double> (1, 2), cameraMatrix.at<double> (0, 0), cameraMatrix.at<double> (1, 1));
PnP.set_maximum_number_of_correspondences(npoints); PnP.set_maximum_number_of_correspondences(npoints);
PnP.reset_correspondences(); PnP.reset_correspondences();
for(int i = 0; i < npoints; i++) { for(int i = 0; i < npoints; i++) {
PnP.add_correspondence(opoints.at<Point3d>(0,i).x, opoints.at<Point3d>(0,i).y, opoints.at<Point3d>(0,i).z, undistortedPoints.at<Point2d>(0,i).x* cameraMatrix.at<double> (0, 0) + cameraMatrix.at<double> (0, 2), PnP.add_correspondence(opoints.at<Point3d>(0,i).x, opoints.at<Point3d>(0,i).y, opoints.at<Point3d>(0,i).z, undistortedPoints.at<Point2d>(0,i).x* cameraMatrix.at<double> (0, 0) + cameraMatrix.at<double> (0, 2),
undistortedPoints.at<Point2d>(0,i).y* cameraMatrix.at<double> (1, 1) + cameraMatrix.at<double> (1, 2)); undistortedPoints.at<Point2d>(0,i).y* cameraMatrix.at<double> (1, 1) + cameraMatrix.at<double> (1, 2));
} }
double R_est[3][3], t_est[3]; double R_est[3][3], t_est[3];
double error = PnP.compute_pose(R_est, t_est); double error = PnP.compute_pose(R_est, t_est);
_tvec.create(3,1,CV_64F); _tvec.create(3,1,CV_64F);
_rvec.create(3,1,CV_64F); _rvec.create(3,1,CV_64F);
Mat(3, 1, CV_64FC1, t_est).copyTo(_tvec.getMat()); Mat t = Mat(3, 1, CV_64FC1, t_est);
Rodrigues(Mat(3, 3, CV_64FC1, R_est), _rvec.getMat()); Mat tvec = _tvec.getMat();
return error; t.copyTo(tvec);
Mat R = Mat(3, 3, CV_64FC1, R_est);
Mat rvec = _rvec.getMat();
Rodrigues(R, rvec);
return error;
} }
} }