last update

modules/calib3d/src/dls.h

-#ifndef DLS_H
-#define DLS_H
+#ifndef DLS_H_
+#define DLS_H_
 
 #include "precomp.hpp"
 
+using namespace std;
+
+
 class dls
 {
 public:
 	dls(const cv::Mat& opoints, const cv::Mat& ipoints);
-	~dls();
+	virtual ~dls();
+
+	template <typename OpointType, typename O, typename IpointType, typename I>
+	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;
+
+			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;
+		}
+	}
 
-	void init_vectors(const cv::Mat& opoints, const cv::Mat& ipoints);
-	void build_coeff_mattrix();
+	void norm_z_vector();
+	void build_coeff_matrix();
 	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 cayley_LS_M(const std::vector<double>& a, const std::vector<double>& b,
+			            const std::vector<double>& c, const std::vector<double>& u);
 
 private:
-	cv::Mat H;			// coeff matrix
-	cv::Mat A;
-	cv::Mat D_mat;
-	std::vector<double> f1coeff;
-	std::vector<double> f2coeff;
-	std::vector<double> f3coeff;
-	cv::Mat p;			// object points
-	cv::Mat z;			// image points
-	int N;				// number of input points
+	cv::Mat H, A, D_mat;	// coeff matrix
+	cv::Mat p;				// object points
+	cv::Mat z;				// image points
+	int N;					// number of input points
+	std::vector<double> f1coeff, f2coeff, f3coeff;
 };
 
 

modules/calib3d/src/solvepnp.cpp

@@ -100,11 +100,10 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
     	cv::Mat undistortedPoints;
     	cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
 
+    	//dls PnP(opoints, undistortedPoints);
+    	dls PnP(opoints, ipoints); // FOR TESTING
 
-    	dls PnP(opoints, undistortedPoints);
-    	//  DO SOMETHING
-
-    	cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
+    	//TODO: DO SOMETHING WITH R and t
 
     	return true;
     }