From 15704c384e17924a46d6bb9933dfa6ac958decd9 Mon Sep 17 00:00:00 2001 From: edgarriba Date: Tue, 19 Aug 2014 00:52:09 +0200 Subject: [PATCH] cleaned dls code --- modules/calib3d/src/dls.cpp | 12 ------- modules/calib3d/src/dls.h | 82 --------------------------------------------- 2 files changed, 94 deletions(-) diff --git a/modules/calib3d/src/dls.cpp b/modules/calib3d/src/dls.cpp index 213e82b..781ca8d 100644 --- a/modules/calib3d/src/dls.cpp +++ b/modules/calib3d/src/dls.cpp @@ -39,9 +39,6 @@ dls::dls(const cv::Mat& opoints, const cv::Mat& ipoints) init_points(opoints, ipoints); else init_points(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) { diff --git a/modules/calib3d/src/dls.h b/modules/calib3d/src/dls.h index ab6b505..30321fc 100644 --- a/modules/calib3d/src/dls.h +++ b/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(i, 3*i + 0) = v.at(0); - mat_.at(i, 3*i + 1) = v.at(1); - mat_.at(i, 3*i + 2) = v.at(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(i, 3*i + 0) = v.at(0); - mat_.at(i, 3*i + 1) = v.at(1); - mat_.at(i, 3*i + 2) = v.at(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: -- 2.7.4