cleaned dls code

modules/calib3d/src/dls.cpp

@@ -39,9 +39,6 @@ dls::dls(const cv::Mat& opoints, const cv::Mat& ipoints)
         init_points<cv::Point3f, cv::Point2d>(opoints, ipoints);
     else
         init_points<cv::Point3d, cv::Point2f>(opoints, ipoints);
-
-    //norm_z_vector();
-
 }
 
 dls::~dls()
@@ -57,13 +54,10 @@ bool dls::compute_pose(cv::Mat& R, cv::Mat& t)
     R_.push_back(roty(CV_PI/2));
     R_.push_back(rotz(CV_PI/2));
 
-    //cv::Mat t_mean = this->mean(p);
-
     // version that calls dls 3 times, to avoid Cayley singularity
     for (int i = 0; i < 3; ++i)
     {
         // Make a random rotation
-        //cv::Mat pp = R_[i] * ( p - cv::repeat(t_mean, 1, p.cols) );
         cv::Mat pp = R_[i] * ( p - cv::repeat(mn, 1, p.cols) );
 
         // clear for new data
@@ -78,7 +72,6 @@ bool dls::compute_pose(cv::Mat& R, cv::Mat& t)
         {
             if( cost_[j] < cost__ )
             {
-                //t_est__ = t_est_[j] - C_est_[j] * R_[i] * t_mean;
                 t_est__ = t_est_[j] - C_est_[j] * R_[i] * mn;
                 C_est__ = C_est_[j] * R_[i];
                 cost__ = cost_[j];
@@ -223,11 +216,6 @@ void dls::build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D)
     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)
     {

modules/calib3d/src/dls.h

@@ -79,88 +79,6 @@ 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: