mirror of
https://github.com/opencv/opencv.git
synced 2026-07-25 21:33:04 +04:00
Alexander Smorkalov
59218f9edd
Geometry module #29175 OpenCV Contrib: https://github.com/opencv/opencv_contrib/pull/4129 CI changes: https://github.com/opencv/ci-gha-workflow/pull/313 Continues - https://github.com/opencv/opencv/pull/28804 - https://github.com/opencv/opencv/pull/29101 - https://github.com/opencv/opencv/pull/29108 - https://github.com/opencv/opencv/pull/28810 Todo for followup PRs: - [x] Rename doxygen groups - [x] Fix JS modules layout and whitelists - [ ] Sort tutorials code/snippets ### 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
1423 lines
56 KiB
C++
1423 lines
56 KiB
C++
// 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 "test_precomp.hpp"
|
|
#include "opencv2/geometry.hpp"
|
|
#include "opencv2/ts/cuda_test.hpp" // EXPECT_MAT_NEAR
|
|
|
|
namespace opencv_test { namespace {
|
|
|
|
#if 0
|
|
class CV_ProjectPointsTest : public cvtest::ArrayTest
|
|
{
|
|
public:
|
|
CV_ProjectPointsTest();
|
|
|
|
protected:
|
|
int read_params( const cv::FileStorage& fs );
|
|
void fill_array( int test_case_idx, int i, int j, Mat& arr );
|
|
int prepare_test_case( int test_case_idx );
|
|
void get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types );
|
|
double get_success_error_level( int test_case_idx, int i, int j );
|
|
void run_func();
|
|
void prepare_to_validation( int );
|
|
|
|
bool calc_jacobians;
|
|
};
|
|
|
|
|
|
CV_ProjectPointsTest::CV_ProjectPointsTest()
|
|
: cvtest::ArrayTest( "3d-ProjectPoints", "cvProjectPoints2", "" )
|
|
{
|
|
test_array[INPUT].push_back(NULL); // rotation vector
|
|
test_array[OUTPUT].push_back(NULL); // rotation matrix
|
|
test_array[OUTPUT].push_back(NULL); // jacobian (J)
|
|
test_array[OUTPUT].push_back(NULL); // rotation vector (backward transform result)
|
|
test_array[OUTPUT].push_back(NULL); // inverse transform jacobian (J1)
|
|
test_array[OUTPUT].push_back(NULL); // J*J1 (or J1*J) == I(3x3)
|
|
test_array[REF_OUTPUT].push_back(NULL);
|
|
test_array[REF_OUTPUT].push_back(NULL);
|
|
test_array[REF_OUTPUT].push_back(NULL);
|
|
test_array[REF_OUTPUT].push_back(NULL);
|
|
test_array[REF_OUTPUT].push_back(NULL);
|
|
|
|
element_wise_relative_error = false;
|
|
calc_jacobians = false;
|
|
}
|
|
|
|
|
|
int CV_ProjectPointsTest::read_params( const cv::FileStorage& fs )
|
|
{
|
|
int code = cvtest::ArrayTest::read_params( fs );
|
|
return code;
|
|
}
|
|
|
|
|
|
void CV_ProjectPointsTest::get_test_array_types_and_sizes(
|
|
int /*test_case_idx*/, vector<vector<Size> >& sizes, vector<vector<int> >& types )
|
|
{
|
|
RNG& rng = ts->get_rng();
|
|
int depth = cvtest::randInt(rng) % 2 == 0 ? CV_32F : CV_64F;
|
|
int i, code;
|
|
|
|
code = cvtest::randInt(rng) % 3;
|
|
types[INPUT][0] = CV_MAKETYPE(depth, 1);
|
|
|
|
if( code == 0 )
|
|
{
|
|
sizes[INPUT][0] = cvSize(1,1);
|
|
types[INPUT][0] = CV_MAKETYPE(depth, 3);
|
|
}
|
|
else if( code == 1 )
|
|
sizes[INPUT][0] = cvSize(3,1);
|
|
else
|
|
sizes[INPUT][0] = cvSize(1,3);
|
|
|
|
sizes[OUTPUT][0] = cvSize(3, 3);
|
|
types[OUTPUT][0] = CV_MAKETYPE(depth, 1);
|
|
|
|
types[OUTPUT][1] = CV_MAKETYPE(depth, 1);
|
|
|
|
if( cvtest::randInt(rng) % 2 )
|
|
sizes[OUTPUT][1] = cvSize(3,9);
|
|
else
|
|
sizes[OUTPUT][1] = cvSize(9,3);
|
|
|
|
types[OUTPUT][2] = types[INPUT][0];
|
|
sizes[OUTPUT][2] = sizes[INPUT][0];
|
|
|
|
types[OUTPUT][3] = types[OUTPUT][1];
|
|
sizes[OUTPUT][3] = cvSize(sizes[OUTPUT][1].height, sizes[OUTPUT][1].width);
|
|
|
|
types[OUTPUT][4] = types[OUTPUT][1];
|
|
sizes[OUTPUT][4] = cvSize(3,3);
|
|
|
|
calc_jacobians = 1;//cvtest::randInt(rng) % 3 != 0;
|
|
if( !calc_jacobians )
|
|
sizes[OUTPUT][1] = sizes[OUTPUT][3] = sizes[OUTPUT][4] = cvSize(0,0);
|
|
|
|
for( i = 0; i < 5; i++ )
|
|
{
|
|
types[REF_OUTPUT][i] = types[OUTPUT][i];
|
|
sizes[REF_OUTPUT][i] = sizes[OUTPUT][i];
|
|
}
|
|
}
|
|
|
|
|
|
double CV_ProjectPointsTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int j )
|
|
{
|
|
return j == 4 ? 1e-2 : 1e-2;
|
|
}
|
|
|
|
|
|
void CV_ProjectPointsTest::fill_array( int /*test_case_idx*/, int /*i*/, int /*j*/, CvMat* arr )
|
|
{
|
|
double r[3], theta0, theta1, f;
|
|
CvMat _r = cvMat( arr->rows, arr->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(arr->type)), r );
|
|
RNG& rng = ts->get_rng();
|
|
|
|
r[0] = cvtest::randReal(rng)*CV_PI*2;
|
|
r[1] = cvtest::randReal(rng)*CV_PI*2;
|
|
r[2] = cvtest::randReal(rng)*CV_PI*2;
|
|
|
|
theta0 = sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]);
|
|
theta1 = fmod(theta0, CV_PI*2);
|
|
|
|
if( theta1 > CV_PI )
|
|
theta1 = -(CV_PI*2 - theta1);
|
|
|
|
f = theta1/(theta0 ? theta0 : 1);
|
|
r[0] *= f;
|
|
r[1] *= f;
|
|
r[2] *= f;
|
|
|
|
cvTsConvert( &_r, arr );
|
|
}
|
|
|
|
|
|
int CV_ProjectPointsTest::prepare_test_case( int test_case_idx )
|
|
{
|
|
int code = cvtest::ArrayTest::prepare_test_case( test_case_idx );
|
|
return code;
|
|
}
|
|
|
|
|
|
void CV_ProjectPointsTest::run_func()
|
|
{
|
|
CvMat *v2m_jac = 0, *m2v_jac = 0;
|
|
if( calc_jacobians )
|
|
{
|
|
v2m_jac = &test_mat[OUTPUT][1];
|
|
m2v_jac = &test_mat[OUTPUT][3];
|
|
}
|
|
|
|
cvProjectPoints2( &test_mat[INPUT][0], &test_mat[OUTPUT][0], v2m_jac );
|
|
cvProjectPoints2( &test_mat[OUTPUT][0], &test_mat[OUTPUT][2], m2v_jac );
|
|
}
|
|
|
|
|
|
void CV_ProjectPointsTest::prepare_to_validation( int /*test_case_idx*/ )
|
|
{
|
|
const CvMat* vec = &test_mat[INPUT][0];
|
|
CvMat* m = &test_mat[REF_OUTPUT][0];
|
|
CvMat* vec2 = &test_mat[REF_OUTPUT][2];
|
|
CvMat* v2m_jac = 0, *m2v_jac = 0;
|
|
double theta0, theta1;
|
|
|
|
if( calc_jacobians )
|
|
{
|
|
v2m_jac = &test_mat[REF_OUTPUT][1];
|
|
m2v_jac = &test_mat[REF_OUTPUT][3];
|
|
}
|
|
|
|
|
|
cvTsProjectPoints( vec, m, v2m_jac );
|
|
cvTsProjectPoints( m, vec2, m2v_jac );
|
|
cvTsCopy( vec, vec2 );
|
|
|
|
theta0 = cvtest::norm( cvarrtomat(vec2), 0, NORM_L2 );
|
|
theta1 = fmod( theta0, CV_PI*2 );
|
|
|
|
if( theta1 > CV_PI )
|
|
theta1 = -(CV_PI*2 - theta1);
|
|
cvScale( vec2, vec2, theta1/(theta0 ? theta0 : 1) );
|
|
|
|
if( calc_jacobians )
|
|
{
|
|
//cvInvert( v2m_jac, m2v_jac, CV_SVD );
|
|
if( cvtest::norm(cvarrtomat(&test_mat[OUTPUT][3]), 0, NORM_INF) < 1000 )
|
|
{
|
|
cvTsGEMM( &test_mat[OUTPUT][1], &test_mat[OUTPUT][3],
|
|
1, 0, 0, &test_mat[OUTPUT][4],
|
|
v2m_jac->rows == 3 ? 0 : CV_GEMM_A_T + CV_GEMM_B_T );
|
|
}
|
|
else
|
|
{
|
|
cvTsSetIdentity( &test_mat[OUTPUT][4], cvScalarAll(1.) );
|
|
cvTsCopy( &test_mat[REF_OUTPUT][2], &test_mat[OUTPUT][2] );
|
|
}
|
|
cvTsSetIdentity( &test_mat[REF_OUTPUT][4], cvScalarAll(1.) );
|
|
}
|
|
}
|
|
|
|
CV_ProjectPointsTest ProjectPoints_test;
|
|
|
|
#endif
|
|
|
|
//----------------------------------------- CV_ProjectPointsTest --------------------------------
|
|
void calcdfdx( const vector<vector<Point2f> >& leftF, const vector<vector<Point2f> >& rightF, double eps, Mat& dfdx )
|
|
{
|
|
const int fdim = 2;
|
|
CV_Assert( !leftF.empty() && !rightF.empty() && !leftF[0].empty() && !rightF[0].empty() );
|
|
CV_Assert( leftF[0].size() == rightF[0].size() );
|
|
CV_Assert( fabs(eps) > std::numeric_limits<double>::epsilon() );
|
|
int fcount = (int)leftF[0].size(), xdim = (int)leftF.size();
|
|
|
|
dfdx.create( fcount*fdim, xdim, CV_64FC1 );
|
|
|
|
vector<vector<Point2f> >::const_iterator arrLeftIt = leftF.begin();
|
|
vector<vector<Point2f> >::const_iterator arrRightIt = rightF.begin();
|
|
for( int xi = 0; xi < xdim; xi++, ++arrLeftIt, ++arrRightIt )
|
|
{
|
|
CV_Assert( (int)arrLeftIt->size() == fcount );
|
|
CV_Assert( (int)arrRightIt->size() == fcount );
|
|
vector<Point2f>::const_iterator lIt = arrLeftIt->begin();
|
|
vector<Point2f>::const_iterator rIt = arrRightIt->begin();
|
|
for( int fi = 0; fi < dfdx.rows; fi+=fdim, ++lIt, ++rIt )
|
|
{
|
|
dfdx.at<double>(fi, xi ) = 0.5 * ((double)(rIt->x - lIt->x)) / eps;
|
|
dfdx.at<double>(fi+1, xi ) = 0.5 * ((double)(rIt->y - lIt->y)) / eps;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
class CV_ProjectPointsTest : public cvtest::BaseTest
|
|
{
|
|
public:
|
|
CV_ProjectPointsTest() {}
|
|
protected:
|
|
void run(int);
|
|
virtual void project( const Mat& objectPoints,
|
|
const Mat& rvec, const Mat& tvec,
|
|
const Mat& cameraMatrix,
|
|
const Mat& distCoeffs,
|
|
vector<Point2f>& imagePoints,
|
|
Mat& dpdrot, Mat& dpdt, Mat& dpdf,
|
|
Mat& dpdc, Mat& dpddist,
|
|
double aspectRatio=0 ) = 0;
|
|
};
|
|
|
|
void CV_ProjectPointsTest::run(int)
|
|
{
|
|
//typedef float matType;
|
|
|
|
int code = cvtest::TS::OK;
|
|
const int pointCount = 100;
|
|
|
|
const float zMinVal = 10.0f, zMaxVal = 100.0f,
|
|
rMinVal = -0.3f, rMaxVal = 0.3f,
|
|
tMinVal = -2.0f, tMaxVal = 2.0f;
|
|
|
|
const float imgPointErr = 1e-3f,
|
|
dEps = 1e-3f;
|
|
|
|
double err;
|
|
|
|
Size imgSize( 600, 800 );
|
|
Mat_<float> objPoints( pointCount, 3), rvec( 1, 3), rmat, tvec( 1, 3 ), cameraMatrix( 3, 3 ), distCoeffs( 1, 4 ),
|
|
leftRvec, rightRvec, leftTvec, rightTvec, leftCameraMatrix, rightCameraMatrix, leftDistCoeffs, rightDistCoeffs;
|
|
|
|
RNG rng = ts->get_rng();
|
|
|
|
// generate data
|
|
cameraMatrix << 300.f, 0.f, imgSize.width/2.f,
|
|
0.f, 300.f, imgSize.height/2.f,
|
|
0.f, 0.f, 1.f;
|
|
distCoeffs << 0.1, 0.01, 0.001, 0.001;
|
|
|
|
rvec(0,0) = rng.uniform( rMinVal, rMaxVal );
|
|
rvec(0,1) = rng.uniform( rMinVal, rMaxVal );
|
|
rvec(0,2) = rng.uniform( rMinVal, rMaxVal );
|
|
rmat = cv::Mat_<float>::zeros(3, 3);
|
|
Rodrigues( rvec, rmat );
|
|
|
|
tvec(0,0) = rng.uniform( tMinVal, tMaxVal );
|
|
tvec(0,1) = rng.uniform( tMinVal, tMaxVal );
|
|
tvec(0,2) = rng.uniform( tMinVal, tMaxVal );
|
|
|
|
for( int y = 0; y < objPoints.rows; y++ )
|
|
{
|
|
Mat point(1, 3, CV_32FC1, objPoints.ptr(y) );
|
|
float z = rng.uniform( zMinVal, zMaxVal );
|
|
point.at<float>(0,2) = z;
|
|
point.at<float>(0,0) = (rng.uniform(2.f,(float)(imgSize.width-2)) - cameraMatrix(0,2)) / cameraMatrix(0,0) * z;
|
|
point.at<float>(0,1) = (rng.uniform(2.f,(float)(imgSize.height-2)) - cameraMatrix(1,2)) / cameraMatrix(1,1) * z;
|
|
point = (point - tvec) * rmat;
|
|
}
|
|
|
|
vector<Point2f> imgPoints;
|
|
vector<vector<Point2f> > leftImgPoints;
|
|
vector<vector<Point2f> > rightImgPoints;
|
|
Mat dpdrot, dpdt, dpdf, dpdc, dpddist,
|
|
valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist;
|
|
|
|
project( objPoints, rvec, tvec, cameraMatrix, distCoeffs,
|
|
imgPoints, dpdrot, dpdt, dpdf, dpdc, dpddist, 0 );
|
|
|
|
// calculate and check image points
|
|
CV_Assert( (int)imgPoints.size() == pointCount );
|
|
vector<Point2f>::const_iterator it = imgPoints.begin();
|
|
for( int i = 0; i < pointCount; i++, ++it )
|
|
{
|
|
Point3d p( objPoints(i,0), objPoints(i,1), objPoints(i,2) );
|
|
double z = p.x*rmat(2,0) + p.y*rmat(2,1) + p.z*rmat(2,2) + tvec(0,2),
|
|
x = (p.x*rmat(0,0) + p.y*rmat(0,1) + p.z*rmat(0,2) + tvec(0,0)) / z,
|
|
y = (p.x*rmat(1,0) + p.y*rmat(1,1) + p.z*rmat(1,2) + tvec(0,1)) / z,
|
|
r2 = x*x + y*y,
|
|
r4 = r2*r2;
|
|
Point2f validImgPoint;
|
|
double a1 = 2*x*y,
|
|
a2 = r2 + 2*x*x,
|
|
a3 = r2 + 2*y*y,
|
|
cdist = 1+distCoeffs(0,0)*r2+distCoeffs(0,1)*r4;
|
|
validImgPoint.x = static_cast<float>((double)cameraMatrix(0,0)*(x*cdist + (double)distCoeffs(0,2)*a1 + (double)distCoeffs(0,3)*a2)
|
|
+ (double)cameraMatrix(0,2));
|
|
validImgPoint.y = static_cast<float>((double)cameraMatrix(1,1)*(y*cdist + (double)distCoeffs(0,2)*a3 + distCoeffs(0,3)*a1)
|
|
+ (double)cameraMatrix(1,2));
|
|
|
|
if( fabs(it->x - validImgPoint.x) > imgPointErr ||
|
|
fabs(it->y - validImgPoint.y) > imgPointErr )
|
|
{
|
|
ts->printf( cvtest::TS::LOG, "bad image point\n" );
|
|
code = cvtest::TS::FAIL_BAD_ACCURACY;
|
|
goto _exit_;
|
|
}
|
|
}
|
|
|
|
// check derivatives
|
|
// 1. rotation
|
|
leftImgPoints.resize(3);
|
|
rightImgPoints.resize(3);
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
rvec.copyTo( leftRvec ); leftRvec(0,i) -= dEps;
|
|
project( objPoints, leftRvec, tvec, cameraMatrix, distCoeffs,
|
|
leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
|
|
rvec.copyTo( rightRvec ); rightRvec(0,i) += dEps;
|
|
project( objPoints, rightRvec, tvec, cameraMatrix, distCoeffs,
|
|
rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
|
|
}
|
|
calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdrot );
|
|
err = cvtest::norm( dpdrot, valDpdrot, NORM_INF );
|
|
if( err > 3 )
|
|
{
|
|
ts->printf( cvtest::TS::LOG, "bad dpdrot: too big difference = %g\n", err );
|
|
code = cvtest::TS::FAIL_BAD_ACCURACY;
|
|
}
|
|
|
|
// 2. translation
|
|
for( int i = 0; i < 3; i++ )
|
|
{
|
|
tvec.copyTo( leftTvec ); leftTvec(0,i) -= dEps;
|
|
project( objPoints, rvec, leftTvec, cameraMatrix, distCoeffs,
|
|
leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
|
|
tvec.copyTo( rightTvec ); rightTvec(0,i) += dEps;
|
|
project( objPoints, rvec, rightTvec, cameraMatrix, distCoeffs,
|
|
rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
|
|
}
|
|
calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdt );
|
|
if( cvtest::norm( dpdt, valDpdt, NORM_INF ) > 0.2 )
|
|
{
|
|
ts->printf( cvtest::TS::LOG, "bad dpdtvec\n" );
|
|
code = cvtest::TS::FAIL_BAD_ACCURACY;
|
|
}
|
|
|
|
// 3. camera matrix
|
|
// 3.1. focus
|
|
leftImgPoints.resize(2);
|
|
rightImgPoints.resize(2);
|
|
cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(0,0) -= dEps;
|
|
project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs,
|
|
leftImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
|
|
cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(1,1) -= dEps;
|
|
project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs,
|
|
leftImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
|
|
cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(0,0) += dEps;
|
|
project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs,
|
|
rightImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
|
|
cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(1,1) += dEps;
|
|
project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs,
|
|
rightImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
|
|
calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdf );
|
|
if ( cvtest::norm( dpdf, valDpdf, NORM_L2 ) > 0.2 )
|
|
{
|
|
ts->printf( cvtest::TS::LOG, "bad dpdf\n" );
|
|
code = cvtest::TS::FAIL_BAD_ACCURACY;
|
|
}
|
|
// 3.2. principal point
|
|
leftImgPoints.resize(2);
|
|
rightImgPoints.resize(2);
|
|
cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(0,2) -= dEps;
|
|
project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs,
|
|
leftImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
|
|
cameraMatrix.copyTo( leftCameraMatrix ); leftCameraMatrix(1,2) -= dEps;
|
|
project( objPoints, rvec, tvec, leftCameraMatrix, distCoeffs,
|
|
leftImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
|
|
cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(0,2) += dEps;
|
|
project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs,
|
|
rightImgPoints[0], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
|
|
cameraMatrix.copyTo( rightCameraMatrix ); rightCameraMatrix(1,2) += dEps;
|
|
project( objPoints, rvec, tvec, rightCameraMatrix, distCoeffs,
|
|
rightImgPoints[1], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
|
|
calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpdc );
|
|
if ( cvtest::norm( dpdc, valDpdc, NORM_L2 ) > 0.2 )
|
|
{
|
|
ts->printf( cvtest::TS::LOG, "bad dpdc\n" );
|
|
code = cvtest::TS::FAIL_BAD_ACCURACY;
|
|
}
|
|
|
|
// 4. distortion
|
|
leftImgPoints.resize(distCoeffs.cols);
|
|
rightImgPoints.resize(distCoeffs.cols);
|
|
for( int i = 0; i < distCoeffs.cols; i++ )
|
|
{
|
|
distCoeffs.copyTo( leftDistCoeffs ); leftDistCoeffs(0,i) -= dEps;
|
|
project( objPoints, rvec, tvec, cameraMatrix, leftDistCoeffs,
|
|
leftImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
|
|
distCoeffs.copyTo( rightDistCoeffs ); rightDistCoeffs(0,i) += dEps;
|
|
project( objPoints, rvec, tvec, cameraMatrix, rightDistCoeffs,
|
|
rightImgPoints[i], valDpdrot, valDpdt, valDpdf, valDpdc, valDpddist, 0 );
|
|
}
|
|
calcdfdx( leftImgPoints, rightImgPoints, dEps, valDpddist );
|
|
if( cvtest::norm( dpddist, valDpddist, NORM_L2 ) > 0.3 )
|
|
{
|
|
ts->printf( cvtest::TS::LOG, "bad dpddist\n" );
|
|
code = cvtest::TS::FAIL_BAD_ACCURACY;
|
|
}
|
|
|
|
_exit_:
|
|
RNG& _rng = ts->get_rng();
|
|
_rng = rng;
|
|
ts->set_failed_test_info( code );
|
|
}
|
|
|
|
//----------------------------------------- CV_ProjectPointsTest_CPP --------------------------------
|
|
class CV_ProjectPointsTest_CPP : public CV_ProjectPointsTest
|
|
{
|
|
public:
|
|
CV_ProjectPointsTest_CPP() {}
|
|
protected:
|
|
virtual void project( const Mat& objectPoints,
|
|
const Mat& rvec, const Mat& tvec,
|
|
const Mat& cameraMatrix,
|
|
const Mat& distCoeffs,
|
|
vector<Point2f>& imagePoints,
|
|
Mat& dpdrot, Mat& dpdt, Mat& dpdf,
|
|
Mat& dpdc, Mat& dpddist,
|
|
double aspectRatio=0 );
|
|
};
|
|
|
|
void CV_ProjectPointsTest_CPP::project( const Mat& objectPoints, const Mat& rvec, const Mat& tvec,
|
|
const Mat& cameraMatrix, const Mat& distCoeffs, vector<Point2f>& imagePoints,
|
|
Mat& dpdrot, Mat& dpdt, Mat& dpdf, Mat& dpdc, Mat& dpddist, double aspectRatio)
|
|
{
|
|
Mat J;
|
|
projectPoints( objectPoints, rvec, tvec, cameraMatrix, distCoeffs, imagePoints, J, aspectRatio);
|
|
J.colRange(0, 3).copyTo(dpdrot);
|
|
J.colRange(3, 6).copyTo(dpdt);
|
|
J.colRange(6, 8).copyTo(dpdf);
|
|
J.colRange(8, 10).copyTo(dpdc);
|
|
J.colRange(10, J.cols).copyTo(dpddist);
|
|
}
|
|
|
|
TEST(Calib3d_ProjectPoints_CPP, regression) { CV_ProjectPointsTest_CPP test; test.safe_run(); }
|
|
|
|
TEST(Calib3d_ProjectPoints_CPP, inputShape)
|
|
{
|
|
Matx31d rvec = Matx31d::zeros();
|
|
Matx31d tvec(0, 0, 1);
|
|
Matx33d cameraMatrix = Matx33d::eye();
|
|
const float L = 0.1f;
|
|
{
|
|
//3xN 1-channel
|
|
Mat objectPoints = (Mat_<float>(3, 2) << -L, L,
|
|
L, L,
|
|
0, 0);
|
|
vector<Point2f> imagePoints;
|
|
projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
|
|
EXPECT_EQ(objectPoints.cols, static_cast<int>(imagePoints.size()));
|
|
EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits<float>::epsilon());
|
|
}
|
|
{
|
|
//Nx2 1-channel
|
|
Mat objectPoints = (Mat_<float>(2, 3) << -L, L, 0,
|
|
L, L, 0);
|
|
vector<Point2f> imagePoints;
|
|
projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
|
|
EXPECT_EQ(objectPoints.rows, static_cast<int>(imagePoints.size()));
|
|
EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits<float>::epsilon());
|
|
}
|
|
{
|
|
//1xN 3-channel
|
|
Mat objectPoints(1, 2, CV_32FC3);
|
|
objectPoints.at<Vec3f>(0,0) = Vec3f(-L, L, 0);
|
|
objectPoints.at<Vec3f>(0,1) = Vec3f(L, L, 0);
|
|
|
|
vector<Point2f> imagePoints;
|
|
projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
|
|
EXPECT_EQ(objectPoints.cols, static_cast<int>(imagePoints.size()));
|
|
EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits<float>::epsilon());
|
|
}
|
|
{
|
|
//Nx1 3-channel
|
|
Mat objectPoints(2, 1, CV_32FC3);
|
|
objectPoints.at<Vec3f>(0,0) = Vec3f(-L, L, 0);
|
|
objectPoints.at<Vec3f>(1,0) = Vec3f(L, L, 0);
|
|
|
|
vector<Point2f> imagePoints;
|
|
projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
|
|
EXPECT_EQ(objectPoints.rows, static_cast<int>(imagePoints.size()));
|
|
EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits<float>::epsilon());
|
|
}
|
|
{
|
|
//vector<Point3f>
|
|
vector<Point3f> objectPoints;
|
|
objectPoints.push_back(Point3f(-L, L, 0));
|
|
objectPoints.push_back(Point3f(L, L, 0));
|
|
|
|
vector<Point2f> imagePoints;
|
|
projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
|
|
EXPECT_EQ(objectPoints.size(), imagePoints.size());
|
|
EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits<float>::epsilon());
|
|
}
|
|
{
|
|
//vector<Point3d>
|
|
vector<Point3d> objectPoints;
|
|
objectPoints.push_back(Point3d(-L, L, 0));
|
|
objectPoints.push_back(Point3d(L, L, 0));
|
|
|
|
vector<Point2d> imagePoints;
|
|
projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
|
|
EXPECT_EQ(objectPoints.size(), imagePoints.size());
|
|
EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits<double>::epsilon());
|
|
EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits<double>::epsilon());
|
|
EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits<double>::epsilon());
|
|
EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits<double>::epsilon());
|
|
}
|
|
}
|
|
|
|
TEST(Calib3d_ProjectPoints_CPP, outputShape)
|
|
{
|
|
Matx31d rvec = Matx31d::zeros();
|
|
Matx31d tvec(0, 0, 1);
|
|
Matx33d cameraMatrix = Matx33d::eye();
|
|
const float L = 0.1f;
|
|
{
|
|
vector<Point3f> objectPoints;
|
|
objectPoints.push_back(Point3f(-L, L, 0));
|
|
objectPoints.push_back(Point3f( L, L, 0));
|
|
objectPoints.push_back(Point3f( L, -L, 0));
|
|
|
|
//Mat --> will be Nx1 2-channel
|
|
Mat imagePoints;
|
|
projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
|
|
EXPECT_EQ(static_cast<int>(objectPoints.size()), imagePoints.rows);
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(0,0)(0), -L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(0,0)(1), L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(1,0)(0), L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(1,0)(1), L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(2,0)(0), L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(2,0)(1), -L, std::numeric_limits<float>::epsilon());
|
|
}
|
|
{
|
|
vector<Point3f> objectPoints;
|
|
objectPoints.push_back(Point3f(-L, L, 0));
|
|
objectPoints.push_back(Point3f( L, L, 0));
|
|
objectPoints.push_back(Point3f( L, -L, 0));
|
|
|
|
//Nx1 2-channel
|
|
Mat imagePoints(3,1,CV_32FC2);
|
|
projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
|
|
EXPECT_EQ(static_cast<int>(objectPoints.size()), imagePoints.rows);
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(0,0)(0), -L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(0,0)(1), L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(1,0)(0), L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(1,0)(1), L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(2,0)(0), L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(2,0)(1), -L, std::numeric_limits<float>::epsilon());
|
|
}
|
|
{
|
|
vector<Point3f> objectPoints;
|
|
objectPoints.push_back(Point3f(-L, L, 0));
|
|
objectPoints.push_back(Point3f( L, L, 0));
|
|
objectPoints.push_back(Point3f( L, -L, 0));
|
|
|
|
//1xN 2-channel
|
|
Mat imagePoints(1,3,CV_32FC2);
|
|
projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
|
|
EXPECT_EQ(static_cast<int>(objectPoints.size()), imagePoints.cols);
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(0,0)(0), -L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(0,0)(1), L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(0,1)(0), L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(0,1)(1), L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(0,2)(0), L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints.at<Vec2f>(0,2)(1), -L, std::numeric_limits<float>::epsilon());
|
|
}
|
|
{
|
|
vector<Point3f> objectPoints;
|
|
objectPoints.push_back(Point3f(-L, L, 0));
|
|
objectPoints.push_back(Point3f(L, L, 0));
|
|
|
|
//vector<Point2f>
|
|
vector<Point2f> imagePoints;
|
|
projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
|
|
EXPECT_EQ(objectPoints.size(), imagePoints.size());
|
|
EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits<float>::epsilon());
|
|
EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits<float>::epsilon());
|
|
}
|
|
{
|
|
vector<Point3d> objectPoints;
|
|
objectPoints.push_back(Point3d(-L, L, 0));
|
|
objectPoints.push_back(Point3d(L, L, 0));
|
|
|
|
//vector<Point2d>
|
|
vector<Point2d> imagePoints;
|
|
projectPoints(objectPoints, rvec, tvec, cameraMatrix, noArray(), imagePoints);
|
|
EXPECT_EQ(objectPoints.size(), imagePoints.size());
|
|
EXPECT_NEAR(imagePoints[0].x, -L, std::numeric_limits<double>::epsilon());
|
|
EXPECT_NEAR(imagePoints[0].y, L, std::numeric_limits<double>::epsilon());
|
|
EXPECT_NEAR(imagePoints[1].x, L, std::numeric_limits<double>::epsilon());
|
|
EXPECT_NEAR(imagePoints[1].y, L, std::numeric_limits<double>::epsilon());
|
|
}
|
|
}
|
|
|
|
#define NUM_DIST_COEFF_TILT 14
|
|
|
|
class cameraCalibrationTiltTest : public ::testing::Test {
|
|
|
|
protected:
|
|
|
|
static const cv::Size m_imageSize;
|
|
static const double m_pixelSize;
|
|
static const double m_circleConfusionPixel;
|
|
static const double m_lensFocalLength;
|
|
static const double m_lensFNumber;
|
|
static const double m_objectDistance;
|
|
static const double m_planeTiltDegree;
|
|
static const double m_pointTargetDist;
|
|
static const int m_pointTargetNum;
|
|
|
|
/** image distance corresponding to working distance */
|
|
double m_imageDistance;
|
|
/** image tilt angle corresponding to the tilt of the object plane */
|
|
double m_imageTiltDegree;
|
|
/** center of the field of view, near and far plane */
|
|
std::vector<cv::Vec3d> m_fovCenter;
|
|
/** normal of the field of view, near and far plane */
|
|
std::vector<cv::Vec3d> m_fovNormal;
|
|
/** points on a plane calibration target */
|
|
std::vector<cv::Point3d> m_pointTarget;
|
|
/** rotations for the calibration target */
|
|
std::vector<cv::Vec3d> m_pointTargetRvec;
|
|
/** translations for the calibration target */
|
|
std::vector<cv::Vec3d> m_pointTargetTvec;
|
|
/** camera matrix */
|
|
cv::Matx33d m_cameraMatrix;
|
|
/** distortion coefficients */
|
|
cv::Vec<double, NUM_DIST_COEFF_TILT> m_distortionCoeff;
|
|
|
|
/** random generator */
|
|
cv::RNG m_rng;
|
|
/** degree to radian conversion factor */
|
|
const double m_toRadian;
|
|
/** radian to degree conversion factor */
|
|
const double m_toDegree;
|
|
|
|
cameraCalibrationTiltTest()
|
|
: m_toRadian(acos(-1.0)/180.0)
|
|
, m_toDegree(180.0/acos(-1.0))
|
|
{}
|
|
|
|
/**
|
|
C *hanges given distortion coefficients randomly by adding
|
|
a uniformly distributed random variable in [-max max]
|
|
\param coeff input
|
|
\param max limits for the random variables
|
|
*/
|
|
void randomDistortionCoeff(
|
|
cv::Vec<double, NUM_DIST_COEFF_TILT>& coeff,
|
|
const cv::Vec<double, NUM_DIST_COEFF_TILT>& max)
|
|
{
|
|
for (int i = 0; i < coeff.rows; ++i)
|
|
coeff(i) += m_rng.uniform(-max(i), max(i));
|
|
}
|
|
|
|
void removeInvalidPoints(
|
|
std::vector<cv::Point2d>& imagePoints,
|
|
std::vector<cv::Point3d>& objectPoints)
|
|
{
|
|
// remove object and imgage points out of range
|
|
std::vector<cv::Point2d>::iterator itImg = imagePoints.begin();
|
|
std::vector<cv::Point3d>::iterator itObj = objectPoints.begin();
|
|
while (itImg != imagePoints.end())
|
|
{
|
|
bool ok =
|
|
itImg->x >= 0 &&
|
|
itImg->x <= m_imageSize.width - 1.0 &&
|
|
itImg->y >= 0 &&
|
|
itImg->y <= m_imageSize.height - 1.0;
|
|
if (ok)
|
|
{
|
|
++itImg;
|
|
++itObj;
|
|
}
|
|
else
|
|
{
|
|
itImg = imagePoints.erase(itImg);
|
|
itObj = objectPoints.erase(itObj);
|
|
}
|
|
}
|
|
}
|
|
|
|
void numericalDerivative(
|
|
cv::Mat& jac,
|
|
double eps,
|
|
const std::vector<cv::Point3d>& obj,
|
|
const cv::Vec3d& rvec,
|
|
const cv::Vec3d& tvec,
|
|
const cv::Matx33d& camera,
|
|
const cv::Vec<double, NUM_DIST_COEFF_TILT>& distor)
|
|
{
|
|
cv::Vec3d r(rvec);
|
|
cv::Vec3d t(tvec);
|
|
cv::Matx33d cm(camera);
|
|
cv::Vec<double, NUM_DIST_COEFF_TILT> dc(distor);
|
|
double* param[10+NUM_DIST_COEFF_TILT] = {
|
|
&r(0), &r(1), &r(2),
|
|
&t(0), &t(1), &t(2),
|
|
&cm(0,0), &cm(1,1), &cm(0,2), &cm(1,2),
|
|
&dc(0), &dc(1), &dc(2), &dc(3), &dc(4), &dc(5), &dc(6),
|
|
&dc(7), &dc(8), &dc(9), &dc(10), &dc(11), &dc(12), &dc(13)};
|
|
std::vector<cv::Point2d> pix0, pix1;
|
|
double invEps = .5/eps;
|
|
|
|
for (int col = 0; col < 10+NUM_DIST_COEFF_TILT; ++col)
|
|
{
|
|
double save = *(param[col]);
|
|
*(param[col]) = save + eps;
|
|
cv::projectPoints(obj, r, t, cm, dc, pix0);
|
|
*(param[col]) = save - eps;
|
|
cv::projectPoints(obj, r, t, cm, dc, pix1);
|
|
*(param[col]) = save;
|
|
|
|
std::vector<cv::Point2d>::const_iterator it0 = pix0.begin();
|
|
std::vector<cv::Point2d>::const_iterator it1 = pix1.begin();
|
|
int row = 0;
|
|
for (;it0 != pix0.end(); ++it0, ++it1)
|
|
{
|
|
cv::Point2d d = invEps*(*it0 - *it1);
|
|
jac.at<double>(row, col) = d.x;
|
|
++row;
|
|
jac.at<double>(row, col) = d.y;
|
|
++row;
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
c *omputes for a given distance of an image or object point
|
|
the distance of the corresponding object or image point
|
|
*/
|
|
double opticalMap(double dist) {
|
|
return m_lensFocalLength*dist/(dist - m_lensFocalLength);
|
|
}
|
|
|
|
/** magnification of the optical map */
|
|
double magnification(double dist) {
|
|
return m_lensFocalLength/(dist - m_lensFocalLength);
|
|
}
|
|
|
|
void SetUp()
|
|
{
|
|
m_imageDistance = opticalMap(m_objectDistance);
|
|
m_imageTiltDegree = m_toDegree * atan2(
|
|
m_imageDistance * tan(m_toRadian * m_planeTiltDegree),
|
|
m_objectDistance);
|
|
// half sensor height
|
|
double tmp = .5 * (m_imageSize.height - 1) * m_pixelSize
|
|
* cos(m_toRadian * m_imageTiltDegree);
|
|
// y-Value of tilted sensor
|
|
double yImage[2] = {tmp, -tmp};
|
|
// change in z because of the tilt
|
|
tmp *= sin(m_toRadian * m_imageTiltDegree);
|
|
// z-values of the sensor lower and upper corner
|
|
double zImage[2] = {
|
|
m_imageDistance + tmp,
|
|
m_imageDistance - tmp};
|
|
// circle of confusion
|
|
double circleConfusion = m_circleConfusionPixel*m_pixelSize;
|
|
// aperture of the lense
|
|
double aperture = m_lensFocalLength/m_lensFNumber;
|
|
// near and far factor on the image side
|
|
double nearFarFactorImage[2] = {
|
|
aperture/(aperture - circleConfusion),
|
|
aperture/(aperture + circleConfusion)};
|
|
// on the object side - points that determine the field of
|
|
// view
|
|
std::vector<cv::Vec3d> fovBottomTop(6);
|
|
std::vector<cv::Vec3d>::iterator itFov = fovBottomTop.begin();
|
|
for (size_t iBottomTop = 0; iBottomTop < 2; ++iBottomTop)
|
|
{
|
|
// mapping sensor to field of view
|
|
*itFov = cv::Vec3d(0,yImage[iBottomTop],zImage[iBottomTop]);
|
|
*itFov *= magnification((*itFov)(2));
|
|
++itFov;
|
|
for (size_t iNearFar = 0; iNearFar < 2; ++iNearFar, ++itFov)
|
|
{
|
|
// scaling to the near and far distance on the
|
|
// image side
|
|
*itFov = cv::Vec3d(0,yImage[iBottomTop],zImage[iBottomTop]) *
|
|
nearFarFactorImage[iNearFar];
|
|
// scaling to the object side
|
|
*itFov *= magnification((*itFov)(2));
|
|
}
|
|
}
|
|
m_fovCenter.resize(3);
|
|
m_fovNormal.resize(3);
|
|
for (size_t i = 0; i < 3; ++i)
|
|
{
|
|
m_fovCenter[i] = .5*(fovBottomTop[i] + fovBottomTop[i+3]);
|
|
m_fovNormal[i] = fovBottomTop[i+3] - fovBottomTop[i];
|
|
m_fovNormal[i] = cv::normalize(m_fovNormal[i]);
|
|
m_fovNormal[i] = cv::Vec3d(
|
|
m_fovNormal[i](0),
|
|
-m_fovNormal[i](2),
|
|
m_fovNormal[i](1));
|
|
// one target position in each plane
|
|
m_pointTargetTvec.push_back(m_fovCenter[i]);
|
|
cv::Vec3d rvec = cv::Vec3d(0,0,1).cross(m_fovNormal[i]);
|
|
rvec = cv::normalize(rvec);
|
|
rvec *= acos(m_fovNormal[i](2));
|
|
m_pointTargetRvec.push_back(rvec);
|
|
}
|
|
// calibration target
|
|
size_t num = 2*m_pointTargetNum + 1;
|
|
m_pointTarget.resize(num*num);
|
|
std::vector<cv::Point3d>::iterator itTarget = m_pointTarget.begin();
|
|
for (int iY = -m_pointTargetNum; iY <= m_pointTargetNum; ++iY)
|
|
{
|
|
for (int iX = -m_pointTargetNum; iX <= m_pointTargetNum; ++iX, ++itTarget)
|
|
{
|
|
*itTarget = cv::Point3d(iX, iY, 0) * m_pointTargetDist;
|
|
}
|
|
}
|
|
// oblique target positions
|
|
// approximate distance to the near and far plane
|
|
double dist = std::max(
|
|
std::abs(m_fovNormal[0].dot(m_fovCenter[0] - m_fovCenter[1])),
|
|
std::abs(m_fovNormal[0].dot(m_fovCenter[0] - m_fovCenter[2])));
|
|
// maximal angle such that target border "reaches" near and far plane
|
|
double maxAngle = atan2(dist, m_pointTargetNum*m_pointTargetDist);
|
|
std::vector<double> angle;
|
|
angle.push_back(-maxAngle);
|
|
angle.push_back(maxAngle);
|
|
cv::Matx33d baseMatrix;
|
|
cv::Rodrigues(m_pointTargetRvec.front(), baseMatrix);
|
|
for (std::vector<double>::const_iterator itAngle = angle.begin(); itAngle != angle.end(); ++itAngle)
|
|
{
|
|
cv::Matx33d rmat;
|
|
for (int i = 0; i < 2; ++i)
|
|
{
|
|
cv::Vec3d rvec(0,0,0);
|
|
rvec(i) = *itAngle;
|
|
cv::Rodrigues(rvec, rmat);
|
|
rmat = baseMatrix*rmat;
|
|
cv::Rodrigues(rmat, rvec);
|
|
m_pointTargetTvec.push_back(m_fovCenter.front());
|
|
m_pointTargetRvec.push_back(rvec);
|
|
}
|
|
}
|
|
// camera matrix
|
|
double cx = .5 * (m_imageSize.width - 1);
|
|
double cy = .5 * (m_imageSize.height - 1);
|
|
double f = m_imageDistance/m_pixelSize;
|
|
m_cameraMatrix = cv::Matx33d(
|
|
f,0,cx,
|
|
0,f,cy,
|
|
0,0,1);
|
|
// distortion coefficients
|
|
m_distortionCoeff = cv::Vec<double, NUM_DIST_COEFF_TILT>::all(0);
|
|
// tauX
|
|
m_distortionCoeff(12) = -m_toRadian*m_imageTiltDegree;
|
|
}
|
|
};
|
|
|
|
/** Number of Pixel of the sensor */
|
|
const cv::Size cameraCalibrationTiltTest::m_imageSize(1600, 1200);
|
|
/** Size of a pixel in mm */
|
|
const double cameraCalibrationTiltTest::m_pixelSize(.005);
|
|
/** Diameter of the circle of confusion */
|
|
const double cameraCalibrationTiltTest::m_circleConfusionPixel(3);
|
|
/** Focal length of the lens */
|
|
const double cameraCalibrationTiltTest::m_lensFocalLength(16.4);
|
|
/** F-Number */
|
|
const double cameraCalibrationTiltTest::m_lensFNumber(8);
|
|
/** Working distance */
|
|
const double cameraCalibrationTiltTest::m_objectDistance(200);
|
|
/** Angle between optical axis and object plane normal */
|
|
const double cameraCalibrationTiltTest::m_planeTiltDegree(55);
|
|
/** the calibration target are points on a square grid with this side length */
|
|
const double cameraCalibrationTiltTest::m_pointTargetDist(5);
|
|
/** the calibration target has (2*n + 1) x (2*n + 1) points */
|
|
const int cameraCalibrationTiltTest::m_pointTargetNum(15);
|
|
|
|
TEST_F(cameraCalibrationTiltTest, projectPoints)
|
|
{
|
|
std::vector<cv::Point2d> imagePoints;
|
|
std::vector<cv::Point3d> objectPoints = m_pointTarget;
|
|
cv::Vec3d rvec = m_pointTargetRvec.front();
|
|
cv::Vec3d tvec = m_pointTargetTvec.front();
|
|
|
|
cv::Vec<double, NUM_DIST_COEFF_TILT> coeffNoiseHalfWidth(
|
|
.1, .1, // k1 k2
|
|
.01, .01, // p1 p2
|
|
.001, .001, .001, .001, // k3 k4 k5 k6
|
|
.001, .001, .001, .001, // s1 s2 s3 s4
|
|
.01, .01); // tauX tauY
|
|
for (size_t numTest = 0; numTest < 10; ++numTest)
|
|
{
|
|
// create random distortion coefficients
|
|
cv::Vec<double, NUM_DIST_COEFF_TILT> distortionCoeff = m_distortionCoeff;
|
|
randomDistortionCoeff(distortionCoeff, coeffNoiseHalfWidth);
|
|
|
|
// projection
|
|
cv::projectPoints(
|
|
objectPoints,
|
|
rvec,
|
|
tvec,
|
|
m_cameraMatrix,
|
|
distortionCoeff,
|
|
imagePoints);
|
|
|
|
// remove object and imgage points out of range
|
|
removeInvalidPoints(imagePoints, objectPoints);
|
|
|
|
int numPoints = (int)imagePoints.size();
|
|
int numParams = 10 + distortionCoeff.rows;
|
|
cv::Mat jacobian(2*numPoints, numParams, CV_64FC1);
|
|
|
|
// projection and jacobian
|
|
cv::projectPoints(
|
|
objectPoints,
|
|
rvec,
|
|
tvec,
|
|
m_cameraMatrix,
|
|
distortionCoeff,
|
|
imagePoints,
|
|
jacobian);
|
|
|
|
// numerical derivatives
|
|
cv::Mat numericJacobian(2*numPoints, numParams, CV_64FC1);
|
|
double eps = 1e-7;
|
|
numericalDerivative(
|
|
numericJacobian,
|
|
eps,
|
|
objectPoints,
|
|
rvec,
|
|
tvec,
|
|
m_cameraMatrix,
|
|
distortionCoeff);
|
|
|
|
#if 0
|
|
for (size_t row = 0; row < 2; ++row)
|
|
{
|
|
std::cout << "------ Row = " << row << " ------\n";
|
|
for (size_t i = 0; i < 10+NUM_DIST_COEFF_TILT; ++i)
|
|
{
|
|
std::cout << i
|
|
<< " jac = " << jacobian.at<double>(row,i)
|
|
<< " num = " << numericJacobian.at<double>(row,i)
|
|
<< " rel. diff = " << abs(numericJacobian.at<double>(row,i) - jacobian.at<double>(row,i))/abs(numericJacobian.at<double>(row,i))
|
|
<< "\n";
|
|
}
|
|
}
|
|
#endif
|
|
// relative difference for large values (rvec and tvec)
|
|
cv::Mat check = abs(jacobian(cv::Range::all(), cv::Range(0,6)) - numericJacobian(cv::Range::all(), cv::Range(0,6)))/
|
|
(1 + abs(jacobian(cv::Range::all(), cv::Range(0,6))));
|
|
double minVal, maxVal;
|
|
cv::minMaxIdx(check, &minVal, &maxVal);
|
|
EXPECT_LE(maxVal, .01);
|
|
// absolute difference for distortion and camera matrix
|
|
EXPECT_MAT_NEAR(jacobian(cv::Range::all(), cv::Range(6,numParams)), numericJacobian(cv::Range::all(), cv::Range(6,numParams)), 1e-5);
|
|
}
|
|
}
|
|
|
|
TEST_F(cameraCalibrationTiltTest, undistortPoints)
|
|
{
|
|
cv::Vec<double, NUM_DIST_COEFF_TILT> coeffNoiseHalfWidth(
|
|
.2, .1, // k1 k2
|
|
.01, .01, // p1 p2
|
|
.01, .01, .01, .01, // k3 k4 k5 k6
|
|
.001, .001, .001, .001, // s1 s2 s3 s4
|
|
.001, .001); // tauX tauY
|
|
double step = 99;
|
|
double toleranceBackProjection = 1e-5;
|
|
|
|
for (size_t numTest = 0; numTest < 10; ++numTest)
|
|
{
|
|
cv::Vec<double, NUM_DIST_COEFF_TILT> distortionCoeff = m_distortionCoeff;
|
|
randomDistortionCoeff(distortionCoeff, coeffNoiseHalfWidth);
|
|
|
|
// distorted points
|
|
std::vector<cv::Point2d> distorted;
|
|
for (double x = 0; x <= m_imageSize.width-1; x += step)
|
|
for (double y = 0; y <= m_imageSize.height-1; y += step)
|
|
distorted.push_back(cv::Point2d(x,y));
|
|
std::vector<cv::Point2d> normalizedUndistorted;
|
|
|
|
// undistort
|
|
cv::undistortPoints(distorted,
|
|
normalizedUndistorted,
|
|
m_cameraMatrix,
|
|
distortionCoeff);
|
|
|
|
// copy normalized points to 3D
|
|
std::vector<cv::Point3d> objectPoints;
|
|
for (std::vector<cv::Point2d>::const_iterator itPnt = normalizedUndistorted.begin();
|
|
itPnt != normalizedUndistorted.end(); ++itPnt)
|
|
objectPoints.push_back(cv::Point3d(itPnt->x, itPnt->y, 1));
|
|
|
|
// project
|
|
std::vector<cv::Point2d> imagePoints(objectPoints.size());
|
|
cv::projectPoints(objectPoints,
|
|
cv::Vec3d(0,0,0),
|
|
cv::Vec3d(0,0,0),
|
|
m_cameraMatrix,
|
|
distortionCoeff,
|
|
imagePoints);
|
|
|
|
EXPECT_MAT_NEAR(distorted, imagePoints, toleranceBackProjection);
|
|
}
|
|
}
|
|
|
|
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);
|
|
|
|
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];
|
|
}
|
|
}
|
|
|
|
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
|
|
{
|
|
Matx33d K1(6137.147949, 0.000000, 644.974609,
|
|
0.000000, 6137.147949, 573.442749,
|
|
0.000000, 0.000000, 1.000000);
|
|
Matx33d K2(6137.147949, 0.000000, 644.674438,
|
|
0.000000, 6137.147949, 573.079834,
|
|
0.000000, 0.000000, 1.000000);
|
|
|
|
Matx34d RT1(1, 0, 0, 0,
|
|
0, 1, 0, 0,
|
|
0, 0, 1, 0);
|
|
|
|
Matx34d RT2(0.998297, 0.0064108, -0.0579766, 143.614334,
|
|
-0.0065818, 0.999975, -0.00275888, -5.160085,
|
|
0.0579574, 0.00313577, 0.998314, 96.066109);
|
|
|
|
Matx34d P1 = K1*RT1;
|
|
Matx34d P2 = K2*RT2;
|
|
|
|
float x1data[] = { 438.f, 19.f };
|
|
float x2data[] = { 452.363600f, 16.452225f };
|
|
float Xdata[] = { -81.049530f, -215.702804f, 2401.645449f };
|
|
Mat x1(2, 1, CV_32F, x1data);
|
|
Mat x2(2, 1, CV_32F, x2data);
|
|
Mat res0(1, 3, CV_32F, Xdata);
|
|
Mat res_, res;
|
|
|
|
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 << "[2]:" << endl;
|
|
cout << "\tres0: " << res0 << endl;
|
|
cout << "\tres: " << res << endl;
|
|
|
|
ASSERT_LE(cvtest::norm(res, res0, NORM_INF), 2);
|
|
}
|
|
}
|
|
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
TEST(CV_RecoverPoseTest, regression_15341)
|
|
{
|
|
// initialize test data
|
|
const int invalid_point_count = 2;
|
|
const float _points1_[] = {
|
|
1537.7f, 166.8f,
|
|
1599.1f, 179.6f,
|
|
1288.0f, 207.5f,
|
|
1507.1f, 193.2f,
|
|
1742.7f, 210.0f,
|
|
1041.6f, 271.7f,
|
|
1591.8f, 247.2f,
|
|
1524.0f, 261.3f,
|
|
1330.3f, 285.0f,
|
|
1403.1f, 284.0f,
|
|
1506.6f, 342.9f,
|
|
1502.8f, 347.3f,
|
|
1344.9f, 364.9f,
|
|
0.0f, 0.0f // last point is initial invalid
|
|
};
|
|
|
|
const float _points2_[] = {
|
|
1533.4f, 532.9f,
|
|
1596.6f, 552.4f,
|
|
1277.0f, 556.4f,
|
|
1502.1f, 557.6f,
|
|
1744.4f, 601.3f,
|
|
1023.0f, 612.6f,
|
|
1589.2f, 621.6f,
|
|
1519.4f, 629.0f,
|
|
1320.3f, 637.3f,
|
|
1395.2f, 642.2f,
|
|
1501.5f, 710.3f,
|
|
1497.6f, 714.2f,
|
|
1335.1f, 719.61f,
|
|
1000.0f, 1000.0f // last point is initial invalid
|
|
};
|
|
|
|
vector<Point2f> _points1; Mat(14, 1, CV_32FC2, (void*)_points1_).copyTo(_points1);
|
|
vector<Point2f> _points2; Mat(14, 1, CV_32FC2, (void*)_points2_).copyTo(_points2);
|
|
|
|
const int point_count = (int) _points1.size();
|
|
CV_Assert(point_count == (int) _points2.size());
|
|
|
|
// camera matrix with both focal lengths = 1, and principal point = (0, 0)
|
|
const Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
|
|
|
|
// camera matrix with focal lengths 0.5 and 0.6 respectively and principal point = (100, 200)
|
|
double cameraMatrix2Data[] = { 0.5, 0, 100,
|
|
0, 0.6, 200,
|
|
0, 0, 1 };
|
|
const Mat cameraMatrix2( 3, 3, CV_64F, cameraMatrix2Data );
|
|
|
|
// zero and nonzero distortion coefficients
|
|
double nonZeroDistCoeffsData[] = { 0.01, 0.0001, 0, 0, 1e-04, 0.2, 0.02, 0.0002 }; // k1, k2, p1, p2, k3, k4, k5, k6
|
|
vector<Mat> distCoeffsList = {Mat::zeros(1, 5, CV_64F), Mat{1, 8, CV_64F, nonZeroDistCoeffsData}};
|
|
const auto &zeroDistCoeffs = distCoeffsList[0];
|
|
|
|
int Inliers = 0;
|
|
|
|
const int ntests = 3;
|
|
for (int testcase = 1; testcase <= ntests; ++testcase)
|
|
{
|
|
if (testcase == 1) // testcase with vector input data
|
|
{
|
|
// init temporary test data
|
|
vector<unsigned char> mask(point_count);
|
|
vector<Point2f> points1(_points1);
|
|
vector<Point2f> points2(_points2);
|
|
|
|
// Estimation of fundamental matrix using the RANSAC algorithm
|
|
Mat E, E2, R, t;
|
|
|
|
// Check pose when camera matrices are different.
|
|
for (const auto &distCoeffs: distCoeffsList)
|
|
{
|
|
E = findEssentialMat(points1, points2, cameraMatrix, distCoeffs, cameraMatrix2, distCoeffs, RANSAC, 0.999, 1.0, mask);
|
|
recoverPose(points1, points2, cameraMatrix, distCoeffs, cameraMatrix2, distCoeffs, E2, R, t, RANSAC, 0.999, 1.0, mask);
|
|
EXPECT_LT(cv::norm(E, E2, NORM_INF), 1e-4) <<
|
|
"Two big difference between the same essential matrices computed using different functions with different cameras, testcase " << testcase;
|
|
EXPECT_EQ(0, (int)mask[13]) << "Detecting outliers in function failed with different cameras, testcase " << testcase;
|
|
}
|
|
|
|
// Check pose when camera matrices are the same.
|
|
E = findEssentialMat(points1, points2, cameraMatrix, RANSAC, 0.999, 1.0, 1000/*maxIters*/, mask);
|
|
E2 = findEssentialMat(points1, points2, cameraMatrix, zeroDistCoeffs, cameraMatrix, zeroDistCoeffs, RANSAC, 0.999, 1.0, mask);
|
|
EXPECT_LT(cv::norm(E, E2, NORM_INF), 1e-4) <<
|
|
"Two big difference between the same essential matrices computed using different functions with same cameras, testcase " << testcase;
|
|
EXPECT_EQ(0, (int)mask[13]) << "Detecting outliers in function findEssentialMat failed with same cameras, testcase " << testcase;
|
|
points2[12] = Point2f(0.0f, 0.0f); // provoke another outlier detection for recover Pose
|
|
Inliers = recoverPose(E, points1, points2, cameraMatrix, R, t, mask);
|
|
EXPECT_EQ(0, (int)mask[12]) << "Detecting outliers in function failed with same cameras, testcase " << testcase;
|
|
}
|
|
else // testcase with mat input data
|
|
{
|
|
Mat points1(_points1, true);
|
|
Mat points2(_points2, true);
|
|
Mat mask;
|
|
|
|
if (testcase == 2)
|
|
{
|
|
// init temporary testdata
|
|
mask = Mat::zeros(point_count, 1, CV_8UC1);
|
|
}
|
|
else // testcase == 3 - with transposed mask
|
|
{
|
|
mask = Mat::zeros(1, point_count, CV_8UC1);
|
|
}
|
|
|
|
// Estimation of fundamental matrix using the RANSAC algorithm
|
|
Mat E, E2, R, t;
|
|
|
|
// Check pose when camera matrices are different.
|
|
for (const auto &distCoeffs: distCoeffsList)
|
|
{
|
|
E = findEssentialMat(points1, points2, cameraMatrix, distCoeffs, cameraMatrix2, distCoeffs, RANSAC, 0.999, 1.0, mask);
|
|
recoverPose(points1, points2, cameraMatrix, distCoeffs, cameraMatrix2, distCoeffs, E2, R, t, RANSAC, 0.999, 1.0, mask);
|
|
EXPECT_LT(cv::norm(E, E2, NORM_INF), 1e-4) <<
|
|
"Two big difference between the same essential matrices computed using different functions with different cameras, testcase " << testcase;
|
|
EXPECT_EQ(0, (int)mask.at<unsigned char>(13)) << "Detecting outliers in function failed with different cameras, testcase " << testcase;
|
|
}
|
|
|
|
// Check pose when camera matrices are the same.
|
|
E = findEssentialMat(points1, points2, cameraMatrix, RANSAC, 0.999, 1.0, 1000/*maxIters*/, mask);
|
|
E2 = findEssentialMat(points1, points2, cameraMatrix, zeroDistCoeffs, cameraMatrix, zeroDistCoeffs, RANSAC, 0.999, 1.0, mask);
|
|
EXPECT_LT(cv::norm(E, E2, NORM_INF), 1e-4) <<
|
|
"Two big difference between the same essential matrices computed using different functions with same cameras, testcase " << testcase;
|
|
EXPECT_EQ(0, (int)mask.at<unsigned char>(13)) << "Detecting outliers in function findEssentialMat failed with same cameras, testcase " << testcase;
|
|
points2.at<Point2f>(12) = Point2f(0.0f, 0.0f); // provoke an outlier detection
|
|
Inliers = recoverPose(E, points1, points2, cameraMatrix, R, t, mask);
|
|
EXPECT_EQ(0, (int)mask.at<unsigned char>(12)) << "Detecting outliers in function failed with same cameras, testcase " << testcase;
|
|
}
|
|
EXPECT_EQ(Inliers, point_count - invalid_point_count) <<
|
|
"Number of inliers differs from expected number of inliers, testcase " << testcase;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------- CV_CalibrationMatrixValuesTest --------------------------------
|
|
|
|
class CV_CalibrationMatrixValuesTest : public cvtest::BaseTest
|
|
{
|
|
public:
|
|
CV_CalibrationMatrixValuesTest() {}
|
|
protected:
|
|
void run(int);
|
|
virtual void calibMatrixValues( const Mat& cameraMatrix, Size imageSize,
|
|
double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength,
|
|
Point2d& principalPoint, double& aspectRatio ) = 0;
|
|
};
|
|
|
|
void CV_CalibrationMatrixValuesTest::run(int)
|
|
{
|
|
int code = cvtest::TS::OK;
|
|
const double fcMinVal = 1e-5;
|
|
const double fcMaxVal = 1000;
|
|
const double apertureMaxVal = 0.01;
|
|
|
|
RNG rng = ts->get_rng();
|
|
|
|
double fx, fy, cx, cy, nx, ny;
|
|
Mat cameraMatrix( 3, 3, CV_64FC1 );
|
|
cameraMatrix.setTo( Scalar(0) );
|
|
fx = cameraMatrix.at<double>(0,0) = rng.uniform( fcMinVal, fcMaxVal );
|
|
fy = cameraMatrix.at<double>(1,1) = rng.uniform( fcMinVal, fcMaxVal );
|
|
cx = cameraMatrix.at<double>(0,2) = rng.uniform( fcMinVal, fcMaxVal );
|
|
cy = cameraMatrix.at<double>(1,2) = rng.uniform( fcMinVal, fcMaxVal );
|
|
cameraMatrix.at<double>(2,2) = 1;
|
|
|
|
Size imageSize( 600, 400 );
|
|
|
|
double apertureWidth = (double)rng * apertureMaxVal,
|
|
apertureHeight = (double)rng * apertureMaxVal;
|
|
|
|
double fovx, fovy, focalLength, aspectRatio,
|
|
goodFovx, goodFovy, goodFocalLength, goodAspectRatio;
|
|
Point2d principalPoint, goodPrincipalPoint;
|
|
|
|
|
|
calibMatrixValues( cameraMatrix, imageSize, apertureWidth, apertureHeight,
|
|
fovx, fovy, focalLength, principalPoint, aspectRatio );
|
|
|
|
// calculate calibration matrix values
|
|
goodAspectRatio = fy / fx;
|
|
|
|
if( apertureWidth != 0.0 && apertureHeight != 0.0 )
|
|
{
|
|
nx = imageSize.width / apertureWidth;
|
|
ny = imageSize.height / apertureHeight;
|
|
}
|
|
else
|
|
{
|
|
nx = 1.0;
|
|
ny = goodAspectRatio;
|
|
}
|
|
|
|
goodFovx = (atan2(cx, fx) + atan2(imageSize.width - cx, fx)) * 180.0 / CV_PI;
|
|
goodFovy = (atan2(cy, fy) + atan2(imageSize.height - cy, fy)) * 180.0 / CV_PI;
|
|
|
|
goodFocalLength = fx / nx;
|
|
|
|
goodPrincipalPoint.x = cx / nx;
|
|
goodPrincipalPoint.y = cy / ny;
|
|
|
|
// check results
|
|
if( fabs(fovx - goodFovx) > FLT_EPSILON )
|
|
{
|
|
ts->printf( cvtest::TS::LOG, "bad fovx (real=%f, good = %f\n", fovx, goodFovx );
|
|
code = cvtest::TS::FAIL_BAD_ACCURACY;
|
|
goto _exit_;
|
|
}
|
|
if( fabs(fovy - goodFovy) > FLT_EPSILON )
|
|
{
|
|
ts->printf( cvtest::TS::LOG, "bad fovy (real=%f, good = %f\n", fovy, goodFovy );
|
|
code = cvtest::TS::FAIL_BAD_ACCURACY;
|
|
goto _exit_;
|
|
}
|
|
if( fabs(focalLength - goodFocalLength) > FLT_EPSILON )
|
|
{
|
|
ts->printf( cvtest::TS::LOG, "bad focalLength (real=%f, good = %f\n", focalLength, goodFocalLength );
|
|
code = cvtest::TS::FAIL_BAD_ACCURACY;
|
|
goto _exit_;
|
|
}
|
|
if( fabs(aspectRatio - goodAspectRatio) > FLT_EPSILON )
|
|
{
|
|
ts->printf( cvtest::TS::LOG, "bad aspectRatio (real=%f, good = %f\n", aspectRatio, goodAspectRatio );
|
|
code = cvtest::TS::FAIL_BAD_ACCURACY;
|
|
goto _exit_;
|
|
}
|
|
if( cv::norm(principalPoint - goodPrincipalPoint) > FLT_EPSILON ) // Point2d
|
|
{
|
|
ts->printf( cvtest::TS::LOG, "bad principalPoint\n" );
|
|
code = cvtest::TS::FAIL_BAD_ACCURACY;
|
|
goto _exit_;
|
|
}
|
|
|
|
_exit_:
|
|
RNG& _rng = ts->get_rng();
|
|
_rng = rng;
|
|
ts->set_failed_test_info( code );
|
|
}
|
|
|
|
//----------------------------------------- CV_CalibrationMatrixValuesTest_CPP --------------------------------
|
|
|
|
class CV_CalibrationMatrixValuesTest_CPP : public CV_CalibrationMatrixValuesTest
|
|
{
|
|
public:
|
|
CV_CalibrationMatrixValuesTest_CPP() {}
|
|
protected:
|
|
virtual void calibMatrixValues( const Mat& cameraMatrix, Size imageSize,
|
|
double apertureWidth, double apertureHeight, double& fovx, double& fovy, double& focalLength,
|
|
Point2d& principalPoint, double& aspectRatio );
|
|
};
|
|
|
|
void CV_CalibrationMatrixValuesTest_CPP::calibMatrixValues( const Mat& cameraMatrix, Size imageSize,
|
|
double apertureWidth, double apertureHeight,
|
|
double& fovx, double& fovy, double& focalLength,
|
|
Point2d& principalPoint, double& aspectRatio )
|
|
{
|
|
calibrationMatrixValues( cameraMatrix, imageSize, apertureWidth, apertureHeight,
|
|
fovx, fovy, focalLength, principalPoint, aspectRatio );
|
|
}
|
|
|
|
TEST(Calib3d_CalibrationMatrixValues_CPP, accuracy) { CV_CalibrationMatrixValuesTest_CPP test; test.safe_run(); }
|
|
|
|
}} // namespace
|