mirror of
https://github.com/opencv/opencv.git
synced 2024-12-12 15:19:11 +08:00
348 lines
14 KiB
C++
348 lines
14 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/ts/cuda_test.hpp> // EXPECT_MAT_NEAR
|
|
#include "opencv2/3d.hpp"
|
|
#include <opencv2/core/utils/logger.hpp>
|
|
|
|
namespace opencv_test { namespace {
|
|
|
|
static bool checkPandROI(const Matx33d& M, const Matx<double, 5, 1>& D,
|
|
const Mat& R, const Mat& P, Size imgsize, Rect roi)
|
|
{
|
|
const double eps = 0.05;
|
|
const int N = 21;
|
|
int x, y, k;
|
|
vector<Point2f> pts, upts;
|
|
|
|
// step 1. check that all the original points belong to the destination image
|
|
for( y = 0; y < N; y++ )
|
|
for( x = 0; x < N; x++ )
|
|
pts.push_back(Point2f((float)x*imgsize.width/(N-1), (float)y*imgsize.height/(N-1)));
|
|
|
|
undistortPoints(pts, upts, M, D, R, P );
|
|
for( k = 0; k < N*N; k++ )
|
|
if( upts[k].x < -imgsize.width*eps || upts[k].x > imgsize.width*(1+eps) ||
|
|
upts[k].y < -imgsize.height*eps || upts[k].y > imgsize.height*(1+eps) )
|
|
{
|
|
CV_LOG_ERROR(NULL, cv::format("The point (%g, %g) was mapped to (%g, %g) which is out of image\n",
|
|
pts[k].x, pts[k].y, upts[k].x, upts[k].y));
|
|
return false;
|
|
}
|
|
|
|
// step 2. check that all the points inside ROI belong to the original source image
|
|
Mat temp(imgsize, CV_8U), utemp, map1, map2;
|
|
temp = Scalar::all(1);
|
|
initUndistortRectifyMap(M, D, R, P, imgsize, CV_16SC2, map1, map2);
|
|
remap(temp, utemp, map1, map2, INTER_LINEAR);
|
|
|
|
if(roi.x < 0 || roi.y < 0 || roi.x + roi.width > imgsize.width || roi.y + roi.height > imgsize.height)
|
|
{
|
|
CV_LOG_ERROR(NULL, cv::format("The ROI=(%d, %d, %d, %d) is outside of the imge rectangle\n",
|
|
roi.x, roi.y, roi.width, roi.height));
|
|
return false;
|
|
}
|
|
double s = sum(utemp(roi))[0];
|
|
if( s > roi.area() || roi.area() - s > roi.area()*(1-eps) )
|
|
{
|
|
CV_LOG_ERROR(NULL, cv::format("The ratio of black pixels inside the valid ROI (~%g%%) is too large\n",
|
|
s*100./roi.area()));
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
TEST(StereoGeometry, stereoRectify)
|
|
{
|
|
// camera parameters are extracted from the original calib3d test CV_StereoCalibrationTest::run
|
|
const Matx33d M1(
|
|
530.4643719672913, 0, 319.5,
|
|
0, 529.7477570329314, 239.5,
|
|
0, 0, 1);
|
|
const Matx<double, 5, 1> D1(-0.2982901576925627, 0.1134645765152131, 0, 0, 0);
|
|
|
|
const Matx33d M2(
|
|
530.4643719672913, 0, 319.5,
|
|
0, 529.7477570329314, 239.5,
|
|
0, 0, 1);
|
|
const Matx<double, 5, 1> D2(-0.2833068597502156, 0.0944810713984697, 0, 0, 0);
|
|
|
|
const Matx33d R(0.9996903750450727, 0.005330951201286465, -0.02430504066096785,
|
|
-0.004837810799471072, 0.9997821583334892, 0.02030348405319902,
|
|
0.02440798289310936, -0.02017961439967296, 0.9994983909610711);
|
|
const Matx31d T(-3.328706469151101, 0.05621025406095936, -0.02956576727262086);
|
|
|
|
const Size imageSize(640, 480);
|
|
|
|
Mat R1, R2, P1, P2, Q;
|
|
Rect roi1, roi2;
|
|
|
|
stereoRectify( M1, D1, M2, D2, imageSize, R, T, R1, R2, P1, P2, Q, 0, 1, imageSize, &roi1, &roi2 );
|
|
|
|
Mat eye33 = Mat::eye(3,3,CV_64F);
|
|
Mat R1t = R1.t(), R2t = R2.t();
|
|
|
|
EXPECT_LE(cvtest::norm(R1t*R1 - eye33, NORM_L2), 0.01) << "R1 is not orthogonal!";
|
|
EXPECT_LE(cvtest::norm(R2t*R2 - eye33, NORM_L2), 0.01) << "R2 is not orthogonal!";
|
|
|
|
//check that Tx after rectification is equal to distance between cameras
|
|
double tx = fabs(P2.at<double>(0, 3) / P2.at<double>(0, 0));
|
|
EXPECT_LE(fabs(tx - cvtest::norm(T, NORM_L2)), 1e-5);
|
|
EXPECT_TRUE(checkPandROI(M1, D1, R1, P1, imageSize, roi1));
|
|
EXPECT_TRUE(checkPandROI(M2, D2, R2, P2, imageSize, roi2));
|
|
|
|
//check that Q reprojects points before the camera
|
|
double testPoint[4] = {0.0, 0.0, 100.0, 1.0};
|
|
Mat reprojectedTestPoint = Q * Mat_<double>(4, 1, testPoint);
|
|
CV_Assert(reprojectedTestPoint.type() == CV_64FC1);
|
|
EXPECT_GT( reprojectedTestPoint.at<double>(2) / reprojectedTestPoint.at<double>(3), 0 ) << \
|
|
"A point after rectification is reprojected behind the camera";
|
|
}
|
|
|
|
TEST(StereoGeometry, regression_10791)
|
|
{
|
|
const Matx33d M1(
|
|
853.1387981631528, 0, 704.154907802121,
|
|
0, 853.6445089162528, 520.3600712930319,
|
|
0, 0, 1
|
|
);
|
|
const Matx33d M2(
|
|
848.6090216909176, 0, 701.6162856852185,
|
|
0, 849.7040162357157, 509.1864036137,
|
|
0, 0, 1
|
|
);
|
|
const Matx<double, 14, 1> D1(-6.463598629567206, 79.00104930508179, -0.0001006144444464403, -0.0005437499822299972,
|
|
12.56900616588467, -6.056719942752855, 76.3842481414836, 45.57460250612659,
|
|
0, 0, 0, 0, 0, 0);
|
|
const Matx<double, 14, 1> D2(0.6123436439798265, -0.4671756923224087, -0.0001261947899033442, -0.000597334584036978,
|
|
-0.05660119809538371, 1.037075740629769, -0.3076042835831711, -0.2502169324283623,
|
|
0, 0, 0, 0, 0, 0);
|
|
|
|
const Matx33d R(
|
|
0.9999926627018476, -0.0001095586963765905, 0.003829169539302921,
|
|
0.0001021735876758584, 0.9999981346680941, 0.0019287874145156,
|
|
-0.003829373712065528, -0.001928382022437616, 0.9999908085776333
|
|
);
|
|
const Matx31d T(-58.9161771697128, -0.01581306249996402, -0.8492960216760961);
|
|
|
|
const Size imageSize(1280, 960);
|
|
|
|
Mat R1, R2, P1, P2, Q;
|
|
Rect roi1, roi2;
|
|
stereoRectify(M1, D1, M2, D2, imageSize, R, T,
|
|
R1, R2, P1, P2, Q,
|
|
STEREO_ZERO_DISPARITY, 1, imageSize, &roi1, &roi2);
|
|
|
|
EXPECT_GE(roi1.area(), 400*300) << roi1;
|
|
EXPECT_GE(roi2.area(), 400*300) << roi2;
|
|
}
|
|
|
|
TEST(StereoGeometry, regression_11131)
|
|
{
|
|
const Matx33d M1(
|
|
1457.572438721727, 0, 1212.945694211622,
|
|
0, 1457.522226502963, 1007.32058848921,
|
|
0, 0, 1
|
|
);
|
|
const Matx33d M2(
|
|
1460.868570835972, 0, 1215.024068023046,
|
|
0, 1460.791367088, 1011.107202932225,
|
|
0, 0, 1
|
|
);
|
|
const Matx<double, 5, 1> D1(0, 0, 0, 0, 0);
|
|
const Matx<double, 5, 1> D2(0, 0, 0, 0, 0);
|
|
|
|
const Matx33d R(
|
|
0.9985404059825475, 0.02963547172078553, -0.04515303352041626,
|
|
-0.03103795276460111, 0.9990471552537432, -0.03068268351343364,
|
|
0.04420071389006859, 0.03203935697372317, 0.9985087763742083
|
|
);
|
|
const Matx31d T(0.9995500167379527, 0.0116311595111068, 0.02764923448462666);
|
|
|
|
const Size imageSize(2456, 2058);
|
|
|
|
Mat R1, R2, P1, P2, Q;
|
|
Rect roi1, roi2;
|
|
stereoRectify(M1, D1, M2, D2, imageSize, R, T,
|
|
R1, R2, P1, P2, Q,
|
|
STEREO_ZERO_DISPARITY, 1, imageSize, &roi1, &roi2);
|
|
|
|
EXPECT_GT(P1.at<double>(0, 0), 0);
|
|
EXPECT_GT(P2.at<double>(0, 0), 0);
|
|
EXPECT_GT(R1.at<double>(0, 0), 0);
|
|
EXPECT_GT(R2.at<double>(0, 0), 0);
|
|
EXPECT_GE(roi1.area(), 400*300) << roi1;
|
|
EXPECT_GE(roi2.area(), 400*300) << roi2;
|
|
}
|
|
|
|
class fisheyeTest : public ::testing::Test {
|
|
|
|
protected:
|
|
const static cv::Size imageSize;
|
|
const static cv::Matx33d K;
|
|
const static cv::Vec4d D;
|
|
const static cv::Matx33d R;
|
|
const static cv::Vec3d T;
|
|
std::string datasets_repository_path;
|
|
|
|
virtual void SetUp() {
|
|
datasets_repository_path = combine(cvtest::TS::ptr()->get_data_path(), "cv/cameracalibration/fisheye");
|
|
}
|
|
|
|
protected:
|
|
std::string combine(const std::string& _item1, const std::string& _item2);
|
|
static void merge4(const cv::Mat& tl, const cv::Mat& tr, const cv::Mat& bl, const cv::Mat& br, cv::Mat& merged);
|
|
};
|
|
|
|
const cv::Size fisheyeTest::imageSize(1280, 800);
|
|
|
|
const cv::Matx33d fisheyeTest::K(558.478087865323, 0, 620.458515360843,
|
|
0, 560.506767351568, 381.939424848348,
|
|
0, 0, 1);
|
|
|
|
const cv::Vec4d fisheyeTest::D(-0.0014613319981768, -0.00329861110580401, 0.00605760088590183, -0.00374209380722371);
|
|
|
|
|
|
const cv::Matx33d fisheyeTest::R ( 9.9756700084424932e-01, 6.9698277640183867e-02, 1.4929569991321144e-03,
|
|
-6.9711825162322980e-02, 9.9748249845531767e-01, 1.2997180766418455e-02,
|
|
-5.8331736398316541e-04,-1.3069635393884985e-02, 9.9991441852366736e-01);
|
|
|
|
const cv::Vec3d fisheyeTest::T(-9.9217369356044638e-02, 3.1741831972356663e-03, 1.8551007952921010e-04);
|
|
|
|
std::string fisheyeTest::combine(const std::string& _item1, const std::string& _item2)
|
|
{
|
|
std::string item1 = _item1, item2 = _item2;
|
|
std::replace(item1.begin(), item1.end(), '\\', '/');
|
|
std::replace(item2.begin(), item2.end(), '\\', '/');
|
|
|
|
if (item1.empty())
|
|
return item2;
|
|
|
|
if (item2.empty())
|
|
return item1;
|
|
|
|
char last = item1[item1.size()-1];
|
|
return item1 + (last != '/' ? "/" : "") + item2;
|
|
}
|
|
|
|
void fisheyeTest::merge4(const cv::Mat& tl, const cv::Mat& tr, const cv::Mat& bl, const cv::Mat& br, cv::Mat& merged)
|
|
{
|
|
int type = tl.type();
|
|
cv::Size sz = tl.size();
|
|
ASSERT_EQ(type, tr.type()); ASSERT_EQ(type, bl.type()); ASSERT_EQ(type, br.type());
|
|
ASSERT_EQ(sz.width, tr.cols); ASSERT_EQ(sz.width, bl.cols); ASSERT_EQ(sz.width, br.cols);
|
|
ASSERT_EQ(sz.height, tr.rows); ASSERT_EQ(sz.height, bl.rows); ASSERT_EQ(sz.height, br.rows);
|
|
|
|
merged.create(cv::Size(sz.width * 2, sz.height * 2), type);
|
|
tl.copyTo(merged(cv::Rect(0, 0, sz.width, sz.height)));
|
|
tr.copyTo(merged(cv::Rect(sz.width, 0, sz.width, sz.height)));
|
|
bl.copyTo(merged(cv::Rect(0, sz.height, sz.width, sz.height)));
|
|
br.copyTo(merged(cv::Rect(sz.width, sz.height, sz.width, sz.height)));
|
|
}
|
|
|
|
TEST_F(fisheyeTest, stereoRectify)
|
|
{
|
|
const std::string folder = combine(datasets_repository_path, "calib-3_stereo_from_JY");
|
|
|
|
cv::Size calibration_size = this->imageSize, requested_size = calibration_size;
|
|
cv::Matx33d K1 = this->K, K2 = K1;
|
|
cv::Mat D1 = cv::Mat(this->D), D2 = D1;
|
|
|
|
cv::Vec3d theT = this->T;
|
|
cv::Matx33d theR = this->R;
|
|
|
|
double balance = 0.0, fov_scale = 1.1;
|
|
cv::Mat R1, R2, P1, P2, Q;
|
|
cv::fisheye::stereoRectify(K1, D1, K2, D2, calibration_size, theR, theT, R1, R2, P1, P2, Q,
|
|
cv::STEREO_ZERO_DISPARITY, requested_size, balance, fov_scale);
|
|
|
|
// Collected with these CMake flags: -DWITH_IPP=OFF -DCV_ENABLE_INTRINSICS=OFF -DCV_DISABLE_OPTIMIZATION=ON -DCMAKE_BUILD_TYPE=Debug
|
|
cv::Matx33d R1_ref(
|
|
0.9992853269091279, 0.03779164101000276, -0.0007920188690205426,
|
|
-0.03778569762983931, 0.9992646472015868, 0.006511981857667881,
|
|
0.001037534936357442, -0.006477400933964018, 0.9999784831677112
|
|
);
|
|
cv::Matx33d R2_ref(
|
|
0.9994868963898833, -0.03197579751378937, -0.001868774538573449,
|
|
0.03196298186616116, 0.9994677442608699, -0.0065265589947392,
|
|
0.002076471801477729, 0.006463478587068991, 0.9999769555891836
|
|
);
|
|
cv::Matx34d P1_ref(
|
|
420.9684016542647, 0, 586.3059567784627, 0,
|
|
0, 420.9684016542647, 374.8571836462291, 0,
|
|
0, 0, 1, 0
|
|
);
|
|
cv::Matx34d P2_ref(
|
|
420.9684016542647, 0, 586.3059567784627, -41.78881938824554,
|
|
0, 420.9684016542647, 374.8571836462291, 0,
|
|
0, 0, 1, 0
|
|
);
|
|
cv::Matx44d Q_ref(
|
|
1, 0, 0, -586.3059567784627,
|
|
0, 1, 0, -374.8571836462291,
|
|
0, 0, 0, 420.9684016542647,
|
|
0, 0, 10.07370889670733, -0
|
|
);
|
|
|
|
const double eps = 1e-10;
|
|
EXPECT_MAT_NEAR(R1_ref, R1, eps);
|
|
EXPECT_MAT_NEAR(R2_ref, R2, eps);
|
|
EXPECT_MAT_NEAR(P1_ref, P1, eps);
|
|
EXPECT_MAT_NEAR(P2_ref, P2, eps);
|
|
EXPECT_MAT_NEAR(Q_ref, Q, eps);
|
|
|
|
if (::testing::Test::HasFailure())
|
|
{
|
|
std::cout << "Actual values are:" << std::endl
|
|
<< "R1 =" << std::endl << R1 << std::endl
|
|
<< "R2 =" << std::endl << R2 << std::endl
|
|
<< "P1 =" << std::endl << P1 << std::endl
|
|
<< "P2 =" << std::endl << P2 << std::endl
|
|
<< "Q =" << std::endl << Q << std::endl;
|
|
}
|
|
|
|
if (cvtest::debugLevel == 0)
|
|
return;
|
|
// DEBUG code is below
|
|
|
|
cv::Mat lmapx, lmapy, rmapx, rmapy;
|
|
//rewrite for fisheye
|
|
cv::fisheye::initUndistortRectifyMap(K1, D1, R1, P1, requested_size, CV_32F, lmapx, lmapy);
|
|
cv::fisheye::initUndistortRectifyMap(K2, D2, R2, P2, requested_size, CV_32F, rmapx, rmapy);
|
|
|
|
cv::Mat l, r, lundist, rundist;
|
|
for (int i = 0; i < 34; ++i)
|
|
{
|
|
SCOPED_TRACE(cv::format("image %d", i));
|
|
l = imread(combine(folder, cv::format("left/stereo_pair_%03d.jpg", i)), cv::IMREAD_COLOR);
|
|
r = imread(combine(folder, cv::format("right/stereo_pair_%03d.jpg", i)), cv::IMREAD_COLOR);
|
|
ASSERT_FALSE(l.empty());
|
|
ASSERT_FALSE(r.empty());
|
|
|
|
int ndisp = 128;
|
|
cv::rectangle(l, cv::Rect(255, 0, 829, l.rows-1), cv::Scalar(0, 0, 255));
|
|
cv::rectangle(r, cv::Rect(255, 0, 829, l.rows-1), cv::Scalar(0, 0, 255));
|
|
cv::rectangle(r, cv::Rect(255-ndisp, 0, 829+ndisp ,l.rows-1), cv::Scalar(0, 0, 255));
|
|
cv::remap(l, lundist, lmapx, lmapy, cv::INTER_LINEAR);
|
|
cv::remap(r, rundist, rmapx, rmapy, cv::INTER_LINEAR);
|
|
|
|
for (int ii = 0; ii < lundist.rows; ii += 20)
|
|
{
|
|
cv::line(lundist, cv::Point(0, ii), cv::Point(lundist.cols, ii), cv::Scalar(0, 255, 0));
|
|
cv::line(rundist, cv::Point(0, ii), cv::Point(lundist.cols, ii), cv::Scalar(0, 255, 0));
|
|
}
|
|
|
|
cv::Mat rectification;
|
|
merge4(l, r, lundist, rundist, rectification);
|
|
|
|
// Add the "--test_debug" to arguments for file output
|
|
if (cvtest::debugLevel > 0)
|
|
cv::imwrite(cv::format("fisheye_rectification_AB_%03d.png", i), rectification);
|
|
}
|
|
}
|
|
|
|
}}
|