1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
8 //
9 //
10 // License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
15 // Third party copyrights are property of their respective owners.
16 //
17 // Redistribution and use in source and binary forms, with or without modification,
18 // are permitted provided that the following conditions are met:
19 //
20 // * Redistribution's of source code must retain the above copyright notice,
21 // this list of conditions and the following disclaimer.
22 //
23 // * Redistribution's in binary form must reproduce the above copyright notice,
24 // this list of conditions and the following disclaimer in the documentation
25 // and/or other materials provided with the distribution.
26 //
27 // * The name of the copyright holders may not be used to endorse or promote products
28 // derived from this software without specific prior written permission.
29 //
30 // This software is provided by the copyright holders and contributors "as is" and
31 // any express or implied warranties, including, but not limited to, the implied
32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
33 // In no event shall the Intel Corporation or contributors be liable for any direct,
34 // indirect, incidental, special, exemplary, or consequential damages
35 // (including, but not limited to, procurement of substitute goods or services;
36 // loss of use, data, or profits; or business interruption) however caused
37 // and on any theory of liability, whether in contract, strict liability,
38 // or tort (including negligence or otherwise) arising in any way out of
39 // the use of this software, even if advised of the possibility of such damage.
40 //
41 //M*/
42
43 #include "test_precomp.hpp"
44 #include "opencv2/imgproc/imgproc_c.h"
45
46 using namespace cv;
47 using namespace std;
48
49 class CV_DefaultNewCameraMatrixTest : public cvtest::ArrayTest
50 {
51 public:
52 CV_DefaultNewCameraMatrixTest();
53 protected:
54 int prepare_test_case (int test_case_idx);
55 void prepare_to_validation( int test_case_idx );
56 void get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types );
57 void run_func();
58
59 private:
60 cv::Size img_size;
61 cv::Mat camera_mat;
62 cv::Mat new_camera_mat;
63
64 int matrix_type;
65
66 bool center_principal_point;
67
68 static const int MAX_X = 2048;
69 static const int MAX_Y = 2048;
70 static const int MAX_VAL = 10000;
71 };
72
CV_DefaultNewCameraMatrixTest()73 CV_DefaultNewCameraMatrixTest::CV_DefaultNewCameraMatrixTest()
74 {
75 test_array[INPUT].push_back(NULL);
76 test_array[OUTPUT].push_back(NULL);
77 test_array[REF_OUTPUT].push_back(NULL);
78
79 matrix_type = 0;
80 center_principal_point = false;
81 }
82
get_test_array_types_and_sizes(int test_case_idx,vector<vector<Size>> & sizes,vector<vector<int>> & types)83 void CV_DefaultNewCameraMatrixTest::get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types )
84 {
85 cvtest::ArrayTest::get_test_array_types_and_sizes(test_case_idx,sizes,types);
86 RNG& rng = ts->get_rng();
87 matrix_type = types[INPUT][0] = types[OUTPUT][0]= types[REF_OUTPUT][0] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F;
88 sizes[INPUT][0] = sizes[OUTPUT][0] = sizes[REF_OUTPUT][0] = cvSize(3,3);
89 }
90
prepare_test_case(int test_case_idx)91 int CV_DefaultNewCameraMatrixTest::prepare_test_case(int test_case_idx)
92 {
93 int code = cvtest::ArrayTest::prepare_test_case( test_case_idx );
94
95 if (code <= 0)
96 return code;
97
98 RNG& rng = ts->get_rng();
99
100 img_size.width = cvtest::randInt(rng) % MAX_X + 1;
101 img_size.height = cvtest::randInt(rng) % MAX_Y + 1;
102
103 center_principal_point = ((cvtest::randInt(rng) % 2)!=0);
104
105 // Generating camera_mat matrix
106 double sz = MAX(img_size.width, img_size.height);
107 double aspect_ratio = cvtest::randReal(rng)*0.6 + 0.7;
108 double a[9] = {0,0,0,0,0,0,0,0,1};
109 Mat _a(3,3,CV_64F,a);
110 a[2] = (img_size.width - 1)*0.5 + cvtest::randReal(rng)*10 - 5;
111 a[5] = (img_size.height - 1)*0.5 + cvtest::randReal(rng)*10 - 5;
112 a[0] = sz/(0.9 - cvtest::randReal(rng)*0.6);
113 a[4] = aspect_ratio*a[0];
114
115 Mat& _a0 = test_mat[INPUT][0];
116 cvtest::convert(_a, _a0, _a0.type());
117 camera_mat = _a0;
118
119 return code;
120
121 }
122
run_func()123 void CV_DefaultNewCameraMatrixTest::run_func()
124 {
125 new_camera_mat = cv::getDefaultNewCameraMatrix(camera_mat,img_size,center_principal_point);
126 }
127
prepare_to_validation(int)128 void CV_DefaultNewCameraMatrixTest::prepare_to_validation( int /*test_case_idx*/ )
129 {
130 const Mat& src = test_mat[INPUT][0];
131 Mat& dst = test_mat[REF_OUTPUT][0];
132 Mat& test_output = test_mat[OUTPUT][0];
133 Mat& output = new_camera_mat;
134 cvtest::convert( output, test_output, test_output.type() );
135 if (!center_principal_point)
136 {
137 cvtest::copy(src, dst);
138 }
139 else
140 {
141 double a[9] = {0,0,0,0,0,0,0,0,1};
142 Mat _a(3,3,CV_64F,a);
143 if (matrix_type == CV_64F)
144 {
145 a[0] = src.at<double>(0,0);
146 a[4] = src.at<double>(1,1);
147 }
148 else
149 {
150 a[0] = src.at<float>(0,0);
151 a[4] = src.at<float>(1,1);
152 }
153 a[2] = (img_size.width - 1)*0.5;
154 a[5] = (img_size.height - 1)*0.5;
155 cvtest::convert( _a, dst, dst.type() );
156 }
157 }
158
159 //---------
160
161 class CV_UndistortPointsTest : public cvtest::ArrayTest
162 {
163 public:
164 CV_UndistortPointsTest();
165 protected:
166 int prepare_test_case (int test_case_idx);
167 void prepare_to_validation( int test_case_idx );
168 void get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types );
169 double get_success_error_level( int test_case_idx, int i, int j );
170 void run_func();
171 void distortPoints(const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
172 const CvMat* _distCoeffs, const CvMat* matR, const CvMat* matP);
173
174 private:
175 bool useCPlus;
176 bool useDstMat;
177 static const int N_POINTS = 10;
178 static const int MAX_X = 2048;
179 static const int MAX_Y = 2048;
180
181 bool zero_new_cam;
182 bool zero_distortion;
183 bool zero_R;
184
185 cv::Size img_size;
186 cv::Mat dst_points_mat;
187
188 cv::Mat camera_mat;
189 cv::Mat R;
190 cv::Mat P;
191 cv::Mat distortion_coeffs;
192 cv::Mat src_points;
193 std::vector<cv::Point2f> dst_points;
194 };
195
CV_UndistortPointsTest()196 CV_UndistortPointsTest::CV_UndistortPointsTest()
197 {
198 test_array[INPUT].push_back(NULL); // points matrix
199 test_array[INPUT].push_back(NULL); // camera matrix
200 test_array[INPUT].push_back(NULL); // distortion coeffs
201 test_array[INPUT].push_back(NULL); // R matrix
202 test_array[INPUT].push_back(NULL); // P matrix
203 test_array[OUTPUT].push_back(NULL); // distorted dst points
204 test_array[TEMP].push_back(NULL); // dst points
205 test_array[REF_OUTPUT].push_back(NULL);
206
207 useCPlus = useDstMat = false;
208 zero_new_cam = zero_distortion = zero_R = false;
209 }
210
get_test_array_types_and_sizes(int test_case_idx,vector<vector<Size>> & sizes,vector<vector<int>> & types)211 void CV_UndistortPointsTest::get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types )
212 {
213 cvtest::ArrayTest::get_test_array_types_and_sizes(test_case_idx,sizes,types);
214 RNG& rng = ts->get_rng();
215 useCPlus = ((cvtest::randInt(rng) % 2)!=0);
216 //useCPlus = 0;
217 if (useCPlus)
218 {
219 types[INPUT][0] = types[OUTPUT][0] = types[REF_OUTPUT][0] = types[TEMP][0]= CV_32FC2;
220 }
221 else
222 {
223 types[INPUT][0] = types[OUTPUT][0] = types[REF_OUTPUT][0] = types[TEMP][0]= cvtest::randInt(rng)%2 ? CV_64FC2 : CV_32FC2;
224 }
225 types[INPUT][1] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F;
226 types[INPUT][2] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F;
227 types[INPUT][3] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F;
228 types[INPUT][4] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F;
229
230 sizes[INPUT][0] = sizes[OUTPUT][0] = sizes[REF_OUTPUT][0] = sizes[TEMP][0]= cvtest::randInt(rng)%2 ? cvSize(1,N_POINTS) : cvSize(N_POINTS,1);
231 sizes[INPUT][1] = sizes[INPUT][3] = cvSize(3,3);
232 sizes[INPUT][4] = cvtest::randInt(rng)%2 ? cvSize(3,3) : cvSize(4,3);
233
234 if (cvtest::randInt(rng)%2)
235 {
236 if (cvtest::randInt(rng)%2)
237 {
238 sizes[INPUT][2] = cvSize(1,4);
239 }
240 else
241 {
242 sizes[INPUT][2] = cvSize(1,5);
243 }
244 }
245 else
246 {
247 if (cvtest::randInt(rng)%2)
248 {
249 sizes[INPUT][2] = cvSize(4,1);
250 }
251 else
252 {
253 sizes[INPUT][2] = cvSize(5,1);
254 }
255 }
256 }
257
prepare_test_case(int test_case_idx)258 int CV_UndistortPointsTest::prepare_test_case(int test_case_idx)
259 {
260 RNG& rng = ts->get_rng();
261 int code = cvtest::ArrayTest::prepare_test_case( test_case_idx );
262
263 if (code <= 0)
264 return code;
265
266 useDstMat = (cvtest::randInt(rng) % 2) == 0;
267
268 img_size.width = cvtest::randInt(rng) % MAX_X + 1;
269 img_size.height = cvtest::randInt(rng) % MAX_Y + 1;
270 int dist_size = test_mat[INPUT][2].cols > test_mat[INPUT][2].rows ? test_mat[INPUT][2].cols : test_mat[INPUT][2].rows;
271 double cam[9] = {0,0,0,0,0,0,0,0,1};
272 vector<double> dist(dist_size);
273 vector<double> proj(test_mat[INPUT][4].cols * test_mat[INPUT][4].rows);
274 vector<Point2d> points(N_POINTS);
275
276 Mat _camera(3,3,CV_64F,cam);
277 Mat _distort(test_mat[INPUT][2].rows,test_mat[INPUT][2].cols,CV_64F,&dist[0]);
278 Mat _proj(test_mat[INPUT][4].size(), CV_64F, &proj[0]);
279 Mat _points(test_mat[INPUT][0].size(), CV_64FC2, &points[0]);
280
281 _proj = Scalar::all(0);
282
283 //Generating points
284 for( int i = 0; i < N_POINTS; i++ )
285 {
286 points[i].x = cvtest::randReal(rng)*img_size.width;
287 points[i].y = cvtest::randReal(rng)*img_size.height;
288 }
289
290 //Generating camera matrix
291 double sz = MAX(img_size.width,img_size.height);
292 double aspect_ratio = cvtest::randReal(rng)*0.6 + 0.7;
293 cam[2] = (img_size.width - 1)*0.5 + cvtest::randReal(rng)*10 - 5;
294 cam[5] = (img_size.height - 1)*0.5 + cvtest::randReal(rng)*10 - 5;
295 cam[0] = sz/(0.9 - cvtest::randReal(rng)*0.6);
296 cam[4] = aspect_ratio*cam[0];
297
298 //Generating distortion coeffs
299 dist[0] = cvtest::randReal(rng)*0.06 - 0.03;
300 dist[1] = cvtest::randReal(rng)*0.06 - 0.03;
301 if( dist[0]*dist[1] > 0 )
302 dist[1] = -dist[1];
303 if( cvtest::randInt(rng)%4 != 0 )
304 {
305 dist[2] = cvtest::randReal(rng)*0.004 - 0.002;
306 dist[3] = cvtest::randReal(rng)*0.004 - 0.002;
307 if (dist_size > 4)
308 dist[4] = cvtest::randReal(rng)*0.004 - 0.002;
309 }
310 else
311 {
312 dist[2] = dist[3] = 0;
313 if (dist_size > 4)
314 dist[4] = 0;
315 }
316
317 //Generating P matrix (projection)
318 if( test_mat[INPUT][4].cols != 4 )
319 {
320 proj[8] = 1;
321 if (cvtest::randInt(rng)%2 == 0) // use identity new camera matrix
322 {
323 proj[0] = 1;
324 proj[4] = 1;
325 }
326 else
327 {
328 proj[0] = cam[0] + (cvtest::randReal(rng) - (double)0.5)*0.2*cam[0]; //10%
329 proj[4] = cam[4] + (cvtest::randReal(rng) - (double)0.5)*0.2*cam[4]; //10%
330 proj[2] = cam[2] + (cvtest::randReal(rng) - (double)0.5)*0.3*img_size.width; //15%
331 proj[5] = cam[5] + (cvtest::randReal(rng) - (double)0.5)*0.3*img_size.height; //15%
332 }
333 }
334 else
335 {
336 proj[10] = 1;
337 proj[0] = cam[0] + (cvtest::randReal(rng) - (double)0.5)*0.2*cam[0]; //10%
338 proj[5] = cam[4] + (cvtest::randReal(rng) - (double)0.5)*0.2*cam[4]; //10%
339 proj[2] = cam[2] + (cvtest::randReal(rng) - (double)0.5)*0.3*img_size.width; //15%
340 proj[6] = cam[5] + (cvtest::randReal(rng) - (double)0.5)*0.3*img_size.height; //15%
341
342 proj[3] = (img_size.height + img_size.width - 1)*0.5 + cvtest::randReal(rng)*10 - 5;
343 proj[7] = (img_size.height + img_size.width - 1)*0.5 + cvtest::randReal(rng)*10 - 5;
344 proj[11] = (img_size.height + img_size.width - 1)*0.5 + cvtest::randReal(rng)*10 - 5;
345 }
346
347 //Generating R matrix
348 Mat _rot(3,3,CV_64F);
349 Mat rotation(1,3,CV_64F);
350 rotation.at<double>(0) = CV_PI*(cvtest::randReal(rng) - (double)0.5); // phi
351 rotation.at<double>(1) = CV_PI*(cvtest::randReal(rng) - (double)0.5); // ksi
352 rotation.at<double>(2) = CV_PI*(cvtest::randReal(rng) - (double)0.5); //khi
353 cvtest::Rodrigues(rotation, _rot);
354
355 //copying data
356 //src_points = &_points;
357 _points.convertTo(test_mat[INPUT][0], test_mat[INPUT][0].type());
358 _camera.convertTo(test_mat[INPUT][1], test_mat[INPUT][1].type());
359 _distort.convertTo(test_mat[INPUT][2], test_mat[INPUT][2].type());
360 _rot.convertTo(test_mat[INPUT][3], test_mat[INPUT][3].type());
361 _proj.convertTo(test_mat[INPUT][4], test_mat[INPUT][4].type());
362
363 zero_distortion = (cvtest::randInt(rng)%2) == 0 ? false : true;
364 zero_new_cam = (cvtest::randInt(rng)%2) == 0 ? false : true;
365 zero_R = (cvtest::randInt(rng)%2) == 0 ? false : true;
366
367 if (useCPlus)
368 {
369 _points.convertTo(src_points, CV_32F);
370
371 camera_mat = test_mat[INPUT][1];
372 distortion_coeffs = test_mat[INPUT][2];
373 R = test_mat[INPUT][3];
374 P = test_mat[INPUT][4];
375 }
376
377 return code;
378 }
379
prepare_to_validation(int)380 void CV_UndistortPointsTest::prepare_to_validation(int /*test_case_idx*/)
381 {
382 int dist_size = test_mat[INPUT][2].cols > test_mat[INPUT][2].rows ? test_mat[INPUT][2].cols : test_mat[INPUT][2].rows;
383 double cam[9] = {0,0,0,0,0,0,0,0,1};
384 double rot[9] = {1,0,0,0,1,0,0,0,1};
385
386 double* dist = new double[dist_size ];
387 double* proj = new double[test_mat[INPUT][4].cols * test_mat[INPUT][4].rows];
388 double* points = new double[N_POINTS*2];
389 double* r_points = new double[N_POINTS*2];
390 //Run reference calculations
391 CvMat ref_points= cvMat(test_mat[INPUT][0].rows,test_mat[INPUT][0].cols,CV_64FC2,r_points);
392 CvMat _camera = cvMat(3,3,CV_64F,cam);
393 CvMat _rot = cvMat(3,3,CV_64F,rot);
394 CvMat _distort = cvMat(test_mat[INPUT][2].rows,test_mat[INPUT][2].cols,CV_64F,dist);
395 CvMat _proj = cvMat(test_mat[INPUT][4].rows,test_mat[INPUT][4].cols,CV_64F,proj);
396 CvMat _points= cvMat(test_mat[TEMP][0].rows,test_mat[TEMP][0].cols,CV_64FC2,points);
397
398 Mat __camera = cvarrToMat(&_camera);
399 Mat __distort = cvarrToMat(&_distort);
400 Mat __rot = cvarrToMat(&_rot);
401 Mat __proj = cvarrToMat(&_proj);
402 Mat __points = cvarrToMat(&_points);
403 Mat _ref_points = cvarrToMat(&ref_points);
404
405 cvtest::convert(test_mat[INPUT][1], __camera, __camera.type());
406 cvtest::convert(test_mat[INPUT][2], __distort, __distort.type());
407 cvtest::convert(test_mat[INPUT][3], __rot, __rot.type());
408 cvtest::convert(test_mat[INPUT][4], __proj, __proj.type());
409
410 if (useCPlus)
411 {
412 if (useDstMat)
413 {
414 CvMat temp = dst_points_mat;
415 for (int i=0;i<N_POINTS*2;i++)
416 {
417 points[i] = temp.data.fl[i];
418 }
419 }
420 else
421 {
422 for (int i=0;i<N_POINTS;i++)
423 {
424 points[2*i] = dst_points[i].x;
425 points[2*i+1] = dst_points[i].y;
426 }
427 }
428 }
429 else
430 {
431 cvtest::convert(test_mat[TEMP][0],__points, __points.type());
432 }
433
434 CvMat* input2 = zero_distortion ? 0 : &_distort;
435 CvMat* input3 = zero_R ? 0 : &_rot;
436 CvMat* input4 = zero_new_cam ? 0 : &_proj;
437 distortPoints(&_points,&ref_points,&_camera,input2,input3,input4);
438
439 Mat& dst = test_mat[REF_OUTPUT][0];
440 cvtest::convert(_ref_points, dst, dst.type());
441
442 cvtest::copy(test_mat[INPUT][0], test_mat[OUTPUT][0]);
443
444 delete[] dist;
445 delete[] proj;
446 delete[] points;
447 delete[] r_points;
448 }
449
run_func()450 void CV_UndistortPointsTest::run_func()
451 {
452
453 if (useCPlus)
454 {
455 cv::Mat input2,input3,input4;
456 input2 = zero_distortion ? cv::Mat() : cv::Mat(test_mat[INPUT][2]);
457 input3 = zero_R ? cv::Mat() : cv::Mat(test_mat[INPUT][3]);
458 input4 = zero_new_cam ? cv::Mat() : cv::Mat(test_mat[INPUT][4]);
459
460 if (useDstMat)
461 {
462 //cv::undistortPoints(src_points,dst_points_mat,camera_mat,distortion_coeffs,R,P);
463 cv::undistortPoints(src_points,dst_points_mat,camera_mat,input2,input3,input4);
464 }
465 else
466 {
467 //cv::undistortPoints(src_points,dst_points,camera_mat,distortion_coeffs,R,P);
468 cv::undistortPoints(src_points,dst_points,camera_mat,input2,input3,input4);
469 }
470 }
471 else
472 {
473 CvMat _input0 = test_mat[INPUT][0], _input1 = test_mat[INPUT][1], _input2, _input3, _input4;
474 CvMat _output = test_mat[TEMP][0];
475 if(!zero_distortion)
476 _input2 = test_mat[INPUT][2];
477 if(!zero_R)
478 _input3 = test_mat[INPUT][3];
479 if(!zero_new_cam)
480 _input4 = test_mat[INPUT][4];
481 cvUndistortPoints(&_input0, &_output, &_input1,
482 zero_distortion ? 0 : &_input2,
483 zero_R ? 0 : &_input3,
484 zero_new_cam ? 0 : &_input4);
485 }
486 }
487
distortPoints(const CvMat * _src,CvMat * _dst,const CvMat * _cameraMatrix,const CvMat * _distCoeffs,const CvMat * matR,const CvMat * matP)488 void CV_UndistortPointsTest::distortPoints(const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
489 const CvMat* _distCoeffs,
490 const CvMat* matR, const CvMat* matP)
491 {
492 double a[9];
493
494 CvMat* __P;
495 if ((!matP)||(matP->cols == 3))
496 __P = cvCreateMat(3,3,CV_64F);
497 else
498 __P = cvCreateMat(3,4,CV_64F);
499 if (matP)
500 {
501 cvtest::convert(cvarrToMat(matP), cvarrToMat(__P), -1);
502 }
503 else
504 {
505 cvZero(__P);
506 __P->data.db[0] = 1;
507 __P->data.db[4] = 1;
508 __P->data.db[8] = 1;
509 }
510 CvMat* __R = cvCreateMat(3,3,CV_64F);
511 if (matR)
512 {
513 cvCopy(matR,__R);
514 }
515 else
516 {
517 cvZero(__R);
518 __R->data.db[0] = 1;
519 __R->data.db[4] = 1;
520 __R->data.db[8] = 1;
521 }
522 for (int i=0;i<N_POINTS;i++)
523 {
524 int movement = __P->cols > 3 ? 1 : 0;
525 double x = (_src->data.db[2*i]-__P->data.db[2])/__P->data.db[0];
526 double y = (_src->data.db[2*i+1]-__P->data.db[5+movement])/__P->data.db[4+movement];
527 CvMat inverse = cvMat(3,3,CV_64F,a);
528 cvInvert(__R,&inverse);
529 double w1 = x*inverse.data.db[6]+y*inverse.data.db[7]+inverse.data.db[8];
530 double _x = (x*inverse.data.db[0]+y*inverse.data.db[1]+inverse.data.db[2])/w1;
531 double _y = (x*inverse.data.db[3]+y*inverse.data.db[4]+inverse.data.db[5])/w1;
532
533 //Distortions
534
535 double __x = _x;
536 double __y = _y;
537 if (_distCoeffs)
538 {
539 double r2 = _x*_x+_y*_y;
540
541 __x = _x*(1+_distCoeffs->data.db[0]*r2+_distCoeffs->data.db[1]*r2*r2)+
542 2*_distCoeffs->data.db[2]*_x*_y+_distCoeffs->data.db[3]*(r2+2*_x*_x);
543 __y = _y*(1+_distCoeffs->data.db[0]*r2+_distCoeffs->data.db[1]*r2*r2)+
544 2*_distCoeffs->data.db[3]*_x*_y+_distCoeffs->data.db[2]*(r2+2*_y*_y);
545 if ((_distCoeffs->cols > 4) || (_distCoeffs->rows > 4))
546 {
547 __x+=_x*_distCoeffs->data.db[4]*r2*r2*r2;
548 __y+=_y*_distCoeffs->data.db[4]*r2*r2*r2;
549 }
550 }
551
552
553 _dst->data.db[2*i] = __x*_cameraMatrix->data.db[0]+_cameraMatrix->data.db[2];
554 _dst->data.db[2*i+1] = __y*_cameraMatrix->data.db[4]+_cameraMatrix->data.db[5];
555
556 }
557
558 cvReleaseMat(&__R);
559 cvReleaseMat(&__P);
560
561 }
562
563
get_success_error_level(int,int,int)564 double CV_UndistortPointsTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ )
565 {
566 return 5e-2;
567 }
568
569 //------------------------------------------------------
570
571 class CV_InitUndistortRectifyMapTest : public cvtest::ArrayTest
572 {
573 public:
574 CV_InitUndistortRectifyMapTest();
575 protected:
576 int prepare_test_case (int test_case_idx);
577 void prepare_to_validation( int test_case_idx );
578 void get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types );
579 double get_success_error_level( int test_case_idx, int i, int j );
580 void run_func();
581
582 private:
583 bool useCPlus;
584 static const int N_POINTS = 100;
585 static const int MAX_X = 2048;
586 static const int MAX_Y = 2048;
587 bool zero_new_cam;
588 bool zero_distortion;
589 bool zero_R;
590
591
592 cv::Size img_size;
593
594 cv::Mat camera_mat;
595 cv::Mat R;
596 cv::Mat new_camera_mat;
597 cv::Mat distortion_coeffs;
598 cv::Mat mapx;
599 cv::Mat mapy;
600 CvMat* _mapx;
601 CvMat* _mapy;
602 int mat_type;
603 };
604
CV_InitUndistortRectifyMapTest()605 CV_InitUndistortRectifyMapTest::CV_InitUndistortRectifyMapTest()
606 {
607 test_array[INPUT].push_back(NULL); // test points matrix
608 test_array[INPUT].push_back(NULL); // camera matrix
609 test_array[INPUT].push_back(NULL); // distortion coeffs
610 test_array[INPUT].push_back(NULL); // R matrix
611 test_array[INPUT].push_back(NULL); // new camera matrix
612 test_array[OUTPUT].push_back(NULL); // distorted dst points
613 test_array[REF_OUTPUT].push_back(NULL);
614
615 useCPlus = false;
616 zero_distortion = zero_new_cam = zero_R = false;
617 _mapx = _mapy = NULL;
618 mat_type = 0;
619 }
620
get_test_array_types_and_sizes(int test_case_idx,vector<vector<Size>> & sizes,vector<vector<int>> & types)621 void CV_InitUndistortRectifyMapTest::get_test_array_types_and_sizes( int test_case_idx, vector<vector<Size> >& sizes, vector<vector<int> >& types )
622 {
623 cvtest::ArrayTest::get_test_array_types_and_sizes(test_case_idx,sizes,types);
624 RNG& rng = ts->get_rng();
625 useCPlus = ((cvtest::randInt(rng) % 2)!=0);
626 //useCPlus = 0;
627 types[INPUT][0] = types[OUTPUT][0] = types[REF_OUTPUT][0] = CV_64FC2;
628
629 types[INPUT][1] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F;
630 types[INPUT][2] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F;
631 types[INPUT][3] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F;
632 types[INPUT][4] = cvtest::randInt(rng)%2 ? CV_64F : CV_32F;
633
634 sizes[INPUT][0] = sizes[OUTPUT][0] = sizes[REF_OUTPUT][0] = cvSize(N_POINTS,1);
635 sizes[INPUT][1] = sizes[INPUT][3] = cvSize(3,3);
636 sizes[INPUT][4] = cvSize(3,3);
637
638 if (cvtest::randInt(rng)%2)
639 {
640 if (cvtest::randInt(rng)%2)
641 {
642 sizes[INPUT][2] = cvSize(1,4);
643 }
644 else
645 {
646 sizes[INPUT][2] = cvSize(1,5);
647 }
648 }
649 else
650 {
651 if (cvtest::randInt(rng)%2)
652 {
653 sizes[INPUT][2] = cvSize(4,1);
654 }
655 else
656 {
657 sizes[INPUT][2] = cvSize(5,1);
658 }
659 }
660 }
661
662
prepare_test_case(int test_case_idx)663 int CV_InitUndistortRectifyMapTest::prepare_test_case(int test_case_idx)
664 {
665 RNG& rng = ts->get_rng();
666 int code = cvtest::ArrayTest::prepare_test_case( test_case_idx );
667
668 if (code <= 0)
669 return code;
670
671 img_size.width = cvtest::randInt(rng) % MAX_X + 1;
672 img_size.height = cvtest::randInt(rng) % MAX_Y + 1;
673
674 if (useCPlus)
675 {
676 mat_type = (cvtest::randInt(rng) % 2) == 0 ? CV_32FC1 : CV_16SC2;
677 if ((cvtest::randInt(rng) % 4) == 0)
678 mat_type = -1;
679 if ((cvtest::randInt(rng) % 4) == 0)
680 mat_type = CV_32FC2;
681 _mapx = 0;
682 _mapy = 0;
683 }
684 else
685 {
686 int typex = (cvtest::randInt(rng) % 2) == 0 ? CV_32FC1 : CV_16SC2;
687 //typex = CV_32FC1; ///!!!!!!!!!!!!!!!!
688 int typey = (typex == CV_32FC1) ? CV_32FC1 : CV_16UC1;
689
690 _mapx = cvCreateMat(img_size.height,img_size.width,typex);
691 _mapy = cvCreateMat(img_size.height,img_size.width,typey);
692
693
694 }
695
696 int dist_size = test_mat[INPUT][2].cols > test_mat[INPUT][2].rows ? test_mat[INPUT][2].cols : test_mat[INPUT][2].rows;
697 double cam[9] = {0,0,0,0,0,0,0,0,1};
698 vector<double> dist(dist_size);
699 vector<double> new_cam(test_mat[INPUT][4].cols * test_mat[INPUT][4].rows);
700 vector<Point2d> points(N_POINTS);
701
702 Mat _camera(3,3,CV_64F,cam);
703 Mat _distort(test_mat[INPUT][2].size(),CV_64F,&dist[0]);
704 Mat _new_cam(test_mat[INPUT][4].size(),CV_64F,&new_cam[0]);
705 Mat _points(test_mat[INPUT][0].size(),CV_64FC2, &points[0]);
706
707 //Generating points
708 for (int i=0;i<N_POINTS;i++)
709 {
710 points[i].x = cvtest::randReal(rng)*img_size.width;
711 points[i].y = cvtest::randReal(rng)*img_size.height;
712 }
713
714 //Generating camera matrix
715 double sz = MAX(img_size.width,img_size.height);
716 double aspect_ratio = cvtest::randReal(rng)*0.6 + 0.7;
717 cam[2] = (img_size.width - 1)*0.5 + cvtest::randReal(rng)*10 - 5;
718 cam[5] = (img_size.height - 1)*0.5 + cvtest::randReal(rng)*10 - 5;
719 cam[0] = sz/(0.9 - cvtest::randReal(rng)*0.6);
720 cam[4] = aspect_ratio*cam[0];
721
722 //Generating distortion coeffs
723 dist[0] = cvtest::randReal(rng)*0.06 - 0.03;
724 dist[1] = cvtest::randReal(rng)*0.06 - 0.03;
725 if( dist[0]*dist[1] > 0 )
726 dist[1] = -dist[1];
727 if( cvtest::randInt(rng)%4 != 0 )
728 {
729 dist[2] = cvtest::randReal(rng)*0.004 - 0.002;
730 dist[3] = cvtest::randReal(rng)*0.004 - 0.002;
731 if (dist_size > 4)
732 dist[4] = cvtest::randReal(rng)*0.004 - 0.002;
733 }
734 else
735 {
736 dist[2] = dist[3] = 0;
737 if (dist_size > 4)
738 dist[4] = 0;
739 }
740
741 //Generating new camera matrix
742 _new_cam = Scalar::all(0);
743 new_cam[8] = 1;
744
745 //new_cam[0] = cam[0];
746 //new_cam[4] = cam[4];
747 //new_cam[2] = cam[2];
748 //new_cam[5] = cam[5];
749
750 new_cam[0] = cam[0] + (cvtest::randReal(rng) - (double)0.5)*0.2*cam[0]; //10%
751 new_cam[4] = cam[4] + (cvtest::randReal(rng) - (double)0.5)*0.2*cam[4]; //10%
752 new_cam[2] = cam[2] + (cvtest::randReal(rng) - (double)0.5)*0.3*img_size.width; //15%
753 new_cam[5] = cam[5] + (cvtest::randReal(rng) - (double)0.5)*0.3*img_size.height; //15%
754
755
756 //Generating R matrix
757 Mat _rot(3,3,CV_64F);
758 Mat rotation(1,3,CV_64F);
759 rotation.at<double>(0) = CV_PI/8*(cvtest::randReal(rng) - (double)0.5); // phi
760 rotation.at<double>(1) = CV_PI/8*(cvtest::randReal(rng) - (double)0.5); // ksi
761 rotation.at<double>(2) = CV_PI/3*(cvtest::randReal(rng) - (double)0.5); //khi
762 cvtest::Rodrigues(rotation, _rot);
763
764 //cvSetIdentity(_rot);
765 //copying data
766 cvtest::convert( _points, test_mat[INPUT][0], test_mat[INPUT][0].type());
767 cvtest::convert( _camera, test_mat[INPUT][1], test_mat[INPUT][1].type());
768 cvtest::convert( _distort, test_mat[INPUT][2], test_mat[INPUT][2].type());
769 cvtest::convert( _rot, test_mat[INPUT][3], test_mat[INPUT][3].type());
770 cvtest::convert( _new_cam, test_mat[INPUT][4], test_mat[INPUT][4].type());
771
772 zero_distortion = (cvtest::randInt(rng)%2) == 0 ? false : true;
773 zero_new_cam = (cvtest::randInt(rng)%2) == 0 ? false : true;
774 zero_R = (cvtest::randInt(rng)%2) == 0 ? false : true;
775
776 if (useCPlus)
777 {
778 camera_mat = test_mat[INPUT][1];
779 distortion_coeffs = test_mat[INPUT][2];
780 R = test_mat[INPUT][3];
781 new_camera_mat = test_mat[INPUT][4];
782 }
783
784 return code;
785 }
786
prepare_to_validation(int)787 void CV_InitUndistortRectifyMapTest::prepare_to_validation(int/* test_case_idx*/)
788 {
789 #if 0
790 int dist_size = test_mat[INPUT][2].cols > test_mat[INPUT][2].rows ? test_mat[INPUT][2].cols : test_mat[INPUT][2].rows;
791 double cam[9] = {0,0,0,0,0,0,0,0,1};
792 double rot[9] = {1,0,0,0,1,0,0,0,1};
793 vector<double> dist(dist_size);
794 vector<double> new_cam(test_mat[INPUT][4].cols * test_mat[INPUT][4].rows);
795 vector<Point2d> points(N_POINTS);
796 vector<Point2d> r_points(N_POINTS);
797 //Run reference calculations
798 Mat ref_points(test_mat[INPUT][0].size(),CV_64FC2,&r_points[0]);
799 Mat _camera(3,3,CV_64F,cam);
800 Mat _rot(3,3,CV_64F,rot);
801 Mat _distort(test_mat[INPUT][2].size(),CV_64F,&dist[0]);
802 Mat _new_cam(test_mat[INPUT][4].size(),CV_64F,&new_cam[0]);
803 Mat _points(test_mat[INPUT][0].size(),CV_64FC2,&points[0]);
804
805 cvtest::convert(test_mat[INPUT][1],_camera,_camera.type());
806 cvtest::convert(test_mat[INPUT][2],_distort,_distort.type());
807 cvtest::convert(test_mat[INPUT][3],_rot,_rot.type());
808 cvtest::convert(test_mat[INPUT][4],_new_cam,_new_cam.type());
809
810 //Applying precalculated undistort rectify map
811 if (!useCPlus)
812 {
813 mapx = cv::Mat(_mapx);
814 mapy = cv::Mat(_mapy);
815 }
816 cv::Mat map1,map2;
817 cv::convertMaps(mapx,mapy,map1,map2,CV_32FC1);
818 CvMat _map1 = map1;
819 CvMat _map2 = map2;
820 const Point2d* sptr = (const Point2d*)test_mat[INPUT][0].data;
821 for( int i = 0;i < N_POINTS; i++ )
822 {
823 int u = saturate_cast<int>(sptr[i].x);
824 int v = saturate_cast<int>(sptr[i].y);
825 points[i].x = _map1.data.fl[v*_map1.cols + u];
826 points[i].y = _map2.data.fl[v*_map2.cols + u];
827 }
828
829 //---
830
831 cv::undistortPoints(_points, ref_points, _camera,
832 zero_distortion ? Mat() : _distort,
833 zero_R ? Mat::eye(3,3,CV_64F) : _rot,
834 zero_new_cam ? _camera : _new_cam);
835 //cvTsDistortPoints(&_points,&ref_points,&_camera,&_distort,&_rot,&_new_cam);
836 cvtest::convert(ref_points, test_mat[REF_OUTPUT][0], test_mat[REF_OUTPUT][0].type());
837 cvtest::copy(test_mat[INPUT][0],test_mat[OUTPUT][0]);
838
839 cvReleaseMat(&_mapx);
840 cvReleaseMat(&_mapy);
841 #else
842 int dist_size = test_mat[INPUT][2].cols > test_mat[INPUT][2].rows ? test_mat[INPUT][2].cols : test_mat[INPUT][2].rows;
843 double cam[9] = {0,0,0,0,0,0,0,0,1};
844 double rot[9] = {1,0,0,0,1,0,0,0,1};
845 double* dist = new double[dist_size ];
846 double* new_cam = new double[test_mat[INPUT][4].cols * test_mat[INPUT][4].rows];
847 double* points = new double[N_POINTS*2];
848 double* r_points = new double[N_POINTS*2];
849 //Run reference calculations
850 CvMat ref_points= cvMat(test_mat[INPUT][0].rows,test_mat[INPUT][0].cols,CV_64FC2,r_points);
851 CvMat _camera = cvMat(3,3,CV_64F,cam);
852 CvMat _rot = cvMat(3,3,CV_64F,rot);
853 CvMat _distort = cvMat(test_mat[INPUT][2].rows,test_mat[INPUT][2].cols,CV_64F,dist);
854 CvMat _new_cam = cvMat(test_mat[INPUT][4].rows,test_mat[INPUT][4].cols,CV_64F,new_cam);
855 CvMat _points= cvMat(test_mat[INPUT][0].rows,test_mat[INPUT][0].cols,CV_64FC2,points);
856
857 CvMat _input1 = test_mat[INPUT][1];
858 CvMat _input2 = test_mat[INPUT][2];
859 CvMat _input3 = test_mat[INPUT][3];
860 CvMat _input4 = test_mat[INPUT][4];
861
862 cvtest::convert(cvarrToMat(&_input1), cvarrToMat(&_camera), -1);
863 cvtest::convert(cvarrToMat(&_input2), cvarrToMat(&_distort), -1);
864 cvtest::convert(cvarrToMat(&_input3), cvarrToMat(&_rot), -1);
865 cvtest::convert(cvarrToMat(&_input4), cvarrToMat(&_new_cam), -1);
866
867 //Applying precalculated undistort rectify map
868 if (!useCPlus)
869 {
870 mapx = cv::cvarrToMat(_mapx);
871 mapy = cv::cvarrToMat(_mapy);
872 }
873 cv::Mat map1,map2;
874 cv::convertMaps(mapx,mapy,map1,map2,CV_32FC1);
875 CvMat _map1 = map1;
876 CvMat _map2 = map2;
877 for (int i=0;i<N_POINTS;i++)
878 {
879 double u = test_mat[INPUT][0].ptr<double>()[2*i];
880 double v = test_mat[INPUT][0].ptr<double>()[2*i+1];
881 _points.data.db[2*i] = (double)_map1.data.fl[(int)v*_map1.cols+(int)u];
882 _points.data.db[2*i+1] = (double)_map2.data.fl[(int)v*_map2.cols+(int)u];
883 }
884
885 //---
886
887 cvUndistortPoints(&_points,&ref_points,&_camera,
888 zero_distortion ? 0 : &_distort, zero_R ? 0 : &_rot, zero_new_cam ? &_camera : &_new_cam);
889 //cvTsDistortPoints(&_points,&ref_points,&_camera,&_distort,&_rot,&_new_cam);
890 CvMat dst = test_mat[REF_OUTPUT][0];
891 cvtest::convert(cvarrToMat(&ref_points), cvarrToMat(&dst), -1);
892
893 cvtest::copy(test_mat[INPUT][0],test_mat[OUTPUT][0]);
894
895 delete[] dist;
896 delete[] new_cam;
897 delete[] points;
898 delete[] r_points;
899 cvReleaseMat(&_mapx);
900 cvReleaseMat(&_mapy);
901 #endif
902 }
903
run_func()904 void CV_InitUndistortRectifyMapTest::run_func()
905 {
906 if (useCPlus)
907 {
908 cv::Mat input2,input3,input4;
909 input2 = zero_distortion ? cv::Mat() : test_mat[INPUT][2];
910 input3 = zero_R ? cv::Mat() : test_mat[INPUT][3];
911 input4 = zero_new_cam ? cv::Mat() : test_mat[INPUT][4];
912 cv::initUndistortRectifyMap(camera_mat,input2,input3,input4,img_size,mat_type,mapx,mapy);
913 }
914 else
915 {
916 CvMat input1 = test_mat[INPUT][1], input2, input3, input4;
917 if( !zero_distortion )
918 input2 = test_mat[INPUT][2];
919 if( !zero_R )
920 input3 = test_mat[INPUT][3];
921 if( !zero_new_cam )
922 input4 = test_mat[INPUT][4];
923 cvInitUndistortRectifyMap(&input1,
924 zero_distortion ? 0 : &input2,
925 zero_R ? 0 : &input3,
926 zero_new_cam ? 0 : &input4,
927 _mapx,_mapy);
928 }
929 }
930
get_success_error_level(int,int,int)931 double CV_InitUndistortRectifyMapTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ )
932 {
933 return 8;
934 }
935
936 //////////////////////////////////////////////////////////////////////////////////////////////////////
937
TEST(Calib3d_DefaultNewCameraMatrix,accuracy)938 TEST(Calib3d_DefaultNewCameraMatrix, accuracy) { CV_DefaultNewCameraMatrixTest test; test.safe_run(); }
TEST(Calib3d_UndistortPoints,accuracy)939 TEST(Calib3d_UndistortPoints, accuracy) { CV_UndistortPointsTest test; test.safe_run(); }
TEST(Calib3d_InitUndistortRectifyMap,accuracy)940 TEST(Calib3d_InitUndistortRectifyMap, accuracy) { CV_InitUndistortRectifyMapTest test; test.safe_run(); }
941