From 649f747f8a418c6df5853b7b0940ac3826573ad2 Mon Sep 17 00:00:00 2001 From: h6197627 <44726212+h6197627@users.noreply.github.com> Date: Mon, 10 Jan 2022 13:51:07 +0200 Subject: [PATCH] Merge pull request #21405 from h6197627:3.4 * Use c++ namespaces explicitly * Add root cv c++ namespace --- modules/calib3d/src/dls.cpp | 208 ++++++++++++++++--------------- modules/calib3d/src/dls.h | 52 ++++---- modules/calib3d/src/solvepnp.cpp | 20 +-- 3 files changed, 141 insertions(+), 139 deletions(-) diff --git a/modules/calib3d/src/dls.cpp b/modules/calib3d/src/dls.cpp index a0ff0c3e1e..f0b9d850e9 100644 --- a/modules/calib3d/src/dls.cpp +++ b/modules/calib3d/src/dls.cpp @@ -21,15 +21,15 @@ # include "opencv2/core/eigen.hpp" #endif -using namespace std; +namespace cv { -dls::dls(const cv::Mat& opoints, const cv::Mat& ipoints) +dls::dls(const Mat& opoints, const 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); + N = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); + p = Mat(3, N, CV_64F); + z = Mat(3, N, CV_64F); + mn = Mat::zeros(3, 1, CV_64F); cost__ = 9999; @@ -40,14 +40,14 @@ dls::dls(const cv::Mat& opoints, const cv::Mat& ipoints) if (opoints.depth() == ipoints.depth()) { if (opoints.depth() == CV_32F) - init_points(opoints, ipoints); + init_points(opoints, ipoints); else - init_points(opoints, ipoints); + init_points(opoints, ipoints); } else if (opoints.depth() == CV_32F) - init_points(opoints, ipoints); + init_points(opoints, ipoints); else - init_points(opoints, ipoints); + init_points(opoints, ipoints); } dls::~dls() @@ -55,10 +55,10 @@ dls::~dls() // TODO Auto-generated destructor stub } -bool dls::compute_pose(cv::Mat& R, cv::Mat& t) +bool dls::compute_pose(Mat& R, Mat& t) { - std::vector R_; + std::vector R_; R_.push_back(rotx(CV_PI/2)); R_.push_back(roty(CV_PI/2)); R_.push_back(rotz(CV_PI/2)); @@ -67,7 +67,7 @@ bool dls::compute_pose(cv::Mat& R, cv::Mat& t) for (int i = 0; i < 3; ++i) { // Make a random rotation - cv::Mat pp = R_[i] * ( p - cv::repeat(mn, 1, p.cols) ); + Mat pp = R_[i] * ( p - repeat(mn, 1, p.cols) ); // clear for new data C_est_.clear(); @@ -99,13 +99,13 @@ bool dls::compute_pose(cv::Mat& R, cv::Mat& t) return false; } -void dls::run_kernel(const cv::Mat& pp) +void dls::run_kernel(const Mat& pp) { - cv::Mat Mtilde(27, 27, CV_64F); - cv::Mat D = cv::Mat::zeros(9, 9, CV_64F); + Mat Mtilde(27, 27, CV_64F); + Mat D = Mat::zeros(9, 9, CV_64F); build_coeff_matrix(pp, Mtilde, D); - cv::Mat eigenval_r, eigenval_i, eigenvec_r, eigenvec_i; + Mat eigenval_r, eigenval_i, eigenvec_r, eigenvec_i; compute_eigenvec(Mtilde, eigenval_r, eigenval_i, eigenvec_r, eigenvec_i); /* @@ -115,16 +115,16 @@ void dls::run_kernel(const cv::Mat& pp) // extract the optimal solutions from the eigen decomposition of the // Multiplication matrix - cv::Mat sols = cv::Mat::zeros(3, 27, CV_64F); + Mat sols = Mat::zeros(3, 27, CV_64F); std::vector cost; int count = 0; for (int k = 0; k < 27; ++k) { // V(:,k) = V(:,k)/V(1,k); - cv::Mat V_kA = eigenvec_r.col(k); // 27x1 - cv::Mat V_kB = cv::Mat(1, 1, z.depth(), V_kA.at(0)); // 1x1 - cv::Mat V_k; cv::solve(V_kB.t(), V_kA.t(), V_k); // A/B = B'\A' - cv::Mat( V_k.t()).copyTo( eigenvec_r.col(k) ); + Mat V_kA = eigenvec_r.col(k); // 27x1 + Mat V_kB = Mat(1, 1, z.depth(), V_kA.at(0)); // 1x1 + Mat V_k; solve(V_kB.t(), V_kA.t(), V_k); // A/B = B'\A' + Mat( V_k.t()).copyTo( eigenvec_r.col(k) ); //if (imag(V(2,k)) == 0) #ifdef HAVE_EIGEN @@ -138,24 +138,24 @@ void dls::run_kernel(const cv::Mat& pp) stmp[1] = eigenvec_r.at(3, k); stmp[2] = eigenvec_r.at(1, k); - cv::Mat H = Hessian(stmp); + Mat H = Hessian(stmp); - cv::Mat eigenvalues, eigenvectors; - cv::eigen(H, eigenvalues, eigenvectors); + Mat eigenvalues, eigenvectors; + eigen(H, eigenvalues, eigenvectors); if(positive_eigenvalues(&eigenvalues)) { // sols(:,i) = stmp; - cv::Mat stmp_mat(3, 1, CV_64F, &stmp); + Mat stmp_mat(3, 1, CV_64F, &stmp); stmp_mat.copyTo( sols.col(count) ); - cv::Mat Cbar = cayley2rotbar(stmp_mat); - cv::Mat Cbarvec = Cbar.reshape(1,1).t(); + Mat Cbar = cayley2rotbar(stmp_mat); + Mat Cbarvec = Cbar.reshape(1,1).t(); // cost(i) = CbarVec' * D * CbarVec; - cv::Mat cost_mat = Cbarvec.t() * D * Cbarvec; + Mat cost_mat = Cbarvec.t() * D * Cbarvec; cost.push_back( cost_mat.at(0) ); count++; @@ -166,30 +166,30 @@ void dls::run_kernel(const cv::Mat& pp) // extract solutions sols = sols.clone().colRange(0, count); - std::vector C_est, t_est; + std::vector C_est, t_est; for (int j = 0; j < sols.cols; ++j) { // recover the optimal orientation // C_est(:,:,j) = 1/(1 + sols(:,j)' * sols(:,j)) * cayley2rotbar(sols(:,j)); - cv::Mat sols_j = sols.col(j); - double sols_mult = 1./(1.+cv::Mat( sols_j.t() * sols_j ).at(0)); - cv::Mat C_est_j = cayley2rotbar(sols_j).mul(sols_mult); + Mat sols_j = sols.col(j); + double sols_mult = 1./(1.+Mat( sols_j.t() * sols_j ).at(0)); + Mat C_est_j = cayley2rotbar(sols_j).mul(sols_mult); C_est.push_back( C_est_j ); - cv::Mat A2 = cv::Mat::zeros(3, 3, CV_64F); - cv::Mat b2 = cv::Mat::zeros(3, 1, CV_64F); + Mat A2 = Mat::zeros(3, 3, CV_64F); + Mat b2 = Mat::zeros(3, 1, CV_64F); for (int i = 0; i < N; ++i) { - cv::Mat eye = cv::Mat::eye(3, 3, CV_64F); - cv::Mat z_mul = z.col(i)*z.col(i).t(); + Mat eye = Mat::eye(3, 3, CV_64F); + Mat z_mul = z.col(i)*z.col(i).t(); A2 += eye - z_mul; b2 += (z_mul - eye) * C_est_j * pp.col(i); } // recover the optimal translation - cv::Mat X2; cv::solve(A2, b2, X2); // A\B + Mat X2; solve(A2, b2, X2); // A\B t_est.push_back(X2); } @@ -197,12 +197,12 @@ void dls::run_kernel(const cv::Mat& pp) // check that the points are infront of the center of perspectivity for (int k = 0; k < sols.cols; ++k) { - cv::Mat cam_points = C_est[k] * pp + cv::repeat(t_est[k], 1, pp.cols); - cv::Mat cam_points_k = cam_points.row(2); + Mat cam_points = C_est[k] * pp + repeat(t_est[k], 1, pp.cols); + Mat cam_points_k = cam_points.row(2); if(is_empty(&cam_points_k)) { - cv::Mat C_valid = C_est[k], t_valid = t_est[k]; + Mat C_valid = C_est[k], t_valid = t_est[k]; double cost_valid = cost[k]; C_est_.push_back(C_valid); @@ -213,20 +213,20 @@ void dls::run_kernel(const cv::Mat& pp) } -void dls::build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D) +void dls::build_coeff_matrix(const Mat& pp, Mat& Mtilde, Mat& D) { CV_Assert(!pp.empty() && N > 0); - cv::Mat eye = cv::Mat::eye(3, 3, CV_64F); + Mat eye = Mat::eye(3, 3, CV_64F); // build coeff matrix // An intermediate matrix, the inverse of what is called "H" in the paper // (see eq. 25) - 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); + Mat H = Mat::zeros(3, 3, CV_64F); + Mat A = Mat::zeros(3, 9, CV_64F); + Mat pp_i(3, 1, CV_64F); - cv::Mat z_i(3, 1, CV_64F); + Mat z_i(3, 1, CV_64F); for (int i = 0; i < N; ++i) { z.col(i).copyTo(z_i); @@ -236,10 +236,10 @@ void dls::build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D) H = eye.mul(N) - z * z.t(); // A\B - cv::solve(H, A, A, cv::DECOMP_NORMAL); + solve(H, A, A, DECOMP_NORMAL); H.release(); - cv::Mat ppi_A(3, 1, CV_64F); + Mat ppi_A(3, 1, CV_64F); for (int i = 0; i < N; ++i) { z.col(i).copyTo(z_i); @@ -253,18 +253,18 @@ void dls::build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D) // generate random samples std::vector u(5); - cv::randn(u, 0, 200); + randn(u, 0, 200); - cv::Mat M2 = cayley_LS_M(f1coeff, f2coeff, f3coeff, u); + Mat M2 = cayley_LS_M(f1coeff, f2coeff, f3coeff, u); - cv::Mat M2_1 = M2(cv::Range(0,27), cv::Range(0,27)); - 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)); + Mat M2_1 = M2(Range(0,27), Range(0,27)); + Mat M2_2 = M2(Range(0,27), Range(27,120)); + Mat M2_3 = M2(Range(27,120), Range(27,120)); + Mat M2_4 = M2(Range(27,120), Range(0,27)); M2.release(); // A/B = B'\A' - cv::Mat M2_5; cv::solve(M2_3.t(), M2_2.t(), M2_5); + Mat M2_5; 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 @@ -273,13 +273,13 @@ void dls::build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D) } -void dls::compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Mat& eigenval_imag, - cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag) +void dls::compute_eigenvec(const Mat& Mtilde, Mat& eigenval_real, Mat& eigenval_imag, + Mat& eigenvec_real, Mat& eigenvec_imag) { #ifdef HAVE_EIGEN Eigen::MatrixXd Mtilde_eig, zeros_eig; - cv::cv2eigen(Mtilde, Mtilde_eig); - cv::cv2eigen(cv::Mat::zeros(27, 27, CV_64F), zeros_eig); + cv2eigen(Mtilde, Mtilde_eig); + cv2eigen(Mat::zeros(27, 27, CV_64F), zeros_eig); Eigen::MatrixXcd Mtilde_eig_cmplx(27, 27); Mtilde_eig_cmplx.real() = Mtilde_eig; @@ -293,20 +293,20 @@ void dls::compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Ma Eigen::MatrixXd eigvec_real = ces.eigenvectors().real(); Eigen::MatrixXd eigvec_imag = ces.eigenvectors().imag(); - cv::eigen2cv(eigval_real, eigenval_real); - cv::eigen2cv(eigval_imag, eigenval_imag); - cv::eigen2cv(eigvec_real, eigenvec_real); - cv::eigen2cv(eigvec_imag, eigenvec_imag); + eigen2cv(eigval_real, eigenval_real); + eigen2cv(eigval_imag, eigenval_imag); + eigen2cv(eigvec_real, eigenvec_real); + eigen2cv(eigvec_imag, eigenvec_imag); #else EigenvalueDecomposition es(Mtilde); eigenval_real = es.eigenvalues(); eigenvec_real = es.eigenvectors(); - eigenval_imag = eigenvec_imag = cv::Mat(); + eigenval_imag = eigenvec_imag = Mat(); #endif } -void dls::fill_coeff(const cv::Mat * D_mat) +void dls::fill_coeff(const Mat * D_mat) { // TODO: shift D and coefficients one position to left @@ -394,9 +394,9 @@ void dls::fill_coeff(const cv::Mat * D_mat) } -cv::Mat dls::LeftMultVec(const cv::Mat& v) +Mat dls::LeftMultVec(const Mat& v) { - cv::Mat mat_ = cv::Mat::zeros(3, 9, CV_64F); + Mat mat_ = Mat::zeros(3, 9, CV_64F); for (int i = 0; i < 3; ++i) { @@ -407,12 +407,12 @@ cv::Mat dls::LeftMultVec(const cv::Mat& v) return mat_; } -cv::Mat dls::cayley_LS_M(const std::vector& a, const std::vector& b, const std::vector& c, const std::vector& u) +Mat dls::cayley_LS_M(const std::vector& a, const std::vector& b, const std::vector& c, const std::vector& u) { // TODO: input matrix pointer // TODO: shift coefficients one position to left - cv::Mat M = cv::Mat::zeros(120, 120, CV_64F); + Mat M = Mat::zeros(120, 120, CV_64F); M.at(0,0)=u[1]; M.at(0,35)=a[1]; M.at(0,83)=b[1]; M.at(0,118)=c[1]; M.at(1,0)=u[4]; M.at(1,1)=u[1]; M.at(1,34)=a[1]; M.at(1,35)=a[10]; M.at(1,54)=b[1]; M.at(1,83)=b[10]; M.at(1,99)=c[1]; M.at(1,118)=c[10]; @@ -538,7 +538,7 @@ cv::Mat dls::cayley_LS_M(const std::vector& a, const std::vector return M.t(); } -cv::Mat dls::Hessian(const double s[]) +Mat dls::Hessian(const double s[]) { // the vector of monomials is // m = [ const ; s1^2 * s2 ; s1 * s2 ; s1 * s3 ; s2 * s3 ; s2^2 * s3 ; s2^3 ; ... @@ -577,73 +577,73 @@ cv::Mat dls::Hessian(const double s[]) Hs3[14]=0; Hs3[15]=3*s[2]*s[2]; Hs3[16]=s[0]*s[1]; Hs3[17]=0; Hs3[18]=s[0]*s[0]; Hs3[19]=0; // fill Hessian matrix - cv::Mat H(3, 3, CV_64F); - H.at(0,0) = cv::Mat(cv::Mat(f1coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs1)).at(0,0); - H.at(0,1) = cv::Mat(cv::Mat(f1coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs2)).at(0,0); - H.at(0,2) = cv::Mat(cv::Mat(f1coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs3)).at(0,0); + Mat H(3, 3, CV_64F); + H.at(0,0) = Mat(Mat(f1coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs1)).at(0,0); + H.at(0,1) = Mat(Mat(f1coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs2)).at(0,0); + H.at(0,2) = Mat(Mat(f1coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs3)).at(0,0); - H.at(1,0) = cv::Mat(cv::Mat(f2coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs1)).at(0,0); - H.at(1,1) = cv::Mat(cv::Mat(f2coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs2)).at(0,0); - H.at(1,2) = cv::Mat(cv::Mat(f2coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs3)).at(0,0); + H.at(1,0) = Mat(Mat(f2coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs1)).at(0,0); + H.at(1,1) = Mat(Mat(f2coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs2)).at(0,0); + H.at(1,2) = Mat(Mat(f2coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs3)).at(0,0); - H.at(2,0) = cv::Mat(cv::Mat(f3coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs1)).at(0,0); - H.at(2,1) = cv::Mat(cv::Mat(f3coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs2)).at(0,0); - H.at(2,2) = cv::Mat(cv::Mat(f3coeff).rowRange(1,21).t()*cv::Mat(20, 1, CV_64F, &Hs3)).at(0,0); + H.at(2,0) = Mat(Mat(f3coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs1)).at(0,0); + H.at(2,1) = Mat(Mat(f3coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs2)).at(0,0); + H.at(2,2) = Mat(Mat(f3coeff).rowRange(1,21).t()*Mat(20, 1, CV_64F, &Hs3)).at(0,0); return H; } -cv::Mat dls::cayley2rotbar(const cv::Mat& s) +Mat dls::cayley2rotbar(const Mat& s) { - double s_mul1 = cv::Mat(s.t()*s).at(0,0); - cv::Mat s_mul2 = s*s.t(); - cv::Mat eye = cv::Mat::eye(3, 3, CV_64F); + double s_mul1 = Mat(s.t()*s).at(0,0); + Mat s_mul2 = s*s.t(); + Mat eye = Mat::eye(3, 3, CV_64F); - return cv::Mat( eye.mul(1.-s_mul1) + skewsymm(&s).mul(2.) + s_mul2.mul(2.) ).t(); + return Mat( eye.mul(1.-s_mul1) + skewsymm(&s).mul(2.) + s_mul2.mul(2.) ).t(); } -cv::Mat dls::skewsymm(const cv::Mat * X1) +Mat dls::skewsymm(const Mat * X1) { - cv::MatConstIterator_ it = X1->begin(); - return (cv::Mat_(3,3) << 0, -*(it+2), *(it+1), - *(it+2), 0, -*(it+0), - -*(it+1), *(it+0), 0); + MatConstIterator_ it = X1->begin(); + return (Mat_(3,3) << 0, -*(it+2), *(it+1), + *(it+2), 0, -*(it+0), + -*(it+1), *(it+0), 0); } -cv::Mat dls::rotx(const double t) +Mat dls::rotx(const double t) { // rotx: rotation about y-axis double ct = cos(t); double st = sin(t); - return (cv::Mat_(3,3) << 1, 0, 0, 0, ct, -st, 0, st, ct); + return (Mat_(3,3) << 1, 0, 0, 0, ct, -st, 0, st, ct); } -cv::Mat dls::roty(const double t) +Mat dls::roty(const double t) { // roty: rotation about y-axis double ct = cos(t); double st = sin(t); - return (cv::Mat_(3,3) << ct, 0, st, 0, 1, 0, -st, 0, ct); + return (Mat_(3,3) << ct, 0, st, 0, 1, 0, -st, 0, ct); } -cv::Mat dls::rotz(const double t) +Mat dls::rotz(const double t) { // rotz: rotation about y-axis double ct = cos(t); double st = sin(t); - return (cv::Mat_(3,3) << ct, -st, 0, st, ct, 0, 0, 0, 1); + return (Mat_(3,3) << ct, -st, 0, st, ct, 0, 0, 0, 1); } -cv::Mat dls::mean(const cv::Mat& M) +Mat dls::mean(const Mat& M) { - cv::Mat m = cv::Mat::zeros(3, 1, CV_64F); + Mat m = Mat::zeros(3, 1, CV_64F); for (int i = 0; i < M.cols; ++i) m += M.col(i); return m.mul(1./(double)M.cols); } -bool dls::is_empty(const cv::Mat * M) +bool dls::is_empty(const Mat * M) { - cv::MatConstIterator_ it = M->begin(), it_end = M->end(); + MatConstIterator_ it = M->begin(), it_end = M->end(); for(; it != it_end; ++it) { if(*it < 0) return false; @@ -651,9 +651,11 @@ bool dls::is_empty(const cv::Mat * M) return true; } -bool dls::positive_eigenvalues(const cv::Mat * eigenvalues) +bool dls::positive_eigenvalues(const Mat * eigenvalues) { CV_Assert(eigenvalues && !eigenvalues->empty()); - cv::MatConstIterator_ it = eigenvalues->begin(); + MatConstIterator_ it = eigenvalues->begin(); return *(it) > 0 && *(it+1) > 0 && *(it+2) > 0; } + +} // namespace cv diff --git a/modules/calib3d/src/dls.h b/modules/calib3d/src/dls.h index f03bee49d7..a3382498c9 100644 --- a/modules/calib3d/src/dls.h +++ b/modules/calib3d/src/dls.h @@ -5,22 +5,21 @@ #include -using namespace std; -using namespace cv; +namespace cv { class dls { public: - dls(const cv::Mat& opoints, const cv::Mat& ipoints); + dls(const Mat& opoints, const Mat& ipoints); ~dls(); - bool compute_pose(cv::Mat& R, cv::Mat& t); + bool compute_pose(Mat& R, Mat& t); private: // initialisation template - void init_points(const cv::Mat& opoints, const cv::Mat& ipoints) + void init_points(const Mat& opoints, const Mat& ipoints) { for(int i = 0; i < N; i++) { @@ -49,33 +48,33 @@ private: } // 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, - cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag); - void fill_coeff(const cv::Mat * D); + Mat LeftMultVec(const Mat& v); + void run_kernel(const Mat& pp); + void build_coeff_matrix(const Mat& pp, Mat& Mtilde, Mat& D); + void compute_eigenvec(const Mat& Mtilde, Mat& eigenval_real, Mat& eigenval_imag, + Mat& eigenvec_real, Mat& eigenvec_imag); + void fill_coeff(const Mat * D); // useful functions - 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[]); - cv::Mat cayley2rotbar(const cv::Mat& s); - cv::Mat skewsymm(const cv::Mat * X1); + Mat cayley_LS_M(const std::vector& a, const std::vector& b, + const std::vector& c, const std::vector& u); + Mat Hessian(const double s[]); + Mat cayley2rotbar(const Mat& s); + Mat skewsymm(const Mat * X1); // extra functions - cv::Mat rotx(const double t); - cv::Mat roty(const double t); - cv::Mat rotz(const double t); - cv::Mat mean(const cv::Mat& M); - bool is_empty(const cv::Mat * v); - bool positive_eigenvalues(const cv::Mat * eigenvalues); + Mat rotx(const double t); + Mat roty(const double t); + Mat rotz(const double t); + Mat mean(const Mat& M); + bool is_empty(const Mat * v); + bool positive_eigenvalues(const Mat * eigenvalues); - cv::Mat p, z, mn; // object-image points + 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 - cv::Mat C_est__, t_est__; // optimal found solution + std::vector C_est_, t_est_; // optimal candidates + Mat C_est__, t_est__; // optimal found solution double cost__; // optimal found solution }; @@ -738,7 +737,7 @@ public: { /*if(isSymmetric(src)) { // Fall back to OpenCV for a symmetric matrix! - cv::eigen(src, _eigenvalues, _eigenvectors); + eigen(src, _eigenvalues, _eigenvectors); } else {*/ Mat tmp; // Convert the given input matrix to double. Is there any way to @@ -770,4 +769,5 @@ public: Mat eigenvectors() { return _eigenvectors; } }; +} // namespace cv #endif // DLS_H diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index ad5c85b222..6ed88edab5 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -100,12 +100,12 @@ void drawFrameAxes(InputOutputArray image, InputArray cameraMatrix, InputArray d CV_Assert(length > 0); // project axes points - vector axesPoints; + std::vector axesPoints; axesPoints.push_back(Point3f(0, 0, 0)); axesPoints.push_back(Point3f(length, 0, 0)); axesPoints.push_back(Point3f(0, length, 0)); axesPoints.push_back(Point3f(0, 0, length)); - vector imagePoints; + std::vector imagePoints; projectPoints(axesPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints); // draw axes lines @@ -120,7 +120,7 @@ bool solvePnP( InputArray opoints, InputArray ipoints, { CV_INSTRUMENT_REGION(); - vector rvecs, tvecs; + std::vector rvecs, tvecs; int solutions = solvePnPGeneric(opoints, ipoints, cameraMatrix, distCoeffs, rvecs, tvecs, useExtrinsicGuess, (SolvePnPMethod)flags, rvec, tvec); if (solutions > 0) @@ -298,8 +298,8 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints, return false; } - vector opoints_inliers; - vector ipoints_inliers; + std::vector opoints_inliers; + std::vector ipoints_inliers; opoints = opoints.reshape(3); ipoints = ipoints.reshape(2); opoints.convertTo(opoints_inliers, CV_64F); @@ -420,7 +420,7 @@ int solveP3P( InputArray _opoints, InputArray _ipoints, else imgPts = imgPts.reshape(1, 2*imgPts.rows); - vector reproj_errors(solutions); + std::vector reproj_errors(solutions); for (size_t i = 0; i < reproj_errors.size(); i++) { Mat rvec; @@ -710,7 +710,7 @@ static void solvePnPRefine(InputArray _objectPoints, InputArray _imagePoints, rvec0.convertTo(rvec, CV_64F); tvec0.convertTo(tvec, CV_64F); - vector ipoints_normalized; + std::vector ipoints_normalized; undistortPoints(ipoints, ipoints_normalized, cameraMatrix, distCoeffs); Mat sd = Mat(ipoints_normalized).reshape(1, npoints*2); Mat objectPoints0 = opoints.reshape(1, npoints); @@ -804,7 +804,7 @@ int solvePnPGeneric( InputArray _opoints, InputArray _ipoints, Mat cameraMatrix = Mat_(cameraMatrix0); Mat distCoeffs = Mat_(distCoeffs0); - vector vec_rvecs, vec_tvecs; + std::vector vec_rvecs, vec_tvecs; if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP) { if (flags == SOLVEPNP_DLS) @@ -829,7 +829,7 @@ int solvePnPGeneric( InputArray _opoints, InputArray _ipoints, } else if (flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P) { - vector rvecs, tvecs; + std::vector rvecs, tvecs; solveP3P(opoints, ipoints, _cameraMatrix, _distCoeffs, rvecs, tvecs, flags); vec_rvecs.insert(vec_rvecs.end(), rvecs.begin(), rvecs.end()); vec_tvecs.insert(vec_tvecs.end(), tvecs.begin(), tvecs.end()); @@ -1082,7 +1082,7 @@ int solvePnPGeneric( InputArray _opoints, InputArray _ipoints, for (size_t i = 0; i < vec_rvecs.size(); i++) { - vector projectedPoints; + std::vector projectedPoints; projectPoints(objectPoints, vec_rvecs[i], vec_tvecs[i], cameraMatrix, distCoeffs, projectedPoints); double rmse = norm(Mat(projectedPoints, false), imagePoints, NORM_L2) / sqrt(2*projectedPoints.size());