From 2653d745fd2b9aa47b0715c8e0dd3daa95dccf6e Mon Sep 17 00:00:00 2001
From: edgarriba <edgar.riba@gmail.com>
Date: Tue, 19 Aug 2014 00:35:22 +0200
Subject: [PATCH] updated sanity data

---
 modules/calib3d/perf/perf_pnp.cpp |  2 +-
 modules/calib3d/src/dls.cpp       | 18 +++++--
 modules/calib3d/src/dls.h         | 83 +++++++++++++++++++++++++++++++
 3 files changed, 99 insertions(+), 4 deletions(-)

diff --git a/modules/calib3d/perf/perf_pnp.cpp b/modules/calib3d/perf/perf_pnp.cpp
index 9ce399dad..87016dd92 100644
--- a/modules/calib3d/perf/perf_pnp.cpp
+++ b/modules/calib3d/perf/perf_pnp.cpp
@@ -19,7 +19,7 @@ typedef perf::TestBaseWithParam<int> PointsNum;
 
 PERF_TEST_P(PointsNum_Algo, solvePnP,
             testing::Combine(
-                testing::Values(/*4,*/ 3*9, 7*13), //TODO: find why results on 4 points are too unstable
+                testing::Values(4, 3*9, 7*13), //TODO: find why results on 4 points are too unstable
                 testing::Values((int)SOLVEPNP_ITERATIVE, (int)SOLVEPNP_EPNP)
                 )
             )
diff --git a/modules/calib3d/src/dls.cpp b/modules/calib3d/src/dls.cpp
index 9881aac80..213e82b0f 100644
--- a/modules/calib3d/src/dls.cpp
+++ b/modules/calib3d/src/dls.cpp
@@ -221,6 +221,12 @@ void dls::build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D)
 
     cv::Mat H = cv::Mat::zeros(3, 3, CV_64F);
     cv::Mat A = cv::Mat::zeros(3, 9, CV_64F);
+    cv::Mat pp_i(3, 1, CV_64F);
+
+    //Parallel_compute_A comp_A(&A, &pp, &z);
+    //cv::parallel_for_(cv::Range(0, N), comp_A);
+
+    //exit(-1);
 
     cv::Mat z_i(3, 1, CV_64F);
     for (int i = 0; i < N; ++i)
@@ -235,6 +241,9 @@ void dls::build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D)
     cv::solve(H, A, A, cv::DECOMP_NORMAL);
     H.release();
 
+    //Parallel_compute_D comp_D(&D, &A, &pp, &z);
+    //cv::parallel_for_(cv::Range(0, N), comp_D);
+
     cv::Mat ppi_A(3, 1, CV_64F);
     for (int i = 0; i < N; ++i)
     {
@@ -393,9 +402,6 @@ void dls::fill_coeff(const cv::Mat * D_mat)
 
 cv::Mat dls::LeftMultVec(const cv::Mat& v)
 {
-    //cv::Mat mat, row1, row2, row3;
-    //cv::Mat zeros16 = cv::Mat::zeros(1, 6, CV_64F);
-    //cv::Mat zeros13 = cv::Mat::zeros(1, 3, CV_64F);
     cv::Mat mat_ = cv::Mat::zeros(3, 9, CV_64F);
 
     for (int i = 0; i < 3; ++i)
@@ -404,6 +410,12 @@ cv::Mat dls::LeftMultVec(const cv::Mat& v)
         mat_.at<double>(i, 3*i + 1) = v.at<double>(1);
         mat_.at<double>(i, 3*i + 2) = v.at<double>(2);
     }
+    /*for (int i = 0; i < 3; ++i)
+    {
+		mat_.data[mat_.step[0]*i + mat_.step[1]* 3*i + 0] = v.at<double>(0);
+		mat_.data[mat_.step[0]*i + mat_.step[1]* 3*i + 1] = v.at<double>(1);
+		mat_.data[mat_.step[0]*i + mat_.step[1]* 3*i + 2] = v.at<double>(2);
+    }*/
 
     return mat_;
 }
diff --git a/modules/calib3d/src/dls.h b/modules/calib3d/src/dls.h
index a35c67a49..ab6b50524 100644
--- a/modules/calib3d/src/dls.h
+++ b/modules/calib3d/src/dls.h
@@ -3,6 +3,8 @@
 
 #include "precomp.hpp"
 
+#include <iostream>
+
 using namespace std;
 using namespace cv;
 
@@ -77,6 +79,87 @@ private:
     double cost__;                            // optimal found solution
 };
 
+class Parallel_compute_A : public cv::ParallelLoopBody
+{
+private:
+  cv::Mat eye, *A;
+  const cv::Mat *pp, *z;
+
+public:
+  Parallel_compute_A( cv::Mat * _A, const cv::Mat * _pp, const cv::Mat * _z)
+      : A(_A), pp(_pp), z(_z)
+  {
+      eye = cv::Mat::eye(3, 3, CV_64F);
+  }
+
+  cv::Mat leftMultVec(const cv::Mat& v) const
+  {
+      cv::Mat mat_(3, 9, CV_64F, 0.0);
+      for (int i = 0; i < 3; ++i)
+      {
+          mat_.at<double>(i, 3*i + 0) = v.at<double>(0);
+          mat_.at<double>(i, 3*i + 1) = v.at<double>(1);
+          mat_.at<double>(i, 3*i + 2) = v.at<double>(2);
+      }
+      return mat_;
+  }
+
+  virtual void operator()( const cv::Range &r ) const {
+    for ( int i = r.start; i != r.end; ++i)
+    {
+    	cv::Mat z_i(3, 1, CV_64F);
+    	cv::Mat pp_i(3, 1, CV_64F);
+    	int index = i;
+
+        z->col(index).copyTo(z_i);
+        pp->col(index).copyTo(pp_i);
+        *A += ( z_i*z_i.t() - eye ) * leftMultVec(pp_i);
+    }
+  }
+
+};
+
+class Parallel_compute_D : public cv::ParallelLoopBody
+{
+private:
+  cv::Mat eye, *D;
+  const cv::Mat *pp, *z, *A;
+
+public:
+  Parallel_compute_D( cv::Mat * _D, const cv::Mat * _A, const cv::Mat * _pp, const cv::Mat * _z)
+      : D(_D), A(_A), pp(_pp), z(_z)
+  {
+      eye = cv::Mat::eye(3, 3, CV_64F);
+  }
+
+  cv::Mat leftMultVec(const cv::Mat& v) const
+  {
+      cv::Mat mat_(3, 9, CV_64F, 0.0);
+      for (int i = 0; i < 3; ++i)
+      {
+          mat_.at<double>(i, 3*i + 0) = v.at<double>(0);
+          mat_.at<double>(i, 3*i + 1) = v.at<double>(1);
+          mat_.at<double>(i, 3*i + 2) = v.at<double>(2);
+      }
+      return mat_;
+  }
+
+  virtual void operator()( const cv::Range &r ) const {
+    for ( int i = r.start; i != r.end; ++i)
+    {
+    	cv::Mat z_i(3, 1, CV_64F);
+		cv::Mat pp_i(3, 1, CV_64F);
+		cv::Mat ppi_A(3, 9, CV_64F);
+		int index = i;
+
+        z->col(index).copyTo(z_i);
+        pp->col(index).copyTo(pp_i);
+		cv::Mat(leftMultVec(pp_i) + *A).copyTo(ppi_A);
+		*D += ppi_A.t() * ( eye - z_i*z_i.t() ) * ppi_A;
+    }
+  }
+
+};
 
 class EigenvalueDecomposition {
 private: