opencv/tests/cv/src/aundistort.cpp

   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 "cvtest.h"
  44
  45
  46 class CV_DefaultNewCameraMatrixTest : public CvArrTest
  47 {
  48 public:
  49         CV_DefaultNewCameraMatrixTest();
  50 protected:
  51         int prepare_test_case (int test_case_idx);
  52         void prepare_to_validation( int test_case_idx );
  53         void get_test_array_types_and_sizes( int test_case_idx, CvSize** sizes, int** types );
  54         void run_func();
  55
  56 private:
  57         cv::Size img_size;
  58         cv::Mat camera_mat;
  59         cv::Mat new_camera_mat;
  60
  61         int matrix_type;
  62
  63         bool center_principal_point;
  64
  65         static const int MAX_X = 2048;
  66         static const int MAX_Y = 2048;
  67         static const int MAX_VAL = 10000;
  68 };
  69
  70 CV_DefaultNewCameraMatrixTest::CV_DefaultNewCameraMatrixTest() : CvArrTest("undistort-getDefaultNewCameraMatrix","getDefaultNewCameraMatrix")
  71 {
  72         test_array[INPUT].push(NULL);
  73         test_array[OUTPUT].push(NULL);
  74         test_array[REF_OUTPUT].push(NULL);
  75 }
  76
  77 void CV_DefaultNewCameraMatrixTest::get_test_array_types_and_sizes( int test_case_idx, CvSize** sizes, int** types )
  78 {
  79         CvArrTest::get_test_array_types_and_sizes(test_case_idx,sizes,types);
  80         CvRNG* rng = ts->get_rng();
  81         matrix_type = types[INPUT][0] = types[OUTPUT][0]= types[REF_OUTPUT][0] = cvTsRandInt(rng)%2 ? CV_64F : CV_32F;
  82         sizes[INPUT][0] = sizes[OUTPUT][0] = sizes[REF_OUTPUT][0] = cvSize(3,3);
  83 }
  84
  85 int CV_DefaultNewCameraMatrixTest::prepare_test_case(int test_case_idx)
  86 {
  87         int code = CvArrTest::prepare_test_case( test_case_idx );
  88
  89         if (code <= 0)
  90                 return code;
  91
  92         CvRNG* rng = ts->get_rng();
  93
  94         img_size.width = cvTsRandInt(rng) % MAX_X + 1;
  95         img_size.height = cvTsRandInt(rng) % MAX_Y + 1;
  96
  97         center_principal_point = ((cvTsRandInt(rng) % 2)!=0);
  98
  99         // Generating camera_mat matrix
 100         double sz = MAX(img_size.width, img_size.height);
 101         double aspect_ratio = cvTsRandReal(rng)*0.6 + 0.7;
 102         double a[9] = {0,0,0,0,0,0,0,0,1};
 103         CvMat _a = cvMat(3,3,CV_64F,a);
 104         a[2] = (img_size.width - 1)*0.5 + cvTsRandReal(rng)*10 - 5;
 105         a[5] = (img_size.height - 1)*0.5 + cvTsRandReal(rng)*10 - 5;
 106         a[0] = sz/(0.9 - cvTsRandReal(rng)*0.6);
 107         a[4] = aspect_ratio*a[0];
 108
 109         //Copying into input array
 110         CvMat* _a0 = &test_mat[INPUT][0];
 111         cvTsConvert( &_a, _a0 );
 112         camera_mat = _a0;
 113         //new_camera_mat = camera_mat;
 114
 115         return code;
 116
 117 }
 118
 119 void CV_DefaultNewCameraMatrixTest::run_func()
 120 {
 121         new_camera_mat = cv::getDefaultNewCameraMatrix(camera_mat,img_size,center_principal_point);
 122 }
 123
 124 void CV_DefaultNewCameraMatrixTest::prepare_to_validation( int /*test_case_idx*/ )
 125 {
 126         const CvMat* src = &test_mat[INPUT][0];
 127         CvMat* dst = &test_mat[REF_OUTPUT][0];
 128         CvMat* test_output = &test_mat[OUTPUT][0];
 129         CvMat output = new_camera_mat;
 130         cvTsConvert( &output, test_output );
 131         if (!center_principal_point)
 132         {
 133                 cvCopy(src,dst);
 134         }
 135         else
 136         {
 137                 double a[9] = {0,0,0,0,0,0,0,0,1};
 138                 CvMat _a = cvMat(3,3,CV_64F,a);
 139                 if (matrix_type == CV_64F)
 140                 {
 141                         a[0] = ((double*)(src->data.ptr + src->step*0))[0];
 142                         a[4] = ((double*)(src->data.ptr + src->step*1))[1];
 143                 }
 144                 else
 145                 {
 146                         a[0] = (double)((float*)(src->data.ptr + src->step*0))[0];
 147                         a[4] = (double)((float*)(src->data.ptr + src->step*1))[1];
 148                 }
 149                 a[2] = (img_size.width - 1)*0.5;
 150                 a[5] = (img_size.height - 1)*0.5;
 151                 cvTsConvert( &_a, dst );
 152         }
 153 }
 154
 155 CV_DefaultNewCameraMatrixTest default_new_camera_matrix_test;
 156
 157 //---------
 158
 159 class CV_UndistortPointsTest : public CvArrTest
 160 {
 161 public:
 162         CV_UndistortPointsTest();
 163 protected:
 164         int prepare_test_case (int test_case_idx);
 165         void prepare_to_validation( int test_case_idx );
 166         void get_test_array_types_and_sizes( int test_case_idx, CvSize** sizes, int** types );
 167         double get_success_error_level( int test_case_idx, int i, int j );
 168         void run_func();
 169         void cvTsDistortPoints(const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
 170                 const CvMat* _distCoeffs,
 171                 const CvMat* _R, const CvMat* _P);
 172
 173 private:
 174         bool useCPlus;
 175         bool useDstMat;
 176         static const int N_POINTS = 10;
 177         static const int MAX_X = 2048;
 178         static const int MAX_Y = 2048;
 179
 180         bool zero_new_cam;
 181         bool zero_distortion;
 182         bool zero_R;
 183
 184         cv::Size img_size;
 185         cv::Mat dst_points_mat;
 186
 187         cv::Mat camera_mat;
 188         cv::Mat R;
 189         cv::Mat P;
 190         cv::Mat distortion_coeffs;
 191         cv::Mat src_points;
 192         std::vector<cv::Point2f> dst_points;
 193 };
 194
 195 CV_UndistortPointsTest::CV_UndistortPointsTest() : CvArrTest("undistort-points","cvUndistortPoints")
 196 {
 197         test_array[INPUT].push(NULL); // points matrix
 198         test_array[INPUT].push(NULL); // camera matrix
 199         test_array[INPUT].push(NULL); // distortion coeffs
 200         test_array[INPUT].push(NULL); // R matrix
 201         test_array[INPUT].push(NULL); // P matrix
 202         test_array[OUTPUT].push(NULL); // distorted dst points
 203         test_array[TEMP].push(NULL); // dst points
 204         test_array[REF_OUTPUT].push(NULL);
 205 }
 206
 207 void CV_UndistortPointsTest::get_test_array_types_and_sizes( int test_case_idx, CvSize** sizes, int** types )
 208 {
 209         CvArrTest::get_test_array_types_and_sizes(test_case_idx,sizes,types);
 210         CvRNG* rng = ts->get_rng();
 211         useCPlus = ((cvTsRandInt(rng) % 2)!=0);
 212         //useCPlus = 0;
 213         if (useCPlus)
 214         {
 215                 types[INPUT][0] = types[OUTPUT][0] = types[REF_OUTPUT][0] = types[TEMP][0]= CV_32FC2;
 216         }
 217         else
 218         {
 219                 types[INPUT][0] = types[OUTPUT][0] = types[REF_OUTPUT][0] = types[TEMP][0]= cvTsRandInt(rng)%2 ? CV_64FC2 : CV_32FC2;
 220         }
 221         types[INPUT][1] = cvTsRandInt(rng)%2 ? CV_64F : CV_32F;
 222         types[INPUT][2] = cvTsRandInt(rng)%2 ? CV_64F : CV_32F;
 223         types[INPUT][3] = cvTsRandInt(rng)%2 ? CV_64F : CV_32F;
 224         types[INPUT][4] = cvTsRandInt(rng)%2 ? CV_64F : CV_32F;
 225
 226         sizes[INPUT][0] = sizes[OUTPUT][0] = sizes[REF_OUTPUT][0] = sizes[TEMP][0]= cvTsRandInt(rng)%2 ? cvSize(1,N_POINTS) : cvSize(N_POINTS,1);
 227         sizes[INPUT][1] = sizes[INPUT][3] = cvSize(3,3);
 228         sizes[INPUT][4] = cvTsRandInt(rng)%2 ? cvSize(3,3) : cvSize(4,3);
 229
 230         if (cvTsRandInt(rng)%2)
 231         {
 232                 if (cvTsRandInt(rng)%2)
 233                 {
 234                         sizes[INPUT][2] = cvSize(1,4);
 235                 }
 236                 else
 237                 {
 238                         sizes[INPUT][2] = cvSize(1,5);
 239                 }
 240         }
 241         else
 242         {
 243                 if (cvTsRandInt(rng)%2)
 244                 {
 245                         sizes[INPUT][2] = cvSize(4,1);
 246                 }
 247                 else
 248                 {
 249                         sizes[INPUT][2] = cvSize(5,1);
 250                 }
 251         }
 252 }
 253
 254 int CV_UndistortPointsTest::prepare_test_case(int test_case_idx)
 255 {
 256         CvRNG* rng = ts->get_rng();
 257         int code = CvArrTest::prepare_test_case( test_case_idx );
 258
 259         if (code <= 0)
 260                 return code;
 261
 262         useDstMat = (cvTsRandInt(rng) % 2) == 0;
 263
 264         img_size.width = cvTsRandInt(rng) % MAX_X + 1;
 265         img_size.height = cvTsRandInt(rng) % MAX_Y + 1;
 266         int dist_size = test_mat[INPUT][2].cols > test_mat[INPUT][2].rows ? test_mat[INPUT][2].cols : test_mat[INPUT][2].rows;
 267         double cam[9] = {0,0,0,0,0,0,0,0,1};
 268         double* dist = new double[dist_size ];
 269         double* proj = new double[test_mat[INPUT][4].cols * test_mat[INPUT][4].rows];
 270         double* points = new double[N_POINTS*2];
 271
 272         CvMat _camera = cvMat(3,3,CV_64F,cam);
 273         CvMat _distort = cvMat(test_mat[INPUT][2].rows,test_mat[INPUT][2].cols,CV_64F,dist);
 274         CvMat _proj = cvMat(test_mat[INPUT][4].rows,test_mat[INPUT][4].cols,CV_64F,proj);
 275         CvMat _points= cvMat(test_mat[INPUT][0].rows,test_mat[INPUT][0].cols,CV_64FC2, points);
 276
 277         for (int i=0;i<test_mat[INPUT][4].cols * test_mat[INPUT][4].rows;i++)
 278         {
 279                 proj[i] = 0;
 280         }
 281
 282         //Generating points
 283         for (int i=0;i<N_POINTS;i++)
 284         {
 285                 points[2*i] = cvTsRandReal(rng)*img_size.width;
 286                 points[2*i+1] = cvTsRandReal(rng)*img_size.height;
 287         }
 288
 289
 290
 291         //Generating camera matrix
 292         double sz = MAX(img_size.width,img_size.height);
 293         double aspect_ratio = cvTsRandReal(rng)*0.6 + 0.7;
 294         cam[2] = (img_size.width - 1)*0.5 + cvTsRandReal(rng)*10 - 5;
 295         cam[5] = (img_size.height - 1)*0.5 + cvTsRandReal(rng)*10 - 5;
 296         cam[0] = sz/(0.9 - cvTsRandReal(rng)*0.6);
 297         cam[4] = aspect_ratio*cam[0];
 298
 299         //Generating distortion coeffs
 300         dist[0] = cvTsRandReal(rng)*0.06 - 0.03;
 301         dist[1] = cvTsRandReal(rng)*0.06 - 0.03;
 302         if( dist[0]*dist[1] > 0 )
 303                 dist[1] = -dist[1];
 304         if( cvTsRandInt(rng)%4 != 0 )
 305         {
 306                 dist[2] = cvTsRandReal(rng)*0.004 - 0.002;
 307                 dist[3] = cvTsRandReal(rng)*0.004 - 0.002;
 308                 if (dist_size > 4)
 309                         dist[4] = cvTsRandReal(rng)*0.004 - 0.002;
 310         }
 311         else
 312         {
 313                 dist[2] = dist[3] = 0;
 314                 if (dist_size > 4)
 315                         dist[4] = 0;
 316         }
 317
 318         //Generating P matrix (projection)
 319         if ( test_mat[INPUT][4].cols != 4)
 320         {
 321                 proj[8] = 1;
 322                 if (cvTsRandInt(rng)%2 == 0) // use identity new camera matrix
 323                 {
 324                         proj[0] = 1;
 325                         proj[4] = 1;
 326                 }
 327                 else
 328                 {
 329                         proj[0] = cam[0] + (cvTsRandReal(rng) - (double)0.5)*0.2*cam[0]; //10%
 330                         proj[4] = cam[4] + (cvTsRandReal(rng) - (double)0.5)*0.2*cam[4]; //10%
 331                         proj[2] = cam[2] + (cvTsRandReal(rng) - (double)0.5)*0.3*img_size.width; //15%
 332                         proj[5] = cam[5] + (cvTsRandReal(rng) - (double)0.5)*0.3*img_size.height; //15%
 333                 }
 334         }
 335         else
 336         {
 337                 proj[10] = 1;
 338                 proj[0] = cam[0] + (cvTsRandReal(rng) - (double)0.5)*0.2*cam[0]; //10%
 339                 proj[5] = cam[4] + (cvTsRandReal(rng) - (double)0.5)*0.2*cam[4]; //10%
 340                 proj[2] = cam[2] + (cvTsRandReal(rng) - (double)0.5)*0.3*img_size.width; //15%
 341                 proj[6] = cam[5] + (cvTsRandReal(rng) - (double)0.5)*0.3*img_size.height; //15%
 342
 343                 proj[3] = (img_size.height + img_size.width - 1)*0.5 + cvTsRandReal(rng)*10 - 5;
 344                 proj[7] = (img_size.height + img_size.width - 1)*0.5 + cvTsRandReal(rng)*10 - 5;
 345                 proj[11] = (img_size.height + img_size.width - 1)*0.5 + cvTsRandReal(rng)*10 - 5;
 346         }
 347
 348         //Generating R matrix
 349         CvMat* _rot = cvCreateMat(3,3,CV_64F);
 350         CvMat* rotation = cvCreateMat(1,3,CV_64F);
 351         rotation->data.db[0] = CV_PI*(cvTsRandReal(rng) - (double)0.5); // phi
 352         rotation->data.db[1] = CV_PI*(cvTsRandReal(rng) - (double)0.5); // ksi
 353         rotation->data.db[2] = CV_PI*(cvTsRandReal(rng) - (double)0.5); //khi
 354         cvRodrigues2(rotation,_rot);
 355         cvReleaseMat(&rotation);
 356
 357         //copying data
 358         //src_points = &_points;
 359         CvMat* dst = &test_mat[INPUT][0];
 360         cvTsConvert( &_points, dst);
 361         dst = &test_mat[INPUT][1];
 362         cvTsConvert( &_camera, dst);
 363         dst = &test_mat[INPUT][2];
 364         cvTsConvert( &_distort, dst);
 365         dst = &test_mat[INPUT][3];
 366         cvTsConvert( _rot, dst);
 367         dst = &test_mat[INPUT][4];
 368         cvTsConvert( &_proj, dst);
 369
 370         zero_distortion = (cvRandInt(rng)%2) == 0 ? false : true;
 371         zero_new_cam = (cvRandInt(rng)%2) == 0 ? false : true;
 372         zero_R = (cvRandInt(rng)%2) == 0 ? false : true;
 373
 374
 375         cvReleaseMat(&_rot);
 376
 377         if (useCPlus)
 378         {
 379                 CvMat* temp = cvCreateMat(test_mat[INPUT][0].rows,test_mat[INPUT][0].cols,CV_32FC2);
 380                 for (int i=0;i<test_mat[INPUT][0].rows*test_mat[INPUT][0].cols*2;i++)
 381                         temp->data.fl[i] = (float)_points.data.db[i];
 382
 383
 384                 src_points = cv::Mat(temp,true);
 385
 386                 cvReleaseMat(&temp);
 387
 388                 camera_mat = &test_mat[INPUT][1];
 389                 distortion_coeffs = &test_mat[INPUT][2];
 390                 R = &test_mat[INPUT][3];
 391                 P = &test_mat[INPUT][4];
 392         }
 393         delete[] dist;
 394         delete[] proj;
 395         delete[] points;
 396
 397         return code;
 398 }
 399
 400 void CV_UndistortPointsTest::prepare_to_validation(int /*test_case_idx*/)
 401 {
 402         int dist_size = test_mat[INPUT][2].cols > test_mat[INPUT][2].rows ? test_mat[INPUT][2].cols : test_mat[INPUT][2].rows;
 403         double cam[9] = {0,0,0,0,0,0,0,0,1};
 404         double rot[9] = {1,0,0,0,1,0,0,0,1};
 405         double* dist = new double[dist_size ];
 406         double* proj = new double[test_mat[INPUT][4].cols * test_mat[INPUT][4].rows];
 407         double* points = new double[N_POINTS*2];
 408         double* r_points = new double[N_POINTS*2];
 409         //Run reference calculations
 410         CvMat ref_points= cvMat(test_mat[INPUT][0].rows,test_mat[INPUT][0].cols,CV_64FC2,r_points);
 411         CvMat _camera = cvMat(3,3,CV_64F,cam);
 412         CvMat _rot = cvMat(3,3,CV_64F,rot);
 413         CvMat _distort = cvMat(test_mat[INPUT][2].rows,test_mat[INPUT][2].cols,CV_64F,dist);
 414         CvMat _proj = cvMat(test_mat[INPUT][4].rows,test_mat[INPUT][4].cols,CV_64F,proj);
 415         CvMat _points= cvMat(test_mat[TEMP][0].rows,test_mat[TEMP][0].cols,CV_64FC2,points);
 416
 417
 418         cvTsConvert(&test_mat[INPUT][1],&_camera);
 419         cvTsConvert(&test_mat[INPUT][2],&_distort);
 420         cvTsConvert(&test_mat[INPUT][3],&_rot);
 421         cvTsConvert(&test_mat[INPUT][4],&_proj);
 422
 423         if (useCPlus)
 424         {
 425                 if (useDstMat)
 426                 {
 427                         CvMat temp = dst_points_mat;
 428                         for (int i=0;i<N_POINTS*2;i++)
 429                         {
 430                                 points[i] = temp.data.fl[i];
 431                         }
 432                 }
 433
 434                 else
 435                 {
 436
 437                         for (int i=0;i<N_POINTS;i++)
 438                         {
 439                                 points[2*i] = dst_points[i].x;
 440                                 points[2*i+1] = dst_points[i].y;
 441                         }
 442                 }
 443         }
 444         else
 445         {
 446                 cvTsConvert(&test_mat[TEMP][0],&_points);
 447         }
 448
 449         CvMat* input2;
 450         CvMat* input3;
 451         CvMat* input4;
 452         input2 = zero_distortion ? 0 : &_distort;
 453         input3 = zero_R ? 0 : &_rot;
 454         input4 = zero_new_cam ? 0 : &_proj;
 455         cvTsDistortPoints(&_points,&ref_points,&_camera,input2,input3,input4);
 456
 457         CvMat* dst = &test_mat[REF_OUTPUT][0];
 458         cvTsConvert(&ref_points,dst);
 459
 460         cvCopy(&test_mat[INPUT][0],&test_mat[OUTPUT][0]);
 461
 462         delete[] dist;
 463         delete[] proj;
 464         delete[] points;
 465         delete[] r_points;
 466 }
 467
 468 void CV_UndistortPointsTest::run_func()
 469 {
 470
 471         if (useCPlus)
 472         {
 473                 cv::Mat input2,input3,input4;
 474                 input2 = zero_distortion ? cv::Mat() : cv::Mat(&test_mat[INPUT][2]);
 475                 input3 = zero_R ? cv::Mat() : cv::Mat(&test_mat[INPUT][3]);
 476                 input4 = zero_new_cam ? cv::Mat() : cv::Mat(&test_mat[INPUT][4]);
 477
 478                 if (useDstMat)
 479                 {
 480                         //cv::undistortPoints(src_points,dst_points_mat,camera_mat,distortion_coeffs,R,P);
 481                         cv::undistortPoints(src_points,dst_points_mat,camera_mat,input2,input3,input4);
 482                 }
 483                 else
 484                 {
 485                         //cv::undistortPoints(src_points,dst_points,camera_mat,distortion_coeffs,R,P);
 486                         cv::undistortPoints(src_points,dst_points,camera_mat,input2,input3,input4);
 487                 }
 488         }
 489         else
 490         {
 491                 CvMat* input2;
 492                 CvMat* input3;
 493                 CvMat* input4;
 494                 input2 = zero_distortion ? 0 : &test_mat[INPUT][2];
 495                 input3 = zero_R ? 0 : &test_mat[INPUT][3];
 496                 input4 = zero_new_cam ? 0 : &test_mat[INPUT][4];
 497                 cvUndistortPoints(&test_mat[INPUT][0],&test_mat[TEMP][0],&test_mat[INPUT][1],input2,input3,input4);
 498         }
 499
 500 }
 501
 502 void CV_UndistortPointsTest::cvTsDistortPoints(const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
 503                                                                                            const CvMat* _distCoeffs,
 504                                                                                            const CvMat* _R, const CvMat* _P)
 505 {
 506         double a[9];
 507
 508         CvMat* __P;
 509         if ((!_P)||(_P->cols == 3))
 510                 __P = cvCreateMat(3,3,CV_64F);
 511         else
 512                 __P = cvCreateMat(3,4,CV_64F);
 513         if (_P)
 514         {
 515                 cvTsConvert(_P,__P);
 516         }
 517         else
 518         {
 519                 cvZero(__P);
 520                 __P->data.db[0] = 1;
 521                 __P->data.db[4] = 1;
 522                 __P->data.db[8] = 1;
 523         }
 524         CvMat* __R = cvCreateMat(3,3,CV_64F);;
 525         if (_R)
 526         {
 527                 cvCopy(_R,__R);
 528         }
 529         else
 530         {
 531                 cvZero(__R);
 532                 __R->data.db[0] = 1;
 533                 __R->data.db[4] = 1;
 534                 __R->data.db[8] = 1;
 535         }
 536         for (int i=0;i<N_POINTS;i++)
 537         {
 538                 int movement = __P->cols > 3 ? 1 : 0;
 539                 double x = (_src->data.db[2*i]-__P->data.db[2])/__P->data.db[0];
 540                 double y = (_src->data.db[2*i+1]-__P->data.db[5+movement])/__P->data.db[4+movement];
 541                 CvMat inverse = cvMat(3,3,CV_64F,a);
 542                 cvInvert(__R,&inverse);
 543                 double w1 = x*inverse.data.db[6]+y*inverse.data.db[7]+inverse.data.db[8];
 544                 double _x = (x*inverse.data.db[0]+y*inverse.data.db[1]+inverse.data.db[2])/w1;
 545                 double _y = (x*inverse.data.db[3]+y*inverse.data.db[4]+inverse.data.db[5])/w1;
 546
 547                 //Distortions
 548
 549                 double __x = _x;
 550                 double __y = _y;
 551                 if (_distCoeffs)
 552                 {
 553                         double r2 = _x*_x+_y*_y;
 554
 555                         __x = _x*(1+_distCoeffs->data.db[0]*r2+_distCoeffs->data.db[1]*r2*r2)+
 556                                 2*_distCoeffs->data.db[2]*_x*_y+_distCoeffs->data.db[3]*(r2+2*_x*_x);
 557                         __y = _y*(1+_distCoeffs->data.db[0]*r2+_distCoeffs->data.db[1]*r2*r2)+
 558                                 2*_distCoeffs->data.db[3]*_x*_y+_distCoeffs->data.db[2]*(r2+2*_y*_y);
 559                         if ((_distCoeffs->cols > 4) || (_distCoeffs->rows > 4))
 560                         {
 561                                 __x+=_x*_distCoeffs->data.db[4]*r2*r2*r2;
 562                                 __y+=_y*_distCoeffs->data.db[4]*r2*r2*r2;
 563                         }
 564                 }
 565
 566
 567                 _dst->data.db[2*i] = __x*_cameraMatrix->data.db[0]+_cameraMatrix->data.db[2];
 568                 _dst->data.db[2*i+1] = __y*_cameraMatrix->data.db[4]+_cameraMatrix->data.db[5];
 569
 570         }
 571
 572         cvReleaseMat(&__R);
 573         cvReleaseMat(&__P);
 574
 575 }
 576
 577 double CV_UndistortPointsTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ )
 578 {
 579         return 5e-2;
 580 }
 581
 582 CV_UndistortPointsTest undistort_points_test;
 583
 584 //------------------------------------------------------
 585
 586 class CV_InitUndistortRectifyMapTest : public CvArrTest
 587 {
 588 public:
 589         CV_InitUndistortRectifyMapTest();
 590 protected:
 591         int prepare_test_case (int test_case_idx);
 592         void prepare_to_validation( int test_case_idx );
 593         void get_test_array_types_and_sizes( int test_case_idx, CvSize** sizes, int** types );
 594         double get_success_error_level( int test_case_idx, int i, int j );
 595         void run_func();
 596
 597 private:
 598         bool useCPlus;
 599         static const int N_POINTS = 100;
 600         static const int MAX_X = 2048;
 601         static const int MAX_Y = 2048;
 602         bool zero_new_cam;
 603         bool zero_distortion;
 604         bool zero_R;
 605
 606
 607         cv::Size img_size;
 608
 609         cv::Mat camera_mat;
 610         cv::Mat R;
 611         cv::Mat new_camera_mat;
 612         cv::Mat distortion_coeffs;
 613         cv::Mat mapx;
 614         cv::Mat mapy;
 615         CvMat* _mapx;
 616         CvMat* _mapy;
 617         int mat_type;
 618 };
 619
 620 CV_InitUndistortRectifyMapTest::CV_InitUndistortRectifyMapTest() : CvArrTest("undistort-undistort_rectify_map","cvInitUndistortRectifyMap")
 621 {
 622         test_array[INPUT].push(NULL); // test points matrix
 623         test_array[INPUT].push(NULL); // camera matrix
 624         test_array[INPUT].push(NULL); // distortion coeffs
 625         test_array[INPUT].push(NULL); // R matrix
 626         test_array[INPUT].push(NULL); // new camera matrix
 627         test_array[OUTPUT].push(NULL); // distorted dst points
 628         test_array[REF_OUTPUT].push(NULL);
 629 }
 630
 631 void CV_InitUndistortRectifyMapTest::get_test_array_types_and_sizes( int test_case_idx, CvSize** sizes, int** types )
 632 {
 633         CvArrTest::get_test_array_types_and_sizes(test_case_idx,sizes,types);
 634         CvRNG* rng = ts->get_rng();
 635         useCPlus = ((cvTsRandInt(rng) % 2)!=0);
 636         //useCPlus = 0;
 637         types[INPUT][0] = types[OUTPUT][0] = types[REF_OUTPUT][0] = CV_64FC2;
 638
 639         types[INPUT][1] = cvTsRandInt(rng)%2 ? CV_64F : CV_32F;
 640         types[INPUT][2] = cvTsRandInt(rng)%2 ? CV_64F : CV_32F;
 641         types[INPUT][3] = cvTsRandInt(rng)%2 ? CV_64F : CV_32F;
 642         types[INPUT][4] = cvTsRandInt(rng)%2 ? CV_64F : CV_32F;
 643
 644         sizes[INPUT][0] = sizes[OUTPUT][0] = sizes[REF_OUTPUT][0] = cvSize(N_POINTS,1);
 645         sizes[INPUT][1] = sizes[INPUT][3] = cvSize(3,3);
 646         sizes[INPUT][4] = cvSize(3,3);
 647
 648         if (cvTsRandInt(rng)%2)
 649         {
 650                 if (cvTsRandInt(rng)%2)
 651                 {
 652                         sizes[INPUT][2] = cvSize(1,4);
 653                 }
 654                 else
 655                 {
 656                         sizes[INPUT][2] = cvSize(1,5);
 657                 }
 658         }
 659         else
 660         {
 661                 if (cvTsRandInt(rng)%2)
 662                 {
 663                         sizes[INPUT][2] = cvSize(4,1);
 664                 }
 665                 else
 666                 {
 667                         sizes[INPUT][2] = cvSize(5,1);
 668                 }
 669         }
 670 }
 671
 672 int CV_InitUndistortRectifyMapTest::prepare_test_case(int test_case_idx)
 673 {
 674         CvRNG* rng = ts->get_rng();
 675         int code = CvArrTest::prepare_test_case( test_case_idx );
 676
 677         if (code <= 0)
 678                 return code;
 679
 680         img_size.width = cvTsRandInt(rng) % MAX_X + 1;
 681         img_size.height = cvTsRandInt(rng) % MAX_Y + 1;
 682
 683         if (useCPlus)
 684         {
 685                 mat_type = (cvTsRandInt(rng) % 2) == 0 ? CV_32FC1 : CV_16SC2;
 686                 if ((cvTsRandInt(rng) % 4) == 0)
 687                         mat_type = -1;
 688                 if ((cvTsRandInt(rng) % 4) == 0)
 689                         mat_type = CV_32FC2;
 690                 _mapx = 0;
 691                 _mapy = 0;
 692         }
 693         else
 694         {
 695                 int typex = (cvTsRandInt(rng) % 2) == 0 ? CV_32FC1 : CV_16SC2;
 696                 //typex = CV_32FC1; ///!!!!!!!!!!!!!!!!
 697                 int typey = (typex == CV_32FC1) ? CV_32FC1 : CV_16UC1;
 698
 699                 _mapx = cvCreateMat(img_size.height,img_size.width,typex);
 700                 _mapy = cvCreateMat(img_size.height,img_size.width,typey);
 701
 702
 703         }
 704
 705         int dist_size = test_mat[INPUT][2].cols > test_mat[INPUT][2].rows ? test_mat[INPUT][2].cols : test_mat[INPUT][2].rows;
 706         double cam[9] = {0,0,0,0,0,0,0,0,1};
 707         double* dist = new double[dist_size ];
 708         double* new_cam = new double[test_mat[INPUT][4].cols * test_mat[INPUT][4].rows];
 709         double* points = new double[N_POINTS*2];
 710
 711         CvMat _camera = cvMat(3,3,CV_64F,cam);
 712         CvMat _distort = cvMat(test_mat[INPUT][2].rows,test_mat[INPUT][2].cols,CV_64F,dist);
 713         CvMat _new_cam = cvMat(test_mat[INPUT][4].rows,test_mat[INPUT][4].cols,CV_64F,new_cam);
 714         CvMat _points= cvMat(test_mat[INPUT][0].rows,test_mat[INPUT][0].cols,CV_64FC2, points);
 715
 716         for (int i=0;i<test_mat[INPUT][4].cols * test_mat[INPUT][4].rows;i++)
 717         {
 718                 new_cam[i] = 0;
 719         }
 720
 721         //Generating points
 722         for (int i=0;i<N_POINTS;i++)
 723         {
 724                 points[2*i] = cvTsRandReal(rng)*img_size.width;
 725                 points[2*i+1] = cvTsRandReal(rng)*img_size.height;
 726         }
 727
 728
 729
 730         //Generating camera matrix
 731         double sz = MAX(img_size.width,img_size.height);
 732         double aspect_ratio = cvTsRandReal(rng)*0.6 + 0.7;
 733         cam[2] = (img_size.width - 1)*0.5 + cvTsRandReal(rng)*10 - 5;
 734         cam[5] = (img_size.height - 1)*0.5 + cvTsRandReal(rng)*10 - 5;
 735         cam[0] = sz/(0.9 - cvTsRandReal(rng)*0.6);
 736         cam[4] = aspect_ratio*cam[0];
 737
 738         //Generating distortion coeffs
 739         dist[0] = cvTsRandReal(rng)*0.06 - 0.03;
 740         dist[1] = cvTsRandReal(rng)*0.06 - 0.03;
 741         if( dist[0]*dist[1] > 0 )
 742                 dist[1] = -dist[1];
 743         if( cvTsRandInt(rng)%4 != 0 )
 744         {
 745                 dist[2] = cvTsRandReal(rng)*0.004 - 0.002;
 746                 dist[3] = cvTsRandReal(rng)*0.004 - 0.002;
 747                 if (dist_size > 4)
 748                         dist[4] = cvTsRandReal(rng)*0.004 - 0.002;
 749         }
 750         else
 751         {
 752                 dist[2] = dist[3] = 0;
 753                 if (dist_size > 4)
 754                         dist[4] = 0;
 755         }
 756
 757         //Generating new camera matrix
 758
 759         new_cam[8] = 1;
 760
 761         //new_cam[0] = cam[0];
 762         //new_cam[4] = cam[4];
 763         //new_cam[2] = cam[2];
 764         //new_cam[5] = cam[5];
 765
 766         new_cam[0] = cam[0] + (cvTsRandReal(rng) - (double)0.5)*0.2*cam[0]; //10%
 767         new_cam[4] = cam[4] + (cvTsRandReal(rng) - (double)0.5)*0.2*cam[4]; //10%
 768         new_cam[2] = cam[2] + (cvTsRandReal(rng) - (double)0.5)*0.3*img_size.width; //15%
 769         new_cam[5] = cam[5] + (cvTsRandReal(rng) - (double)0.5)*0.3*img_size.height; //15%
 770
 771
 772         //Generating R matrix
 773         CvMat* _rot = cvCreateMat(3,3,CV_64F);
 774         CvMat* rotation = cvCreateMat(1,3,CV_64F);
 775         rotation->data.db[0] = CV_PI/8*(cvTsRandReal(rng) - (double)0.5); // phi
 776         rotation->data.db[1] = CV_PI/8*(cvTsRandReal(rng) - (double)0.5); // ksi
 777         rotation->data.db[2] = CV_PI/3*(cvTsRandReal(rng) - (double)0.5); //khi
 778         cvRodrigues2(rotation,_rot);
 779         cvReleaseMat(&rotation);
 780
 781         //cvSetIdentity(_rot);
 782         //copying data
 783         CvMat* dst = &test_mat[INPUT][0];
 784         cvTsConvert( &_points, dst);
 785         dst = &test_mat[INPUT][1];
 786         cvTsConvert( &_camera, dst);
 787         dst = &test_mat[INPUT][2];
 788         cvTsConvert( &_distort, dst);
 789         dst = &test_mat[INPUT][3];
 790         cvTsConvert( _rot, dst);
 791         dst = &test_mat[INPUT][4];
 792         cvTsConvert( &_new_cam, dst);
 793
 794         zero_distortion = (cvRandInt(rng)%2) == 0 ? false : true;
 795         zero_new_cam = (cvRandInt(rng)%2) == 0 ? false : true;
 796         zero_R = (cvRandInt(rng)%2) == 0 ? false : true;
 797
 798         cvReleaseMat(&_rot);
 799
 800         if (useCPlus)
 801         {
 802                 camera_mat = &test_mat[INPUT][1];
 803                 distortion_coeffs = &test_mat[INPUT][2];
 804                 R = &test_mat[INPUT][3];
 805                 new_camera_mat = &test_mat[INPUT][4];
 806         }
 807         delete[] dist;
 808         delete[] new_cam;
 809         delete[] points;
 810
 811         return code;
 812 }
 813
 814 void CV_InitUndistortRectifyMapTest::prepare_to_validation(int/* test_case_idx*/)
 815 {
 816         int dist_size = test_mat[INPUT][2].cols > test_mat[INPUT][2].rows ? test_mat[INPUT][2].cols : test_mat[INPUT][2].rows;
 817         double cam[9] = {0,0,0,0,0,0,0,0,1};
 818         double rot[9] = {1,0,0,0,1,0,0,0,1};
 819         double* dist = new double[dist_size ];
 820         double* new_cam = new double[test_mat[INPUT][4].cols * test_mat[INPUT][4].rows];
 821         double* points = new double[N_POINTS*2];
 822         double* r_points = new double[N_POINTS*2];
 823         //Run reference calculations
 824         CvMat ref_points= cvMat(test_mat[INPUT][0].rows,test_mat[INPUT][0].cols,CV_64FC2,r_points);
 825         CvMat _camera = cvMat(3,3,CV_64F,cam);
 826         CvMat _rot = cvMat(3,3,CV_64F,rot);
 827         CvMat _distort = cvMat(test_mat[INPUT][2].rows,test_mat[INPUT][2].cols,CV_64F,dist);
 828         CvMat _new_cam = cvMat(test_mat[INPUT][4].rows,test_mat[INPUT][4].cols,CV_64F,new_cam);
 829         CvMat _points= cvMat(test_mat[INPUT][0].rows,test_mat[INPUT][0].cols,CV_64FC2,points);
 830
 831         cvTsConvert(&test_mat[INPUT][1],&_camera);
 832         cvTsConvert(&test_mat[INPUT][2],&_distort);
 833         cvTsConvert(&test_mat[INPUT][3],&_rot);
 834         cvTsConvert(&test_mat[INPUT][4],&_new_cam);
 835
 836         //Applying precalculated undistort rectify map
 837         if (!useCPlus)
 838         {
 839                 mapx = cv::Mat(_mapx);
 840                 mapy = cv::Mat(_mapy);
 841         }
 842         cv::Mat map1,map2;
 843         cv::convertMaps(mapx,mapy,map1,map2,CV_32FC1);
 844         CvMat _map1 = map1;
 845         CvMat _map2 = map2;
 846         for (int i=0;i<N_POINTS;i++)
 847         {
 848                 double u = test_mat[INPUT][0].data.db[2*i];
 849                 double v = test_mat[INPUT][0].data.db[2*i+1];
 850                 _points.data.db[2*i] = (double)_map1.data.fl[(int)v*_map1.cols+(int)u];
 851                 _points.data.db[2*i+1] = (double)_map2.data.fl[(int)v*_map2.cols+(int)u];
 852         }
 853
 854         //---
 855
 856         cvUndistortPoints(&_points,&ref_points,&_camera,
 857                 zero_distortion ? 0 : &_distort, zero_R ? 0 : &_rot, zero_new_cam ? &_camera : &_new_cam);
 858         //cvTsDistortPoints(&_points,&ref_points,&_camera,&_distort,&_rot,&_new_cam);
 859         CvMat* dst = &test_mat[REF_OUTPUT][0];
 860         cvTsConvert(&ref_points,dst);
 861
 862         cvCopy(&test_mat[INPUT][0],&test_mat[OUTPUT][0]);
 863
 864         delete[] dist;
 865         delete[] new_cam;
 866         delete[] points;
 867         delete[] r_points;
 868         if (_mapx)
 869         {
 870                 cvReleaseMat(&_mapx);
 871                 _mapx = 0;
 872         }
 873         if (_mapy)
 874         {
 875                 cvReleaseMat(&_mapy);
 876                 _mapy = 0;
 877         }
 878 }
 879
 880 void CV_InitUndistortRectifyMapTest::run_func()
 881 {
 882         if (useCPlus)
 883         {
 884                 cv::Mat input2,input3,input4;
 885                 input2 = zero_distortion ? cv::Mat() : cv::Mat(&test_mat[INPUT][2]);
 886                 input3 = zero_R ? cv::Mat() : cv::Mat(&test_mat[INPUT][3]);
 887                 input4 = zero_new_cam ? cv::Mat() : cv::Mat(&test_mat[INPUT][4]);
 888                 cv::initUndistortRectifyMap(camera_mat,input2,input3,input4,img_size,mat_type,mapx,mapy);
 889         }
 890         else
 891         {
 892                 CvMat* input2;
 893                 CvMat* input3;
 894                 CvMat* input4;
 895                 input2 = zero_distortion ? 0 : &test_mat[INPUT][2];
 896                 input3 = zero_R ? 0 : &test_mat[INPUT][3];
 897                 input4 = zero_new_cam ? 0 : &test_mat[INPUT][4];
 898                 cvInitUndistortRectifyMap(&test_mat[INPUT][1],input2,input3,input4,_mapx,_mapy);
 899         }
 900 }
 901
 902 double CV_InitUndistortRectifyMapTest::get_success_error_level( int /*test_case_idx*/, int /*i*/, int /*j*/ )
 903 {
 904         return 8;
 905 }
 906
 907 CV_InitUndistortRectifyMapTest init_undistort_rectify_map_test;