DLS working

modules/calib3d/src/dls.h

@@ -12,52 +12,58 @@ public:
 	dls(const cv::Mat& opoints, const cv::Mat& ipoints);
 	~dls();
 
-	void compute_pose(cv::Mat& R, cv::Mat& t);
+	bool compute_pose(cv::Mat& R, cv::Mat& t);
 
 private:
 
-	template <typename OpointType, typename O, typename IpointType, typename I>
+	// initialisation
+	template <typename OpointType, typename IpointType>
 	void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
 	{
 		for(int i = 0; i < N; i++)
 		{
-			p.at<O>(0,i) = opoints.at<OpointType>(0,i).x;
-			p.at<O>(1,i) = opoints.at<OpointType>(0,i).y;
-			p.at<O>(2,i) = opoints.at<OpointType>(0,i).z;
+			p.at<double>(0,i) = opoints.at<OpointType>(0,i).x;
+			p.at<double>(1,i) = opoints.at<OpointType>(0,i).y;
+			p.at<double>(2,i) = opoints.at<OpointType>(0,i).z;
 
-			z.at<I>(0,i) = ipoints.at<IpointType>(0,i).x;
-			z.at<I>(1,i) = ipoints.at<IpointType>(0,i).y;
-			z.at<I>(2,i) = (I)1;
+			z.at<double>(0,i) = ipoints.at<IpointType>(0,i).x;
+			z.at<double>(1,i) = ipoints.at<IpointType>(0,i).y;
+			z.at<double>(2,i) = (double)1;
 		}
 	}
 
 	void norm_z_vector();
+
+	// main algorithm
+	void run_kernel(const cv::Mat& pp);
 	void build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D);
 	void compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Mat& eigenval_imag,
 			                                     cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag);
-	double min_val(const std::vector<double>& values);
+	void fill_coeff(const cv::Mat * D);
+
+	// useful functions
+	cv::Mat LeftMultVec(const cv::Mat& v);
+	cv::Mat cayley_LS_M(const std::vector<double>& a, const std::vector<double>& b,
+			            const std::vector<double>& c, const std::vector<double>& u);
+	cv::Mat Hessian(const double s[]);
+	cv::Mat cayley2rotbar(const cv::Mat& s);
+	cv::Mat skewsymm(const cv::Mat * X1);
+
+	// extra functions
 	cv::Mat rotx(const double t);
 	cv::Mat roty(const double t);
 	cv::Mat rotz(const double t);
 	cv::Mat mean(const cv::Mat& M);
-	void fill_coeff(const cv::Mat * D);
-	cv::Mat LeftMultVec(const cv::Mat& v);
-	cv::Mat cayley_LS_M(const std::vector<double>& a, const std::vector<double>& b,
-			            const std::vector<double>& c, const std::vector<double>& u);
-	bool positive_eigenvalues(const cv::Mat& eigenvalues);
-	cv::Mat Hessian(const double s[]);
-	cv::Mat cayley2rotbar(const double s[]);
-	cv::Mat skewsymm(const double X1[]);
 	bool is_empty(const cv::Mat * v);
-	void run_kernel(const cv::Mat& pp);
+	bool positive_eigenvalues(const cv::Mat * eigenvalues);
+
 
 	cv::Mat p, z;		// object-image points
 	int N;				// number of input points
-	std::vector<double> f1coeff, f2coeff, f3coeff, cost_;
-	std::vector<cv::Mat> C_est_, t_est_;	// vector to store candidate
-	cv::Mat C_est__, t_est__;	// best found solution
-	double cost__;
-
+	std::vector<double> f1coeff, f2coeff, f3coeff, cost_; // coefficient for coefficients matrix
+	std::vector<cv::Mat> C_est_, t_est_;	// optimal candidates
+	cv::Mat C_est__, t_est__;				// optimal found solution
+	double cost__;							// optimal found solution
 };
 
 #endif // DLS_H

modules/calib3d/src/solvepnp.cpp

@@ -49,6 +49,11 @@
 #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 )
@@ -96,23 +101,16 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
     }
     else if (flags == DLS)
     {
-    	std::cout << "DLS" << std::endl;
     	cv::Mat undistortedPoints;
-    	//cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
+    	cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
+
+    	dls PnP(opoints, undistortedPoints);
 
     	cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
-
-    	//dls PnP(opoints, undistortedPoints);
-    	dls PnP(opoints, ipoints); // FOR TESTING
-
-    	PnP.compute_pose(R, tvec);
-
-    	cout << "after dls compute pose" << endl;
-
-    	//TODO: DO SOMETHING WITH R and t
-        //cv::Rodrigues(R, rvec);
-
-    	return true;
+    	bool result = PnP.compute_pose(R, tvec);
+        if (result)
+        	cv::Rodrigues(R, rvec);
+        return result;
     }
     else
         CV_Error(CV_StsBadArg, "The flags argument must be one of CV_ITERATIVE, CV_P3P or CV_EPNP");