potok/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2026-07-21 19:33:03 +04:00
Code Packages Releases Wiki Activity

Merge pull request #28825 from vrabaud:cxx

Browse Source 
Bump some calib3d files to C++ #28825

This is just copy/pasting files from 5.x (first commit) and applying some light patch for compilation (second commit).

### Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [x] The PR is proposed to the proper branch
- [ ] There is a reference to the original bug report and related work
- [ ] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
      Patch to opencv_extra has the same branch name.
- [ ] The feature is well documented and sample code can be built with the project CMake
This commit is contained in:
Vincent Rabaud
2026-04-28 09:16:42 +02:00
committed by GitHub
parent 049917f793
commit 597c360b9d
5 changed files with 405 additions and 487 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/calib3d/src/epnp.cpp
+92 -96
View File
@@ -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 <iostream>
#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<double>(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<double>(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<double>(i, 0) = L_6x10.at<double>(i, 0);
L_6x4.at<double>(i, 1) = L_6x10.at<double>(i, 1);
L_6x4.at<double>(i, 2) = L_6x10.at<double>(i, 3);
L_6x4.at<double>(i, 3) = L_6x10.at<double>(i, 6);
}
cvSolve(&L_6x4, Rho, &B4, CV_SVD);
solve(L_6x4, Rho, B4, DECOMP_SVD);
CV_Assert(B4.ptr<double>() == 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<double>(i, 0) = L_6x10.at<double>(i, 0);
L_6x3.at<double>(i, 1) = L_6x10.at<double>(i, 1);
L_6x3.at<double>(i, 2) = L_6x10.at<double>(i, 2);
}
cvSolve(&L_6x3, Rho, &B3, CV_SVD);
solve(L_6x3, Rho, B3, DECOMP_SVD);
CV_Assert(B3.ptr<double>() == 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<double>(i, 0) = L_6x10.at<double>(i, 0);
L_6x5.at<double>(i, 1) = L_6x10.at<double>(i, 1);
L_6x5.at<double>(i, 2) = L_6x10.at<double>(i, 2);
L_6x5.at<double>(i, 3) = L_6x10.at<double>(i, 3);
L_6x5.at<double>(i, 4) = L_6x10.at<double>(i, 4);
}
cvSolve(&L_6x5, Rho, &B5, CV_SVD);
solve(L_6x5, Rho, B5, DECOMP_SVD);
CV_Assert(B5.ptr<double>() == 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<double>(i);
double * rowA = A.ptr<double>(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<double>(i) = Rho.at<double>(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<double>(), * 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<double>();
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<double>();
pX[nc - 1] = pb[nc - 1] / A2[nc - 1];
for(int i = nc - 2; i >= 0; i--)
{
modules/calib3d/src/epnp.h
+13 -11
View File
@@ -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]);
modules/calib3d/src/triangulate.cpp
+195 -285
View File
@@ -40,83 +40,111 @@
//M*/
#include "precomp.hpp"
#include "opencv2/core/core_c.h"
#include <opencv2/core/hal/hal.hpp>
// 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<float>() : 0;
const float *p2f = depth2 == CV_32F ? points2.ptr<float>() : 0;
const double *p1d = depth1 == CV_64F ? points1.ptr<double>() : 0;
const double *p2d = depth2 == CV_64F ? points2.ptr<double>() : 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<double, 4, 4> matrA;
cv::Matx<double, 4, 4> matrU;
cv::Matx<double, 4, 1> matrW;
cv::Matx<double, 4, 4> 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<double, 4, 4> matrA;
Matx<double, 4, 4> matrU;
Matx<double, 4, 1> matrW;
Matx<double, 4, 4> matrV;
size_t step4 = 4*sizeof(double);
Matx<double, 3, 4> P1;
Matx<double, 3, 4> 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<float>(0, i) = (float)matrV(3, 0);
points4D.at<float>(1, i) = (float)matrV(3, 1);
points4D.at<float>(2, i) = (float)matrV(3, 2);
points4D.at<float>(3, i) = (float)matrV(3, 3);
}
else
{
points4D.at<double>(0, i) = matrV(3, 0);
points4D.at<double>(1, i) = matrV(3, 1);
points4D.at<double>(2, i) = matrV(3, 2);
points4D.at<double>(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<CvMat> tmp33;
cv::Ptr<CvMat> tmp31, tmp31_2;
cv::Ptr<CvMat> T1i, T2i;
cv::Ptr<CvMat> R1, R2;
cv::Ptr<CvMat> TFT, TFTt, RTFTR;
cv::Ptr<CvMat> U, S, V;
cv::Ptr<CvMat> e1, e2;
cv::Ptr<CvMat> polynomial;
cv::Ptr<CvMat> result;
cv::Ptr<CvMat> points1, points2;
cv::Ptr<CvMat> 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<Point2f>(p);
Point2f p2 = points2.at<Point2f>(p);
x1 = p1.x; y1 = p1.y;
x2 = p2.x; y2 = p2.y;
} else {
Point2d p1 = points1.at<Point2d>(p);
Point2d p2 = points2.at<Point2d>(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<double, 7> 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<Vec2d>(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<Point2f>(p) = Point2f((float)x1, (float)y1);
newPoints2.at<Point2f>(p) = Point2f((float)x2, (float)y2);
} else {
newPoints1.at<Point2d>(p) = Point2d(x1, y1);
newPoints2.at<Point2d>(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<int>(points1.total())).t();
if((points2.rows == 1 || points2.cols == 1) && points2.channels() == 2)
points2 = points2.reshape(1, static_cast<int>(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);
}
modules/calib3d/src/undistort.dispatch.cpp
+59 -77
View File
@@ -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<ParallelLoopBody> getInitUndistortRectifyMapComputer(Size _size, Mat &_map1, Mat &_map2, int _m1type,
static Ptr<ParallelLoopBody> 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<ParallelLoopBody> 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<double>() == k);
if (k[12] != 0 || k[13] != 0)
{
cv::detail::computeTiltProjectionMatrix<double>(k[12], k[13], NULL, NULL, NULL, &invMatTilt);
cv::detail::computeTiltProjectionMatrix<double>(k[12], k[13], &matTilt, NULL, NULL);
detail::computeTiltProjectionMatrix<double>(k[12], k[13], NULL, NULL, NULL, &invMatTilt);
detail::computeTiltProjectionMatrix<double>(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<double>() == &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<double>() == &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<double>(0,0), fy = cameraMatrix.at<double>(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 */
modules/calib3d/test/test_cameracalibration.cpp
+46 -18
View File
@@ -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<cv::Point2f, num_points> x1;
x1.fill({x1data[0], x1data[1]});
std::vector<cv::Point2f> 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