Initial DLS add

modules/calib3d/include/opencv2/calib3d.hpp

@@ -57,8 +57,9 @@ enum { LMEDS  = 4, //!< least-median algorithm
 
 enum { ITERATIVE = 0,
        EPNP      = 1, // F.Moreno-Noguer, V.Lepetit and P.Fua "EPnP: Efficient Perspective-n-Point Camera Pose Estimation"
-       P3P       = 2  // X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem"
-     };
+       P3P       = 2, // X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem"
+       DLS       = 3  // Joel A. Hesch and Stergios I. Roumeliotis. "A Direct Least-Squares (DLS) Method for PnP"
+};
 
 enum { CALIB_CB_ADAPTIVE_THRESH = 1,
        CALIB_CB_NORMALIZE_IMAGE = 2,

modules/calib3d/src/dls.cpp

+#include <iostream>
+#include "dls.h"
+
+
+dls::dls()
+{
+  // TODO Auto-generated constructor stub
+}
+
+
+dls::~dls()
+{
+  // TODO Auto-generated destructor stub
+}

modules/calib3d/src/dls.h

+#ifndef dls_h
+#define dls_h
+
+#include "opencv2/core/core_c.h"
+
+class dls
+{
+public:
+	dls();
+	~dls();
+
+private:
+
+};
+
+
+#endif

modules/calib3d/src/solvepnp.cpp

@@ -44,6 +44,7 @@
 #include "epnp.h"
 #include "p3p.h"
 #include "opencv2/calib3d/calib3d_c.h"
+#include "dls.h"
 
 #include <iostream>
 using namespace cv;
@@ -92,6 +93,19 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
                                      c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0,
                                      &c_rvec, &c_tvec, useExtrinsicGuess );
         return true;
+    }
+    else if (flags == DLS)
+    {
+    	std::cout << "DLS" << std::endl;
+    	cv::Mat undistortedPoints;
+    	cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
+
+    	//dls PnP;
+    	//  DO SOMETHING
+
+    	cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
+
+
     }
     else
         CV_Error(CV_StsBadArg, "The flags argument must be one of CV_ITERATIVE, CV_P3P or CV_EPNP");