MACRO for EIGEN libraries

diff --git a/modules/calib3d/src/dls.cpp b/modules/calib3d/src/dls.cpp
index 3a56010..c8d9260 100644 (file)
--- a/modules/calib3d/src/dls.cpp
+++ b/modules/calib3d/src/dls.cpp
@@ -2,30 +2,30 @@
 #include "dls.h"
 
 #include <iostream>
-#include <fstream>
 
-/*
 #ifdef HAVE_EIGEN
 #  if defined __GNUC__ && defined __APPLE__
 #    pragma GCC diagnostic ignored "-Wshadow"
 #  endif
 #  include <Eigen/Core>
+#  include <Eigen/Eigenvalues>
 #  include "opencv2/core/eigen.hpp"
 #endif
-*/
 
-#include <Eigen/Eigenvalues>
-#include <Eigen/Core>
+
+//#include <Eigen/Eigenvalues>
+//#include <Eigen/Core>
 
 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<double>::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<double>(k,0) >= -epsilon && eigenval_i.at<double>(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 4c4ef36..73eda92 100644 (file)
--- a/modules/calib3d/src/solvepnp.cpp
+++ b/modules/calib3d/src/solvepnp.cpp
@@ -49,11 +49,6 @@
 #include <iostream>
 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;
 }