MACRO for EIGEN libraries

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
 
 }
 

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;
 }