diff --git a/modules/calib3d/src/epnp.cpp b/modules/calib3d/src/epnp.cpp index 9a887c8067..09bff11362 100644 --- a/modules/calib3d/src/epnp.cpp +++ b/modules/calib3d/src/epnp.cpp @@ -1,9 +1,12 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + #include #include "precomp.hpp" #include "epnp.h" -namespace cv -{ +namespace cv { epnp::epnp(const Mat& cameraMatrix, const Mat& opoints, const Mat& ipoints) { @@ -58,21 +61,22 @@ void epnp::choose_control_points(void) // Take C1, C2, and C3 from PCA on the reference points: - CvMat * PW0 = cvCreateMat(number_of_correspondences, 3, CV_64F); + Mat PW0(number_of_correspondences, 3, CV_64F); double pw0tpw0[3 * 3] = {}, dc[3] = {}, uct[3 * 3] = {}; - CvMat PW0tPW0 = cvMat(3, 3, CV_64F, pw0tpw0); - CvMat DC = cvMat(3, 1, CV_64F, dc); - CvMat UCt = cvMat(3, 3, CV_64F, uct); + Mat PW0tPW0(3, 3, CV_64F, pw0tpw0); + Mat DC(3, 1, CV_64F, dc); + Mat UCt(3, 3, CV_64F, uct); - for(int i = 0; i < number_of_correspondences; i++) + for(int i = 0; i < number_of_correspondences; i++) { + double* PW0row = PW0.ptr(i); for(int j = 0; j < 3; j++) - PW0->data.db[3 * i + j] = pws[3 * i + j] - cws[0][j]; + PW0row[j] = pws[3 * i + j] - cws[0][j]; + } - cvMulTransposed(PW0, &PW0tPW0, 1); - cvSVD(&PW0tPW0, &DC, &UCt, 0, CV_SVD_MODIFY_A | CV_SVD_U_T); - - cvReleaseMat(&PW0); + mulTransposed(PW0, PW0tPW0, true); + SVDecomp(PW0tPW0, DC, UCt, noArray(), SVD::MODIFY_A); + transpose(UCt, UCt); for(int i = 1; i < 4; i++) { double k = sqrt(dc[i - 1] / number_of_correspondences); @@ -83,16 +87,14 @@ void epnp::choose_control_points(void) void epnp::compute_barycentric_coordinates(void) { - double cc[3 * 3] = {}, cc_inv[3 * 3] = {}; - CvMat CC = cvMat(3, 3, CV_64F, cc); - CvMat CC_inv = cvMat(3, 3, CV_64F, cc_inv); + Matx33d CC, CC_inv; for(int i = 0; i < 3; i++) for(int j = 1; j < 4; j++) - cc[3 * i + j - 1] = cws[j][i] - cws[0][i]; + CC(i, j - 1) = cws[j][i] - cws[0][i]; - cvInvert(&CC, &CC_inv, CV_SVD); - double * ci = cc_inv; + cv::invert(CC, CC_inv, DECOMP_SVD); + double * ci = CC_inv.val; for(int i = 0; i < number_of_correspondences; i++) { double * pi = &pws[0] + 3 * i; double * a = &alphas[0] + 4 * i; @@ -108,10 +110,10 @@ void epnp::compute_barycentric_coordinates(void) } } -void epnp::fill_M(CvMat * M, +void epnp::fill_M(Mat& M, const int row, const double * as, const double u, const double v) { - double * M1 = M->data.db + row * 12; + double * M1 = M.ptr(row); double * M2 = M1 + 12; for(int i = 0; i < 4; i++) { @@ -154,23 +156,23 @@ void epnp::compute_pose(Mat& R, Mat& t) choose_control_points(); compute_barycentric_coordinates(); - CvMat * M = cvCreateMat(2 * number_of_correspondences, 12, CV_64F); + Mat M(2 * number_of_correspondences, 12, CV_64F); for(int i = 0; i < number_of_correspondences; i++) fill_M(M, 2 * i, &alphas[0] + 4 * i, us[2 * i], us[2 * i + 1]); double mtm[12 * 12] = {}, d[12] = {}, ut[12 * 12] = {}; - CvMat MtM = cvMat(12, 12, CV_64F, mtm); - CvMat D = cvMat(12, 1, CV_64F, d); - CvMat Ut = cvMat(12, 12, CV_64F, ut); + Mat MtM(12, 12, CV_64F, mtm); + Mat D(12, 1, CV_64F, d); + Mat Ut(12, 12, CV_64F, ut); - cvMulTransposed(M, &MtM, 1); - cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T); - cvReleaseMat(&M); + mulTransposed(M, MtM, true); + SVDecomp(MtM, D, Ut, noArray(), SVD::MODIFY_A); + transpose(Ut, Ut); double l_6x10[6 * 10] = {}, rho[6] = {}; - CvMat L_6x10 = cvMat(6, 10, CV_64F, l_6x10); - CvMat Rho = cvMat(6, 1, CV_64F, rho); + Mat L_6x10(6, 10, CV_64F, l_6x10); + Mat Rho(6, 1, CV_64F, rho); compute_L_6x10(ut, l_6x10); compute_rho(rho); @@ -178,16 +180,16 @@ void epnp::compute_pose(Mat& R, Mat& t) double Betas[4][4] = {}, rep_errors[4] = {}; double Rs[4][3][3] = {}, ts[4][3] = {}; - find_betas_approx_1(&L_6x10, &Rho, Betas[1]); - gauss_newton(&L_6x10, &Rho, Betas[1]); + find_betas_approx_1(L_6x10, Rho, Betas[1]); + gauss_newton(L_6x10, Rho, Betas[1]); rep_errors[1] = compute_R_and_t(ut, Betas[1], Rs[1], ts[1]); - find_betas_approx_2(&L_6x10, &Rho, Betas[2]); - gauss_newton(&L_6x10, &Rho, Betas[2]); + find_betas_approx_2(L_6x10, Rho, Betas[2]); + gauss_newton(L_6x10, Rho, Betas[2]); rep_errors[2] = compute_R_and_t(ut, Betas[2], Rs[2], ts[2]); - find_betas_approx_3(&L_6x10, &Rho, Betas[3]); - gauss_newton(&L_6x10, &Rho, Betas[3]); + find_betas_approx_3(L_6x10, Rho, Betas[3]); + gauss_newton(L_6x10, Rho, Betas[3]); rep_errors[3] = compute_R_and_t(ut, Betas[3], Rs[3], ts[3]); int N = 1; @@ -242,13 +244,13 @@ void epnp::estimate_R_and_t(double R[3][3], double t[3]) pw0[j] /= number_of_correspondences; } - double abt[3 * 3] = {}, abt_d[3] = {}, abt_u[3 * 3] = {}, abt_v[3 * 3] = {}; - CvMat ABt = cvMat(3, 3, CV_64F, abt); - CvMat ABt_D = cvMat(3, 1, CV_64F, abt_d); - CvMat ABt_U = cvMat(3, 3, CV_64F, abt_u); - CvMat ABt_V = cvMat(3, 3, CV_64F, abt_v); + double abt[3 * 3] = {}, abt_d[3] = {}, abt_u[3 * 3] = {}, abt_vt[3 * 3] = {}; + Mat ABt(3, 3, CV_64F, abt); + Mat ABt_D(3, 1, CV_64F, abt_d); + Mat ABt_U(3, 3, CV_64F, abt_u); + Mat ABt_Vt(3, 3, CV_64F, abt_vt); - cvSetZero(&ABt); + ABt.setTo(Scalar::all(0.)); for(int i = 0; i < number_of_correspondences; i++) { double * pc = &pcs[3 * i]; double * pw = &pws[3 * i]; @@ -260,15 +262,11 @@ void epnp::estimate_R_and_t(double R[3][3], double t[3]) } } - cvSVD(&ABt, &ABt_D, &ABt_U, &ABt_V, CV_SVD_MODIFY_A); + SVDecomp(ABt, ABt_D, ABt_U, ABt_Vt, SVD::MODIFY_A); + Mat mR(3, 3, CV_64F, R); + gemm(ABt_U, ABt_Vt, 1, noArray(), 0, mR); - for(int i = 0; i < 3; i++) - for(int j = 0; j < 3; j++) - R[i][j] = dot(abt_u + 3 * i, abt_v + 3 * j); - - const double det = - R[0][0] * R[1][1] * R[2][2] + R[0][1] * R[1][2] * R[2][0] + R[0][2] * R[1][0] * R[2][1] - - R[0][2] * R[1][1] * R[2][0] - R[0][1] * R[1][0] * R[2][2] - R[0][0] * R[1][2] * R[2][1]; + const double det = determinant(mR); if (det < 0) { R[2][0] = -R[2][0]; @@ -331,21 +329,21 @@ double epnp::reprojection_error(const double R[3][3], const double t[3]) // betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44] // betas_approx_1 = [B11 B12 B13 B14] -void epnp::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, - double * betas) +void epnp::find_betas_approx_1(const Mat& L_6x10, const Mat& Rho, double* betas) { double l_6x4[6 * 4] = {}, b4[4] = {}; - CvMat L_6x4 = cvMat(6, 4, CV_64F, l_6x4); - CvMat B4 = cvMat(4, 1, CV_64F, b4); + Mat L_6x4(6, 4, CV_64F, l_6x4); + Mat B4(4, 1, CV_64F, b4); for(int i = 0; i < 6; i++) { - cvmSet(&L_6x4, i, 0, cvmGet(L_6x10, i, 0)); - cvmSet(&L_6x4, i, 1, cvmGet(L_6x10, i, 1)); - cvmSet(&L_6x4, i, 2, cvmGet(L_6x10, i, 3)); - cvmSet(&L_6x4, i, 3, cvmGet(L_6x10, i, 6)); + L_6x4.at(i, 0) = L_6x10.at(i, 0); + L_6x4.at(i, 1) = L_6x10.at(i, 1); + L_6x4.at(i, 2) = L_6x10.at(i, 3); + L_6x4.at(i, 3) = L_6x10.at(i, 6); } - cvSolve(&L_6x4, Rho, &B4, CV_SVD); + solve(L_6x4, Rho, B4, DECOMP_SVD); + CV_Assert(B4.ptr() == b4); if (b4[0] < 0) { betas[0] = sqrt(-b4[0]); @@ -363,20 +361,20 @@ void epnp::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, // betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44] // betas_approx_2 = [B11 B12 B22 ] -void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, - double * betas) +void epnp::find_betas_approx_2(const Mat& L_6x10, const Mat& Rho, double* betas) { double l_6x3[6 * 3] = {}, b3[3] = {}; - CvMat L_6x3 = cvMat(6, 3, CV_64F, l_6x3); - CvMat B3 = cvMat(3, 1, CV_64F, b3); + Mat L_6x3(6, 3, CV_64F, l_6x3); + Mat B3(3, 1, CV_64F, b3); for(int i = 0; i < 6; i++) { - cvmSet(&L_6x3, i, 0, cvmGet(L_6x10, i, 0)); - cvmSet(&L_6x3, i, 1, cvmGet(L_6x10, i, 1)); - cvmSet(&L_6x3, i, 2, cvmGet(L_6x10, i, 2)); + L_6x3.at(i, 0) = L_6x10.at(i, 0); + L_6x3.at(i, 1) = L_6x10.at(i, 1); + L_6x3.at(i, 2) = L_6x10.at(i, 2); } - cvSolve(&L_6x3, Rho, &B3, CV_SVD); + solve(L_6x3, Rho, B3, DECOMP_SVD); + CV_Assert(B3.ptr() == b3); if (b3[0] < 0) { betas[0] = sqrt(-b3[0]); @@ -387,7 +385,6 @@ void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, } if (b3[1] < 0) betas[0] = -betas[0]; - betas[2] = 0.0; betas[3] = 0.0; } @@ -395,22 +392,22 @@ void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, // betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44] // betas_approx_3 = [B11 B12 B22 B13 B23 ] -void epnp::find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, - double * betas) +void epnp::find_betas_approx_3(const Mat& L_6x10, const Mat& Rho, double * betas) { double l_6x5[6 * 5] = {}, b5[5] = {}; - CvMat L_6x5 = cvMat(6, 5, CV_64F, l_6x5); - CvMat B5 = cvMat(5, 1, CV_64F, b5); + Mat L_6x5(6, 5, CV_64F, l_6x5); + Mat B5(5, 1, CV_64F, b5); for(int i = 0; i < 6; i++) { - cvmSet(&L_6x5, i, 0, cvmGet(L_6x10, i, 0)); - cvmSet(&L_6x5, i, 1, cvmGet(L_6x10, i, 1)); - cvmSet(&L_6x5, i, 2, cvmGet(L_6x10, i, 2)); - cvmSet(&L_6x5, i, 3, cvmGet(L_6x10, i, 3)); - cvmSet(&L_6x5, i, 4, cvmGet(L_6x10, i, 4)); + L_6x5.at(i, 0) = L_6x10.at(i, 0); + L_6x5.at(i, 1) = L_6x10.at(i, 1); + L_6x5.at(i, 2) = L_6x10.at(i, 2); + L_6x5.at(i, 3) = L_6x10.at(i, 3); + L_6x5.at(i, 4) = L_6x10.at(i, 4); } - cvSolve(&L_6x5, Rho, &B5, CV_SVD); + solve(L_6x5, Rho, B5, DECOMP_SVD); + CV_Assert(B5.ptr() == b5); if (b5[0] < 0) { betas[0] = sqrt(-b5[0]); @@ -476,19 +473,19 @@ void epnp::compute_rho(double * rho) rho[5] = dist2(cws[2], cws[3]); } -void epnp::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho, - const double betas[4], CvMat * A, CvMat * b) +void epnp::compute_A_and_b_gauss_newton(const Mat& L_6x10, const Mat& Rho, + const double betas[4], Mat& A, Mat& b) { for(int i = 0; i < 6; i++) { - const double * rowL = l_6x10 + i * 10; - double * rowA = A->data.db + i * 4; + const double * rowL = L_6x10.ptr(i); + double * rowA = A.ptr(i); rowA[0] = 2 * rowL[0] * betas[0] + rowL[1] * betas[1] + rowL[3] * betas[2] + rowL[6] * betas[3]; rowA[1] = rowL[1] * betas[0] + 2 * rowL[2] * betas[1] + rowL[4] * betas[2] + rowL[7] * betas[3]; rowA[2] = rowL[3] * betas[0] + rowL[4] * betas[1] + 2 * rowL[5] * betas[2] + rowL[8] * betas[3]; rowA[3] = rowL[6] * betas[0] + rowL[7] * betas[1] + rowL[8] * betas[2] + 2 * rowL[9] * betas[3]; - cvmSet(b, i, 0, rho[i] - + b.at(i) = Rho.at(i) - ( rowL[0] * betas[0] * betas[0] + rowL[1] * betas[0] * betas[1] + @@ -500,33 +497,32 @@ void epnp::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rh rowL[7] * betas[1] * betas[3] + rowL[8] * betas[2] * betas[3] + rowL[9] * betas[3] * betas[3] - )); + ); } } -void epnp::gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double betas[4]) +void epnp::gauss_newton(const Mat& L_6x10, const Mat& Rho, double betas[4]) { const int iterations_number = 5; double a[6*4] = {}, b[6] = {}, x[4] = {}; - CvMat A = cvMat(6, 4, CV_64F, a); - CvMat B = cvMat(6, 1, CV_64F, b); - CvMat X = cvMat(4, 1, CV_64F, x); + Mat A(6, 4, CV_64F, a); + Mat B(6, 1, CV_64F, b); + Mat X(4, 1, CV_64F, x); for(int k = 0; k < iterations_number; k++) { - compute_A_and_b_gauss_newton(L_6x10->data.db, Rho->data.db, - betas, &A, &B); - qr_solve(&A, &B, &X); + compute_A_and_b_gauss_newton(L_6x10, Rho, betas, A, B); + qr_solve(A, B, X); for(int i = 0; i < 4; i++) betas[i] += x[i]; } } -void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X) +void epnp::qr_solve(Mat& A, Mat& b, Mat& X) { - const int nr = A->rows; - const int nc = A->cols; + const int nr = A.rows; + const int nc = A.cols; if (nc <= 0 || nr <= 0) return; @@ -542,7 +538,7 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X) A2 = new double[nr]; } - double * pA = A->data.db, * ppAkk = pA; + double * pA = A.ptr(), * ppAkk = pA; for(int k = 0; k < nc; k++) { double * ppAik1 = ppAkk, eta = fabs(*ppAik1); @@ -594,7 +590,7 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X) } // b <- Qt b - double * ppAjj = pA, * pb = b->data.db; + double * ppAjj = pA, * pb = b.ptr(); for(int j = 0; j < nc; j++) { double * ppAij = ppAjj, tau = 0; @@ -614,7 +610,7 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X) } // X = R-1 b - double * pX = X->data.db; + double * pX = X.ptr(); pX[nc - 1] = pb[nc - 1] / A2[nc - 1]; for(int i = nc - 2; i >= 0; i--) { diff --git a/modules/calib3d/src/epnp.h b/modules/calib3d/src/epnp.h index a66596b368..3e047b3b04 100644 --- a/modules/calib3d/src/epnp.h +++ b/modules/calib3d/src/epnp.h @@ -1,11 +1,13 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html + #ifndef epnp_h #define epnp_h #include "precomp.hpp" -#include "opencv2/core/core_c.h" -namespace cv -{ +namespace cv { class epnp { public: @@ -43,16 +45,16 @@ class epnp { double reprojection_error(const double R[3][3], const double t[3]); void choose_control_points(void); void compute_barycentric_coordinates(void); - void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v); + void fill_M(Mat& M, const int row, const double * alphas, const double u, const double v); void compute_ccs(const double * betas, const double * ut); void compute_pcs(void); void solve_for_sign(void); - void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas); - void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas); - void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas); - void qr_solve(CvMat * A, CvMat * b, CvMat * X); + void find_betas_approx_1(const Mat& L_6x10, const Mat& Rho, double * betas); + void find_betas_approx_2(const Mat& L_6x10, const Mat& Rho, double * betas); + void find_betas_approx_3(const Mat& L_6x10, const Mat& Rho, double * betas); + void qr_solve(Mat& A, Mat& b, Mat& X); double dot(const double * v1, const double * v2); double dist2(const double * p1, const double * p2); @@ -60,9 +62,9 @@ class epnp { void compute_rho(double * rho); void compute_L_6x10(const double * ut, double * l_6x10); - void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]); - void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho, - const double cb[4], CvMat * A, CvMat * b); + void gauss_newton(const Mat& L_6x10, const Mat& Rho, double current_betas[4]); + void compute_A_and_b_gauss_newton(const Mat& L_6x10, const Mat& Rho, + const double cb[4], Mat& A, Mat& b); double compute_R_and_t(const double * ut, const double * betas, double R[3][3], double t[3]); diff --git a/modules/calib3d/src/triangulate.cpp b/modules/calib3d/src/triangulate.cpp index 851ca5d7be..07f19d6b24 100644 --- a/modules/calib3d/src/triangulate.cpp +++ b/modules/calib3d/src/triangulate.cpp @@ -40,83 +40,111 @@ //M*/ #include "precomp.hpp" -#include "opencv2/core/core_c.h" +#include -// cvCorrectMatches function is Copyright (C) 2009, Jostein Austvik Jacobsen. -// cvTriangulatePoints function is derived from icvReconstructPointsFor3View, originally by Valery Mosyagin. +namespace cv { + +// correctMatches function is Copyright (C) 2009, Jostein Austvik Jacobsen. +// triangulatePoints function is derived from reconstructPointsFor3View, originally by Valery Mosyagin. // HZ, R. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision, Cambridge Univ. Press, 2003. - - -// This method is the same as icvReconstructPointsFor3View, with only a few numbers adjusted for two-view geometry -static void -icvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2, CvMat* projPoints1, CvMat* projPoints2, CvMat* points4D) +// This method is the same as reconstructPointsFor3View, with only a few numbers adjusted for two-view geometry +void triangulatePoints( InputArray _P1, InputArray _P2, + InputArray _points1, InputArray _points2, + OutputArray _points4D ) { - if( projMatr1 == 0 || projMatr2 == 0 || - projPoints1 == 0 || projPoints2 == 0 || - points4D == 0) - CV_Error( cv::Error::StsNullPtr, "Some of parameters is a NULL pointer" ); + CV_INSTRUMENT_REGION(); - if( !CV_IS_MAT(projMatr1) || !CV_IS_MAT(projMatr2) || - !CV_IS_MAT(projPoints1) || !CV_IS_MAT(projPoints2) || - !CV_IS_MAT(points4D) ) - CV_Error( cv::Error::StsUnsupportedFormat, "Input parameters must be matrices" ); + Mat points1 = _points1.getMat(), points2 = _points2.getMat(); + int depth1 = points1.depth(), depth2 = points2.depth(); + const float *p1f = depth1 == CV_32F ? points1.ptr() : 0; + const float *p2f = depth2 == CV_32F ? points2.ptr() : 0; + const double *p1d = depth1 == CV_64F ? points1.ptr() : 0; + const double *p2d = depth2 == CV_64F ? points2.ptr() : 0; + int pstep1, ystep1, pstep2, ystep2, npoints1, npoints2; - int numPoints = projPoints1->cols; + CV_Assert(depth1 == depth2 && (depth1 == CV_32F || depth1 == CV_64F)); - if( numPoints < 1 ) - CV_Error( cv::Error::StsOutOfRange, "Number of points must be more than zero" ); - - if( projPoints2->cols != numPoints || points4D->cols != numPoints ) - CV_Error( cv::Error::StsUnmatchedSizes, "Number of points must be the same" ); - - if( projPoints1->rows != 2 || projPoints2->rows != 2) - CV_Error( cv::Error::StsUnmatchedSizes, "Number of proj points coordinates must be == 2" ); - - if( points4D->rows != 4 ) - CV_Error( cv::Error::StsUnmatchedSizes, "Number of world points coordinates must be == 4" ); - - if( projMatr1->cols != 4 || projMatr1->rows != 3 || - projMatr2->cols != 4 || projMatr2->rows != 3) - CV_Error( cv::Error::StsUnmatchedSizes, "Size of projection matrices must be 3x4" ); - - // preallocate SVD matrices on stack - cv::Matx matrA; - cv::Matx matrU; - cv::Matx matrW; - cv::Matx matrV; - - CvMat* projPoints[2] = {projPoints1, projPoints2}; - CvMat* projMatrs[2] = {projMatr1, projMatr2}; - - /* Solve system for each point */ - for( int i = 0; i < numPoints; i++ )/* For each point */ + if ((points1.rows == 1 || points1.cols == 1) && points1.channels() == 2) { - /* Fill matrix for current point */ - for( int j = 0; j < 2; j++ )/* For each view */ - { - double x,y; - x = cvmGet(projPoints[j],0,i); - y = cvmGet(projPoints[j],1,i); - for( int k = 0; k < 4; k++ ) - { - matrA(j*2+0, k) = x * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],0,k); - matrA(j*2+1, k) = y * cvmGet(projMatrs[j],2,k) - cvmGet(projMatrs[j],1,k); - } - } - /* Solve system for current point */ - cv::SVD::compute(matrA, matrW, matrU, matrV); + npoints1 = points1.rows + points1.cols - 1; + ystep1 = 1; + pstep1 = 2; + } + else + { + npoints1 = points1.cols; + ystep1 = (int)(points1.step/points1.elemSize()); + pstep1 = 1; + } - /* Copy computed point */ - cvmSet(points4D,0,i,matrV(3,0));/* X */ - cvmSet(points4D,1,i,matrV(3,1));/* Y */ - cvmSet(points4D,2,i,matrV(3,2));/* Z */ - cvmSet(points4D,3,i,matrV(3,3));/* W */ + if ((points2.rows == 1 || points2.cols == 1) && points2.channels() == 2) + { + npoints2 = points2.rows + points2.cols - 1; + ystep2 = 1; + pstep2 = 2; + } + else + { + npoints2 = points2.cols; + ystep2 = (int)(points2.step/points2.elemSize()); + pstep2 = 1; + } + + CV_Assert(npoints1 == npoints2); + + _points4D.create(4, npoints1, depth1); + Mat points4D = _points4D.getMat(); + + Matx matrA; + Matx matrU; + Matx matrW; + Matx matrV; + size_t step4 = 4*sizeof(double); + Matx P1; + Matx P2; + _P1.getMat().convertTo(P1, CV_64F); + _P2.getMat().convertTo(P2, CV_64F); + + // Solve system for each point + for( int i = 0; i < npoints1; i++ ) + { + // Fill matrix for current point + double x1 = p1f ? (double)p1f[pstep1*i] : p1d[pstep1*i]; + double y1 = p1f ? (double)p1f[pstep1*i + ystep1] : p1d[pstep1*i + ystep1]; + double x2 = p2f ? (double)p2f[pstep2*i] : p2d[pstep2*i]; + double y2 = p2f ? (double)p2f[pstep2*i + ystep2] : p2d[pstep2*i + ystep2]; + + for(int k = 0; k < 4; k++) + { + matrA(k, 0) = x1*P1(2, k) - P1(0, k); + matrA(k, 1) = y1*P1(2, k) - P1(1, k); + matrA(k, 2) = x2*P2(2, k) - P2(0, k); + matrA(k, 3) = y2*P2(2, k) - P2(1, k); + } + + // Solve system for current point + hal::SVD64f(matrA.val, step4, matrW.val, matrU.val, step4, matrV.val, step4, 4, 4, 4); + + // Copy computed point + if(depth1 == CV_32F) + { + points4D.at(0, i) = (float)matrV(3, 0); + points4D.at(1, i) = (float)matrV(3, 1); + points4D.at(2, i) = (float)matrV(3, 2); + points4D.at(3, i) = (float)matrV(3, 3); + } + else + { + points4D.at(0, i) = matrV(3, 0); + points4D.at(1, i) = matrV(3, 1); + points4D.at(2, i) = matrV(3, 2); + points4D.at(3, i) = matrV(3, 3); + } } } - /* * The Optimal Triangulation Method (see HZ for details) * For each given point correspondence points1[i] <-> points2[i], and a fundamental matrix F, @@ -125,182 +153,118 @@ icvTriangulatePoints(CvMat* projMatr1, CvMat* projMatr2, CvMat* projPoints1, CvM * is the geometric distance between points a and b) subject to the epipolar constraint * new_points2' * F * new_points1 = 0. * - * F_ : 3x3 fundamental matrix - * points1_ : 1xN matrix containing the first set of points - * points2_ : 1xN matrix containing the second set of points - * new_points1 : the optimized points1_. if this is NULL, the corrected points are placed back in points1_ - * new_points2 : the optimized points2_. if this is NULL, the corrected points are placed back in points2_ + * _F : 3x3 fundamental matrix + * _points1 : 1xN matrix containing the first set of points + * _points2 : 1xN matrix containing the second set of points + * _newPoints1 : the optimized _points1. + * _newPoints2 : the optimized -points2. */ -static void -icvCorrectMatches(CvMat *F_, CvMat *points1_, CvMat *points2_, CvMat *new_points1, CvMat *new_points2) +void correctMatches( InputArray _F, InputArray _points1, InputArray _points2, + OutputArray _newPoints1, OutputArray _newPoints2 ) { - cv::Ptr tmp33; - cv::Ptr tmp31, tmp31_2; - cv::Ptr T1i, T2i; - cv::Ptr R1, R2; - cv::Ptr TFT, TFTt, RTFTR; - cv::Ptr U, S, V; - cv::Ptr e1, e2; - cv::Ptr polynomial; - cv::Ptr result; - cv::Ptr points1, points2; - cv::Ptr F; + CV_INSTRUMENT_REGION(); - if (!CV_IS_MAT(F_) || !CV_IS_MAT(points1_) || !CV_IS_MAT(points2_) ) - CV_Error( cv::Error::StsUnsupportedFormat, "Input parameters must be matrices" ); - if (!( F_->cols == 3 && F_->rows == 3)) - CV_Error( cv::Error::StsUnmatchedSizes, "The fundamental matrix must be a 3x3 matrix"); - if (!(((F_->type & CV_MAT_TYPE_MASK) >> 3) == 0 )) - CV_Error( cv::Error::StsUnsupportedFormat, "The fundamental matrix must be a single-channel matrix" ); - if (!(points1_->rows == 1 && points2_->rows == 1 && points1_->cols == points2_->cols)) - CV_Error( cv::Error::StsUnmatchedSizes, "The point-matrices must have one row, and an equal number of columns" ); - if (((points1_->type & CV_MAT_TYPE_MASK) >> 3) != 1 ) + Mat points1 = _points1.getMat(), points2 = _points2.getMat(); + + int depth1 = points1.depth(), depth2 = points2.depth(); + CV_Assert((depth1 == CV_32F || depth1 == CV_64F) && depth1 == depth2); + + CV_Assert(points1.size() == points2.size()); + CV_Assert(points1.rows == 1 || points1.cols == 1); + if (points1.channels() != 2) CV_Error( cv::Error::StsUnmatchedSizes, "The first set of points must contain two channels; one for x and one for y" ); - if (((points2_->type & CV_MAT_TYPE_MASK) >> 3) != 1 ) + if (points2.channels() != 2) CV_Error( cv::Error::StsUnmatchedSizes, "The second set of points must contain two channels; one for x and one for y" ); - if (new_points1 != NULL) { - CV_Assert(CV_IS_MAT(new_points1)); - if (new_points1->cols != points1_->cols || new_points1->rows != 1) - CV_Error( cv::Error::StsUnmatchedSizes, "The first output matrix must have the same dimensions as the input matrices" ); - if (CV_MAT_CN(new_points1->type) != 2) - CV_Error( cv::Error::StsUnsupportedFormat, "The first output matrix must have two channels; one for x and one for y" ); - } - if (new_points2 != NULL) { - CV_Assert(CV_IS_MAT(new_points2)); - if (new_points2->cols != points2_->cols || new_points2->rows != 1) - CV_Error( cv::Error::StsUnmatchedSizes, "The second output matrix must have the same dimensions as the input matrices" ); - if (CV_MAT_CN(new_points2->type) != 2) - CV_Error( cv::Error::StsUnsupportedFormat, "The second output matrix must have two channels; one for x and one for y" ); - } + + _newPoints1.create(points1.size(), points1.type()); + _newPoints2.create(points2.size(), points2.type()); + Mat newPoints1 = _newPoints1.getMat(), newPoints2 = _newPoints2.getMat(); + + Matx33d F, U, Vt; + Matx31d S; + int npoints = points1.rows + points1.cols - 1; // Make sure F uses double precision - F.reset(cvCreateMat(3,3,CV_64FC1)); - cvConvert(F_, F); + _F.getMat().convertTo(F, CV_64F); - // Make sure points1 uses double precision - points1.reset(cvCreateMat(points1_->rows,points1_->cols,CV_64FC2)); - cvConvert(points1_, points1); - - // Make sure points2 uses double precision - points2.reset(cvCreateMat(points2_->rows,points2_->cols,CV_64FC2)); - cvConvert(points2_, points2); - - tmp33.reset(cvCreateMat(3,3,CV_64FC1)); - tmp31.reset(cvCreateMat(3,1,CV_64FC1)), tmp31_2.reset(cvCreateMat(3,1,CV_64FC1)); - T1i.reset(cvCreateMat(3,3,CV_64FC1)), T2i.reset(cvCreateMat(3,3,CV_64FC1)); - R1.reset(cvCreateMat(3,3,CV_64FC1)), R2.reset(cvCreateMat(3,3,CV_64FC1)); - TFT.reset(cvCreateMat(3,3,CV_64FC1)), TFTt.reset(cvCreateMat(3,3,CV_64FC1)), RTFTR.reset(cvCreateMat(3,3,CV_64FC1)); - U.reset(cvCreateMat(3,3,CV_64FC1)); - S.reset(cvCreateMat(3,3,CV_64FC1)); - V.reset(cvCreateMat(3,3,CV_64FC1)); - e1.reset(cvCreateMat(3,1,CV_64FC1)), e2.reset(cvCreateMat(3,1,CV_64FC1)); - - double x1, y1, x2, y2; - double scale; - double f1, f2, a, b, c, d; - polynomial.reset(cvCreateMat(1,7,CV_64FC1)); - result.reset(cvCreateMat(1,6,CV_64FC2)); - double t_min, s_val, t, s; - for (int p = 0; p < points1->cols; ++p) { + for (int p = 0; p < npoints; ++p) { // Replace F by T2-t * F * T1-t - x1 = points1->data.db[p*2]; - y1 = points1->data.db[p*2+1]; - x2 = points2->data.db[p*2]; - y2 = points2->data.db[p*2+1]; + double x1, y1, x2, y2; + if (depth1 == CV_32F) { + Point2f p1 = points1.at(p); + Point2f p2 = points2.at(p); + x1 = p1.x; y1 = p1.y; + x2 = p2.x; y2 = p2.y; + } else { + Point2d p1 = points1.at(p); + Point2d p2 = points2.at(p); + x1 = p1.x; y1 = p1.y; + x2 = p2.x; y2 = p2.y; + } - cvSetZero(T1i); - cvSetReal2D(T1i,0,0,1); - cvSetReal2D(T1i,1,1,1); - cvSetReal2D(T1i,2,2,1); - cvSetReal2D(T1i,0,2,x1); - cvSetReal2D(T1i,1,2,y1); - cvSetZero(T2i); - cvSetReal2D(T2i,0,0,1); - cvSetReal2D(T2i,1,1,1); - cvSetReal2D(T2i,2,2,1); - cvSetReal2D(T2i,0,2,x2); - cvSetReal2D(T2i,1,2,y2); - cvGEMM(T2i,F,1,0,0,tmp33,CV_GEMM_A_T); - cvSetZero(TFT); - cvGEMM(tmp33,T1i,1,0,0,TFT); + Matx33d T1i(1, 0, x1, + 0, 1, y1, + 0, 0, 1); + Matx33d T2i(1, 0, x2, + 0, 1, y2, + 0, 0, 1); + Matx33d TFT = T2i.t()*F*T1i; // Compute the right epipole e1 from F * e1 = 0 - cvSetZero(U); - cvSetZero(S); - cvSetZero(V); - cvSVD(TFT,S,U,V); - scale = sqrt(cvGetReal2D(V,0,2)*cvGetReal2D(V,0,2) + cvGetReal2D(V,1,2)*cvGetReal2D(V,1,2)); - cvSetReal2D(e1,0,0,cvGetReal2D(V,0,2)/scale); - cvSetReal2D(e1,1,0,cvGetReal2D(V,1,2)/scale); - cvSetReal2D(e1,2,0,cvGetReal2D(V,2,2)/scale); - if (cvGetReal2D(e1,2,0) < 0) { - cvSetReal2D(e1,0,0,-cvGetReal2D(e1,0,0)); - cvSetReal2D(e1,1,0,-cvGetReal2D(e1,1,0)); - cvSetReal2D(e1,2,0,-cvGetReal2D(e1,2,0)); - } + SVDecomp(TFT, S, U, Vt); + double scale = sqrt(Vt(2, 0)*Vt(2, 0) + Vt(2, 1)*Vt(2, 1)); + + Vec3d e1(Vt(2, 0)/scale, Vt(2, 1)/scale, Vt(2, 2)/scale); + if (e1(2) < 0) + e1 = -e1; // Compute the left epipole e2 from e2' * F = 0 => F' * e2 = 0 - cvSetZero(TFTt); - cvTranspose(TFT, TFTt); - cvSetZero(U); - cvSetZero(S); - cvSetZero(V); - cvSVD(TFTt,S,U,V); - cvSetZero(e2); - scale = sqrt(cvGetReal2D(V,0,2)*cvGetReal2D(V,0,2) + cvGetReal2D(V,1,2)*cvGetReal2D(V,1,2)); - cvSetReal2D(e2,0,0,cvGetReal2D(V,0,2)/scale); - cvSetReal2D(e2,1,0,cvGetReal2D(V,1,2)/scale); - cvSetReal2D(e2,2,0,cvGetReal2D(V,2,2)/scale); - if (cvGetReal2D(e2,2,0) < 0) { - cvSetReal2D(e2,0,0,-cvGetReal2D(e2,0,0)); - cvSetReal2D(e2,1,0,-cvGetReal2D(e2,1,0)); - cvSetReal2D(e2,2,0,-cvGetReal2D(e2,2,0)); - } + scale = sqrt(U(0, 2)*U(0, 2) + U(1, 2)*U(1, 2)); + + Vec3d e2(U(0, 2)/scale, U(1, 2)/scale, U(2, 2)/scale); + if (e2(2) < 0) + e2 = -e2; // Replace F by R2 * F * R1' - cvSetZero(R1); - cvSetReal2D(R1,0,0,cvGetReal2D(e1,0,0)); - cvSetReal2D(R1,0,1,cvGetReal2D(e1,1,0)); - cvSetReal2D(R1,1,0,-cvGetReal2D(e1,1,0)); - cvSetReal2D(R1,1,1,cvGetReal2D(e1,0,0)); - cvSetReal2D(R1,2,2,1); - cvSetZero(R2); - cvSetReal2D(R2,0,0,cvGetReal2D(e2,0,0)); - cvSetReal2D(R2,0,1,cvGetReal2D(e2,1,0)); - cvSetReal2D(R2,1,0,-cvGetReal2D(e2,1,0)); - cvSetReal2D(R2,1,1,cvGetReal2D(e2,0,0)); - cvSetReal2D(R2,2,2,1); - cvGEMM(R2,TFT,1,0,0,tmp33); - cvGEMM(tmp33,R1,1,0,0,RTFTR,CV_GEMM_B_T); + Matx33d R1_t(e1(0), -e1(1), 0, + e1(1), e1(0), 0, + 0, 0, 1); + Matx33d R2(e2(0), e2(1), 0, + -e2(1), e2(0), 0, + 0, 0, 1); + Matx33d RTFTR = R2*TFT*R1_t; // Set f1 = e1(3), f2 = e2(3), a = F22, b = F23, c = F32, d = F33 - f1 = cvGetReal2D(e1,2,0); - f2 = cvGetReal2D(e2,2,0); - a = cvGetReal2D(RTFTR,1,1); - b = cvGetReal2D(RTFTR,1,2); - c = cvGetReal2D(RTFTR,2,1); - d = cvGetReal2D(RTFTR,2,2); + double f1 = e1(2); + double f2 = e2(2); + double a = RTFTR(1,1); + double b = RTFTR(1,2); + double c = RTFTR(2,1); + double d = RTFTR(2,2); // Form the polynomial g(t) = k6*t^6 + k5*t^5 + k4*t^4 + k3*t^3 + k2*t^2 + k1*t + k0 // from f1, f2, a, b, c and d - cvSetReal2D(polynomial,0,6,( +b*c*c*f1*f1*f1*f1*a-a*a*d*f1*f1*f1*f1*c )); - cvSetReal2D(polynomial,0,5,( +f2*f2*f2*f2*c*c*c*c+2*a*a*f2*f2*c*c-a*a*d*d*f1*f1*f1*f1+b*b*c*c*f1*f1*f1*f1+a*a*a*a )); - cvSetReal2D(polynomial,0,4,( +4*a*a*a*b+2*b*c*c*f1*f1*a+4*f2*f2*f2*f2*c*c*c*d+4*a*b*f2*f2*c*c+4*a*a*f2*f2*c*d-2*a*a*d*f1*f1*c-a*d*d*f1*f1*f1*f1*b+b*b*c*f1*f1*f1*f1*d )); - cvSetReal2D(polynomial,0,3,( +6*a*a*b*b+6*f2*f2*f2*f2*c*c*d*d+2*b*b*f2*f2*c*c+2*a*a*f2*f2*d*d-2*a*a*d*d*f1*f1+2*b*b*c*c*f1*f1+8*a*b*f2*f2*c*d )); - cvSetReal2D(polynomial,0,2,( +4*a*b*b*b+4*b*b*f2*f2*c*d+4*f2*f2*f2*f2*c*d*d*d-a*a*d*c+b*c*c*a+4*a*b*f2*f2*d*d-2*a*d*d*f1*f1*b+2*b*b*c*f1*f1*d )); - cvSetReal2D(polynomial,0,1,( +f2*f2*f2*f2*d*d*d*d+b*b*b*b+2*b*b*f2*f2*d*d-a*a*d*d+b*b*c*c )); - cvSetReal2D(polynomial,0,0,( -a*d*d*b+b*b*c*d )); + Vec polynomial( + -a*d*d*b+b*b*c*d, + +f2*f2*f2*f2*d*d*d*d+b*b*b*b+2*b*b*f2*f2*d*d-a*a*d*d+b*b*c*c, + +4*a*b*b*b+4*b*b*f2*f2*c*d+4*f2*f2*f2*f2*c*d*d*d-a*a*d*c+b*c*c*a+4*a*b*f2*f2*d*d-2*a*d*d*f1*f1*b+2*b*b*c*f1*f1*d, + +6*a*a*b*b+6*f2*f2*f2*f2*c*c*d*d+2*b*b*f2*f2*c*c+2*a*a*f2*f2*d*d-2*a*a*d*d*f1*f1+2*b*b*c*c*f1*f1+8*a*b*f2*f2*c*d, + +4*a*a*a*b+2*b*c*c*f1*f1*a+4*f2*f2*f2*f2*c*c*c*d+4*a*b*f2*f2*c*c+4*a*a*f2*f2*c*d-2*a*a*d*f1*f1*c-a*d*d*f1*f1*f1*f1*b+b*b*c*f1*f1*f1*f1*d, + +f2*f2*f2*f2*c*c*c*c+2*a*a*f2*f2*c*c-a*a*d*d*f1*f1*f1*f1+b*b*c*c*f1*f1*f1*f1+a*a*a*a, + +b*c*c*f1*f1*f1*f1*a-a*a*d*f1*f1*f1*f1*c); // Solve g(t) for t to get 6 roots - cvSetZero(result); - cvSolvePoly(polynomial, result, 100, 20); + double rdata[6*2]; + Mat result(6, 1, CV_64FC2, rdata); + solvePoly(polynomial, result); // Evaluate the cost function s(t) at the real part of the 6 roots - t_min = DBL_MAX; - s_val = 1./(f1*f1) + (c*c)/(a*a+f2*f2*c*c); + double t_min = DBL_MAX; + double s_val = 1./(f1*f1) + (c*c)/(a*a+f2*f2*c*c); for (int ti = 0; ti < 6; ++ti) { - t = result->data.db[2*ti]; - s = (t*t)/(1 + f1*f1*t*t) + ((c*t + d)*(c*t + d))/((a*t + b)*(a*t + b) + f2*f2*(c*t + d)*(c*t + d)); + Vec2d root_i = result.at(ti); + double t = root_i(0); + double s = (t*t)/(1 + f1*f1*t*t) + ((c*t + d)*(c*t + d))/((a*t + b)*(a*t + b) + f2*f2*(c*t + d)*(c*t + d)); if (s < s_val) { s_val = s; t_min = t; @@ -308,81 +272,27 @@ icvCorrectMatches(CvMat *F_, CvMat *points1_, CvMat *points2_, CvMat *new_points } // find the optimal x1 and y1 as the points on l1 and l2 closest to the origin - tmp31->data.db[0] = t_min*t_min*f1; - tmp31->data.db[1] = t_min; - tmp31->data.db[2] = t_min*t_min*f1*f1+1; - tmp31->data.db[0] /= tmp31->data.db[2]; - tmp31->data.db[1] /= tmp31->data.db[2]; - tmp31->data.db[2] /= tmp31->data.db[2]; - cvGEMM(T1i,R1,1,0,0,tmp33,CV_GEMM_B_T); - cvGEMM(tmp33,tmp31,1,0,0,tmp31_2); - x1 = tmp31_2->data.db[0]; - y1 = tmp31_2->data.db[1]; + scale = t_min*t_min*f1*f1+1; + Vec3d tmp31(t_min*t_min*f1/scale, t_min/scale, 1); + Vec3d tmp31_2 = T1i*(R1_t*tmp31); + x1 = tmp31_2(0); + y1 = tmp31_2(1); - tmp31->data.db[0] = f2*std::pow(c*t_min+d,2); - tmp31->data.db[1] = -(a*t_min+b)*(c*t_min+d); - tmp31->data.db[2] = f2*f2*std::pow(c*t_min+d,2) + std::pow(a*t_min+b,2); - tmp31->data.db[0] /= tmp31->data.db[2]; - tmp31->data.db[1] /= tmp31->data.db[2]; - tmp31->data.db[2] /= tmp31->data.db[2]; - cvGEMM(T2i,R2,1,0,0,tmp33,CV_GEMM_B_T); - cvGEMM(tmp33,tmp31,1,0,0,tmp31_2); - x2 = tmp31_2->data.db[0]; - y2 = tmp31_2->data.db[1]; + scale = f2*f2*(c*t_min+d)*(c*t_min+d) + (a*t_min+b)*(a*t_min+b); + tmp31 = Vec3d(f2*(c*t_min+d)*(c*t_min+d)/scale, -(a*t_min+b)*(c*t_min+d)/scale, 1); + tmp31_2 = T2i*(R2.t()*tmp31); + x2 = tmp31_2(0); + y2 = tmp31_2(1); // Return the points in the matrix format that the user wants - points1->data.db[p*2] = x1; - points1->data.db[p*2+1] = y1; - points2->data.db[p*2] = x2; - points2->data.db[p*2+1] = y2; + if (depth1 == CV_32F) { + newPoints1.at(p) = Point2f((float)x1, (float)y1); + newPoints2.at(p) = Point2f((float)x2, (float)y2); + } else { + newPoints1.at(p) = Point2d(x1, y1); + newPoints2.at(p) = Point2d(x2, y2); + } } - - if( new_points1 ) - cvConvert( points1, new_points1 ); - if( new_points2 ) - cvConvert( points2, new_points2 ); } -void cv::triangulatePoints( InputArray _projMatr1, InputArray _projMatr2, - InputArray _projPoints1, InputArray _projPoints2, - OutputArray _points4D ) -{ - CV_INSTRUMENT_REGION(); - - Mat matr1 = _projMatr1.getMat(), matr2 = _projMatr2.getMat(); - Mat points1 = _projPoints1.getMat(), points2 = _projPoints2.getMat(); - - if((points1.rows == 1 || points1.cols == 1) && points1.channels() == 2) - points1 = points1.reshape(1, static_cast(points1.total())).t(); - - if((points2.rows == 1 || points2.cols == 1) && points2.channels() == 2) - points2 = points2.reshape(1, static_cast(points2.total())).t(); - - CvMat cvMatr1 = cvMat(matr1), cvMatr2 = cvMat(matr2); - CvMat cvPoints1 = cvMat(points1), cvPoints2 = cvMat(points2); - - _points4D.create(4, points1.cols, points1.type()); - Mat cvPoints4D_ = _points4D.getMat(); - CvMat cvPoints4D = cvMat(cvPoints4D_); - - icvTriangulatePoints(&cvMatr1, &cvMatr2, &cvPoints1, &cvPoints2, &cvPoints4D); -} - -void cv::correctMatches( InputArray _F, InputArray _points1, InputArray _points2, - OutputArray _newPoints1, OutputArray _newPoints2 ) -{ - CV_INSTRUMENT_REGION(); - - Mat F = _F.getMat(); - Mat points1 = _points1.getMat(), points2 = _points2.getMat(); - - CvMat cvPoints1 = cvMat(points1), cvPoints2 = cvMat(points2); - CvMat cvF = cvMat(F); - - _newPoints1.create(points1.size(), points1.type()); - _newPoints2.create(points2.size(), points2.type()); - Mat cvNewPoints1_ = _newPoints1.getMat(), cvNewPoints2_ = _newPoints2.getMat(); - CvMat cvNewPoints1 = cvMat(cvNewPoints1_), cvNewPoints2 = cvMat(cvNewPoints2_); - - icvCorrectMatches(&cvF, &cvPoints1, &cvPoints2, &cvNewPoints1, &cvNewPoints2); } diff --git a/modules/calib3d/src/undistort.dispatch.cpp b/modules/calib3d/src/undistort.dispatch.cpp index c56d1d7e7b..2a53c254d5 100644 --- a/modules/calib3d/src/undistort.dispatch.cpp +++ b/modules/calib3d/src/undistort.dispatch.cpp @@ -40,7 +40,6 @@ // //M*/ -#include "opencv2/core/core_c.h" #include "opencv2/core/types.hpp" #include "precomp.hpp" #include "distortion_model.hpp" @@ -48,8 +47,7 @@ #include "undistort.simd.hpp" #include "undistort.simd_declarations.hpp" // defines CV_CPU_DISPATCH_MODES_ALL=AVX2,...,BASELINE based on CMakeLists.txt content -namespace cv -{ +namespace cv { Mat getDefaultNewCameraMatrix( InputArray _cameraMatrix, Size imgsize, bool centerPrincipalPoint ) @@ -68,8 +66,7 @@ Mat getDefaultNewCameraMatrix( InputArray _cameraMatrix, Size imgsize, return newCameraMatrix; } -namespace { -Ptr getInitUndistortRectifyMapComputer(Size _size, Mat &_map1, Mat &_map2, int _m1type, +static Ptr getInitUndistortRectifyMapComputer(Size _size, Mat &_map1, Mat &_map2, int _m1type, const double* _ir, Matx33d &_matTilt, double _u0, double _v0, double _fx, double _fy, double _k1, double _k2, double _p1, double _p2, @@ -81,7 +78,6 @@ Ptr getInitUndistortRectifyMapComputer(Size _size, Mat &_map1, CV_CPU_DISPATCH(getInitUndistortRectifyMapComputer, (_size, _map1, _map2, _m1type, _ir, _matTilt, _u0, _v0, _fx, _fy, _k1, _k2, _p1, _p2, _k3, _k4, _k5, _k6, _s1, _s2, _s3, _s4), CV_CPU_DISPATCH_MODES_ALL); } -} void initUndistortRectifyMap( InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _matR, InputArray _newCameraMatrix, @@ -331,78 +327,74 @@ void undistort( InputArray _src, OutputArray _dst, InputArray _cameraMatrix, } } -} - -static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix, - const CvMat* _distCoeffs, - const CvMat* matR, const CvMat* matP, cv::TermCriteria criteria) +static void undistortPointsInternal( const Mat& _src, Mat& _dst, const Mat& _cameraMatrix, + const Mat& _distCoeffs, const Mat& matR, const Mat& matP, TermCriteria criteria) { CV_Assert(criteria.isValid()); double A[3][3], RR[3][3], k[14]={0,0,0,0,0,0,0,0,0,0,0,0,0,0}; - CvMat matA=cvMat(3, 3, CV_64F, A), _Dk; - CvMat _RR=cvMat(3, 3, CV_64F, RR); + Mat matA(3, 3, CV_64F, A), _Dk; + Mat _RR(3, 3, CV_64F, RR); cv::Matx33d invMatTilt = cv::Matx33d::eye(); cv::Matx33d matTilt = cv::Matx33d::eye(); + bool haveDistCoeffs = !_distCoeffs.empty(); - CV_Assert( CV_IS_MAT(_src) && CV_IS_MAT(_dst) && - (_src->rows == 1 || _src->cols == 1) && - (_dst->rows == 1 || _dst->cols == 1) && - _src->cols + _src->rows - 1 == _dst->rows + _dst->cols - 1 && - (CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) && - (CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2)); + CV_Assert( (_src.rows == 1 || _src.cols == 1) && + (_dst.rows == 1 || _dst.cols == 1) && + _src.cols + _src.rows - 1 == _dst.rows + _dst.cols - 1 && + (_src.type() == CV_32FC2 || _src.type() == CV_64FC2) && + (_dst.type() == CV_32FC2 || _dst.type() == CV_64FC2)); - CV_Assert( CV_IS_MAT(_cameraMatrix) && - _cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 ); + CV_Assert( _cameraMatrix.rows == 3 && _cameraMatrix.cols == 3 && _cameraMatrix.channels() == 1 ); + _cameraMatrix.convertTo(matA, CV_64F); - cvConvert( _cameraMatrix, &matA ); - - - if( _distCoeffs ) + if( haveDistCoeffs ) { - CV_Assert( CV_IS_MAT(_distCoeffs) && - (_distCoeffs->rows == 1 || _distCoeffs->cols == 1) && - (_distCoeffs->rows*_distCoeffs->cols == 4 || - _distCoeffs->rows*_distCoeffs->cols == 5 || - _distCoeffs->rows*_distCoeffs->cols == 8 || - _distCoeffs->rows*_distCoeffs->cols == 12 || - _distCoeffs->rows*_distCoeffs->cols == 14)); + CV_Assert( + (_distCoeffs.rows == 1 || _distCoeffs.cols == 1) && + (_distCoeffs.rows*_distCoeffs.cols == 4 || + _distCoeffs.rows*_distCoeffs.cols == 5 || + _distCoeffs.rows*_distCoeffs.cols == 8 || + _distCoeffs.rows*_distCoeffs.cols == 12 || + _distCoeffs.rows*_distCoeffs.cols == 14)); - _Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols, - CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k); - - cvConvert( _distCoeffs, &_Dk ); + _Dk = Mat( _distCoeffs.rows, _distCoeffs.cols, + CV_MAKETYPE(CV_64F,_distCoeffs.channels()), k); + _distCoeffs.convertTo(_Dk, CV_64F); + CV_Assert(_Dk.ptr() == k); if (k[12] != 0 || k[13] != 0) { - cv::detail::computeTiltProjectionMatrix(k[12], k[13], NULL, NULL, NULL, &invMatTilt); - cv::detail::computeTiltProjectionMatrix(k[12], k[13], &matTilt, NULL, NULL); + detail::computeTiltProjectionMatrix(k[12], k[13], NULL, NULL, NULL, &invMatTilt); + detail::computeTiltProjectionMatrix(k[12], k[13], &matTilt, NULL, NULL); } } - if( matR ) + if( !matR.empty() ) { - CV_Assert( CV_IS_MAT(matR) && matR->rows == 3 && matR->cols == 3 ); - cvConvert( matR, &_RR ); + CV_Assert( matR.rows == 3 && matR.cols == 3 && matR.channels() == 1 ); + matR.convertTo(_RR, CV_64F); + CV_Assert(_RR.ptr() == &RR[0][0]); } else - cvSetIdentity(&_RR); + setIdentity(_RR); - if( matP ) + if( !matP.empty() ) { double PP[3][3]; - CvMat _P3x3, _PP=cvMat(3, 3, CV_64F, PP); - CV_Assert( CV_IS_MAT(matP) && matP->rows == 3 && (matP->cols == 3 || matP->cols == 4)); - cvConvert( cvGetCols(matP, &_P3x3, 0, 3), &_PP ); - cvMatMul( &_PP, &_RR, &_RR ); + Mat _PP(3, 3, CV_64F, PP); + CV_Assert( matP.rows == 3 && (matP.cols == 3 || matP.cols == 4)); + matP.colRange(0, 3).convertTo(_PP, CV_64F); + CV_Assert(_PP.ptr() == &PP[0][0]); + _RR = _PP*_RR; } - const CvPoint2D32f* srcf = (const CvPoint2D32f*)_src->data.ptr; - const CvPoint2D64f* srcd = (const CvPoint2D64f*)_src->data.ptr; - CvPoint2D32f* dstf = (CvPoint2D32f*)_dst->data.ptr; - CvPoint2D64f* dstd = (CvPoint2D64f*)_dst->data.ptr; - int stype = CV_MAT_TYPE(_src->type); - int dtype = CV_MAT_TYPE(_dst->type); - int sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype); - int dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype); + const Point2f* srcf = (const Point2f*)_src.data; + const Point2d* srcd = (const Point2d*)_src.data; + Point2f* dstf = (Point2f*)_dst.data; + Point2d* dstd = (Point2d*)_dst.data; + int stype = _src.type(); + int dtype = _dst.type(); + int sstep = _src.rows == 1 ? 1 : (int)(_src.step/_src.elemSize()); + int dstep = _dst.rows == 1 ? 1 : (int)(_dst.step/_dst.elemSize()); double fx = A[0][0]; double fy = A[1][1]; @@ -411,7 +403,7 @@ static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvM double cx = A[0][2]; double cy = A[1][2]; - int n = _src->rows + _src->cols - 1; + int n = _src.rows + _src.cols - 1; for( int i = 0; i < n; i++ ) { double x, y, x0 = 0, y0 = 0, u, v; @@ -431,7 +423,7 @@ static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvM x = (x - cx)*ifx; y = (y - cy)*ify; - if( _distCoeffs ) { + if( haveDistCoeffs ) { // compensate tilt distortion // s * [x''', y''', 1]^T = matTilt * [x'', y'', 1]^T => // s * matTilt^{-1} * [x''', y''', 1]^T = [x'', y'', 1]^T => @@ -453,9 +445,9 @@ static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvM for( int j = 0; ; j++ ) { - if ((criteria.type & cv::TermCriteria::COUNT) && j >= criteria.maxCount) + if ((criteria.type & TermCriteria::COUNT) && j >= criteria.maxCount) break; - if ((criteria.type & cv::TermCriteria::EPS) && error < criteria.epsilon) + if ((criteria.type & TermCriteria::EPS) && error < criteria.epsilon) break; // r^2 = x'^2 + y'^2 double r2 = x*x + y*y; @@ -480,11 +472,11 @@ static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvM double new_x = (1. - alpha)*x + alpha*(x0 - deltaX)*icdist; double new_y = (1. - alpha)*y + alpha*(y0 - deltaY)*icdist; - if(criteria.type & cv::TermCriteria::EPS) + if(criteria.type & TermCriteria::EPS) { double r4, r6, a1, a2, a3, cdist, icdist2; double xd, yd, xd0, yd0; - cv::Vec3d vecTilt; + Vec3d vecTilt; // r^2 = x'^2 + y'^2 r2 = new_x*new_x + new_y*new_y; @@ -528,7 +520,7 @@ static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvM } } - if( matR || matP ) + if( !matR.empty() || !matP.empty() ) { double xx = RR[0][0]*x + RR[0][1]*y + RR[0][2]; double yy = RR[1][0]*x + RR[1][1]*y + RR[1][2]; @@ -550,8 +542,6 @@ static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvM } } -namespace cv { - void undistortPoints(InputArray _src, OutputArray _dst, InputArray _cameraMatrix, InputArray _distCoeffs, @@ -583,15 +573,7 @@ void undistortPoints(InputArray _src, OutputArray _dst, _dst.create(npoints, 1, CV_MAKETYPE(depth, 2), -1, true); Mat dst = _dst.getMat(); - CvMat _csrc = cvMat(src), _cdst = cvMat(dst), _ccameraMatrix = cvMat(cameraMatrix); - CvMat matR, matP, _cdistCoeffs, *pR=0, *pP=0, *pD=0; - if( !R.empty() ) - pR = &(matR = cvMat(R)); - if( !P.empty() ) - pP = &(matP = cvMat(P)); - if( !distCoeffs.empty() ) - pD = &(_cdistCoeffs = cvMat(distCoeffs)); - cvUndistortPointsInternal(&_csrc, &_cdst, &_ccameraMatrix, pD, pR, pP, criteria); + undistortPointsInternal(src, dst, cameraMatrix, distCoeffs, R, P, criteria); } void undistortImagePoints(InputArray src, OutputArray dst, InputArray cameraMatrix, InputArray distCoeffs, TermCriteria termCriteria) @@ -629,7 +611,7 @@ static Point2f mapPointSpherical(const Point2f& p, float alpha, Vec4d* J, enum U double fy1 = iR/std::sqrt(1 - y1*y1); *J = Vec4d(fx1*(kx*x + k), fx1*ky*x, fy1*kx*y, fy1*(ky*y + k)); } - return Point2f((float)asin(x1), (float)asin(y1)); + return Point2f((float)std::asin(x1), (float)std::asin(y1)); } CV_Error(Error::StsBadArg, "Unknown projection type"); } @@ -710,8 +692,8 @@ float initWideAngleProjMap(InputArray _cameraMatrix0, InputArray _distCoeffs0, Mat mapxy(dsize, CV_32FC2); double k1 = k[0], k2 = k[1], k3 = k[2], p1 = k[3], p2 = k[4], k4 = k[5], k5 = k[6], k6 = k[7], s1 = k[8], s2 = k[9], s3 = k[10], s4 = k[11]; double fx = cameraMatrix.at(0,0), fy = cameraMatrix.at(1,1), cx = scenter.x, cy = scenter.y; - cv::Matx33d matTilt; - cv::detail::computeTiltProjectionMatrix(k[12], k[13], &matTilt); + Matx33d matTilt; + detail::computeTiltProjectionMatrix(k[12], k[13], &matTilt); for( int y = 0; y < dsize.height; y++ ) { @@ -752,5 +734,5 @@ float initWideAngleProjMap(InputArray _cameraMatrix0, InputArray _distCoeffs0, return scale; } -} // namespace +} /* End of file */ diff --git a/modules/calib3d/test/test_cameracalibration.cpp b/modules/calib3d/test/test_cameracalibration.cpp index 41751e15ee..4997f8f23b 100644 --- a/modules/calib3d/test/test_cameracalibration.cpp +++ b/modules/calib3d/test/test_cameracalibration.cpp @@ -2203,28 +2203,56 @@ TEST(Calib3d_Triangulate, accuracy) { // the testcase from http://code.opencv.org/issues/4334 { - double P1data[] = { 250, 0, 200, 0, 0, 250, 150, 0, 0, 0, 1, 0 }; - double P2data[] = { 250, 0, 200, -250, 0, 250, 150, 0, 0, 0, 1, 0 }; - Mat P1(3, 4, CV_64F, P1data), P2(3, 4, CV_64F, P2data); + double P1data[] = { 250, 0, 200, 0, 0, 250, 150, 0, 0, 0, 1, 0 }; + double P2data[] = { 250, 0, 200, -250, 0, 250, 150, 0, 0, 0, 1, 0 }; + Mat P1(3, 4, CV_64F, P1data), P2(3, 4, CV_64F, P2data); - float x1data[] = { 200.f, 0.f }; - float x2data[] = { 170.f, 1.f }; - float Xdata[] = { 0.f, -5.f, 25/3.f }; - Mat x1(2, 1, CV_32F, x1data); - Mat x2(2, 1, CV_32F, x2data); - Mat res0(1, 3, CV_32F, Xdata); - Mat res_, res; + float x1data[] = { 200.f, 0.f }; + float x2data[] = { 170.f, 1.f }; + float Xdata[] = { 0.f, -5.f, 25/3.f }; + constexpr int num_points = 5; + Mat1f res0(num_points, 3); + for(int i = 0; i < num_points; ++i) { + for(int j = 0; j < 3; ++j) { + res0(i, j) = Xdata[j]; + } + } - triangulatePoints(P1, P2, x1, x2, res_); - cv::transpose(res_, res_); // TODO cvtest (transpose doesn't support inplace) - convertPointsFromHomogeneous(res_, res); - res = res.reshape(1, 1); - cout << "[1]:" << endl; - cout << "\tres0: " << res0 << endl; - cout << "\tres: " << res << endl; - ASSERT_LE(cvtest::norm(res, res0, NORM_INF), 1e-1); + Mat res_, res; + + + + for(int test : {0, 1}) { + if (test == 0) { + // 2xN and 1xN 2-channel. + Mat1f x1(2, num_points); + Mat2f x2(1, num_points); + for(int i = 0; i < 2; ++i) { + for(int j = 0; j < num_points; ++j) { + x1(i, j) = x1data[i]; + x2(0, j)[i] = x2data[i]; + } + } + triangulatePoints(P1, P2, x1, x2, res_); + } else { + // Array and vector. + std::array x1; + x1.fill({x1data[0], x1data[1]}); + std::vector x2(num_points, {x2data[0], x2data[1]}); + triangulatePoints(P1, P2, x1, x2, res_); + } + cv::transpose(res_, res_); // TODO cvtest (transpose doesn't support inplace) + convertPointsFromHomogeneous(res_, res); + res = res.reshape(1, num_points); + + cout << "[1]:" << endl; + cout << "\tres0: " << res0 << endl; + cout << "\tres: " << res << endl; + + ASSERT_LE(cvtest::norm(res, res0, NORM_INF), 1e-1); + } } // another testcase http://code.opencv.org/issues/3461