From d1f4f6f4b82c6442ea01e9b243c47c512d1355f6 Mon Sep 17 00:00:00 2001 From: edgarriba Date: Tue, 12 Aug 2014 16:15:19 +0200 Subject: [PATCH] DLS performance modifications --- modules/calib3d/src/dls.cpp | 56 +++++++++++++------ modules/calib3d/src/dls.h | 27 ++++++--- modules/calib3d/src/solvepnp.cpp | 2 +- modules/calib3d/test/test_solvepnp_ransac.cpp | 2 +- 4 files changed, 60 insertions(+), 27 deletions(-) diff --git a/modules/calib3d/src/dls.cpp b/modules/calib3d/src/dls.cpp index b4ede53a2c..b6de1bc1e1 100644 --- a/modules/calib3d/src/dls.cpp +++ b/modules/calib3d/src/dls.cpp @@ -20,6 +20,7 @@ dls::dls(const cv::Mat& opoints, const cv::Mat& ipoints) N = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); p = cv::Mat(3, N, CV_64F); z = cv::Mat(3, N, CV_64F); + mn = cv::Mat::zeros(3, 1, CV_64F); cost__ = 9999; @@ -39,7 +40,7 @@ dls::dls(const cv::Mat& opoints, const cv::Mat& ipoints) else init_points(opoints, ipoints); - norm_z_vector(); + //norm_z_vector(); } @@ -48,7 +49,7 @@ dls::~dls() // TODO Auto-generated destructor stub } -void dls::norm_z_vector() +/*void dls::norm_z_vector() { // make z into unit vectors from normalized pixel coords for (int i = 0; i < N; ++i) @@ -63,7 +64,7 @@ void dls::norm_z_vector() z.at(1, i) /= sr; z.at(2, i) /= sr; } -} +}*/ bool dls::compute_pose(cv::Mat& R, cv::Mat& t) { @@ -73,13 +74,14 @@ 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); + //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(t_mean, 1, p.cols) ); + cv::Mat pp = R_[i] * ( p - cv::repeat(mn, 1, p.cols) ); // clear for new data C_est_.clear(); @@ -93,7 +95,8 @@ 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] * t_mean; + t_est__ = t_est_[j] - C_est_[j] * R_[i] * mn; C_est__ = C_est_[j] * R_[i]; cost__ = cost_[j]; } @@ -236,21 +239,28 @@ 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 z_i(3, 1, CV_64F); for (int i = 0; i < N; ++i) { - cv::Mat z_dot = z.col(i)*z.col(i).t(); - H += eye - z_dot; - A += ( z_dot - eye ) * LeftMultVec(pp.col(i)); + z.col(i).copyTo(z_i); + A += ( z_i*z_i.t() - eye ) * LeftMultVec(pp.col(i)); } + H = eye.mul(N) - z * z.t(); + // A\B - cv::solve(H, A, A); + cv::solve(H, A, A, cv::DECOMP_NORMAL); + H.release(); + cv::Mat ppi_A(3, 1, CV_64F); for (int i = 0; i < N; ++i) { - cv::Mat z_dot = z.col(i)*z.col(i).t(); - D += cv::Mat( LeftMultVec(pp.col(i)) + A ).t() * (eye-z_dot) * ( LeftMultVec(pp.col(i)) + A ); + //z_i = z.col(i); + z.col(i).copyTo(z_i); + ppi_A = LeftMultVec(pp.col(i)) + A; + D += ppi_A.t() * ( eye - z_i*z_i.t() ) * ppi_A; } + A.release(); // fill the coefficients fill_coeff(&D); @@ -265,9 +275,11 @@ void dls::build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D) cv::Mat M2_2 = M2(cv::Range(0,27), cv::Range(27,120)); cv::Mat M2_3 = M2(cv::Range(27,120), cv::Range(27,120)); cv::Mat M2_4 = M2(cv::Range(27,120), cv::Range(0,27)); + M2.release(); // A/B = B'\A' cv::Mat M2_5; cv::solve(M2_3.t(), M2_2.t(), M2_5); + M2_2.release(); M2_3.release(); // construct the multiplication matrix via schur compliment of the Macaulay // matrix @@ -398,20 +410,28 @@ 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, 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); - cv::hconcat(v.clone().t(), zeros16, row1); // first row + 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); + } + + /*cv::hconcat(v.clone().t(), zeros16, row1); // first row cv::hconcat(zeros13, v.clone().t(), row2); // second row cv::hconcat(row2, zeros13, row2); // second row cv::hconcat(zeros16, v.clone().t(), row3); // third row mat.push_back(row1); mat.push_back(row2); - mat.push_back(row3); + mat.push_back(row3);*/ - return mat; + return mat_; } cv::Mat dls::cayley_LS_M(const std::vector& a, const std::vector& b, const std::vector& c, const std::vector& u) diff --git a/modules/calib3d/src/dls.h b/modules/calib3d/src/dls.h index ba6c40a79a..aba7c7c571 100644 --- a/modules/calib3d/src/dls.h +++ b/modules/calib3d/src/dls.h @@ -26,15 +26,30 @@ private: p.at(1,i) = opoints.at(0,i).y; p.at(2,i) = opoints.at(0,i).z; - z.at(0,i) = ipoints.at(0,i).x; - z.at(1,i) = ipoints.at(0,i).y; - z.at(2,i) = (double)1; + // compute mean of object points + mn.at(0) += p.at(0,i); + mn.at(1) += p.at(1,i); + mn.at(2) += p.at(2,i); + + // make z into unit vectors from normalized pixel coords + double sr = std::pow(ipoints.at(0,i).x, 2) + + std::pow(ipoints.at(0,i).y, 2) + (double)1; + sr = std::sqrt(sr); + + z.at(0,i) = ipoints.at(0,i).x / sr; + z.at(1,i) = ipoints.at(0,i).y / sr; + z.at(2,i) = (double)1 / sr; } + + mn.at(0) /= (double)N; + mn.at(1) /= (double)N; + mn.at(2) /= (double)N; } - void norm_z_vector(); + //void norm_z_vector(); // main algorithm + cv::Mat LeftMultVec(const cv::Mat& v); void run_kernel(const cv::Mat& pp); void build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D); void compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Mat& eigenval_imag, @@ -42,7 +57,6 @@ private: void fill_coeff(const cv::Mat * D); // useful functions - cv::Mat LeftMultVec(const cv::Mat& v); cv::Mat cayley_LS_M(const std::vector& a, const std::vector& b, const std::vector& c, const std::vector& u); cv::Mat Hessian(const double s[]); @@ -57,8 +71,7 @@ private: bool is_empty(const cv::Mat * v); bool positive_eigenvalues(const cv::Mat * eigenvalues); - - cv::Mat p, z; // object-image points + cv::Mat p, z, mn; // object-image points int N; // number of input points std::vector f1coeff, f2coeff, f3coeff, cost_; // coefficient for coefficients matrix std::vector C_est_, t_est_; // optimal candidates diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index c4cacb4ce7..f31928483e 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -108,7 +108,7 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints, return result; } else - CV_Error(CV_StsBadArg, "The flags argument must be one of CV_ITERATIVE, CV_P3P, CV_EPNP or CV_DLS"); + CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS"); return false; } diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp index 167415eda1..7780462c15 100644 --- a/modules/calib3d/test/test_solvepnp_ransac.cpp +++ b/modules/calib3d/test/test_solvepnp_ransac.cpp @@ -196,7 +196,7 @@ public: eps[SOLVEPNP_ITERATIVE] = 1.0e-6; eps[SOLVEPNP_EPNP] = 1.0e-6; eps[SOLVEPNP_P3P] = 1.0e-4; - eps[SOLVEPNP_DLS] = 1.0e-6; + eps[SOLVEPNP_DLS] = 1.0e-4; totalTestsCount = 1000; }