From fb67ab12570ecead1645d64eff0333b8760ff4aa Mon Sep 17 00:00:00 2001
From: edgarriba <edgar.riba@gmail.com>
Date: Tue, 15 Jul 2014 09:58:49 +0200
Subject: [PATCH] Initial DLS add

---
 modules/calib3d/include/opencv2/calib3d.hpp |  5 +++--
 modules/calib3d/src/dls.cpp                 | 14 ++++++++++++++
 modules/calib3d/src/dls.h                   | 17 +++++++++++++++++
 modules/calib3d/src/solvepnp.cpp            | 14 ++++++++++++++
 4 files changed, 48 insertions(+), 2 deletions(-)
 create mode 100644 modules/calib3d/src/dls.cpp
 create mode 100644 modules/calib3d/src/dls.h

diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp
index ba7eba930d..c0a050e526 100644
--- a/modules/calib3d/include/opencv2/calib3d.hpp
+++ b/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,
diff --git a/modules/calib3d/src/dls.cpp b/modules/calib3d/src/dls.cpp
new file mode 100644
index 0000000000..8fbeb3e47d
--- /dev/null
+++ b/modules/calib3d/src/dls.cpp
@@ -0,0 +1,14 @@
+#include <iostream>
+#include "dls.h"
+
+
+dls::dls()
+{
+  // TODO Auto-generated constructor stub
+}
+
+
+dls::~dls()
+{
+  // TODO Auto-generated destructor stub
+}
diff --git a/modules/calib3d/src/dls.h b/modules/calib3d/src/dls.h
new file mode 100644
index 0000000000..329e8042a1
--- /dev/null
+++ b/modules/calib3d/src/dls.h
@@ -0,0 +1,17 @@
+#ifndef dls_h
+#define dls_h
+
+#include "opencv2/core/core_c.h"
+
+class dls
+{
+public:
+	dls();
+	~dls();
+
+private:
+
+};
+
+
+#endif
diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp
index b76c46e6cb..f204463d6f 100644
--- a/modules/calib3d/src/solvepnp.cpp
+++ b/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");