test_undistort_points.cpp
5.52 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
// 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"
namespace opencv_test { namespace {
class CV_UndistortTest : public cvtest::BaseTest
{
public:
CV_UndistortTest();
~CV_UndistortTest();
protected:
void run(int);
private:
void generate3DPointCloud(vector<Point3f>& points, Point3f pmin = Point3f(-1,
-1, 5), Point3f pmax = Point3f(1, 1, 10));
void generateCameraMatrix(Mat& cameraMatrix);
void generateDistCoeffs(Mat& distCoeffs, int count);
double thresh;
RNG rng;
};
CV_UndistortTest::CV_UndistortTest()
{
thresh = 1.0e-2;
}
CV_UndistortTest::~CV_UndistortTest() {}
void CV_UndistortTest::generate3DPointCloud(vector<Point3f>& points, Point3f pmin, Point3f pmax)
{
RNG rng_Point = cv::theRNG(); // fix the seed to use "fixed" input 3D points
for (size_t i = 0; i < points.size(); i++)
{
float _x = rng_Point.uniform(pmin.x, pmax.x);
float _y = rng_Point.uniform(pmin.y, pmax.y);
float _z = rng_Point.uniform(pmin.z, pmax.z);
points[i] = Point3f(_x, _y, _z);
}
}
void CV_UndistortTest::generateCameraMatrix(Mat& cameraMatrix)
{
const double fcMinVal = 1e-3;
const double fcMaxVal = 100;
cameraMatrix.create(3, 3, CV_64FC1);
cameraMatrix.setTo(Scalar(0));
cameraMatrix.at<double>(0,0) = rng.uniform(fcMinVal, fcMaxVal);
cameraMatrix.at<double>(1,1) = rng.uniform(fcMinVal, fcMaxVal);
cameraMatrix.at<double>(0,2) = rng.uniform(fcMinVal, fcMaxVal);
cameraMatrix.at<double>(1,2) = rng.uniform(fcMinVal, fcMaxVal);
cameraMatrix.at<double>(2,2) = 1;
}
void CV_UndistortTest::generateDistCoeffs(Mat& distCoeffs, int count)
{
distCoeffs = Mat::zeros(count, 1, CV_64FC1);
for (int i = 0; i < count; i++)
distCoeffs.at<double>(i,0) = rng.uniform(0.0, 1.0e-3);
}
void CV_UndistortTest::run(int /* start_from */)
{
Mat intrinsics, distCoeffs;
generateCameraMatrix(intrinsics);
vector<Point3f> points(500);
generate3DPointCloud(points);
vector<Point2f> projectedPoints;
projectedPoints.resize(points.size());
int modelMembersCount[] = {4,5,8};
for (int idx = 0; idx < 3; idx++)
{
generateDistCoeffs(distCoeffs, modelMembersCount[idx]);
projectPoints(Mat(points), Mat::zeros(3,1,CV_64FC1), Mat::zeros(3,1,CV_64FC1), intrinsics, distCoeffs, projectedPoints);
vector<Point2f> realUndistortedPoints;
projectPoints(Mat(points), Mat::zeros(3,1,CV_64FC1), Mat::zeros(3,1,CV_64FC1), intrinsics, Mat::zeros(4,1,CV_64FC1), realUndistortedPoints);
Mat undistortedPoints;
undistortPoints(Mat(projectedPoints), undistortedPoints, intrinsics, distCoeffs);
Mat p;
perspectiveTransform(undistortedPoints, p, intrinsics);
undistortedPoints = p;
double diff = cvtest::norm(Mat(realUndistortedPoints), undistortedPoints, NORM_L2);
if (diff > thresh)
{
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
return;
}
ts->set_failed_test_info(cvtest::TS::OK);
}
}
TEST(Calib3d_Undistort, accuracy) { CV_UndistortTest test; test.safe_run(); }
TEST(Calib3d_Undistort, stop_criteria)
{
Mat cameraMatrix = (Mat_<double>(3,3,CV_64F) << 857.48296979, 0, 968.06224829,
0, 876.71824265, 556.37145899,
0, 0, 1);
Mat distCoeffs = (Mat_<double>(5,1,CV_64F) <<
-2.57614020e-01, 8.77086999e-02, -2.56970803e-04, -5.93390389e-04, -1.52194091e-02);
RNG rng(2);
Point2d pt_distorted(rng.uniform(0.0, 1920.0), rng.uniform(0.0, 1080.0));
std::vector<Point2d> pt_distorted_vec;
pt_distorted_vec.push_back(pt_distorted);
const double maxError = 1e-6;
TermCriteria criteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 100, maxError);
std::vector<Point2d> pt_undist_vec;
undistortPoints(pt_distorted_vec, pt_undist_vec, cameraMatrix, distCoeffs, noArray(), noArray(), criteria);
std::vector<Point2d> pt_redistorted_vec;
std::vector<Point3d> pt_undist_vec_homogeneous;
pt_undist_vec_homogeneous.push_back( Point3d(pt_undist_vec[0].x, pt_undist_vec[0].y, 1.0) );
projectPoints(pt_undist_vec_homogeneous, Mat::zeros(3,1,CV_64F), Mat::zeros(3,1,CV_64F), cameraMatrix, distCoeffs, pt_redistorted_vec);
const double obtainedError = sqrt( pow(pt_distorted.x - pt_redistorted_vec[0].x, 2) + pow(pt_distorted.y - pt_redistorted_vec[0].y, 2) );
ASSERT_LE(obtainedError, maxError);
}
TEST(undistortPoints, regression_14583)
{
const int col = 720;
// const int row = 540;
float camera_matrix_value[] = {
437.8995f, 0.0f, 342.9241f,
0.0f, 438.8216f, 273.7163f,
0.0f, 0.0f, 1.0f
};
cv::Mat camera_interior(3, 3, CV_32F, camera_matrix_value);
float camera_distort_value[] = {-0.34329f, 0.11431f, 0.0f, 0.0f, -0.017375f};
cv::Mat camera_distort(1, 5, CV_32F, camera_distort_value);
float distort_points_value[] = {col, 0.};
cv::Mat distort_pt(1, 1, CV_32FC2, distort_points_value);
cv::Mat undistort_pt;
cv::undistortPoints(distort_pt, undistort_pt, camera_interior,
camera_distort, cv::Mat(), camera_interior);
EXPECT_NEAR(distort_pt.at<Vec2f>(0)[0], undistort_pt.at<Vec2f>(0)[0], col / 2)
<< "distort point: " << distort_pt << std::endl
<< "undistort point: " << undistort_pt;
}
}} // namespace