modules/legacy/src/oneway.cpp

   1 /*
   2  *  cvoneway.cpp
   3  *  one_way_sample
   4  *
   5  *  Created by Victor  Eruhimov on 3/23/10.
   6  *  Copyright 2010 Argus Corp. All rights reserved.
   7  *
   8  */
   9
  10 #include "precomp.hpp"
  11 #include "opencv2/opencv_modules.hpp"
  12 #ifdef HAVE_OPENCV_HIGHGUI
  13 #  include "opencv2/highgui.hpp"
  14 #endif
  15 #include <stdio.h>
  16
  17 namespace cv{
  18
  19     inline int round(float value)
  20     {
  21         if(value > 0)
  22         {
  23             return int(value + 0.5f);
  24         }
  25         else
  26         {
  27             return int(value - 0.5f);
  28         }
  29     }
  30
  31     inline CvRect resize_rect(CvRect rect, float alpha)
  32     {
  33         return cvRect(rect.x + round((float)(0.5*(1 - alpha)*rect.width)), rect.y + round((float)(0.5*(1 - alpha)*rect.height)),
  34                       round(rect.width*alpha), round(rect.height*alpha));
  35     }
  36
  37     CvMat* ConvertImageToMatrix(IplImage* patch);
  38
  39     class CvCameraPose
  40         {
  41         public:
  42             CvCameraPose()
  43             {
  44                 m_rotation = cvCreateMat(1, 3, CV_32FC1);
  45                 m_translation = cvCreateMat(1, 3, CV_32FC1);
  46             };
  47
  48             ~CvCameraPose()
  49             {
  50                 cvReleaseMat(&m_rotation);
  51                 cvReleaseMat(&m_translation);
  52             };
  53
  54             void SetPose(CvMat* rotation, CvMat* translation)
  55             {
  56                 cvCopy(rotation, m_rotation);
  57                 cvCopy(translation, m_translation);
  58             };
  59
  60             CvMat* GetRotation() {return m_rotation;}
  61             CvMat* GetTranslation() {return m_translation;}
  62
  63         protected:
  64             CvMat* m_rotation;
  65             CvMat* m_translation;
  66         };
  67
  68     // AffineTransformPatch: generates an affine transformed image patch.
  69     // - src: source image (roi is supported)
  70     // - dst: output image. ROI of dst image should be 2 times smaller than ROI of src.
  71     // - pose: parameters of an affine transformation
  72     void AffineTransformPatch(IplImage* src, IplImage* dst, CvAffinePose pose);
  73
  74     // GenerateAffineTransformFromPose: generates an affine transformation matrix from CvAffinePose instance
  75     // - size: the size of image patch
  76     // - pose: affine transformation
  77     // - transform: 2x3 transformation matrix
  78     void GenerateAffineTransformFromPose(CvSize size, CvAffinePose pose, CvMat* transform);
  79
  80     // Generates a random affine pose
  81     CvAffinePose GenRandomAffinePose();
  82
  83
  84     const static int num_mean_components = 500;
  85     const static float noise_intensity = 0.15f;
  86
  87
  88     static inline CvPoint rect_center(CvRect rect)
  89     {
  90         return cvPoint(rect.x + rect.width/2, rect.y + rect.height/2);
  91     }
  92
  93     // static void homography_transform(IplImage* frontal, IplImage* result, CvMat* homography)
  94     // {
  95     //     cvWarpPerspective(frontal, result, homography);
  96     // }
  97
  98     static CvAffinePose perturbate_pose(CvAffinePose pose, float noise)
  99     {
 100         // perturbate the matrix
 101         float noise_mult_factor = 1 + (0.5f - float(rand())/RAND_MAX)*noise;
 102         float noise_add_factor = noise_mult_factor - 1;
 103
 104         CvAffinePose pose_pert = pose;
 105         pose_pert.phi += noise_add_factor;
 106         pose_pert.theta += noise_mult_factor;
 107         pose_pert.lambda1 *= noise_mult_factor;
 108         pose_pert.lambda2 *= noise_mult_factor;
 109
 110         return pose_pert;
 111     }
 112
 113     static void generate_mean_patch(IplImage* frontal, IplImage* result, CvAffinePose pose, int pose_count, float noise)
 114     {
 115         IplImage* sum = cvCreateImage(cvSize(result->width, result->height), IPL_DEPTH_32F, 1);
 116         IplImage* workspace = cvCloneImage(result);
 117         IplImage* workspace_float = cvCloneImage(sum);
 118
 119         cvSetZero(sum);
 120         for(int i = 0; i < pose_count; i++)
 121         {
 122             CvAffinePose pose_pert = perturbate_pose(pose, noise);
 123
 124             AffineTransformPatch(frontal, workspace, pose_pert);
 125             cvConvertScale(workspace, workspace_float);
 126             cvAdd(sum, workspace_float, sum);
 127         }
 128
 129         cvConvertScale(sum, result, 1.0f/pose_count);
 130
 131         cvReleaseImage(&workspace);
 132         cvReleaseImage(&sum);
 133         cvReleaseImage(&workspace_float);
 134     }
 135
 136     // static void generate_mean_patch_fast(IplImage* /*frontal*/, IplImage* /*result*/, CvAffinePose /*pose*/,
 137     //                               CvMat* /*pca_hr_avg*/, CvMat* /*pca_hr_eigenvectors*/, const OneWayDescriptor* /*pca_descriptors*/)
 138     // {
 139     //     /*for(int i = 0; i < pca_hr_eigenvectors->cols; i++)
 140     //     {
 141
 142     //     }*/
 143     // }
 144
 145     void readPCAFeatures(const char *filename, CvMat** avg, CvMat** eigenvectors, const char *postfix = "");
 146     void readPCAFeatures(const FileNode &fn, CvMat** avg, CvMat** eigenvectors, const char* postfix = "");
 147     void savePCAFeatures(FileStorage &fs, const char* postfix, CvMat* avg, CvMat* eigenvectors);
 148     void calcPCAFeatures(std::vector<IplImage*>& patches, FileStorage &fs, const char* postfix, CvMat** avg,
 149                          CvMat** eigenvectors);
 150     void loadPCAFeatures(const char* path, const char* images_list, std::vector<IplImage*>& patches, CvSize patch_size);
 151     void generatePCAFeatures(const char* path, const char* img_filename, FileStorage& fs, const char* postfix,
 152                              CvSize patch_size, CvMat** avg, CvMat** eigenvectors);
 153
 154     void eigenvector2image(CvMat* eigenvector, IplImage* img);
 155
 156     void FindOneWayDescriptor(int desc_count, const OneWayDescriptor* descriptors, IplImage* patch, int& desc_idx, int& pose_idx, float& distance,
 157                               CvMat* avg = 0, CvMat* eigenvalues = 0);
 158
 159     void FindOneWayDescriptor(int desc_count, const OneWayDescriptor* descriptors, IplImage* patch, int n,
 160                               std::vector<int>& desc_idxs, std::vector<int>&  pose_idxs, std::vector<float>& distances,
 161                               CvMat* avg = 0, CvMat* eigenvalues = 0);
 162
 163     void FindOneWayDescriptor(cv::flann::Index* m_pca_descriptors_tree, CvSize patch_size, int m_pca_dim_low, int m_pose_count, IplImage* patch, int& desc_idx, int& pose_idx, float& distance,
 164                               CvMat* avg = 0, CvMat* eigenvalues = 0);
 165
 166     void FindOneWayDescriptorEx(int desc_count, const OneWayDescriptor* descriptors, IplImage* patch,
 167                                 float scale_min, float scale_max, float scale_step,
 168                                 int& desc_idx, int& pose_idx, float& distance, float& scale,
 169                                 CvMat* avg, CvMat* eigenvectors);
 170
 171     void FindOneWayDescriptorEx(int desc_count, const OneWayDescriptor* descriptors, IplImage* patch,
 172                                 float scale_min, float scale_max, float scale_step,
 173                                 int n, std::vector<int>& desc_idxs, std::vector<int>& pose_idxs,
 174                                 std::vector<float>& distances, std::vector<float>& scales,
 175                                 CvMat* avg, CvMat* eigenvectors);
 176
 177     void FindOneWayDescriptorEx(cv::flann::Index* m_pca_descriptors_tree, CvSize patch_size, int m_pca_dim_low, int m_pose_count, IplImage* patch,
 178                                 float scale_min, float scale_max, float scale_step,
 179                                 int& desc_idx, int& pose_idx, float& distance, float& scale,
 180                                 CvMat* avg, CvMat* eigenvectors);
 181
 182     inline CvRect fit_rect_roi_fixedsize(CvRect rect, CvRect roi)
 183     {
 184         CvRect fit = rect;
 185         fit.x = MAX(fit.x, roi.x);
 186         fit.y = MAX(fit.y, roi.y);
 187         fit.x = MIN(fit.x, roi.x + roi.width - fit.width - 1);
 188         fit.y = MIN(fit.y, roi.y + roi.height - fit.height - 1);
 189         return(fit);
 190     }
 191
 192     inline CvRect fit_rect_fixedsize(CvRect rect, IplImage* img)
 193     {
 194         CvRect roi = cvGetImageROI(img);
 195         return fit_rect_roi_fixedsize(rect, roi);
 196     }
 197
 198     OneWayDescriptor::OneWayDescriptor()
 199     {
 200         m_pose_count = 0;
 201         m_samples = 0;
 202         m_input_patch = 0;
 203         m_train_patch = 0;
 204         m_pca_coeffs = 0;
 205         m_affine_poses = 0;
 206         m_transforms = 0;
 207         m_pca_dim_low = 100;
 208         m_pca_dim_high = 100;
 209     }
 210
 211     OneWayDescriptor::~OneWayDescriptor()
 212     {
 213         if(m_pose_count)
 214         {
 215             for(int i = 0; i < m_pose_count; i++)
 216             {
 217                 cvReleaseImage(&m_samples[i]);
 218                 cvReleaseMat(&m_pca_coeffs[i]);
 219             }
 220             cvReleaseImage(&m_input_patch);
 221             cvReleaseImage(&m_train_patch);
 222             delete []m_samples;
 223             delete []m_pca_coeffs;
 224
 225             if(!m_transforms)
 226             {
 227                 delete []m_affine_poses;
 228             }
 229         }
 230     }
 231
 232     void OneWayDescriptor::Allocate(int pose_count, CvSize size, int nChannels)
 233     {
 234         m_pose_count = pose_count;
 235         m_samples = new IplImage* [m_pose_count];
 236         m_pca_coeffs = new CvMat* [m_pose_count];
 237         m_patch_size = cvSize(size.width/2, size.height/2);
 238
 239         if(!m_transforms)
 240         {
 241             m_affine_poses = new CvAffinePose[m_pose_count];
 242         }
 243
 244         int length = m_pca_dim_low;//roi.width*roi.height;
 245         for(int i = 0; i < m_pose_count; i++)
 246         {
 247             m_samples[i] = cvCreateImage(cvSize(size.width/2, size.height/2), IPL_DEPTH_32F, nChannels);
 248             m_pca_coeffs[i] = cvCreateMat(1, length, CV_32FC1);
 249         }
 250
 251         m_input_patch = cvCreateImage(GetPatchSize(), IPL_DEPTH_8U, 1);
 252         m_train_patch = cvCreateImage(GetInputPatchSize(), IPL_DEPTH_8U, 1);
 253     }
 254
 255     // static void cvmSet2DPoint(CvMat* matrix, int row, int col, CvPoint2D32f point)
 256     // {
 257     //     cvmSet(matrix, row, col, point.x);
 258     //     cvmSet(matrix, row, col + 1, point.y);
 259     // }
 260
 261     // static void cvmSet3DPoint(CvMat* matrix, int row, int col, CvPoint3D32f point)
 262     // {
 263     //     cvmSet(matrix, row, col, point.x);
 264     //     cvmSet(matrix, row, col + 1, point.y);
 265     //     cvmSet(matrix, row, col + 2, point.z);
 266     // }
 267
 268     CvAffinePose GenRandomAffinePose()
 269     {
 270         const float scale_min = 0.8f;
 271         const float scale_max = 1.2f;
 272         CvAffinePose pose;
 273         pose.theta = float(rand())/RAND_MAX*120 - 60;
 274         pose.phi = float(rand())/RAND_MAX*360;
 275         pose.lambda1 = scale_min + float(rand())/RAND_MAX*(scale_max - scale_min);
 276         pose.lambda2 = scale_min + float(rand())/RAND_MAX*(scale_max - scale_min);
 277
 278         return pose;
 279     }
 280
 281     void GenerateAffineTransformFromPose(CvSize size, CvAffinePose pose, CvMat* transform)
 282     {
 283         CvMat* temp = cvCreateMat(3, 3, CV_32FC1);
 284         CvMat* final = cvCreateMat(3, 3, CV_32FC1);
 285         cvmSet(temp, 2, 0, 0.0f);
 286         cvmSet(temp, 2, 1, 0.0f);
 287         cvmSet(temp, 2, 2, 1.0f);
 288
 289         CvMat rotation;
 290         cvGetSubRect(temp, &rotation, cvRect(0, 0, 3, 2));
 291
 292         cv2DRotationMatrix(cvPoint2D32f(size.width/2, size.height/2), pose.phi, 1.0, &rotation);
 293         cvCopy(temp, final);
 294
 295         cvmSet(temp, 0, 0, pose.lambda1);
 296         cvmSet(temp, 0, 1, 0.0f);
 297         cvmSet(temp, 1, 0, 0.0f);
 298         cvmSet(temp, 1, 1, pose.lambda2);
 299         cvmSet(temp, 0, 2, size.width/2*(1 - pose.lambda1));
 300         cvmSet(temp, 1, 2, size.height/2*(1 - pose.lambda2));
 301         cvMatMul(temp, final, final);
 302
 303         cv2DRotationMatrix(cvPoint2D32f(size.width/2, size.height/2), pose.theta - pose.phi, 1.0, &rotation);
 304         cvMatMul(temp, final, final);
 305
 306         cvGetSubRect(final, &rotation, cvRect(0, 0, 3, 2));
 307         cvCopy(&rotation, transform);
 308
 309         cvReleaseMat(&temp);
 310         cvReleaseMat(&final);
 311     }
 312
 313     void AffineTransformPatch(IplImage* src, IplImage* dst, CvAffinePose pose)
 314     {
 315         CvRect src_large_roi = cvGetImageROI(src);
 316
 317         IplImage* temp = cvCreateImage(cvSize(src_large_roi.width, src_large_roi.height), IPL_DEPTH_32F, src->nChannels);
 318         cvSetZero(temp);
 319         IplImage* temp2 = cvCloneImage(temp);
 320         CvMat* rotation_phi = cvCreateMat(2, 3, CV_32FC1);
 321
 322         CvSize new_size = cvSize(cvRound(temp->width*pose.lambda1), cvRound(temp->height*pose.lambda2));
 323         IplImage* temp3 = cvCreateImage(new_size, IPL_DEPTH_32F, src->nChannels);
 324
 325         cvConvertScale(src, temp);
 326         cvResetImageROI(temp);
 327
 328
 329         cv2DRotationMatrix(cvPoint2D32f(temp->width/2, temp->height/2), pose.phi, 1.0, rotation_phi);
 330         cvWarpAffine(temp, temp2, rotation_phi);
 331
 332         cvSetZero(temp);
 333
 334         cvResize(temp2, temp3);
 335
 336         cv2DRotationMatrix(cvPoint2D32f(temp3->width/2, temp3->height/2), pose.theta - pose.phi, 1.0, rotation_phi);
 337         cvWarpAffine(temp3, temp, rotation_phi);
 338
 339         cvSetImageROI(temp, cvRect(temp->width/2 - src_large_roi.width/4, temp->height/2 - src_large_roi.height/4,
 340                                    src_large_roi.width/2, src_large_roi.height/2));
 341         cvConvertScale(temp, dst);
 342         cvReleaseMat(&rotation_phi);
 343
 344         cvReleaseImage(&temp3);
 345         cvReleaseImage(&temp2);
 346         cvReleaseImage(&temp);
 347     }
 348
 349     void OneWayDescriptor::GenerateSamples(int pose_count, IplImage* frontal, int norm)
 350     {
 351         /*    if(m_transforms)
 352          {
 353          GenerateSamplesWithTransforms(pose_count, frontal);
 354          return;
 355          }
 356          */
 357         CvRect roi = cvGetImageROI(frontal);
 358         IplImage* patch_8u = cvCreateImage(cvSize(roi.width/2, roi.height/2), frontal->depth, frontal->nChannels);
 359         for(int i = 0; i < pose_count; i++)
 360         {
 361             if(!m_transforms)
 362             {
 363                 m_affine_poses[i] = GenRandomAffinePose();
 364             }
 365             //AffineTransformPatch(frontal, patch_8u, m_affine_poses[i]);
 366             generate_mean_patch(frontal, patch_8u, m_affine_poses[i], num_mean_components, noise_intensity);
 367
 368             double scale = 1.0f;
 369             if(norm)
 370             {
 371                 double sum = cvSum(patch_8u).val[0];
 372                 scale = 1/sum;
 373             }
 374             cvConvertScale(patch_8u, m_samples[i], scale);
 375
 376 #if 0
 377             double maxval;
 378             cvMinMaxLoc(m_samples[i], 0, &maxval);
 379             IplImage* test = cvCreateImage(cvSize(roi.width/2, roi.height/2), IPL_DEPTH_8U, 1);
 380             cvConvertScale(m_samples[i], test, 255.0/maxval);
 381             cvNamedWindow("1", 1);
 382             cvShowImage("1", test);
 383             cvWaitKey(0);
 384 #endif
 385         }
 386         cvReleaseImage(&patch_8u);
 387     }
 388
 389     void OneWayDescriptor::GenerateSamplesFast(IplImage* frontal, CvMat* pca_hr_avg,
 390                                                CvMat* pca_hr_eigenvectors, OneWayDescriptor* pca_descriptors)
 391     {
 392         CvRect roi = cvGetImageROI(frontal);
 393         if(roi.width != GetInputPatchSize().width || roi.height != GetInputPatchSize().height)
 394         {
 395             cvResize(frontal, m_train_patch);
 396             frontal = m_train_patch;
 397         }
 398
 399         CvMat* pca_coeffs = cvCreateMat(1, pca_hr_eigenvectors->cols, CV_32FC1);
 400         double maxval;
 401         cvMinMaxLoc(frontal, 0, &maxval);
 402         CvMat* frontal_data = ConvertImageToMatrix(frontal);
 403
 404         double sum = cvSum(frontal_data).val[0];
 405         cvConvertScale(frontal_data, frontal_data, 1.0f/sum);
 406         cvProjectPCA(frontal_data, pca_hr_avg, pca_hr_eigenvectors, pca_coeffs);
 407         for(int i = 0; i < m_pose_count; i++)
 408         {
 409             cvSetZero(m_samples[i]);
 410             for(int j = 0; j < m_pca_dim_high; j++)
 411             {
 412                 double coeff = cvmGet(pca_coeffs, 0, j);
 413                 IplImage* patch = pca_descriptors[j + 1].GetPatch(i);
 414                 cvAddWeighted(m_samples[i], 1.0, patch, coeff, 0, m_samples[i]);
 415
 416 #if 0
 417                 printf("coeff%d = %f\n", j, coeff);
 418                 IplImage* test = cvCreateImage(cvSize(12, 12), IPL_DEPTH_8U, 1);
 419                 double maxval;
 420                 cvMinMaxLoc(patch, 0, &maxval);
 421                 cvConvertScale(patch, test, 255.0/maxval);
 422                 cvNamedWindow("1", 1);
 423                 cvShowImage("1", test);
 424                 cvWaitKey(0);
 425 #endif
 426             }
 427
 428             cvAdd(pca_descriptors[0].GetPatch(i), m_samples[i], m_samples[i]);
 429             double sm = cvSum(m_samples[i]).val[0];
 430             cvConvertScale(m_samples[i], m_samples[i], 1.0/sm);
 431
 432 #if 0
 433             IplImage* test = cvCreateImage(cvSize(12, 12), IPL_DEPTH_8U, 1);
 434             /*        IplImage* temp1 = cvCreateImage(cvSize(12, 12), IPL_DEPTH_32F, 1);
 435              eigenvector2image(pca_hr_avg, temp1);
 436              IplImage* test = cvCreateImage(cvSize(12, 12), IPL_DEPTH_8U, 1);
 437              cvAdd(m_samples[i], temp1, temp1);
 438              cvMinMaxLoc(temp1, 0, &maxval);
 439              cvConvertScale(temp1, test, 255.0/maxval);*/
 440             cvMinMaxLoc(m_samples[i], 0, &maxval);
 441             cvConvertScale(m_samples[i], test, 255.0/maxval);
 442
 443             cvNamedWindow("1", 1);
 444             cvShowImage("1", frontal);
 445             cvNamedWindow("2", 1);
 446             cvShowImage("2", test);
 447             cvWaitKey(0);
 448 #endif
 449         }
 450
 451         cvReleaseMat(&pca_coeffs);
 452         cvReleaseMat(&frontal_data);
 453     }
 454
 455     void OneWayDescriptor::SetTransforms(CvAffinePose* poses, CvMat** transforms)
 456     {
 457         if(m_affine_poses)
 458         {
 459             delete []m_affine_poses;
 460         }
 461
 462         m_affine_poses = poses;
 463         m_transforms = transforms;
 464     }
 465
 466     void OneWayDescriptor::Initialize(int pose_count, IplImage* frontal, const char* feature_name, int norm)
 467     {
 468         m_feature_name = String(feature_name);
 469         CvRect roi = cvGetImageROI(frontal);
 470         m_center = rect_center(roi);
 471
 472         Allocate(pose_count, cvSize(roi.width, roi.height), frontal->nChannels);
 473
 474         GenerateSamples(pose_count, frontal, norm);
 475     }
 476
 477     void OneWayDescriptor::InitializeFast(int pose_count, IplImage* frontal, const char* feature_name,
 478                                           CvMat* pca_hr_avg, CvMat* pca_hr_eigenvectors, OneWayDescriptor* pca_descriptors)
 479     {
 480         if(pca_hr_avg == 0)
 481         {
 482             Initialize(pose_count, frontal, feature_name, 1);
 483             return;
 484         }
 485         m_feature_name = String(feature_name);
 486         CvRect roi = cvGetImageROI(frontal);
 487         m_center = rect_center(roi);
 488
 489         Allocate(pose_count, cvSize(roi.width, roi.height), frontal->nChannels);
 490
 491         GenerateSamplesFast(frontal, pca_hr_avg, pca_hr_eigenvectors, pca_descriptors);
 492     }
 493
 494     void OneWayDescriptor::InitializePCACoeffs(CvMat* avg, CvMat* eigenvectors)
 495     {
 496         for(int i = 0; i < m_pose_count; i++)
 497         {
 498             ProjectPCASample(m_samples[i], avg, eigenvectors, m_pca_coeffs[i]);
 499         }
 500     }
 501
 502     void OneWayDescriptor::ProjectPCASample(IplImage* patch, CvMat* avg, CvMat* eigenvectors, CvMat* pca_coeffs) const
 503     {
 504         CvMat* patch_mat = ConvertImageToMatrix(patch);
 505         //    CvMat eigenvectorsr;
 506         //    cvGetSubRect(eigenvectors, &eigenvectorsr, cvRect(0, 0, eigenvectors->cols, pca_coeffs->cols));
 507         CvMat* temp = cvCreateMat(1, eigenvectors->cols, CV_32FC1);
 508         cvProjectPCA(patch_mat, avg, eigenvectors, temp);
 509         CvMat temp1;
 510         cvGetSubRect(temp, &temp1, cvRect(0, 0, pca_coeffs->cols, 1));
 511         cvCopy(&temp1, pca_coeffs);
 512
 513         cvReleaseMat(&temp);
 514         cvReleaseMat(&patch_mat);
 515     }
 516
 517     void OneWayDescriptor::EstimatePosePCA(CvArr* patch, int& pose_idx, float& distance, CvMat* avg, CvMat* eigenvectors) const
 518     {
 519         if(avg == 0)
 520         {
 521             // do not use pca
 522             if (!CV_IS_MAT(patch))
 523             {
 524                 EstimatePose((IplImage*)patch, pose_idx, distance);
 525             }
 526             else
 527             {
 528
 529             }
 530             return;
 531         }
 532         CvRect roi;
 533         if (!CV_IS_MAT(patch))
 534         {
 535             roi = cvGetImageROI((IplImage*)patch);
 536             if(roi.width != GetPatchSize().width || roi.height != GetPatchSize().height)
 537             {
 538                 cvResize(patch, m_input_patch);
 539                 patch = m_input_patch;
 540                 roi = cvGetImageROI((IplImage*)patch);
 541             }
 542         }
 543
 544         CvMat* pca_coeffs = cvCreateMat(1, m_pca_dim_low, CV_32FC1);
 545
 546         if (CV_IS_MAT(patch))
 547         {
 548             cvCopy((CvMat*)patch, pca_coeffs);
 549         }
 550         else
 551         {
 552             IplImage* patch_32f = cvCreateImage(cvSize(roi.width, roi.height), IPL_DEPTH_32F, 1);
 553             double sum = cvSum(patch).val[0];
 554             cvConvertScale(patch, patch_32f, 1.0f/sum);
 555             ProjectPCASample(patch_32f, avg, eigenvectors, pca_coeffs);
 556             cvReleaseImage(&patch_32f);
 557         }
 558
 559
 560         distance = 1e10;
 561         pose_idx = -1;
 562
 563         for(int i = 0; i < m_pose_count; i++)
 564         {
 565             double dist = cvNorm(m_pca_coeffs[i], pca_coeffs);
 566             //      float dist = 0;
 567             //      float data1, data2;
 568             //      //CvMat* pose_pca_coeffs = m_pca_coeffs[i];
 569             //      for (int x=0; x < pca_coeffs->width; x++)
 570             //          for (int y =0 ; y < pca_coeffs->height; y++)
 571             //          {
 572             //              data1 = ((float*)(pca_coeffs->data.ptr + pca_coeffs->step*x))[y];
 573             //              data2 = ((float*)(m_pca_coeffs[i]->data.ptr + m_pca_coeffs[i]->step*x))[y];
 574             //              dist+=(data1-data2)*(data1-data2);
 575             //          }
 576             ////#if 1
 577             //      for (int j = 0; j < m_pca_dim_low; j++)
 578             //      {
 579             //          dist += (pose_pca_coeffs->data.fl[j]- pca_coeffs->data.fl[j])*(pose_pca_coeffs->data.fl[j]- pca_coeffs->data.fl[j]);
 580             //      }
 581             //#else
 582             //      for (int j = 0; j <= m_pca_dim_low - 4; j += 4)
 583             //      {
 584             //          dist += (pose_pca_coeffs->data.fl[j]- pca_coeffs->data.fl[j])*
 585             //              (pose_pca_coeffs->data.fl[j]- pca_coeffs->data.fl[j]);
 586             //          dist += (pose_pca_coeffs->data.fl[j+1]- pca_coeffs->data.fl[j+1])*
 587             //              (pose_pca_coeffs->data.fl[j+1]- pca_coeffs->data.fl[j+1]);
 588             //          dist += (pose_pca_coeffs->data.fl[j+2]- pca_coeffs->data.fl[j+2])*
 589             //              (pose_pca_coeffs->data.fl[j+2]- pca_coeffs->data.fl[j+2]);
 590             //          dist += (pose_pca_coeffs->data.fl[j+3]- pca_coeffs->data.fl[j+3])*
 591             //              (pose_pca_coeffs->data.fl[j+3]- pca_coeffs->data.fl[j+3]);
 592             //      }
 593             //#endif
 594             if(dist < distance)
 595             {
 596                 distance = (float)dist;
 597                 pose_idx = i;
 598             }
 599         }
 600
 601         cvReleaseMat(&pca_coeffs);
 602     }
 603
 604     void OneWayDescriptor::EstimatePose(IplImage* patch, int& pose_idx, float& distance) const
 605     {
 606         distance = 1e10;
 607         pose_idx = -1;
 608
 609         CvRect roi = cvGetImageROI(patch);
 610         IplImage* patch_32f = cvCreateImage(cvSize(roi.width, roi.height), IPL_DEPTH_32F, patch->nChannels);
 611         double sum = cvSum(patch).val[0];
 612         cvConvertScale(patch, patch_32f, 1/sum);
 613
 614         for(int i = 0; i < m_pose_count; i++)
 615         {
 616             if(m_samples[i]->width != patch_32f->width || m_samples[i]->height != patch_32f->height)
 617             {
 618                 continue;
 619             }
 620             double dist = cvNorm(m_samples[i], patch_32f);
 621             //float dist = 0.0f;
 622             //float i1,i2;
 623
 624             //for (int y = 0; y<patch_32f->height; y++)
 625             //  for (int x = 0; x< patch_32f->width; x++)
 626             //  {
 627             //      i1 = ((float*)(m_samples[i]->imageData + m_samples[i]->widthStep*y))[x];
 628             //      i2 = ((float*)(patch_32f->imageData + patch_32f->widthStep*y))[x];
 629             //      dist+= (i1-i2)*(i1-i2);
 630             //  }
 631
 632             if(dist < distance)
 633             {
 634                 distance = (float)dist;
 635                 pose_idx = i;
 636             }
 637
 638 #if 0
 639             IplImage* img1 = cvCreateImage(cvSize(roi.width, roi.height), IPL_DEPTH_8U, 1);
 640             IplImage* img2 = cvCreateImage(cvSize(roi.width, roi.height), IPL_DEPTH_8U, 1);
 641             double maxval;
 642             cvMinMaxLoc(m_samples[i], 0, &maxval);
 643             cvConvertScale(m_samples[i], img1, 255.0/maxval);
 644             cvMinMaxLoc(patch_32f, 0, &maxval);
 645             cvConvertScale(patch_32f, img2, 255.0/maxval);
 646
 647             cvNamedWindow("1", 1);
 648             cvShowImage("1", img1);
 649             cvNamedWindow("2", 1);
 650             cvShowImage("2", img2);
 651             printf("Distance = %f\n", dist);
 652             cvWaitKey(0);
 653 #endif
 654         }
 655
 656         cvReleaseImage(&patch_32f);
 657     }
 658
 659     void OneWayDescriptor::Save(const char* path)
 660     {
 661         for(int i = 0; i < m_pose_count; i++)
 662         {
 663             char buf[1024];
 664             sprintf(buf, "%s/patch_%04d.png", path, i);
 665             IplImage* patch = cvCreateImage(cvSize(m_samples[i]->width, m_samples[i]->height), IPL_DEPTH_8U, m_samples[i]->nChannels);
 666
 667             double maxval;
 668             cvMinMaxLoc(m_samples[i], 0, &maxval);
 669             cvConvertScale(m_samples[i], patch, 255/maxval);
 670
 671 #ifdef HAVE_OPENCV_HIGHGUI
 672             cv::imwrite(buf, cv::cvarrToMat(patch));
 673 #else
 674             CV_Error(CV_StsNotImplemented, "OpenCV has been compiled without image I/O support");
 675 #endif
 676
 677             cvReleaseImage(&patch);
 678         }
 679     }
 680
 681     void OneWayDescriptor::Write(CvFileStorage* fs, const char* name)
 682     {
 683         CvMat* mat = cvCreateMat(m_pose_count, m_samples[0]->width*m_samples[0]->height, CV_32FC1);
 684
 685         // prepare data to write as a single matrix
 686         for(int i = 0; i < m_pose_count; i++)
 687         {
 688             for(int y = 0; y < m_samples[i]->height; y++)
 689             {
 690                 for(int x = 0; x < m_samples[i]->width; x++)
 691                 {
 692                     float val = *((float*)(m_samples[i]->imageData + m_samples[i]->widthStep*y) + x);
 693                     cvmSet(mat, i, y*m_samples[i]->width + x, val);
 694                 }
 695             }
 696         }
 697
 698         cvWrite(fs, name, mat);
 699
 700         cvReleaseMat(&mat);
 701     }
 702
 703     int OneWayDescriptor::ReadByName(const FileNode &parent, const char* name)
 704     {
 705         CvMat* mat = reinterpret_cast<CvMat*> (parent[name].readObj ());
 706         if(!mat)
 707         {
 708             return 0;
 709         }
 710
 711
 712         for(int i = 0; i < m_pose_count; i++)
 713         {
 714             for(int y = 0; y < m_samples[i]->height; y++)
 715             {
 716                 for(int x = 0; x < m_samples[i]->width; x++)
 717                 {
 718                     float val = (float)cvmGet(mat, i, y*m_samples[i]->width + x);
 719                     *((float*)(m_samples[i]->imageData + y*m_samples[i]->widthStep) + x) = val;
 720                 }
 721             }
 722         }
 723
 724         cvReleaseMat(&mat);
 725         return 1;
 726     }
 727
 728     int OneWayDescriptor::ReadByName(CvFileStorage* fs, CvFileNode* parent, const char* name)
 729     {
 730         return ReadByName (FileNode (fs, parent), name);
 731     }
 732
 733     IplImage* OneWayDescriptor::GetPatch(int index)
 734     {
 735         return m_samples[index];
 736     }
 737
 738     CvAffinePose OneWayDescriptor::GetPose(int index) const
 739     {
 740         return m_affine_poses[index];
 741     }
 742
 743     void FindOneWayDescriptor(int desc_count, const OneWayDescriptor* descriptors, IplImage* patch, int& desc_idx, int& pose_idx, float& distance,
 744                               CvMat* avg, CvMat* eigenvectors)
 745     {
 746         desc_idx = -1;
 747         pose_idx = -1;
 748         distance = 1e10;
 749         //--------
 750         //PCA_coeffs precalculating
 751         int m_pca_dim_low = descriptors[0].GetPCADimLow();
 752         CvMat* pca_coeffs = cvCreateMat(1, m_pca_dim_low, CV_32FC1);
 753         int patch_width = descriptors[0].GetPatchSize().width;
 754         int patch_height = descriptors[0].GetPatchSize().height;
 755         if (avg)
 756         {
 757             CvRect _roi = cvGetImageROI((IplImage*)patch);
 758             IplImage* test_img = cvCreateImage(cvSize(patch_width,patch_height), IPL_DEPTH_8U, 1);
 759             if(_roi.width != patch_width|| _roi.height != patch_height)
 760             {
 761
 762                 cvResize(patch, test_img);
 763                 _roi = cvGetImageROI(test_img);
 764             }
 765             else
 766             {
 767                 cvCopy(patch,test_img);
 768             }
 769             IplImage* patch_32f = cvCreateImage(cvSize(_roi.width, _roi.height), IPL_DEPTH_32F, 1);
 770             double sum = cvSum(test_img).val[0];
 771             cvConvertScale(test_img, patch_32f, 1.0f/sum);
 772
 773             //ProjectPCASample(patch_32f, avg, eigenvectors, pca_coeffs);
 774             //Projecting PCA
 775             CvMat* patch_mat = ConvertImageToMatrix(patch_32f);
 776             CvMat* temp = cvCreateMat(1, eigenvectors->cols, CV_32FC1);
 777             cvProjectPCA(patch_mat, avg, eigenvectors, temp);
 778             CvMat temp1;
 779             cvGetSubRect(temp, &temp1, cvRect(0, 0, pca_coeffs->cols, 1));
 780             cvCopy(&temp1, pca_coeffs);
 781             cvReleaseMat(&temp);
 782             cvReleaseMat(&patch_mat);
 783             //End of projecting
 784
 785             cvReleaseImage(&patch_32f);
 786             cvReleaseImage(&test_img);
 787         }
 788
 789         //--------
 790
 791
 792
 793         for(int i = 0; i < desc_count; i++)
 794         {
 795             int _pose_idx = -1;
 796             float _distance = 0;
 797
 798 #if 0
 799             descriptors[i].EstimatePose(patch, _pose_idx, _distance);
 800 #else
 801             if (!avg)
 802             {
 803                 descriptors[i].EstimatePosePCA(patch, _pose_idx, _distance, avg, eigenvectors);
 804             }
 805             else
 806             {
 807                 descriptors[i].EstimatePosePCA(pca_coeffs, _pose_idx, _distance, avg, eigenvectors);
 808             }
 809 #endif
 810
 811             if(_distance < distance)
 812             {
 813                 desc_idx = i;
 814                 pose_idx = _pose_idx;
 815                 distance = _distance;
 816             }
 817         }
 818         cvReleaseMat(&pca_coeffs);
 819     }
 820
 821 #if defined(_KDTREE)
 822
 823     void FindOneWayDescriptor(cv::flann::Index* m_pca_descriptors_tree, CvSize patch_size, int m_pca_dim_low, int m_pose_count, IplImage* patch, int& desc_idx, int& pose_idx, float& distance,
 824                               CvMat* avg, CvMat* eigenvectors)
 825     {
 826         desc_idx = -1;
 827         pose_idx = -1;
 828         distance = 1e10;
 829         //--------
 830         //PCA_coeffs precalculating
 831         CvMat* pca_coeffs = cvCreateMat(1, m_pca_dim_low, CV_32FC1);
 832         int patch_width = patch_size.width;
 833         int patch_height = patch_size.height;
 834         //if (avg)
 835         //{
 836         CvRect _roi = cvGetImageROI((IplImage*)patch);
 837         IplImage* test_img = cvCreateImage(cvSize(patch_width,patch_height), IPL_DEPTH_8U, 1);
 838         if(_roi.width != patch_width|| _roi.height != patch_height)
 839         {
 840
 841             cvResize(patch, test_img);
 842             _roi = cvGetImageROI(test_img);
 843         }
 844         else
 845         {
 846             cvCopy(patch,test_img);
 847         }
 848         IplImage* patch_32f = cvCreateImage(cvSize(_roi.width, _roi.height), IPL_DEPTH_32F, 1);
 849         float sum = cvSum(test_img).val[0];
 850         cvConvertScale(test_img, patch_32f, 1.0f/sum);
 851
 852         //ProjectPCASample(patch_32f, avg, eigenvectors, pca_coeffs);
 853         //Projecting PCA
 854         CvMat* patch_mat = ConvertImageToMatrix(patch_32f);
 855         CvMat* temp = cvCreateMat(1, eigenvectors->cols, CV_32FC1);
 856         cvProjectPCA(patch_mat, avg, eigenvectors, temp);
 857         CvMat temp1;
 858         cvGetSubRect(temp, &temp1, cvRect(0, 0, pca_coeffs->cols, 1));
 859         cvCopy(&temp1, pca_coeffs);
 860         cvReleaseMat(&temp);
 861         cvReleaseMat(&patch_mat);
 862         //End of projecting
 863
 864         cvReleaseImage(&patch_32f);
 865         cvReleaseImage(&test_img);
 866         //  }
 867
 868         //--------
 869
 870         //float* target = new float[m_pca_dim_low];
 871         //::cvflann::KNNResultSet res(1,pca_coeffs->data.fl,m_pca_dim_low);
 872         //::cvflann::SearchParams params;
 873         //params.checks = -1;
 874
 875         //int maxDepth = 1000000;
 876         //int neighbors_count = 1;
 877         //int* neighborsIdx = new int[neighbors_count];
 878         //float* distances = new float[neighbors_count];
 879         //if (m_pca_descriptors_tree->findNearest(pca_coeffs->data.fl,neighbors_count,maxDepth,neighborsIdx,0,distances) > 0)
 880         //{
 881         //  desc_idx = neighborsIdx[0] / m_pose_count;
 882         //  pose_idx = neighborsIdx[0] % m_pose_count;
 883         //  distance = distances[0];
 884         //}
 885         //delete[] neighborsIdx;
 886         //delete[] distances;
 887
 888         cv::Mat m_object(1, m_pca_dim_low, CV_32F);
 889         cv::Mat m_indices(1, 1, CV_32S);
 890         cv::Mat m_dists(1, 1, CV_32F);
 891
 892         float* object_ptr = m_object.ptr<float>(0);
 893         for (int i=0;i<m_pca_dim_low;i++)
 894         {
 895             object_ptr[i] = pca_coeffs->data.fl[i];
 896         }
 897
 898         m_pca_descriptors_tree->knnSearch(m_object, m_indices, m_dists, 1, cv::flann::SearchParams(-1) );
 899
 900         desc_idx = ((int*)(m_indices.ptr<int>(0)))[0] / m_pose_count;
 901         pose_idx = ((int*)(m_indices.ptr<int>(0)))[0] % m_pose_count;
 902         distance = ((float*)(m_dists.ptr<float>(0)))[0];
 903
 904         //  delete[] target;
 905
 906
 907         //    for(int i = 0; i < desc_count; i++)
 908         //    {
 909         //        int _pose_idx = -1;
 910         //        float _distance = 0;
 911         //
 912         //#if 0
 913         //        descriptors[i].EstimatePose(patch, _pose_idx, _distance);
 914         //#else
 915         //      if (!avg)
 916         //      {
 917         //          descriptors[i].EstimatePosePCA(patch, _pose_idx, _distance, avg, eigenvectors);
 918         //      }
 919         //      else
 920         //      {
 921         //          descriptors[i].EstimatePosePCA(pca_coeffs, _pose_idx, _distance, avg, eigenvectors);
 922         //      }
 923         //#endif
 924         //
 925         //        if(_distance < distance)
 926         //        {
 927         //            desc_idx = i;
 928         //            pose_idx = _pose_idx;
 929         //            distance = _distance;
 930         //        }
 931         //    }
 932         cvReleaseMat(&pca_coeffs);
 933     }
 934 #endif
 935     //**
 936     void FindOneWayDescriptor(int desc_count, const OneWayDescriptor* descriptors, IplImage* patch, int n,
 937                               std::vector<int>& desc_idxs, std::vector<int>&  pose_idxs, std::vector<float>& distances,
 938                               CvMat* avg, CvMat* eigenvectors)
 939     {
 940         for (int i=0;i<n;i++)
 941         {
 942             desc_idxs[i] = -1;
 943             pose_idxs[i] = -1;
 944             distances[i] = 1e10;
 945         }
 946         //--------
 947         //PCA_coeffs precalculating
 948         int m_pca_dim_low = descriptors[0].GetPCADimLow();
 949         CvMat* pca_coeffs = cvCreateMat(1, m_pca_dim_low, CV_32FC1);
 950         int patch_width = descriptors[0].GetPatchSize().width;
 951         int patch_height = descriptors[0].GetPatchSize().height;
 952         if (avg)
 953         {
 954             CvRect _roi = cvGetImageROI((IplImage*)patch);
 955             IplImage* test_img = cvCreateImage(cvSize(patch_width,patch_height), IPL_DEPTH_8U, 1);
 956             if(_roi.width != patch_width|| _roi.height != patch_height)
 957             {
 958
 959                 cvResize(patch, test_img);
 960                 _roi = cvGetImageROI(test_img);
 961             }
 962             else
 963             {
 964                 cvCopy(patch,test_img);
 965             }
 966             IplImage* patch_32f = cvCreateImage(cvSize(_roi.width, _roi.height), IPL_DEPTH_32F, 1);
 967             double sum = cvSum(test_img).val[0];
 968             cvConvertScale(test_img, patch_32f, 1.0f/sum);
 969
 970             //ProjectPCASample(patch_32f, avg, eigenvectors, pca_coeffs);
 971             //Projecting PCA
 972             CvMat* patch_mat = ConvertImageToMatrix(patch_32f);
 973             CvMat* temp = cvCreateMat(1, eigenvectors->cols, CV_32FC1);
 974             cvProjectPCA(patch_mat, avg, eigenvectors, temp);
 975             CvMat temp1;
 976             cvGetSubRect(temp, &temp1, cvRect(0, 0, pca_coeffs->cols, 1));
 977             cvCopy(&temp1, pca_coeffs);
 978             cvReleaseMat(&temp);
 979             cvReleaseMat(&patch_mat);
 980             //End of projecting
 981
 982             cvReleaseImage(&patch_32f);
 983             cvReleaseImage(&test_img);
 984         }
 985         //--------
 986
 987
 988
 989         for(int i = 0; i < desc_count; i++)
 990         {
 991             int _pose_idx = -1;
 992             float _distance = 0;
 993
 994 #if 0
 995             descriptors[i].EstimatePose(patch, _pose_idx, _distance);
 996 #else
 997             if (!avg)
 998             {
 999                 descriptors[i].EstimatePosePCA(patch, _pose_idx, _distance, avg, eigenvectors);
1000             }
1001             else
1002             {
1003                 descriptors[i].EstimatePosePCA(pca_coeffs, _pose_idx, _distance, avg, eigenvectors);
1004             }
1005 #endif
1006
1007             for (int j=0;j<n;j++)
1008             {
1009                 if(_distance < distances[j])
1010                 {
1011                     for (int k=(n-1);k > j;k--)
1012                     {
1013                         desc_idxs[k] = desc_idxs[k-1];
1014                         pose_idxs[k] = pose_idxs[k-1];
1015                         distances[k] = distances[k-1];
1016                     }
1017                     desc_idxs[j] = i;
1018                     pose_idxs[j] = _pose_idx;
1019                     distances[j] = _distance;
1020                     break;
1021                 }
1022             }
1023         }
1024         cvReleaseMat(&pca_coeffs);
1025     }
1026
1027     void FindOneWayDescriptorEx(int desc_count, const OneWayDescriptor* descriptors, IplImage* patch,
1028                                 float scale_min, float scale_max, float scale_step,
1029                                 int& desc_idx, int& pose_idx, float& distance, float& scale,
1030                                 CvMat* avg, CvMat* eigenvectors)
1031     {
1032         CvSize patch_size = descriptors[0].GetPatchSize();
1033         IplImage* input_patch;
1034         CvRect roi;
1035
1036         input_patch= cvCreateImage(patch_size, IPL_DEPTH_8U, 1);
1037         roi = cvGetImageROI((IplImage*)patch);
1038
1039         int _desc_idx, _pose_idx;
1040         float _distance;
1041         distance = 1e10;
1042         for(float cur_scale = scale_min; cur_scale < scale_max; cur_scale *= scale_step)
1043         {
1044             //        printf("Scale = %f\n", cur_scale);
1045
1046             CvRect roi_scaled = resize_rect(roi, cur_scale);
1047             cvSetImageROI(patch, roi_scaled);
1048             cvResize(patch, input_patch);
1049
1050
1051 #if 0
1052             if(roi.x > 244 && roi.y < 200)
1053             {
1054                 cvNamedWindow("1", 1);
1055                 cvShowImage("1", input_patch);
1056                 cvWaitKey(0);
1057             }
1058 #endif
1059
1060             FindOneWayDescriptor(desc_count, descriptors, input_patch, _desc_idx, _pose_idx, _distance, avg, eigenvectors);
1061             if(_distance < distance)
1062             {
1063                 distance = _distance;
1064                 desc_idx = _desc_idx;
1065                 pose_idx = _pose_idx;
1066                 scale = cur_scale;
1067             }
1068         }
1069
1070
1071         cvSetImageROI((IplImage*)patch, roi);
1072         cvReleaseImage(&input_patch);
1073
1074     }
1075
1076     void FindOneWayDescriptorEx(int desc_count, const OneWayDescriptor* descriptors, IplImage* patch,
1077                                 float scale_min, float scale_max, float scale_step,
1078                                 int n, std::vector<int>& desc_idxs, std::vector<int>& pose_idxs,
1079                                 std::vector<float>& distances, std::vector<float>& scales,
1080                                 CvMat* avg, CvMat* eigenvectors)
1081     {
1082         CvSize patch_size = descriptors[0].GetPatchSize();
1083         IplImage* input_patch;
1084         CvRect roi;
1085
1086         input_patch= cvCreateImage(patch_size, IPL_DEPTH_8U, 1);
1087         roi = cvGetImageROI((IplImage*)patch);
1088
1089         //  float min_distance = 1e10;
1090         std::vector<int> _desc_idxs;
1091         _desc_idxs.resize(n);
1092         std::vector<int> _pose_idxs;
1093         _pose_idxs.resize(n);
1094         std::vector<float> _distances;
1095         _distances.resize(n);
1096
1097
1098         for (int i=0;i<n;i++)
1099         {
1100             distances[i] = 1e10;
1101         }
1102
1103         for(float cur_scale = scale_min; cur_scale < scale_max; cur_scale *= scale_step)
1104         {
1105
1106             CvRect roi_scaled = resize_rect(roi, cur_scale);
1107             cvSetImageROI(patch, roi_scaled);
1108             cvResize(patch, input_patch);
1109
1110
1111
1112             FindOneWayDescriptor(desc_count, descriptors, input_patch, n,_desc_idxs, _pose_idxs, _distances, avg, eigenvectors);
1113             for (int i=0;i<n;i++)
1114             {
1115                 if(_distances[i] < distances[i])
1116                 {
1117                     distances[i] = _distances[i];
1118                     desc_idxs[i] = _desc_idxs[i];
1119                     pose_idxs[i] = _pose_idxs[i];
1120                     scales[i] = cur_scale;
1121                 }
1122             }
1123         }
1124
1125
1126
1127         cvSetImageROI((IplImage*)patch, roi);
1128         cvReleaseImage(&input_patch);
1129     }
1130
1131 #if defined(_KDTREE)
1132     void FindOneWayDescriptorEx(cv::flann::Index* m_pca_descriptors_tree, CvSize patch_size, int m_pca_dim_low,
1133                                 int m_pose_count, IplImage* patch,
1134                                 float scale_min, float scale_max, float scale_step,
1135                                 int& desc_idx, int& pose_idx, float& distance, float& scale,
1136                                 CvMat* avg, CvMat* eigenvectors)
1137     {
1138         IplImage* input_patch;
1139         CvRect roi;
1140
1141         input_patch= cvCreateImage(patch_size, IPL_DEPTH_8U, 1);
1142         roi = cvGetImageROI((IplImage*)patch);
1143
1144         int _desc_idx, _pose_idx;
1145         float _distance;
1146         distance = 1e10;
1147         for(float cur_scale = scale_min; cur_scale < scale_max; cur_scale *= scale_step)
1148         {
1149             //        printf("Scale = %f\n", cur_scale);
1150
1151             CvRect roi_scaled = resize_rect(roi, cur_scale);
1152             cvSetImageROI(patch, roi_scaled);
1153             cvResize(patch, input_patch);
1154
1155             FindOneWayDescriptor(m_pca_descriptors_tree, patch_size, m_pca_dim_low, m_pose_count, input_patch, _desc_idx, _pose_idx, _distance, avg, eigenvectors);
1156             if(_distance < distance)
1157             {
1158                 distance = _distance;
1159                 desc_idx = _desc_idx;
1160                 pose_idx = _pose_idx;
1161                 scale = cur_scale;
1162             }
1163         }
1164
1165
1166         cvSetImageROI((IplImage*)patch, roi);
1167         cvReleaseImage(&input_patch);
1168
1169     }
1170 #endif
1171
1172     const char* OneWayDescriptor::GetFeatureName() const
1173     {
1174         return m_feature_name.c_str();
1175     }
1176
1177     CvPoint OneWayDescriptor::GetCenter() const
1178     {
1179         return m_center;
1180     }
1181
1182     int OneWayDescriptor::GetPCADimLow() const
1183     {
1184         return m_pca_dim_low;
1185     }
1186
1187     int OneWayDescriptor::GetPCADimHigh() const
1188     {
1189         return m_pca_dim_high;
1190     }
1191
1192     CvMat* ConvertImageToMatrix(IplImage* patch)
1193     {
1194         CvRect roi = cvGetImageROI(patch);
1195         CvMat* mat = cvCreateMat(1, roi.width*roi.height, CV_32FC1);
1196
1197         if(patch->depth == 32)
1198         {
1199             for(int y = 0; y < roi.height; y++)
1200             {
1201                 for(int x = 0; x < roi.width; x++)
1202                 {
1203                     mat->data.fl[y*roi.width + x] = *((float*)(patch->imageData + (y + roi.y)*patch->widthStep) + x + roi.x);
1204                 }
1205             }
1206         }
1207         else if(patch->depth == 8)
1208         {
1209             for(int y = 0; y < roi.height; y++)
1210             {
1211                 for(int x = 0; x < roi.width; x++)
1212                 {
1213                     mat->data.fl[y*roi.width + x] = (float)(unsigned char)patch->imageData[(y + roi.y)*patch->widthStep + x + roi.x];
1214                 }
1215             }
1216         }
1217         else
1218         {
1219             printf("Image depth %d is not supported\n", patch->depth);
1220             return 0;
1221         }
1222
1223         return mat;
1224     }
1225
1226     OneWayDescriptorBase::OneWayDescriptorBase(CvSize patch_size, int pose_count, const char* train_path,
1227                                                const char* pca_config, const char* pca_hr_config,
1228                                                const char* pca_desc_config, int pyr_levels,
1229                                                int pca_dim_high, int pca_dim_low)
1230     : m_pca_dim_high(pca_dim_high), m_pca_dim_low(pca_dim_low), scale_min (0.7f), scale_max(1.5f), scale_step (1.2f)
1231     {
1232 #if defined(_KDTREE)
1233         m_pca_descriptors_matrix = 0;
1234         m_pca_descriptors_tree = 0;
1235 #endif
1236         //  m_pca_descriptors_matrix = 0;
1237         m_patch_size = patch_size;
1238         m_pose_count = pose_count;
1239         m_pyr_levels = pyr_levels;
1240         m_poses = 0;
1241         m_transforms = 0;
1242
1243         m_pca_avg = 0;
1244         m_pca_eigenvectors = 0;
1245         m_pca_hr_avg = 0;
1246         m_pca_hr_eigenvectors = 0;
1247         m_pca_descriptors = 0;
1248
1249         m_descriptors = 0;
1250
1251         if(train_path == 0 || strlen(train_path) == 0)
1252         {
1253             // skip pca loading
1254             return;
1255         }
1256         char pca_config_filename[1024];
1257         sprintf(pca_config_filename, "%s/%s", train_path, pca_config);
1258         readPCAFeatures(pca_config_filename, &m_pca_avg, &m_pca_eigenvectors);
1259         if(pca_hr_config && strlen(pca_hr_config) > 0)
1260         {
1261             char pca_hr_config_filename[1024];
1262             sprintf(pca_hr_config_filename, "%s/%s", train_path, pca_hr_config);
1263             readPCAFeatures(pca_hr_config_filename, &m_pca_hr_avg, &m_pca_hr_eigenvectors);
1264         }
1265
1266         m_pca_descriptors = new OneWayDescriptor[m_pca_dim_high + 1];
1267
1268 #if !defined(_GH_REGIONS)
1269         if(pca_desc_config && strlen(pca_desc_config) > 0)
1270             //    if(0)
1271         {
1272             //printf("Loading the descriptors...");
1273             char pca_desc_config_filename[1024];
1274             sprintf(pca_desc_config_filename, "%s/%s", train_path, pca_desc_config);
1275             LoadPCADescriptors(pca_desc_config_filename);
1276             //printf("done.\n");
1277         }
1278         else
1279         {
1280             printf("Initializing the descriptors...\n");
1281             InitializePoseTransforms();
1282             CreatePCADescriptors();
1283             SavePCADescriptors("pca_descriptors.yml");
1284         }
1285 #endif //_GH_REGIONS
1286         //    SavePCADescriptors("./pca_descriptors.yml");
1287
1288     }
1289
1290     OneWayDescriptorBase::OneWayDescriptorBase(CvSize patch_size, int pose_count, const String &pca_filename,
1291                                                const String &train_path, const String &images_list, float _scale_min, float _scale_max,
1292                                                float _scale_step, int pyr_levels,
1293                                                int pca_dim_high, int pca_dim_low)
1294     : m_pca_dim_high(pca_dim_high), m_pca_dim_low(pca_dim_low), scale_min(_scale_min), scale_max(_scale_max), scale_step(_scale_step)
1295     {
1296 #if defined(_KDTREE)
1297         m_pca_descriptors_matrix = 0;
1298         m_pca_descriptors_tree = 0;
1299 #endif
1300         m_patch_size = patch_size;
1301         m_pose_count = pose_count;
1302         m_pyr_levels = pyr_levels;
1303         m_poses = 0;
1304         m_transforms = 0;
1305
1306         m_pca_avg = 0;
1307         m_pca_eigenvectors = 0;
1308         m_pca_hr_avg = 0;
1309         m_pca_hr_eigenvectors = 0;
1310         m_pca_descriptors = 0;
1311
1312         m_descriptors = 0;
1313
1314
1315         if (pca_filename.length() == 0)
1316         {
1317             return;
1318         }
1319
1320         CvFileStorage* fs = cvOpenFileStorage(pca_filename.c_str(), NULL, CV_STORAGE_READ);
1321         if (fs != 0)
1322         {
1323             cvReleaseFileStorage(&fs);
1324
1325             readPCAFeatures(pca_filename.c_str(), &m_pca_avg, &m_pca_eigenvectors, "_lr");
1326             readPCAFeatures(pca_filename.c_str(), &m_pca_hr_avg, &m_pca_hr_eigenvectors, "_hr");
1327             m_pca_descriptors = new OneWayDescriptor[m_pca_dim_high + 1];
1328 #if !defined(_GH_REGIONS)
1329             LoadPCADescriptors(pca_filename.c_str());
1330 #endif //_GH_REGIONS
1331         }
1332         else
1333         {
1334             GeneratePCA(train_path.c_str(), images_list.c_str());
1335             m_pca_descriptors = new OneWayDescriptor[m_pca_dim_high + 1];
1336             char pca_default_filename[1024];
1337             sprintf(pca_default_filename, "%s/%s", train_path.c_str(), GetPCAFilename().c_str());
1338             LoadPCADescriptors(pca_default_filename);
1339         }
1340     }
1341
1342     void OneWayDescriptorBase::Read (const FileNode &fn)
1343     {
1344         clear ();
1345
1346         m_pose_count = fn["poseCount"];
1347         int patch_width = fn["patchWidth"];
1348         int patch_height = fn["patchHeight"];
1349         m_patch_size = cvSize (patch_width, patch_height);
1350         m_pyr_levels = fn["pyrLevels"];
1351         m_pca_dim_high = fn["pcaDimHigh"];
1352         m_pca_dim_low = fn["pcaDimLow"];
1353         scale_min = fn["minScale"];
1354         scale_max = fn["maxScale"];
1355         scale_step = fn["stepScale"];
1356
1357     LoadPCAall (fn);
1358     }
1359
1360     void OneWayDescriptorBase::LoadPCAall (const FileNode &fn)
1361     {
1362         readPCAFeatures(fn, &m_pca_avg, &m_pca_eigenvectors, "_lr");
1363         readPCAFeatures(fn, &m_pca_hr_avg, &m_pca_hr_eigenvectors, "_hr");
1364         m_pca_descriptors = new OneWayDescriptor[m_pca_dim_high + 1];
1365 #if !defined(_GH_REGIONS)
1366         LoadPCADescriptors(fn);
1367 #endif //_GH_REGIONS
1368     }
1369
1370     OneWayDescriptorBase::~OneWayDescriptorBase()
1371     {
1372         cvReleaseMat(&m_pca_avg);
1373         cvReleaseMat(&m_pca_eigenvectors);
1374
1375         if(m_pca_hr_eigenvectors)
1376         {
1377             delete[] m_pca_descriptors;
1378             cvReleaseMat(&m_pca_hr_avg);
1379             cvReleaseMat(&m_pca_hr_eigenvectors);
1380         }
1381
1382
1383         if(m_descriptors)
1384             delete []m_descriptors;
1385
1386         if(m_poses)
1387             delete []m_poses;
1388
1389         if (m_transforms)
1390         {
1391             for(int i = 0; i < m_pose_count; i++)
1392             {
1393                 cvReleaseMat(&m_transforms[i]);
1394             }
1395             delete []m_transforms;
1396         }
1397 #if defined(_KDTREE)
1398         if (m_pca_descriptors_matrix)
1399         {
1400             cvReleaseMat(&m_pca_descriptors_matrix);
1401         }
1402         if (m_pca_descriptors_tree)
1403         {
1404             delete m_pca_descriptors_tree;
1405         }
1406 #endif
1407     }
1408
1409     void OneWayDescriptorBase::clear(){
1410         if (m_descriptors)
1411         {
1412             delete []m_descriptors;
1413             m_descriptors = 0;
1414         }
1415
1416 #if defined(_KDTREE)
1417         if (m_pca_descriptors_matrix)
1418         {
1419             cvReleaseMat(&m_pca_descriptors_matrix);
1420             m_pca_descriptors_matrix = 0;
1421         }
1422         if (m_pca_descriptors_tree)
1423         {
1424             delete m_pca_descriptors_tree;
1425             m_pca_descriptors_tree = 0;
1426         }
1427 #endif
1428     }
1429
1430     void OneWayDescriptorBase::InitializePoses()
1431     {
1432         m_poses = new CvAffinePose[m_pose_count];
1433         for(int i = 0; i < m_pose_count; i++)
1434         {
1435             m_poses[i] = GenRandomAffinePose();
1436         }
1437     }
1438
1439     void OneWayDescriptorBase::InitializeTransformsFromPoses()
1440     {
1441         m_transforms = new CvMat*[m_pose_count];
1442         for(int i = 0; i < m_pose_count; i++)
1443         {
1444             m_transforms[i] = cvCreateMat(2, 3, CV_32FC1);
1445             GenerateAffineTransformFromPose(cvSize(m_patch_size.width*2, m_patch_size.height*2), m_poses[i], m_transforms[i]);
1446         }
1447     }
1448
1449     void OneWayDescriptorBase::InitializePoseTransforms()
1450     {
1451         InitializePoses();
1452         InitializeTransformsFromPoses();
1453     }
1454
1455     void OneWayDescriptorBase::InitializeDescriptor(int desc_idx, IplImage* train_image, const KeyPoint& keypoint, const char* feature_label)
1456     {
1457
1458         // TBD add support for octave != 0
1459         CvPoint center = keypoint.pt;
1460
1461         CvRect roi = cvRect(center.x - m_patch_size.width/2, center.y - m_patch_size.height/2, m_patch_size.width, m_patch_size.height);
1462         cvResetImageROI(train_image);
1463         roi = fit_rect_fixedsize(roi, train_image);
1464         cvSetImageROI(train_image, roi);
1465         if(roi.width != m_patch_size.width || roi.height != m_patch_size.height)
1466         {
1467             return;
1468         }
1469
1470         InitializeDescriptor(desc_idx, train_image, feature_label);
1471         cvResetImageROI(train_image);
1472     }
1473
1474     void OneWayDescriptorBase::InitializeDescriptor(int desc_idx, IplImage* train_image, const char* feature_label)
1475     {
1476         m_descriptors[desc_idx].SetPCADimHigh(m_pca_dim_high);
1477         m_descriptors[desc_idx].SetPCADimLow(m_pca_dim_low);
1478         m_descriptors[desc_idx].SetTransforms(m_poses, m_transforms);
1479
1480         if(!m_pca_hr_eigenvectors)
1481         {
1482             m_descriptors[desc_idx].Initialize(m_pose_count, train_image, feature_label);
1483         }
1484         else
1485         {
1486             m_descriptors[desc_idx].InitializeFast(m_pose_count, train_image, feature_label,
1487                                                    m_pca_hr_avg, m_pca_hr_eigenvectors, m_pca_descriptors);
1488         }
1489
1490         if(m_pca_avg)
1491         {
1492             m_descriptors[desc_idx].InitializePCACoeffs(m_pca_avg, m_pca_eigenvectors);
1493         }
1494     }
1495
1496     void OneWayDescriptorBase::FindDescriptor(IplImage* src, cv::Point2f pt, int& desc_idx, int& pose_idx, float& distance) const
1497     {
1498         CvRect roi = cvRect(cvRound(pt.x - m_patch_size.width/4),
1499                             cvRound(pt.y - m_patch_size.height/4),
1500                             m_patch_size.width/2, m_patch_size.height/2);
1501         cvSetImageROI(src, roi);
1502
1503         FindDescriptor(src, desc_idx, pose_idx, distance);
1504         cvResetImageROI(src);
1505     }
1506
1507     void OneWayDescriptorBase::FindDescriptor(IplImage* patch, int& desc_idx, int& pose_idx, float& distance, float* _scale, float* scale_ranges) const
1508     {
1509 #if 0
1510         ::FindOneWayDescriptor(m_train_feature_count, m_descriptors, patch, desc_idx, pose_idx, distance, m_pca_avg, m_pca_eigenvectors);
1511 #else
1512         float min = scale_min;
1513         float max = scale_max;
1514         float step = scale_step;
1515
1516         if (scale_ranges)
1517         {
1518             min = scale_ranges[0];
1519             max = scale_ranges[1];
1520         }
1521
1522         float scale = 1.0f;
1523
1524 #if !defined(_KDTREE)
1525         cv::FindOneWayDescriptorEx(m_train_feature_count, m_descriptors, patch,
1526                                    min, max, step, desc_idx, pose_idx, distance, scale,
1527                                    m_pca_avg, m_pca_eigenvectors);
1528 #else
1529         cv::FindOneWayDescriptorEx(m_pca_descriptors_tree, m_descriptors[0].GetPatchSize(), m_descriptors[0].GetPCADimLow(), m_pose_count, patch,
1530                                    min, max, step, desc_idx, pose_idx, distance, scale,
1531                                    m_pca_avg, m_pca_eigenvectors);
1532 #endif
1533
1534         if (_scale)
1535             *_scale = scale;
1536
1537 #endif
1538     }
1539
1540     void OneWayDescriptorBase::FindDescriptor(IplImage* patch, int n, std::vector<int>& desc_idxs, std::vector<int>& pose_idxs,
1541                                               std::vector<float>& distances, std::vector<float>& _scales, float* scale_ranges) const
1542     {
1543         float min = scale_min;
1544         float max = scale_max;
1545         float step = scale_step;
1546
1547         if (scale_ranges)
1548         {
1549             min = scale_ranges[0];
1550             max = scale_ranges[1];
1551         }
1552
1553         distances.resize(n);
1554         _scales.resize(n);
1555         desc_idxs.resize(n);
1556         pose_idxs.resize(n);
1557         /*float scales = 1.0f;*/
1558
1559         cv::FindOneWayDescriptorEx(m_train_feature_count, m_descriptors, patch,
1560                                    min, max, step ,n, desc_idxs, pose_idxs, distances, _scales,
1561                                    m_pca_avg, m_pca_eigenvectors);
1562
1563     }
1564
1565     void OneWayDescriptorBase::SetPCAHigh(CvMat* avg, CvMat* eigenvectors)
1566     {
1567         m_pca_hr_avg = cvCloneMat(avg);
1568         m_pca_hr_eigenvectors = cvCloneMat(eigenvectors);
1569     }
1570
1571     void OneWayDescriptorBase::SetPCALow(CvMat* avg, CvMat* eigenvectors)
1572     {
1573         m_pca_avg = cvCloneMat(avg);
1574         m_pca_eigenvectors = cvCloneMat(eigenvectors);
1575     }
1576
1577     void OneWayDescriptorBase::AllocatePCADescriptors()
1578     {
1579         m_pca_descriptors = new OneWayDescriptor[m_pca_dim_high + 1];
1580         for(int i = 0; i < m_pca_dim_high + 1; i++)
1581         {
1582             m_pca_descriptors[i].SetPCADimHigh(m_pca_dim_high);
1583             m_pca_descriptors[i].SetPCADimLow(m_pca_dim_low);
1584         }
1585     }
1586
1587     void OneWayDescriptorBase::CreatePCADescriptors()
1588     {
1589         if(m_pca_descriptors == 0)
1590         {
1591             AllocatePCADescriptors();
1592         }
1593         IplImage* frontal = cvCreateImage(m_patch_size, IPL_DEPTH_32F, 1);
1594
1595         eigenvector2image(m_pca_hr_avg, frontal);
1596         m_pca_descriptors[0].SetTransforms(m_poses, m_transforms);
1597         m_pca_descriptors[0].Initialize(m_pose_count, frontal, "", 0);
1598
1599         for(int j = 0; j < m_pca_dim_high; j++)
1600         {
1601             CvMat eigenvector;
1602             cvGetSubRect(m_pca_hr_eigenvectors, &eigenvector, cvRect(0, j, m_pca_hr_eigenvectors->cols, 1));
1603             eigenvector2image(&eigenvector, frontal);
1604
1605             m_pca_descriptors[j + 1].SetTransforms(m_poses, m_transforms);
1606             m_pca_descriptors[j + 1].Initialize(m_pose_count, frontal, "", 0);
1607
1608             printf("Created descriptor for PCA component %d\n", j);
1609         }
1610
1611         cvReleaseImage(&frontal);
1612     }
1613
1614
1615     int OneWayDescriptorBase::LoadPCADescriptors(const char* filename)
1616     {
1617         FileStorage fs = FileStorage (filename, FileStorage::READ);
1618         if(!fs.isOpened ())
1619         {
1620             printf("File %s not found...\n", filename);
1621             return 0;
1622         }
1623
1624         LoadPCADescriptors (fs.root ());
1625
1626         printf("Successfully read %d pca components\n", m_pca_dim_high);
1627         fs.release ();
1628
1629         return 1;
1630     }
1631
1632     int OneWayDescriptorBase::LoadPCADescriptors(const FileNode &fn)
1633     {
1634         // read affine poses
1635 //            FileNode* node = cvGetFileNodeByName(fs, 0, "affine poses");
1636         CvMat* poses = reinterpret_cast<CvMat*> (fn["affine_poses"].readObj ());
1637         if (poses == 0)
1638         {
1639             poses = reinterpret_cast<CvMat*> (fn["affine poses"].readObj ());
1640             if (poses == 0)
1641                 return 0;
1642         }
1643
1644
1645         if(m_poses)
1646         {
1647             delete m_poses;
1648         }
1649         m_poses = new CvAffinePose[m_pose_count];
1650         for(int i = 0; i < m_pose_count; i++)
1651         {
1652             m_poses[i].phi = (float)cvmGet(poses, i, 0);
1653             m_poses[i].theta = (float)cvmGet(poses, i, 1);
1654             m_poses[i].lambda1 = (float)cvmGet(poses, i, 2);
1655             m_poses[i].lambda2 = (float)cvmGet(poses, i, 3);
1656         }
1657         cvReleaseMat(&poses);
1658
1659         // now initialize pose transforms
1660         InitializeTransformsFromPoses();
1661
1662         m_pca_dim_high = (int) fn["pca_components_number"];
1663         if (m_pca_dim_high == 0)
1664         {
1665             m_pca_dim_high = (int) fn["pca components number"];
1666         }
1667         if(m_pca_descriptors)
1668         {
1669             delete []m_pca_descriptors;
1670         }
1671         AllocatePCADescriptors();
1672         for(int i = 0; i < m_pca_dim_high + 1; i++)
1673         {
1674             m_pca_descriptors[i].Allocate(m_pose_count, m_patch_size, 1);
1675             m_pca_descriptors[i].SetTransforms(m_poses, m_transforms);
1676             char buf[1024];
1677             sprintf(buf, "descriptor_for_pca_component_%d", i);
1678
1679             if (! m_pca_descriptors[i].ReadByName(fn, buf))
1680             {
1681                 sprintf(buf, "descriptor for pca component %d", i);
1682                 m_pca_descriptors[i].ReadByName(fn, buf);
1683             }
1684         }
1685         return 1;
1686     }
1687
1688
1689     void savePCAFeatures(FileStorage &fs, const char* postfix, CvMat* avg, CvMat* eigenvectors)
1690     {
1691         char buf[1024];
1692         sprintf(buf, "avg_%s", postfix);
1693         fs.writeObj(buf, avg);
1694         sprintf(buf, "eigenvectors_%s", postfix);
1695         fs.writeObj(buf, eigenvectors);
1696     }
1697
1698     void calcPCAFeatures(std::vector<IplImage*>& patches, FileStorage &fs, const char* postfix, CvMat** avg,
1699                          CvMat** eigenvectors)
1700     {
1701         int width = patches[0]->width;
1702         int height = patches[0]->height;
1703         int length = width * height;
1704         int patch_count = (int)patches.size();
1705
1706         CvMat* data = cvCreateMat(patch_count, length, CV_32FC1);
1707         *avg = cvCreateMat(1, length, CV_32FC1);
1708         CvMat* eigenvalues = cvCreateMat(1, length, CV_32FC1);
1709         *eigenvectors = cvCreateMat(length, length, CV_32FC1);
1710
1711         for (int i = 0; i < patch_count; i++)
1712         {
1713             float nf = (float)(1./cvSum(patches[i]).val[0]);
1714             for (int y = 0; y < height; y++)
1715             {
1716                 for (int x = 0; x < width; x++)
1717                 {
1718                     *((float*)(data->data.ptr + data->step * i) + y * width + x)
1719                             = (unsigned char)patches[i]->imageData[y * patches[i]->widthStep + x] * nf;
1720                 }
1721             }
1722         }
1723
1724         //printf("Calculating PCA...");
1725         cvCalcPCA(data, *avg, eigenvalues, *eigenvectors, CV_PCA_DATA_AS_ROW);
1726         //printf("done\n");
1727
1728         // save pca data
1729         savePCAFeatures(fs, postfix, *avg, *eigenvectors);
1730
1731         cvReleaseMat(&data);
1732         cvReleaseMat(&eigenvalues);
1733     }
1734
1735     static void extractPatches (IplImage *img, std::vector<IplImage*>& patches, CvSize patch_size)
1736     {
1737         std::vector<KeyPoint> features;
1738         Ptr<FeatureDetector> surf_extractor = FeatureDetector::create("SURF");
1739         if( !surf_extractor )
1740             CV_Error(CV_StsNotImplemented, "OpenCV was built without SURF support");
1741         surf_extractor->set("hessianThreshold", 1.0);
1742         //printf("Extracting SURF features...");
1743         surf_extractor->detect(cv::cvarrToMat(img), features);
1744         //printf("done\n");
1745
1746         for (int j = 0; j < (int)features.size(); j++)
1747         {
1748             int patch_width = patch_size.width;
1749             int patch_height = patch_size.height;
1750
1751             CvPoint center = features[j].pt;
1752
1753             CvRect roi = cvRect(center.x - patch_width / 2, center.y - patch_height / 2, patch_width, patch_height);
1754             cvSetImageROI(img, roi);
1755             roi = cvGetImageROI(img);
1756             if (roi.width != patch_width || roi.height != patch_height)
1757             {
1758                 continue;
1759             }
1760
1761             IplImage* patch = cvCreateImage(cvSize(patch_width, patch_height), IPL_DEPTH_8U, 1);
1762             cvCopy(img, patch);
1763             patches.push_back(patch);
1764             cvResetImageROI(img);
1765         }
1766         //printf("Completed file, extracted %d features\n", (int)features.size());
1767     }
1768
1769 /*
1770     void loadPCAFeatures(const FileNode &fn, std::vector<IplImage*>& patches, CvSize patch_size)
1771     {
1772         FileNodeIterator begin = fn.begin();
1773         for (FileNodeIterator i = fn.begin(); i != fn.end(); i++)
1774         {
1775             IplImage *img = reinterpret_cast<IplImage*> ((*i).readObj());
1776             extractPatches (img, patches, patch_size);
1777             cvReleaseImage(&img);
1778         }
1779     }
1780 */
1781
1782     void loadPCAFeatures(const char* path, const char* images_list, std::vector<IplImage*>& patches, CvSize patch_size)
1783     {
1784         char images_filename[1024];
1785         sprintf(images_filename, "%s/%s", path, images_list);
1786         FILE *pFile = fopen(images_filename, "r");
1787         if (pFile == 0)
1788         {
1789             printf("Cannot open images list file %s\n", images_filename);
1790             return;
1791         }
1792         while (!feof(pFile))
1793         {
1794             char imagename[1024];
1795             if (fscanf(pFile, "%s", imagename) <= 0)
1796             {
1797                 break;
1798             }
1799
1800             char filename[1024];
1801             sprintf(filename, "%s/%s", path, imagename);
1802
1803             //printf("Reading image %s...", filename);
1804             IplImage img;
1805 #ifdef HAVE_OPENCV_HIGHGUI
1806             Mat img2 = cv::imread(filename, IMREAD_GRAYSCALE);
1807             img = img2;
1808 #else
1809             CV_Error(CV_StsNotImplemented, "OpenCV has been compiled without image I/O support");
1810 #endif
1811             //printf("done\n");
1812
1813             extractPatches (&img, patches, patch_size);
1814         }
1815         fclose(pFile);
1816     }
1817
1818     void generatePCAFeatures(const char* path, const char* img_filename, FileStorage& fs, const char* postfix,
1819                              CvSize patch_size, CvMat** avg, CvMat** eigenvectors)
1820     {
1821         std::vector<IplImage*> patches;
1822         loadPCAFeatures(path, img_filename, patches, patch_size);
1823         calcPCAFeatures(patches, fs, postfix, avg, eigenvectors);
1824     }
1825
1826 /*
1827     void generatePCAFeatures(const FileNode &fn, const char* postfix,
1828                              CvSize patch_size, CvMat** avg, CvMat** eigenvectors)
1829     {
1830         std::vector<IplImage*> patches;
1831         loadPCAFeatures(fn, patches, patch_size);
1832         calcPCAFeatures(patches, fs, postfix, avg, eigenvectors);
1833     }
1834
1835
1836     void OneWayDescriptorBase::GeneratePCA(const FileNode &fn, int pose_count)
1837     {
1838         generatePCAFeatures(fn, "hr", m_patch_size, &m_pca_hr_avg, &m_pca_hr_eigenvectors);
1839         generatePCAFeatures(fn, "lr", cvSize(m_patch_size.width / 2, m_patch_size.height / 2),
1840                             &m_pca_avg, &m_pca_eigenvectors);
1841
1842
1843         OneWayDescriptorBase descriptors(m_patch_size, pose_count);
1844         descriptors.SetPCAHigh(m_pca_hr_avg, m_pca_hr_eigenvectors);
1845         descriptors.SetPCALow(m_pca_avg, m_pca_eigenvectors);
1846
1847         printf("Calculating %d PCA descriptors (you can grab a coffee, this will take a while)...\n",
1848                descriptors.GetPCADimHigh());
1849         descriptors.InitializePoseTransforms();
1850         descriptors.CreatePCADescriptors();
1851         descriptors.SavePCADescriptors(*fs);
1852     }
1853 */
1854
1855     void OneWayDescriptorBase::GeneratePCA(const char* img_path, const char* images_list, int pose_count)
1856     {
1857         char pca_filename[1024];
1858         sprintf(pca_filename, "%s/%s", img_path, GetPCAFilename().c_str());
1859         FileStorage fs = FileStorage(pca_filename, FileStorage::WRITE);
1860
1861         generatePCAFeatures(img_path, images_list, fs, "hr", m_patch_size, &m_pca_hr_avg, &m_pca_hr_eigenvectors);
1862         generatePCAFeatures(img_path, images_list, fs, "lr", cvSize(m_patch_size.width / 2, m_patch_size.height / 2),
1863                             &m_pca_avg, &m_pca_eigenvectors);
1864
1865         OneWayDescriptorBase descriptors(m_patch_size, pose_count);
1866         descriptors.SetPCAHigh(m_pca_hr_avg, m_pca_hr_eigenvectors);
1867         descriptors.SetPCALow(m_pca_avg, m_pca_eigenvectors);
1868
1869         printf("Calculating %d PCA descriptors (you can grab a coffee, this will take a while)...\n",
1870                descriptors.GetPCADimHigh());
1871         descriptors.InitializePoseTransforms();
1872         descriptors.CreatePCADescriptors();
1873         descriptors.SavePCADescriptors(*fs);
1874
1875         fs.release();
1876     }
1877
1878     void OneWayDescriptorBase::Write (FileStorage &fs) const
1879     {
1880         fs << "poseCount" << m_pose_count;
1881         fs << "patchWidth" << m_patch_size.width;
1882         fs << "patchHeight" << m_patch_size.height;
1883         fs << "minScale" << scale_min;
1884         fs << "maxScale" << scale_max;
1885         fs << "stepScale" << scale_step;
1886         fs << "pyrLevels" << m_pyr_levels;
1887         fs << "pcaDimHigh" << m_pca_dim_high;
1888         fs << "pcaDimLow" << m_pca_dim_low;
1889
1890         SavePCAall (fs);
1891     }
1892
1893     void OneWayDescriptorBase::SavePCAall (FileStorage &fs) const
1894     {
1895         savePCAFeatures(fs, "hr", m_pca_hr_avg, m_pca_hr_eigenvectors);
1896         savePCAFeatures(fs, "lr", m_pca_avg, m_pca_eigenvectors);
1897         SavePCADescriptors(*fs);
1898     }
1899
1900     void OneWayDescriptorBase::SavePCADescriptors(const char* filename)
1901     {
1902         CvMemStorage* storage = cvCreateMemStorage();
1903         CvFileStorage* fs = cvOpenFileStorage(filename, storage, CV_STORAGE_WRITE);
1904
1905         SavePCADescriptors (fs);
1906
1907         cvReleaseMemStorage(&storage);
1908         cvReleaseFileStorage(&fs);
1909     }
1910
1911     void OneWayDescriptorBase::SavePCADescriptors(CvFileStorage *fs) const
1912     {
1913         cvWriteInt(fs, "pca_components_number", m_pca_dim_high);
1914         cvWriteComment(
1915                        fs,
1916                        "The first component is the average Vector, so the total number of components is <pca components number> + 1",
1917                        0);
1918         cvWriteInt(fs, "patch_width", m_patch_size.width);
1919         cvWriteInt(fs, "patch_height", m_patch_size.height);
1920
1921         // pack the affine transforms into a single CvMat and write them
1922         CvMat* poses = cvCreateMat(m_pose_count, 4, CV_32FC1);
1923         for (int i = 0; i < m_pose_count; i++)
1924         {
1925             cvmSet(poses, i, 0, m_poses[i].phi);
1926             cvmSet(poses, i, 1, m_poses[i].theta);
1927             cvmSet(poses, i, 2, m_poses[i].lambda1);
1928             cvmSet(poses, i, 3, m_poses[i].lambda2);
1929         }
1930         cvWrite(fs, "affine_poses", poses);
1931         cvReleaseMat(&poses);
1932
1933         for (int i = 0; i < m_pca_dim_high + 1; i++)
1934         {
1935             char buf[1024];
1936             sprintf(buf, "descriptor_for_pca_component_%d", i);
1937             m_pca_descriptors[i].Write(fs, buf);
1938         }
1939     }
1940
1941
1942     void OneWayDescriptorBase::Allocate(int train_feature_count)
1943     {
1944         m_train_feature_count = train_feature_count;
1945         m_descriptors = new OneWayDescriptor[m_train_feature_count];
1946         for(int i = 0; i < m_train_feature_count; i++)
1947         {
1948             m_descriptors[i].SetPCADimHigh(m_pca_dim_high);
1949             m_descriptors[i].SetPCADimLow(m_pca_dim_low);
1950         }
1951     }
1952
1953     void OneWayDescriptorBase::InitializeDescriptors(IplImage* train_image, const std::vector<KeyPoint>& features,
1954                                                      const char* feature_label, int desc_start_idx)
1955     {
1956         for(int i = 0; i < (int)features.size(); i++)
1957         {
1958             InitializeDescriptor(desc_start_idx + i, train_image, features[i], feature_label);
1959
1960         }
1961         cvResetImageROI(train_image);
1962
1963 #if defined(_KDTREE)
1964         ConvertDescriptorsArrayToTree();
1965 #endif
1966     }
1967
1968     void OneWayDescriptorBase::CreateDescriptorsFromImage(IplImage* src, const std::vector<KeyPoint>& features)
1969     {
1970         m_train_feature_count = (int)features.size();
1971
1972         m_descriptors = new OneWayDescriptor[m_train_feature_count];
1973
1974         InitializeDescriptors(src, features);
1975
1976     }
1977
1978 #if defined(_KDTREE)
1979     void OneWayDescriptorBase::ConvertDescriptorsArrayToTree()
1980     {
1981         int n = this->GetDescriptorCount();
1982         if (n<1)
1983             return;
1984         int pca_dim_low = this->GetDescriptor(0)->GetPCADimLow();
1985
1986         //if (!m_pca_descriptors_matrix)
1987         //  m_pca_descriptors_matrix = new ::cvflann::Matrix<float>(n*m_pose_count,pca_dim_low);
1988         //else
1989         //{
1990         //  if ((m_pca_descriptors_matrix->cols != pca_dim_low)&&(m_pca_descriptors_matrix->rows != n*m_pose_count))
1991         //  {
1992         //      delete m_pca_descriptors_matrix;
1993         //      m_pca_descriptors_matrix = new ::cvflann::Matrix<float>(n*m_pose_count,pca_dim_low);
1994         //  }
1995         //}
1996
1997         m_pca_descriptors_matrix = cvCreateMat(n*m_pose_count,pca_dim_low,CV_32FC1);
1998         for (int i=0;i<n;i++)
1999         {
2000             CvMat** pca_coeffs = m_descriptors[i].GetPCACoeffs();
2001             for (int j = 0;j<m_pose_count;j++)
2002             {
2003                 for (int k=0;k<pca_dim_low;k++)
2004                 {
2005                     m_pca_descriptors_matrix->data.fl[(i*m_pose_count+j)*m_pca_dim_low + k] = pca_coeffs[j]->data.fl[k];
2006                 }
2007             }
2008         }
2009         cv::Mat pca_descriptors_mat(m_pca_descriptors_matrix,false);
2010
2011         //::cvflann::KDTreeIndexParams params;
2012         //params.trees = 1;
2013         //m_pca_descriptors_tree = new KDTree(pca_descriptors_mat);
2014         m_pca_descriptors_tree = new cv::flann::Index(pca_descriptors_mat,cv::flann::KDTreeIndexParams(1));
2015         //cvReleaseMat(&m_pca_descriptors_matrix);
2016         //m_pca_descriptors_tree->buildIndex();
2017     }
2018 #endif
2019
2020     void OneWayDescriptorObject::Allocate(int train_feature_count, int object_feature_count)
2021     {
2022         OneWayDescriptorBase::Allocate(train_feature_count);
2023         m_object_feature_count = object_feature_count;
2024
2025         m_part_id = new int[m_object_feature_count];
2026     }
2027
2028
2029     void OneWayDescriptorObject::InitializeObjectDescriptors(IplImage* train_image, const std::vector<KeyPoint>& features,
2030                                                              const char* feature_label, int desc_start_idx, float scale, int is_background)
2031     {
2032         InitializeDescriptors(train_image, features, feature_label, desc_start_idx);
2033
2034         for(int i = 0; i < (int)features.size(); i++)
2035         {
2036             CvPoint center = features[i].pt;
2037
2038             if(!is_background)
2039             {
2040                 // remember descriptor part id
2041                 CvPoint center_scaled = cvPoint(round(center.x*scale), round(center.y*scale));
2042                 m_part_id[i + desc_start_idx] = MatchPointToPart(center_scaled);
2043             }
2044         }
2045         cvResetImageROI(train_image);
2046     }
2047
2048     int OneWayDescriptorObject::IsDescriptorObject(int desc_idx) const
2049     {
2050         return desc_idx < m_object_feature_count ? 1 : 0;
2051     }
2052
2053     int OneWayDescriptorObject::MatchPointToPart(CvPoint pt) const
2054     {
2055         int idx = -1;
2056         const int max_dist = 10;
2057         for(int i = 0; i < (int)m_train_features.size(); i++)
2058         {
2059             if(norm(Point2f(pt) - m_train_features[i].pt) < max_dist)
2060             {
2061                 idx = i;
2062                 break;
2063             }
2064         }
2065
2066         return idx;
2067     }
2068
2069     int OneWayDescriptorObject::GetDescriptorPart(int desc_idx) const
2070     {
2071         //    return MatchPointToPart(GetDescriptor(desc_idx)->GetCenter());
2072         return desc_idx < m_object_feature_count ? m_part_id[desc_idx] : -1;
2073     }
2074
2075     OneWayDescriptorObject::OneWayDescriptorObject(CvSize patch_size, int pose_count, const char* train_path,
2076                                                    const char* pca_config, const char* pca_hr_config, const char* pca_desc_config, int pyr_levels) :
2077     OneWayDescriptorBase(patch_size, pose_count, train_path, pca_config, pca_hr_config, pca_desc_config, pyr_levels)
2078     {
2079         m_part_id = 0;
2080     }
2081
2082     OneWayDescriptorObject::OneWayDescriptorObject(CvSize patch_size, int pose_count, const String &pca_filename,
2083                                                    const String &train_path, const String &images_list, float _scale_min, float _scale_max, float _scale_step, int pyr_levels) :
2084     OneWayDescriptorBase(patch_size, pose_count, pca_filename, train_path, images_list, _scale_min, _scale_max, _scale_step, pyr_levels)
2085     {
2086         m_part_id = 0;
2087     }
2088
2089     OneWayDescriptorObject::~OneWayDescriptorObject()
2090     {
2091         if (m_part_id)
2092             delete []m_part_id;
2093     }
2094
2095     std::vector<KeyPoint> OneWayDescriptorObject::_GetLabeledFeatures() const
2096     {
2097         std::vector<KeyPoint> features;
2098         for(size_t i = 0; i < m_train_features.size(); i++)
2099         {
2100             features.push_back(m_train_features[i]);
2101         }
2102
2103         return features;
2104     }
2105
2106     void eigenvector2image(CvMat* eigenvector, IplImage* img)
2107     {
2108         CvRect roi = cvGetImageROI(img);
2109         if(img->depth == 32)
2110         {
2111             for(int y = 0; y < roi.height; y++)
2112             {
2113                 for(int x = 0; x < roi.width; x++)
2114                 {
2115                     float val = (float)cvmGet(eigenvector, 0, roi.width*y + x);
2116                     *((float*)(img->imageData + (roi.y + y)*img->widthStep) + roi.x + x) = val;
2117                 }
2118             }
2119         }
2120         else
2121         {
2122             for(int y = 0; y < roi.height; y++)
2123             {
2124                 for(int x = 0; x < roi.width; x++)
2125                 {
2126                     float val = (float)cvmGet(eigenvector, 0, roi.width*y + x);
2127                     img->imageData[(roi.y + y)*img->widthStep + roi.x + x] = (unsigned char)val;
2128                 }
2129             }
2130         }
2131     }
2132
2133     void readPCAFeatures(const char* filename, CvMat** avg, CvMat** eigenvectors, const char* postfix)
2134     {
2135         FileStorage fs = FileStorage(filename, FileStorage::READ);
2136         if (!fs.isOpened ())
2137         {
2138             printf("Cannot open file %s! Exiting!", filename);
2139         }
2140
2141         readPCAFeatures (fs.root (), avg, eigenvectors, postfix);
2142         fs.release ();
2143     }
2144
2145     void readPCAFeatures(const FileNode &fn, CvMat** avg, CvMat** eigenvectors, const char* postfix)
2146     {
2147         String str = String ("avg") + postfix;
2148         CvMat* _avg = reinterpret_cast<CvMat*> (fn[str].readObj());
2149         if (_avg != 0)
2150         {
2151             *avg = cvCloneMat(_avg);
2152             cvReleaseMat(&_avg);
2153         }
2154
2155         str = String ("eigenvectors") + postfix;
2156         CvMat* _eigenvectors = reinterpret_cast<CvMat*> (fn[str].readObj());
2157         if (_eigenvectors != 0)
2158         {
2159             *eigenvectors = cvCloneMat(_eigenvectors);
2160             cvReleaseMat(&_eigenvectors);
2161         }
2162     }
2163
2164     /****************************************************************************************\
2165      *                                OneWayDescriptorMatcher                                  *
2166      \****************************************************************************************/
2167
2168     OneWayDescriptorMatcher::Params::Params( int _poseCount, Size _patchSize, String _pcaFilename,
2169                                             String _trainPath, String _trainImagesList,
2170                                             float _minScale, float _maxScale, float _stepScale ) :
2171     poseCount(_poseCount), patchSize(_patchSize), pcaFilename(_pcaFilename),
2172     trainPath(_trainPath), trainImagesList(_trainImagesList),
2173     minScale(_minScale), maxScale(_maxScale), stepScale(_stepScale)
2174     {}
2175
2176
2177     OneWayDescriptorMatcher::OneWayDescriptorMatcher( const Params& _params)
2178     {
2179         initialize(_params);
2180     }
2181
2182     OneWayDescriptorMatcher::~OneWayDescriptorMatcher()
2183     {}
2184
2185     void OneWayDescriptorMatcher::initialize( const Params& _params, const Ptr<OneWayDescriptorBase>& _base )
2186     {
2187         clear();
2188
2189         if( !_base )
2190             base = _base;
2191
2192         params = _params;
2193     }
2194
2195     void OneWayDescriptorMatcher::clear()
2196     {
2197         GenericDescriptorMatcher::clear();
2198
2199         prevTrainCount = 0;
2200         if( base )
2201             base->clear();
2202     }
2203
2204     void OneWayDescriptorMatcher::train()
2205     {
2206         if( !base || prevTrainCount < (int)trainPointCollection.keypointCount() )
2207         {
2208             base.reset(
2209                 new OneWayDescriptorObject( params.patchSize, params.poseCount, params.pcaFilename,
2210                                             params.trainPath, params.trainImagesList, params.minScale, params.maxScale, params.stepScale ));
2211
2212             base->Allocate( (int)trainPointCollection.keypointCount() );
2213             prevTrainCount = (int)trainPointCollection.keypointCount();
2214
2215             const std::vector<std::vector<KeyPoint> >& points = trainPointCollection.getKeypoints();
2216             int count = 0;
2217             for( size_t i = 0; i < points.size(); i++ )
2218             {
2219                 IplImage _image = trainPointCollection.getImage((int)i);
2220                 for( size_t j = 0; j < points[i].size(); j++ )
2221                     base->InitializeDescriptor( count++, &_image, points[i][j], "" );
2222             }
2223
2224 #if defined(_KDTREE)
2225             base->ConvertDescriptorsArrayToTree();
2226 #endif
2227         }
2228     }
2229
2230     bool OneWayDescriptorMatcher::isMaskSupported()
2231     {
2232         return false;
2233     }
2234
2235     void OneWayDescriptorMatcher::knnMatchImpl( InputArray _queryImage, std::vector<KeyPoint>& queryKeypoints,
2236                                                std::vector<std::vector<DMatch> >& matches, int knn,
2237                                                const std::vector<Mat>& /*masks*/, bool /*compactResult*/ )
2238     {
2239         Mat queryImage = _queryImage.getMat();
2240         train();
2241
2242         CV_Assert( knn == 1 ); // knn > 1 unsupported because of bug in OneWayDescriptorBase for this case
2243
2244         matches.resize( queryKeypoints.size() );
2245         IplImage _qimage = queryImage;
2246         for( size_t i = 0; i < queryKeypoints.size(); i++ )
2247         {
2248             int descIdx = -1, poseIdx = -1;
2249             float distance;
2250             base->FindDescriptor( &_qimage, queryKeypoints[i].pt, descIdx, poseIdx, distance );
2251             matches[i].push_back( DMatch((int)i, descIdx, distance) );
2252         }
2253     }
2254
2255     void OneWayDescriptorMatcher::radiusMatchImpl( InputArray _queryImage, std::vector<KeyPoint>& queryKeypoints,
2256                                                   std::vector<std::vector<DMatch> >& matches, float maxDistance,
2257                                                   const std::vector<Mat>& /*masks*/, bool /*compactResult*/ )
2258     {
2259         Mat queryImage = _queryImage.getMat();
2260
2261         train();
2262
2263         matches.resize( queryKeypoints.size() );
2264         IplImage _qimage = queryImage;
2265         for( size_t i = 0; i < queryKeypoints.size(); i++ )
2266         {
2267             int descIdx = -1, poseIdx = -1;
2268             float distance;
2269             base->FindDescriptor( &_qimage, queryKeypoints[i].pt, descIdx, poseIdx, distance );
2270             if( distance < maxDistance )
2271                 matches[i].push_back( DMatch((int)i, descIdx, distance) );
2272         }
2273     }
2274
2275     void OneWayDescriptorMatcher::read( const FileNode &fn )
2276     {
2277         base.reset(
2278             new OneWayDescriptorObject( params.patchSize, params.poseCount, String (), String (), String (),
2279                                         params.minScale, params.maxScale, params.stepScale ));
2280         base->Read (fn);
2281     }
2282
2283     void OneWayDescriptorMatcher::write( FileStorage& fs ) const
2284     {
2285         base->Write (fs);
2286     }
2287
2288     bool OneWayDescriptorMatcher::empty() const
2289     {
2290         return !base || base->empty();
2291     }
2292
2293     Ptr<GenericDescriptorMatcher> OneWayDescriptorMatcher::clone( bool emptyTrainData ) const
2294     {
2295         Ptr<OneWayDescriptorMatcher> matcher = makePtr<OneWayDescriptorMatcher>( params );
2296
2297         if( !emptyTrainData )
2298         {
2299             CV_Error( CV_StsNotImplemented, "deep clone functionality is not implemented, because "
2300                      "OneWayDescriptorBase has not copy constructor or clone method ");
2301
2302             //matcher->base;
2303             matcher->params = params;
2304             matcher->prevTrainCount = prevTrainCount;
2305             matcher->trainPointCollection = trainPointCollection;
2306         }
2307         return matcher;
2308     }
2309 }