diff --git a/modules/calib3d/src/dls.cpp b/modules/calib3d/src/dls.cpp index 3a5601043a..c8d9260fba 100644 --- a/modules/calib3d/src/dls.cpp +++ b/modules/calib3d/src/dls.cpp @@ -2,30 +2,30 @@ #include "dls.h" #include -#include -/* #ifdef HAVE_EIGEN # if defined __GNUC__ && defined __APPLE__ # pragma GCC diagnostic ignored "-Wshadow" # endif # include +# include # include "opencv2/core/eigen.hpp" #endif -*/ -#include -#include + +//#include +//#include using namespace std; dls::dls(const cv::Mat& opoints, const cv::Mat& ipoints) { + N = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); p = cv::Mat(3, N, CV_64F); z = cv::Mat(3, N, CV_64F); - cost__ = std::numeric_limits::infinity(); + cost__ = 9999; f1coeff.resize(21); f2coeff.resize(21); @@ -117,7 +117,6 @@ bool dls::compute_pose(cv::Mat& R, cv::Mat& t) void dls::run_kernel(const cv::Mat& pp) { - cv::Mat Mtilde(27, 27, CV_64F); cv::Mat D = cv::Mat::zeros(9, 9, CV_64F); build_coeff_matrix(pp, Mtilde, D); @@ -125,7 +124,6 @@ void dls::run_kernel(const cv::Mat& pp) cv::Mat eigenval_r, eigenval_i, eigenvec_r, eigenvec_i; compute_eigenvec(Mtilde, eigenval_r, eigenval_i, eigenvec_r, eigenvec_i); - /* * Now check the solutions */ @@ -146,8 +144,6 @@ void dls::run_kernel(const cv::Mat& pp) cv::Mat V_k; cv::solve(V_kB.t(), V_kA.t(), V_k); // A/B = B'\A' cv::Mat( V_k.t()).copyTo( eigenvec_r.col(k) ); - // TODO: check imaginari part to filter solutions - //if (imag(V(2,k)) == 0) const double epsilon = 1e-4; if( eigenval_i.at(k,0) >= -epsilon && eigenval_i.at(k,0) <= epsilon ) @@ -230,6 +226,7 @@ void dls::run_kernel(const cv::Mat& pp) cost_.push_back(cost_valid); } } + } void dls::build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D) @@ -287,6 +284,7 @@ void dls::compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Ma { // EIGENVALUES AND EIGENVECTORS +#ifdef HAVE_EIGEN Eigen::MatrixXd Mtilde_eig, zeros_eig; cv::cv2eigen(Mtilde, Mtilde_eig); cv::cv2eigen(cv::Mat::zeros(27, 27, CV_64F), zeros_eig); @@ -307,6 +305,7 @@ void dls::compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Ma cv::eigen2cv(eigval_imag, eigenval_imag); cv::eigen2cv(eigvec_real, eigenvec_real); cv::eigen2cv(eigvec_imag, eigenvec_imag); +#endif } diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 4c4ef36301..73eda92674 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -49,11 +49,6 @@ #include using namespace cv; -void MatrixSize(const cv::Mat& mat) -{ - cout << mat.rows << "x" << mat.cols << endl; -} - bool cv::solvePnP( InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags ) @@ -101,19 +96,25 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints, } else if (flags == DLS) { + bool result = false; +#ifdef HAVE_EIGEN + cv::Mat undistortedPoints; cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); dls PnP(opoints, undistortedPoints); cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); - bool result = PnP.compute_pose(R, tvec); + result = PnP.compute_pose(R, tvec); if (result) cv::Rodrigues(R, rvec); +#else + std::cout << "EIGEN library needed for DLS" << std::endl; +#endif return result; } else - CV_Error(CV_StsBadArg, "The flags argument must be one of CV_ITERATIVE, CV_P3P or CV_EPNP"); + CV_Error(CV_StsBadArg, "The flags argument must be one of CV_ITERATIVE, CV_P3P, CV_EPNP or CV_DLS"); return false; }