modules/legacy/src/planardetect.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                           License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
  14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
  15 // Third party copyrights are property of their respective owners.
  16 //
  17 // Redistribution and use in source and binary forms, with or without modification,
  18 // are permitted provided that the following conditions are met:
  19 //
  20 //   * Redistribution's of source code must retain the above copyright notice,
  21 //     this list of conditions and the following disclaimer.
  22 //
  23 //   * Redistribution's in binary form must reproduce the above copyright notice,
  24 //     this list of conditions and the following disclaimer in the documentation
  25 //     and/or other materials provided with the distribution.
  26 //
  27 //   * The name of the copyright holders may not be used to endorse or promote products
  28 //     derived from this software without specific prior written permission.
  29 //
  30 // This software is provided by the copyright holders and contributors "as is" and
  31 // any express or implied warranties, including, but not limited to, the implied
  32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  33 // In no event shall the Intel Corporation or contributors be liable for any direct,
  34 // indirect, incidental, special, exemplary, or consequential damages
  35 // (including, but not limited to, procurement of substitute goods or services;
  36 // loss of use, data, or profits; or business interruption) however caused
  37 // and on any theory of liability, whether in contract, strict liability,
  38 // or tort (including negligence or otherwise) arising in any way out of
  39 // the use of this software, even if advised of the possibility of such damage.
  40 //
  41 //M*/
  42
  43 #include "precomp.hpp"
  44 #include "opencv2/calib3d.hpp"
  45 #include <stdio.h>
  46
  47 namespace cv
  48 {
  49
  50 /*
  51   The code below implements keypoint detector, fern-based point classifier and a planar object detector.
  52
  53   References:
  54    1. Mustafa Özuysal, Michael Calonder, Vincent Lepetit, Pascal Fua,
  55       "Fast KeyPoint Recognition Using Random Ferns,"
  56       IEEE Transactions on Pattern Analysis and Machine Intelligence, 15 Jan. 2009.
  57
  58    2. Vincent Lepetit, Pascal Fua,
  59       "Towards Recognizing Feature Points Using Classification Trees,"
  60       Technical Report IC/2004/74, EPFL, 2004.
  61 */
  62
  63 const int progressBarSize = 50;
  64
  65 //////////////////////////// Patch Generator //////////////////////////////////
  66
  67 static const double DEFAULT_BACKGROUND_MIN = 0;
  68 static const double DEFAULT_BACKGROUND_MAX = 256;
  69 static const double DEFAULT_NOISE_RANGE = 5;
  70 static const double DEFAULT_LAMBDA_MIN = 0.6;
  71 static const double DEFAULT_LAMBDA_MAX = 1.5;
  72 static const double DEFAULT_THETA_MIN = -CV_PI;
  73 static const double DEFAULT_THETA_MAX = CV_PI;
  74 static const double DEFAULT_PHI_MIN = -CV_PI;
  75 static const double DEFAULT_PHI_MAX = CV_PI;
  76
  77 PatchGenerator::PatchGenerator()
  78 : backgroundMin(DEFAULT_BACKGROUND_MIN), backgroundMax(DEFAULT_BACKGROUND_MAX),
  79 noiseRange(DEFAULT_NOISE_RANGE), randomBlur(true), lambdaMin(DEFAULT_LAMBDA_MIN),
  80 lambdaMax(DEFAULT_LAMBDA_MAX), thetaMin(DEFAULT_THETA_MIN),
  81 thetaMax(DEFAULT_THETA_MAX), phiMin(DEFAULT_PHI_MIN),
  82 phiMax(DEFAULT_PHI_MAX)
  83 {
  84 }
  85
  86
  87 PatchGenerator::PatchGenerator(double _backgroundMin, double _backgroundMax,
  88                                double _noiseRange, bool _randomBlur,
  89                                double _lambdaMin, double _lambdaMax,
  90                                double _thetaMin, double _thetaMax,
  91                                double _phiMin, double _phiMax )
  92 : backgroundMin(_backgroundMin), backgroundMax(_backgroundMax),
  93 noiseRange(_noiseRange), randomBlur(_randomBlur),
  94 lambdaMin(_lambdaMin), lambdaMax(_lambdaMax),
  95 thetaMin(_thetaMin), thetaMax(_thetaMax),
  96 phiMin(_phiMin), phiMax(_phiMax)
  97 {
  98 }
  99
 100
 101 void PatchGenerator::generateRandomTransform(Point2f srcCenter, Point2f dstCenter,
 102                                              Mat& transform, RNG& rng, bool inverse) const
 103 {
 104     double lambda1 = rng.uniform(lambdaMin, lambdaMax);
 105     double lambda2 = rng.uniform(lambdaMin, lambdaMax);
 106     double theta = rng.uniform(thetaMin, thetaMax);
 107     double phi = rng.uniform(phiMin, phiMax);
 108
 109     // Calculate random parameterized affine transformation A,
 110     // A = T(patch center) * R(theta) * R(phi)' *
 111     //     S(lambda1, lambda2) * R(phi) * T(-pt)
 112     double st = sin(theta);
 113     double ct = cos(theta);
 114     double sp = sin(phi);
 115     double cp = cos(phi);
 116     double c2p = cp*cp;
 117     double s2p = sp*sp;
 118
 119     double A = lambda1*c2p + lambda2*s2p;
 120     double B = (lambda2 - lambda1)*sp*cp;
 121     double C = lambda1*s2p + lambda2*c2p;
 122
 123     double Ax_plus_By = A*srcCenter.x + B*srcCenter.y;
 124     double Bx_plus_Cy = B*srcCenter.x + C*srcCenter.y;
 125
 126     transform.create(2, 3, CV_64F);
 127     Mat_<double>& T = (Mat_<double>&)transform;
 128     T(0,0) = A*ct - B*st;
 129     T(0,1) = B*ct - C*st;
 130     T(0,2) = -ct*Ax_plus_By + st*Bx_plus_Cy + dstCenter.x;
 131     T(1,0) = A*st + B*ct;
 132     T(1,1) = B*st + C*ct;
 133     T(1,2) = -st*Ax_plus_By - ct*Bx_plus_Cy + dstCenter.y;
 134
 135     if( inverse )
 136         invertAffineTransform(T, T);
 137 }
 138
 139
 140 void PatchGenerator::operator ()(const Mat& image, Point2f pt, Mat& patch, Size patchSize, RNG& rng) const
 141 {
 142     double buffer[6];
 143     Mat_<double> T(2, 3, buffer);
 144
 145     generateRandomTransform(pt, Point2f((patchSize.width-1)*0.5f, (patchSize.height-1)*0.5f), T, rng);
 146     (*this)(image, T, patch, patchSize, rng);
 147 }
 148
 149
 150 void PatchGenerator::operator ()(const Mat& image, const Mat& T,
 151                                  Mat& patch, Size patchSize, RNG& rng) const
 152 {
 153     patch.create( patchSize, image.type() );
 154     if( backgroundMin != backgroundMax )
 155     {
 156         rng.fill(patch, RNG::UNIFORM, Scalar::all(backgroundMin), Scalar::all(backgroundMax));
 157         warpAffine(image, patch, T, patchSize, INTER_LINEAR, BORDER_TRANSPARENT);
 158     }
 159     else
 160         warpAffine(image, patch, T, patchSize, INTER_LINEAR, BORDER_CONSTANT, Scalar::all(backgroundMin));
 161
 162     int ksize = randomBlur ? (unsigned)rng % 9 - 5 : 0;
 163     if( ksize > 0 )
 164     {
 165         ksize = ksize*2 + 1;
 166         GaussianBlur(patch, patch, Size(ksize, ksize), 0, 0);
 167     }
 168
 169     if( noiseRange > 0 )
 170     {
 171         AutoBuffer<uchar> _noiseBuf( patchSize.width*patchSize.height*image.elemSize() );
 172         Mat noise(patchSize, image.type(), (uchar*)_noiseBuf);
 173         int delta = image.depth() == CV_8U ? 128 : image.depth() == CV_16U ? 32768 : 0;
 174         rng.fill(noise, RNG::NORMAL, Scalar::all(delta), Scalar::all(noiseRange));
 175         if( backgroundMin != backgroundMax )
 176             addWeighted(patch, 1, noise, 1, -delta, patch);
 177         else
 178         {
 179             for( int i = 0; i < patchSize.height; i++ )
 180             {
 181                 uchar* prow = patch.ptr<uchar>(i);
 182                 const uchar* nrow =  noise.ptr<uchar>(i);
 183                 for( int j = 0; j < patchSize.width; j++ )
 184                     if( prow[j] != backgroundMin )
 185                         prow[j] = saturate_cast<uchar>(prow[j] + nrow[j] - delta);
 186             }
 187         }
 188     }
 189 }
 190
 191 void PatchGenerator::warpWholeImage(const Mat& image, Mat& matT, Mat& buf,
 192                                     Mat& warped, int border, RNG& rng) const
 193 {
 194     Mat_<double> T = matT;
 195     Rect roi(INT_MAX, INT_MAX, INT_MIN, INT_MIN);
 196
 197     for( int k = 0; k < 4; k++ )
 198     {
 199         Point2f pt0, pt1;
 200         pt0.x = (float)(k == 0 || k == 3 ? 0 : image.cols);
 201         pt0.y = (float)(k < 2 ? 0 : image.rows);
 202         pt1.x = (float)(T(0,0)*pt0.x + T(0,1)*pt0.y + T(0,2));
 203         pt1.y = (float)(T(1,0)*pt0.x + T(1,1)*pt0.y + T(1,2));
 204
 205         roi.x = std::min(roi.x, cvFloor(pt1.x));
 206         roi.y = std::min(roi.y, cvFloor(pt1.y));
 207         roi.width = std::max(roi.width, cvCeil(pt1.x));
 208         roi.height = std::max(roi.height, cvCeil(pt1.y));
 209     }
 210
 211     roi.width -= roi.x - 1;
 212     roi.height -= roi.y - 1;
 213     int dx = border - roi.x;
 214     int dy = border - roi.y;
 215
 216     if( (roi.width+border*2)*(roi.height+border*2) > buf.cols )
 217         buf.create(1, (roi.width+border*2)*(roi.height+border*2), image.type());
 218
 219     warped = Mat(roi.height + border*2, roi.width + border*2,
 220                  image.type(), buf.data);
 221
 222     T(0,2) += dx;
 223     T(1,2) += dy;
 224     (*this)(image, T, warped, warped.size(), rng);
 225
 226     if( T.data != matT.data )
 227         T.convertTo(matT, matT.type());
 228 }
 229
 230
 231 // Params are assumed to be symmetrical: lambda w.r.t. 1, theta and phi w.r.t. 0
 232 void PatchGenerator::setAffineParam(double lambda, double theta, double phi)
 233 {
 234    lambdaMin = 1. - lambda;
 235    lambdaMax = 1. + lambda;
 236    thetaMin = -theta;
 237    thetaMax = theta;
 238    phiMin = -phi;
 239    phiMax = phi;
 240 }
 241
 242
 243 /////////////////////////////////////// LDetector //////////////////////////////////////////////
 244
 245 LDetector::LDetector() : radius(7), threshold(20), nOctaves(3), nViews(1000),
 246     verbose(false), baseFeatureSize(32), clusteringDistance(2)
 247 {
 248 }
 249
 250 LDetector::LDetector(int _radius, int _threshold, int _nOctaves, int _nViews,
 251                      double _baseFeatureSize, double _clusteringDistance)
 252 : radius(_radius), threshold(_threshold), nOctaves(_nOctaves), nViews(_nViews),
 253     verbose(false), baseFeatureSize(_baseFeatureSize), clusteringDistance(_clusteringDistance)
 254 {
 255 }
 256
 257 static void getDiscreteCircle(int R, std::vector<Point>& circle, std::vector<int>& filledHCircle)
 258 {
 259     int x = R, y = 0;
 260     for( ;;y++ )
 261     {
 262         x = cvRound(std::sqrt((double)R*R - y*y));
 263         if( x < y )
 264             break;
 265         circle.push_back(Point(x,y));
 266         if( x == y )
 267             break;
 268     }
 269
 270     int i, n8 = (int)circle.size() - (x == y), n8_ = n8 - (x != y), n4 = n8 + n8_, n = n4*4;
 271     CV_Assert(n8 > 0);
 272     circle.resize(n);
 273
 274     for( i = 0; i < n8; i++ )
 275     {
 276         Point p = circle[i];
 277         circle[i+n4] = Point(-p.y, p.x);
 278         circle[i+n4*2] = Point(-p.x, -p.y);
 279         circle[i+n4*3] = Point(p.y, -p.x);
 280     }
 281
 282     for( i = n8; i < n4; i++ )
 283     {
 284         Point p = circle[n4 - i], q = Point(p.y, p.x);
 285         circle[i] = q;
 286         circle[i+n4] = Point(-q.y, q.x);
 287         circle[i+n4*2] = Point(-q.x, -q.y);
 288         circle[i+n4*3] = Point(q.y, -q.x);
 289     }
 290
 291     // the filled upper half of the circle is encoded as sequence of integers,
 292     // i-th element is the coordinate of right-most circle point in each horizontal line y=i.
 293     // the left-most point will be -filledHCircle[i].
 294     for( i = 0, y = -1; i < n4; i++ )
 295     {
 296         Point p = circle[i];
 297         if( p.y != y )
 298         {
 299             filledHCircle.push_back(p.x);
 300             y = p.y;
 301             if( y == R )
 302                 break;
 303         }
 304     }
 305 }
 306
 307
 308 struct CmpKeypointScores
 309 {
 310     bool operator ()(const KeyPoint& a, const KeyPoint& b) const { return std::abs(a.response) > std::abs(b.response); }
 311 };
 312
 313
 314 void LDetector::getMostStable2D(const Mat& image, std::vector<KeyPoint>& keypoints,
 315                                 int maxPoints, const PatchGenerator& _patchGenerator) const
 316 {
 317     PatchGenerator patchGenerator = _patchGenerator;
 318     patchGenerator.backgroundMin = patchGenerator.backgroundMax = 128;
 319
 320     Mat warpbuf, warped;
 321     Mat matM(2, 3, CV_64F), _iM(2, 3, CV_64F);
 322     double *M = (double*)matM.data, *iM = (double*)_iM.data;
 323     RNG& rng = theRNG();
 324     int i, k;
 325     std::vector<KeyPoint> tempKeypoints;
 326     double d2 = clusteringDistance*clusteringDistance;
 327     keypoints.clear();
 328
 329     // TODO: this loop can be run in parallel, for that we need
 330     // a separate accumulator keypoint lists for different threads.
 331     for( i = 0; i < nViews; i++ )
 332     {
 333         // 1. generate random transform
 334         // 2. map the source image corners and compute the ROI in canvas
 335         // 3. select the ROI in canvas, adjust the transformation matrix
 336         // 4. apply the transformation
 337         // 5. run keypoint detector in pyramids
 338         // 6. map each point back and update the lists of most stable points
 339
 340         if(verbose && (i+1)*progressBarSize/nViews != i*progressBarSize/nViews)
 341             putchar('.');
 342
 343         if( i > 0 )
 344             patchGenerator.generateRandomTransform(Point2f(), Point2f(), matM, rng);
 345         else
 346         {
 347             // identity transformation
 348             M[0] = M[4] = 1;
 349             M[1] = M[3] = M[2] = M[5] = 0;
 350         }
 351
 352         patchGenerator.warpWholeImage(image, matM, warpbuf, warped, cvCeil(baseFeatureSize*0.5+radius), rng);
 353         (*this)(warped, tempKeypoints, maxPoints*3);
 354         invertAffineTransform(matM, _iM);
 355
 356         int j, sz0 = (int)tempKeypoints.size(), sz1;
 357         for( j = 0; j < sz0; j++ )
 358         {
 359             KeyPoint kpt1 = tempKeypoints[j];
 360             KeyPoint kpt0((float)(iM[0]*kpt1.pt.x + iM[1]*kpt1.pt.y + iM[2]),
 361                           (float)(iM[3]*kpt1.pt.x + iM[4]*kpt1.pt.y + iM[5]),
 362                           kpt1.size, -1.f, 1.f, kpt1.octave);
 363             float r = kpt1.size*0.5f;
 364             if( kpt0.pt.x < r || kpt0.pt.x >= image.cols - r ||
 365                kpt0.pt.y < r || kpt0.pt.y >= image.rows - r )
 366                 continue;
 367
 368             sz1 = (int)keypoints.size();
 369             for( k = 0; k < sz1; k++ )
 370             {
 371                 KeyPoint kpt = keypoints[k];
 372                 if( kpt.octave != kpt0.octave )
 373                     continue;
 374                 double dx = kpt.pt.x - kpt0.pt.x, dy = kpt.pt.y - kpt0.pt.y;
 375                 if( dx*dx + dy*dy <= d2*(1 << kpt.octave*2) )
 376                 {
 377                     keypoints[k] = KeyPoint((kpt.pt.x*kpt.response + kpt0.pt.x)/(kpt.response+1),
 378                                             (kpt.pt.y*kpt.response + kpt0.pt.y)/(kpt.response+1),
 379                                             kpt.size, -1.f, kpt.response + 1, kpt.octave);
 380                     break;
 381                 }
 382             }
 383             if( k == sz1 )
 384                 keypoints.push_back(kpt0);
 385         }
 386     }
 387
 388     if( verbose )
 389         putchar('\n');
 390
 391     if( (int)keypoints.size() > maxPoints )
 392     {
 393         std::sort(keypoints.begin(), keypoints.end(), CmpKeypointScores());
 394         keypoints.resize(maxPoints);
 395     }
 396 }
 397
 398
 399 static inline int computeLResponse(const uchar* ptr, const int* cdata, int csize)
 400 {
 401     int i, csize2 = csize/2, sum = -ptr[0]*csize;
 402     for( i = 0; i < csize2; i++ )
 403     {
 404         int ofs = cdata[i];
 405         sum += ptr[ofs] + ptr[-ofs];
 406     }
 407     return sum;
 408 }
 409
 410
 411 static Point2f adjustCorner(const float* fval, float& fvaln)
 412 {
 413     double bx = (fval[3] - fval[5])*0.5;
 414     double by = (fval[2] - fval[7])*0.5;
 415     double Axx = fval[3] - fval[4]*2 + fval[5];
 416     double Axy = (fval[0] - fval[2] - fval[6] + fval[8])*0.25;
 417     double Ayy = fval[1] - fval[4]*2 + fval[7];
 418     double D = Axx*Ayy - Axy*Axy;
 419     D = D != 0 ? 1./D : 0;
 420     double dx = (bx*Ayy - by*Axy)*D;
 421     double dy = (by*Axx - bx*Axy)*D;
 422     dx = std::min(std::max(dx, -1.), 1.);
 423     dy = std::min(std::max(dy, -1.), 1.);
 424     fvaln = (float)(fval[4] + (bx*dx + by*dy)*0.5);
 425     if(fvaln*fval[4] < 0 || std::abs(fvaln) < std::abs(fval[4]))
 426         fvaln = fval[4];
 427
 428     return Point2f((float)dx, (float)dy);
 429 }
 430
 431 void LDetector::operator()(const Mat& image, std::vector<KeyPoint>& keypoints, int maxCount, bool scaleCoords) const
 432 {
 433     std::vector<Mat> pyr;
 434     buildPyramid(image, pyr, std::max(nOctaves-1, 0));
 435     (*this)(pyr, keypoints, maxCount, scaleCoords);
 436 }
 437
 438 void LDetector::operator()(const std::vector<Mat>& pyr, std::vector<KeyPoint>& keypoints, int maxCount, bool scaleCoords) const
 439 {
 440     const int lthreshold = 3;
 441     int L, x, y, i, j, k, tau = lthreshold;
 442     Mat scoreBuf(pyr[0].size(), CV_16S), maskBuf(pyr[0].size(), CV_8U);
 443     int scoreElSize = (int)scoreBuf.elemSize();
 444     std::vector<Point> circle0;
 445     std::vector<int> fhcircle0, circle, fcircle_s, fcircle;
 446     getDiscreteCircle(radius, circle0, fhcircle0);
 447     CV_Assert(fhcircle0.size() == (size_t)(radius+1) && circle0.size() % 2 == 0);
 448     keypoints.clear();
 449
 450     for( L = 0; L < nOctaves; L++ )
 451     {
 452         //  Pyramidal keypoint detector body:
 453         //    1. build next pyramid layer
 454         //    2. scan points, check the circular neighborhood, compute the score
 455         //    3. do non-maxima suppression
 456         //    4. adjust the corners (sub-pix)
 457         double cscale = scaleCoords ? 1 << L : 1;
 458         Size layerSize = pyr[L].size();
 459         if( layerSize.width < radius*2 + 3 || layerSize.height < radius*2 + 3 )
 460             break;
 461         Mat scoreLayer(layerSize, scoreBuf.type(), scoreBuf.data);
 462         Mat maskLayer(layerSize, maskBuf.type(), maskBuf.data);
 463         const Mat& pyrLayer = pyr[L];
 464         int sstep = (int)(scoreLayer.step/sizeof(short));
 465         int mstep = (int)maskLayer.step;
 466
 467         int csize = (int)circle0.size(), csize2 = csize/2;
 468         circle.resize(csize*3);
 469         for( i = 0; i < csize; i++ )
 470             circle[i] = circle[i+csize] = circle[i+csize*2] = (int)((-circle0[i].y)*pyrLayer.step + circle0[i].x);
 471         fcircle.clear();
 472         fcircle_s.clear();
 473         for( i = -radius; i <= radius; i++ )
 474         {
 475             x = fhcircle0[std::abs(i)];
 476             for( j = -x; j <= x; j++ )
 477             {
 478                 fcircle_s.push_back(i*sstep + j);
 479                 fcircle.push_back((int)(i*pyrLayer.step + j));
 480             }
 481         }
 482         int nsize = (int)fcircle.size();
 483         const int* cdata = &circle[0];
 484         const int* ndata = &fcircle[0];
 485         const int* ndata_s = &fcircle_s[0];
 486
 487         for( y = 0; y < radius; y++ )
 488         {
 489             memset( scoreLayer.ptr<short>(y), 0, layerSize.width*scoreElSize );
 490             memset( scoreLayer.ptr<short>(layerSize.height-y-1), 0, layerSize.width*scoreElSize );
 491             memset( maskLayer.ptr<uchar>(y), 0, layerSize.width );
 492             memset( maskLayer.ptr<uchar>(layerSize.height-y-1), 0, layerSize.width );
 493         }
 494
 495         int vradius = (int)(radius*pyrLayer.step);
 496
 497         for( y = radius; y < layerSize.height - radius; y++ )
 498         {
 499             const uchar* img = pyrLayer.ptr<uchar>(y) + radius;
 500             short* scores = scoreLayer.ptr<short>(y);
 501             uchar* mask = maskLayer.ptr<uchar>(y);
 502
 503             for( x = 0; x < radius; x++ )
 504             {
 505                 scores[x] = scores[layerSize.width - 1 - x] = 0;
 506                 mask[x] = mask[layerSize.width - 1 - x] = 0;
 507             }
 508
 509             for( x = radius; x < layerSize.width - radius; x++, img++ )
 510             {
 511                 int val0 = *img;
 512                 if( (std::abs(val0 - img[radius]) < tau && std::abs(val0 - img[-radius]) < tau) ||
 513                    (std::abs(val0 - img[vradius]) < tau && std::abs(val0 - img[-vradius]) < tau))
 514                 {
 515                     scores[x] = 0;
 516                     mask[x] = 0;
 517                     continue;
 518                 }
 519
 520                 for( k = 0; k < csize; k++ )
 521                 {
 522                     if( std::abs(val0 - img[cdata[k]]) < tau &&
 523                        (std::abs(val0 - img[cdata[k + csize2]]) < tau ||
 524                         std::abs(val0 - img[cdata[k + csize2 - 1]]) < tau ||
 525                         std::abs(val0 - img[cdata[k + csize2 + 1]]) < tau ||
 526                         std::abs(val0 - img[cdata[k + csize2 - 2]]) < tau ||
 527                         std::abs(val0 - img[cdata[k + csize2 + 2]]) < tau/* ||
 528                      std::abs(val0 - img[cdata[k + csize2 - 3]]) < tau ||
 529                      std::abs(val0 - img[cdata[k + csize2 + 3]]) < tau*/) )
 530                         break;
 531                 }
 532
 533                 if( k < csize )
 534                 {
 535                     scores[x] = 0;
 536                     mask[x] = 0;
 537                 }
 538                 else
 539                 {
 540                     scores[x] = (short)computeLResponse(img, cdata, csize);
 541                     mask[x] = 1;
 542                 }
 543             }
 544         }
 545
 546         for( y = radius+1; y < layerSize.height - radius-1; y++ )
 547         {
 548             const uchar* img = pyrLayer.ptr<uchar>(y) + radius+1;
 549             short* scores = scoreLayer.ptr<short>(y) + radius+1;
 550             const uchar* mask = maskLayer.ptr<uchar>(y) + radius+1;
 551
 552             for( x = radius+1; x < layerSize.width - radius-1; x++, img++, scores++, mask++ )
 553             {
 554                 int val0 = *scores;
 555                 if( !*mask || std::abs(val0) < lthreshold ||
 556                    (mask[-1] + mask[1] + mask[-mstep-1] + mask[-mstep] + mask[-mstep+1]+
 557                     mask[mstep-1] + mask[mstep] + mask[mstep+1] < 3))
 558                     continue;
 559                 bool recomputeZeroScores = radius*2 < y && y < layerSize.height - radius*2 &&
 560                 radius*2 < x && x < layerSize.width - radius*2;
 561
 562                 if( val0 > 0 )
 563                 {
 564                     for( k = 0; k < nsize; k++ )
 565                     {
 566                         int val = scores[ndata_s[k]];
 567                         if( val == 0 && recomputeZeroScores )
 568                             scores[ndata_s[k]] = (short)(val =
 569                                 computeLResponse(img + ndata[k], cdata, csize));
 570                         if( val0 < val )
 571                             break;
 572                     }
 573                 }
 574                 else
 575                 {
 576                     for( k = 0; k < nsize; k++ )
 577                     {
 578                         int val = scores[ndata_s[k]];
 579                         if( val == 0 && recomputeZeroScores )
 580                             scores[ndata_s[k]] = (short)(val =
 581                                 computeLResponse(img + ndata[k], cdata, csize));
 582                         if( val0 > val )
 583                             break;
 584                     }
 585                 }
 586                 if( k < nsize )
 587                     continue;
 588                 float fval[9], fvaln = 0;
 589                 for( int i1 = -1; i1 <= 1; i1++ )
 590                     for( int j1 = -1; j1 <= 1; j1++ )
 591                     {
 592                         fval[(i1+1)*3 + j1 + 1] = (float)(scores[sstep*i1+j1] ? scores[sstep*i1+j1] :
 593                             computeLResponse(img + pyrLayer.step*i1 + j1, cdata, csize));
 594                     }
 595                 Point2f pt = adjustCorner(fval, fvaln);
 596                 pt.x += x;
 597                 pt.y += y;
 598                 keypoints.push_back(KeyPoint((float)(pt.x*cscale), (float)(pt.y*cscale),
 599                                              (float)(baseFeatureSize*cscale), -1, fvaln, L));
 600             }
 601         }
 602     }
 603
 604     if( maxCount > 0 && keypoints.size() > (size_t)maxCount )
 605     {
 606         std::sort(keypoints.begin(), keypoints.end(), CmpKeypointScores());
 607         keypoints.resize(maxCount);
 608     }
 609 }
 610
 611 void LDetector::read(const FileNode& objnode)
 612 {
 613     radius = (int)objnode["radius"];
 614     threshold = (int)objnode["threshold"];
 615     nOctaves = (int)objnode["noctaves"];
 616     nViews = (int)objnode["nviews"];
 617     baseFeatureSize = (int)objnode["base-feature-size"];
 618     clusteringDistance = (int)objnode["clustering-distance"];
 619 }
 620
 621 void LDetector::write(FileStorage& fs, const String& name) const
 622 {
 623     internal::WriteStructContext ws(fs, name, CV_NODE_MAP);
 624
 625     fs << "radius" << radius
 626     << "threshold" << threshold
 627     << "noctaves" << nOctaves
 628     << "nviews" << nViews
 629     << "base-feature-size" << baseFeatureSize
 630     << "clustering-distance" << clusteringDistance;
 631 }
 632
 633 void LDetector::setVerbose(bool _verbose)
 634 {
 635     verbose = _verbose;
 636 }
 637
 638 /////////////////////////////////////// FernClassifier ////////////////////////////////////////////
 639
 640 FernClassifier::FernClassifier()
 641 {
 642     verbose = false;
 643     clear();
 644 }
 645
 646
 647 FernClassifier::FernClassifier(const FileNode& node)
 648 {
 649     verbose = false;
 650     clear();
 651     read(node);
 652 }
 653
 654 FernClassifier::~FernClassifier()
 655 {
 656 }
 657
 658
 659 int FernClassifier::getClassCount() const
 660 {
 661     return nclasses;
 662 }
 663
 664
 665 int FernClassifier::getStructCount() const
 666 {
 667     return nstructs;
 668 }
 669
 670
 671 int FernClassifier::getStructSize() const
 672 {
 673     return structSize;
 674 }
 675
 676
 677 int FernClassifier::getSignatureSize() const
 678 {
 679     return signatureSize;
 680 }
 681
 682
 683 int FernClassifier::getCompressionMethod() const
 684 {
 685     return compressionMethod;
 686 }
 687
 688
 689 Size FernClassifier::getPatchSize() const
 690 {
 691     return patchSize;
 692 }
 693
 694
 695 FernClassifier::FernClassifier(const std::vector<std::vector<Point2f> >& points,
 696                                const std::vector<Mat>& refimgs,
 697                                const std::vector<std::vector<int> >& labels,
 698                                int _nclasses, int _patchSize,
 699                                int _signatureSize, int _nstructs,
 700                                int _structSize, int _nviews, int _compressionMethod,
 701                                const PatchGenerator& patchGenerator)
 702 {
 703     verbose = false;
 704     clear();
 705     train(points, refimgs, labels, _nclasses, _patchSize,
 706           _signatureSize, _nstructs, _structSize, _nviews,
 707           _compressionMethod, patchGenerator);
 708 }
 709
 710
 711 void FernClassifier::write(FileStorage& fs, const String& objname) const
 712 {
 713     internal::WriteStructContext ws(fs, objname, CV_NODE_MAP);
 714
 715     cv::write(fs, "nstructs", nstructs);
 716     cv::write(fs, "struct-size", structSize);
 717     cv::write(fs, "nclasses", nclasses);
 718     cv::write(fs, "signature-size", signatureSize);
 719     cv::write(fs, "compression-method", compressionMethod);
 720     cv::write(fs, "patch-size", patchSize.width);
 721     {
 722         internal::WriteStructContext wsf(fs, "features", CV_NODE_SEQ + CV_NODE_FLOW);
 723         int i, nfeatures = (int)features.size();
 724         for( i = 0; i < nfeatures; i++ )
 725         {
 726             cv::write(fs, features[i].y1*patchSize.width + features[i].x1);
 727             cv::write(fs, features[i].y2*patchSize.width + features[i].x2);
 728         }
 729     }
 730     {
 731         internal::WriteStructContext wsp(fs, "posteriors", CV_NODE_SEQ + CV_NODE_FLOW);
 732         cv::write(fs, posteriors);
 733     }
 734 }
 735
 736
 737 void FernClassifier::read(const FileNode& objnode)
 738 {
 739     clear();
 740
 741     nstructs = (int)objnode["nstructs"];
 742     structSize = (int)objnode["struct-size"];
 743     nclasses = (int)objnode["nclasses"];
 744     signatureSize = (int)objnode["signature-size"];
 745     compressionMethod = (int)objnode["compression-method"];
 746     patchSize.width = patchSize.height = (int)objnode["patch-size"];
 747     leavesPerStruct = 1 << structSize;
 748
 749     FileNode _nodes = objnode["features"];
 750     int i, nfeatures = structSize*nstructs;
 751     features.resize(nfeatures);
 752     FileNodeIterator it = _nodes.begin(), it_end = _nodes.end();
 753     for( i = 0; i < nfeatures && it != it_end; i++ )
 754     {
 755         int ofs1, ofs2;
 756         it >> ofs1 >> ofs2;
 757         features[i] = Feature(ofs1%patchSize.width, ofs1/patchSize.width,
 758                               ofs2%patchSize.width, ofs2/patchSize.width);
 759     }
 760
 761     FileNode _posteriors = objnode["posteriors"];
 762     int psz = leavesPerStruct*nstructs*signatureSize;
 763     posteriors.reserve(psz);
 764     _posteriors >> posteriors;
 765 }
 766
 767
 768 void FernClassifier::clear()
 769 {
 770     signatureSize = nclasses = nstructs = structSize = compressionMethod = leavesPerStruct = 0;
 771     std::vector<Feature>().swap(features);
 772     std::vector<float>().swap(posteriors);
 773 }
 774
 775 bool FernClassifier::empty() const
 776 {
 777     return features.empty();
 778 }
 779
 780 int FernClassifier::getLeaf(int fern, const Mat& _patch) const
 781 {
 782     assert( 0 <= fern && fern < nstructs );
 783     size_t fofs = fern*structSize, idx = 0;
 784     const Mat_<uchar>& patch = (const Mat_<uchar>&)_patch;
 785
 786     for( int i = 0; i < structSize; i++ )
 787     {
 788         const Feature& f = features[fofs + i];
 789         idx = (idx << 1) + f(patch);
 790     }
 791
 792     return (int)(fern*leavesPerStruct + idx);
 793 }
 794
 795
 796 void FernClassifier::prepare(int _nclasses, int _patchSize, int _signatureSize,
 797                              int _nstructs, int _structSize,
 798                              int _nviews, int _compressionMethod)
 799 {
 800     clear();
 801
 802     CV_Assert( _nclasses > 1 && _patchSize >= 5 && _nstructs > 0 &&
 803               _nviews > 0 && _structSize > 0 &&
 804               (_compressionMethod == COMPRESSION_NONE ||
 805                _compressionMethod == COMPRESSION_RANDOM_PROJ ||
 806                _compressionMethod == COMPRESSION_PCA) );
 807
 808     nclasses = _nclasses;
 809     patchSize = Size(_patchSize, _patchSize);
 810     nstructs = _nstructs;
 811     structSize = _structSize;
 812     signatureSize = _compressionMethod == COMPRESSION_NONE ? nclasses : std::min(_signatureSize, nclasses);
 813     compressionMethod = signatureSize == nclasses ? COMPRESSION_NONE : _compressionMethod;
 814
 815     leavesPerStruct = 1 << structSize;
 816
 817     int i, nfeatures = structSize*nstructs;
 818
 819     features = std::vector<Feature>( nfeatures );
 820     posteriors = std::vector<float>( leavesPerStruct*nstructs*nclasses, 1.f );
 821     classCounters = std::vector<int>( nclasses, leavesPerStruct );
 822
 823     CV_Assert( patchSize.width <= 256 && patchSize.height <= 256 );
 824     RNG& rng = theRNG();
 825
 826     for( i = 0; i < nfeatures; i++ )
 827     {
 828         int x1 = (unsigned)rng % patchSize.width;
 829         int y1 = (unsigned)rng % patchSize.height;
 830         int x2 = (unsigned)rng % patchSize.width;
 831         int y2 = (unsigned)rng % patchSize.height;
 832         features[i] = Feature(x1, y1, x2, y2);
 833     }
 834 }
 835
 836 static int calcNumPoints( const std::vector<std::vector<Point2f> >& points )
 837 {
 838     size_t count = 0;
 839     for( size_t i = 0; i < points.size(); i++ )
 840         count += points[i].size();
 841     return (int)count;
 842 }
 843
 844 void FernClassifier::train(const std::vector<std::vector<Point2f> >& points,
 845                            const std::vector<Mat>& refimgs,
 846                            const std::vector<std::vector<int> >& labels,
 847                            int _nclasses, int _patchSize,
 848                            int _signatureSize, int _nstructs,
 849                            int _structSize, int _nviews, int _compressionMethod,
 850                            const PatchGenerator& patchGenerator)
 851 {
 852     CV_Assert( points.size() == refimgs.size() );
 853     int numPoints = calcNumPoints( points );
 854     _nclasses = (!labels.empty() && _nclasses>0) ? _nclasses : numPoints;
 855     CV_Assert( labels.empty() || labels.size() == points.size() );
 856
 857
 858     prepare(_nclasses, _patchSize, _signatureSize, _nstructs,
 859             _structSize, _nviews, _compressionMethod);
 860
 861     // pass all the views of all the samples through the generated trees and accumulate
 862     // the statistics (posterior probabilities) in leaves.
 863     Mat patch;
 864     RNG& rng = theRNG();
 865
 866     int globalPointIdx = 0;
 867     for( size_t imgIdx = 0; imgIdx < points.size(); imgIdx++ )
 868     {
 869         const Point2f* imgPoints = &points[imgIdx][0];
 870         const int* imgLabels = labels.empty() ? 0 : &labels[imgIdx][0];
 871         for( size_t pointIdx = 0; pointIdx < points[imgIdx].size(); pointIdx++, globalPointIdx++ )
 872         {
 873             Point2f pt = imgPoints[pointIdx];
 874             const Mat& src = refimgs[imgIdx];
 875             int classId = imgLabels==0 ? globalPointIdx : imgLabels[pointIdx];
 876             if( verbose && (globalPointIdx+1)*progressBarSize/numPoints != globalPointIdx*progressBarSize/numPoints )
 877                 putchar('.');
 878             CV_Assert( 0 <= classId && classId < nclasses );
 879             classCounters[classId] += _nviews;
 880             for( int v = 0; v < _nviews; v++ )
 881             {
 882                 patchGenerator(src, pt, patch, patchSize, rng);
 883                 for( int f = 0; f < nstructs; f++ )
 884                     posteriors[getLeaf(f, patch)*nclasses + classId]++;
 885             }
 886         }
 887     }
 888     if( verbose )
 889         putchar('\n');
 890
 891     finalize(rng);
 892 }
 893
 894
 895 void FernClassifier::trainFromSingleView(const Mat& image,
 896                                          const std::vector<KeyPoint>& keypoints,
 897                                          int _patchSize, int _signatureSize,
 898                                          int _nstructs, int _structSize,
 899                                          int _nviews, int _compressionMethod,
 900                                          const PatchGenerator& patchGenerator)
 901 {
 902     prepare((int)keypoints.size(), _patchSize, _signatureSize, _nstructs,
 903             _structSize, _nviews, _compressionMethod);
 904     int i, j, k, nsamples = (int)keypoints.size(), maxOctave = 0;
 905     for( i = 0; i < nsamples; i++ )
 906     {
 907         classCounters[i] = _nviews;
 908         maxOctave = std::max(maxOctave, keypoints[i].octave);
 909     }
 910
 911     double maxScale = patchGenerator.lambdaMax*2;
 912     Mat canvas(cvRound(std::max(image.cols,image.rows)*maxScale + patchSize.width*2 + 10),
 913                cvRound(std::max(image.cols,image.rows)*maxScale + patchSize.width*2 + 10), image.type());
 914     Mat noisebuf;
 915     std::vector<Mat> pyrbuf(maxOctave+1), pyr(maxOctave+1);
 916     Point2f center0((image.cols-1)*0.5f, (image.rows-1)*0.5f),
 917     center1((canvas.cols - 1)*0.5f, (canvas.rows - 1)*0.5f);
 918     Mat matM(2, 3, CV_64F);
 919     double *M = (double*)matM.data;
 920     RNG& rng = theRNG();
 921
 922     Mat patch(patchSize, CV_8U);
 923
 924     for( i = 0; i < _nviews; i++ )
 925     {
 926         patchGenerator.generateRandomTransform(center0, center1, matM, rng);
 927
 928         CV_Assert(matM.type() == CV_64F);
 929         Rect roi(INT_MAX, INT_MAX, INT_MIN, INT_MIN);
 930
 931         for( k = 0; k < 4; k++ )
 932         {
 933             Point2f pt0, pt1;
 934             pt0.x = (float)(k == 0 || k == 3 ? 0 : image.cols);
 935             pt0.y = (float)(k < 2 ? 0 : image.rows);
 936             pt1.x = (float)(M[0]*pt0.x + M[1]*pt0.y + M[2]);
 937             pt1.y = (float)(M[3]*pt0.x + M[4]*pt0.y + M[5]);
 938
 939             roi.x = std::min(roi.x, cvFloor(pt1.x));
 940             roi.y = std::min(roi.y, cvFloor(pt1.y));
 941             roi.width = std::max(roi.width, cvCeil(pt1.x));
 942             roi.height = std::max(roi.height, cvCeil(pt1.y));
 943         }
 944
 945         roi.width -= roi.x + 1;
 946         roi.height -= roi.y + 1;
 947
 948         Mat canvas_roi(canvas, roi);
 949         M[2] -= roi.x;
 950         M[5] -= roi.y;
 951
 952         Size size = canvas_roi.size();
 953         rng.fill(canvas_roi, RNG::UNIFORM, Scalar::all(0), Scalar::all(256));
 954         warpAffine( image, canvas_roi, matM, size, INTER_LINEAR, BORDER_TRANSPARENT);
 955
 956         pyr[0] = canvas_roi;
 957         for( j = 1; j <= maxOctave; j++ )
 958         {
 959             size = Size((size.width+1)/2, (size.height+1)/2);
 960             if( pyrbuf[j].cols < size.width*size.height )
 961                 pyrbuf[j].create(1, size.width*size.height, image.type());
 962             pyr[j] = Mat(size, image.type(), pyrbuf[j].data);
 963             pyrDown(pyr[j-1], pyr[j]);
 964         }
 965
 966         if( patchGenerator.noiseRange > 0 )
 967         {
 968             const int noiseDelta = 128;
 969             if( noisebuf.cols < pyr[0].cols*pyr[0].rows )
 970                 noisebuf.create(1, pyr[0].cols*pyr[0].rows, image.type());
 971             for( j = 0; j <= maxOctave; j++ )
 972             {
 973                 Mat noise(pyr[j].size(), image.type(), noisebuf.data);
 974                 rng.fill(noise, RNG::UNIFORM, Scalar::all(-patchGenerator.noiseRange + noiseDelta),
 975                          Scalar::all(patchGenerator.noiseRange + noiseDelta));
 976                 addWeighted(pyr[j], 1, noise, 1, -noiseDelta, pyr[j]);
 977             }
 978         }
 979
 980         for( j = 0; j < nsamples; j++ )
 981         {
 982             KeyPoint kpt = keypoints[j];
 983             float scale = 1.f/(1 << kpt.octave);
 984             Point2f pt((float)((M[0]*kpt.pt.x + M[1]*kpt.pt.y + M[2])*scale),
 985                        (float)((M[3]*kpt.pt.x + M[4]*kpt.pt.y + M[5])*scale));
 986             getRectSubPix(pyr[kpt.octave], patchSize, pt, patch, patch.type());
 987             for( int f = 0; f < nstructs; f++ )
 988                 posteriors[getLeaf(f, patch)*nclasses + j]++;
 989         }
 990
 991         if( verbose && (i+1)*progressBarSize/_nviews != i*progressBarSize/_nviews )
 992             putchar('.');
 993     }
 994     if( verbose )
 995         putchar('\n');
 996
 997     finalize(rng);
 998 }
 999
1000
1001 int FernClassifier::operator()(const Mat& img, Point2f pt, std::vector<float>& signature) const
1002 {
1003     Mat patch;
1004     getRectSubPix(img, patchSize, pt, patch, img.type());
1005     return (*this)(patch, signature);
1006 }
1007
1008
1009 int FernClassifier::operator()(const Mat& patch, std::vector<float>& signature) const
1010 {
1011     if( posteriors.empty() )
1012         CV_Error(CV_StsNullPtr,
1013                  "The descriptor has not been trained or "
1014                  "the floating-point posteriors have been deleted");
1015     CV_Assert(patch.size() == patchSize);
1016
1017     int i, j, sz = signatureSize;
1018     signature.resize(sz);
1019     float* s = &signature[0];
1020
1021     for( j = 0; j < sz; j++ )
1022         s[j] = 0;
1023
1024     for( i = 0; i < nstructs; i++ )
1025     {
1026         int lf = getLeaf(i, patch);
1027         const float* ldata = &posteriors[lf*signatureSize];
1028         for( j = 0; j <= sz - 4; j += 4 )
1029         {
1030             float t0 = s[j] + ldata[j];
1031             float t1 = s[j+1] + ldata[j+1];
1032             s[j] = t0; s[j+1] = t1;
1033             t0 = s[j+2] + ldata[j+2];
1034             t1 = s[j+3] + ldata[j+3];
1035             s[j+2] = t0; s[j+3] = t1;
1036         }
1037         for( ; j < sz; j++ )
1038             s[j] += ldata[j];
1039     }
1040
1041     j = 0;
1042     if( signatureSize == nclasses && compressionMethod == COMPRESSION_NONE )
1043     {
1044         for( i = 1; i < nclasses; i++ )
1045             if( s[j] < s[i] )
1046                 j = i;
1047     }
1048     return j;
1049 }
1050
1051
1052 void FernClassifier::finalize(RNG&)
1053 {
1054     int i, j, k, n = nclasses;
1055     std::vector<double> invClassCounters(n);
1056     Mat_<double> _temp(1, n);
1057     double* temp = &_temp(0,0);
1058
1059     for( i = 0; i < n; i++ )
1060         invClassCounters[i] = 1./classCounters[i];
1061
1062     for( i = 0; i < nstructs; i++ )
1063     {
1064         for( j = 0; j < leavesPerStruct; j++ )
1065         {
1066             float* P = &posteriors[(i*leavesPerStruct + j)*nclasses];
1067             double sum = 0;
1068             for( k = 0; k < n; k++ )
1069                 sum += P[k]*invClassCounters[k];
1070             sum = 1./sum;
1071             for( k = 0; k < n; k++ )
1072                 temp[k] = P[k]*invClassCounters[k]*sum;
1073             log(_temp, _temp);
1074             for( k = 0; k < n; k++ )
1075                 P[k] = (float)temp[k];
1076         }
1077     }
1078
1079 #if 0
1080     // do the first pass over the data.
1081     if( compressionMethod == COMPRESSION_RANDOM_PROJ )
1082     {
1083         // In case of random projection
1084         // we generate a random m x n matrix consisting of -1's and 1's
1085         // (called Bernoulli matrix) and multiply it by each vector
1086         // of posterior probabilities.
1087         // the product is stored back into the same input vector.
1088
1089         Mat_<uchar> csmatrix;
1090         if( m < n )
1091         {
1092             // generate random Bernoulli matrix:
1093             //   -1's are replaced with 0's and 1's stay 1's.
1094             csmatrix.create(m, n);
1095             rng.fill(csmatrix, RNG::UNIFORM, Scalar::all(0), Scalar::all(2));
1096         }
1097         std::vector<float> dst(m);
1098
1099         for( i = 0; i < totalLeaves; i++ )
1100         {
1101             int S = sampleCounters[i];
1102             if( S == 0 )
1103                 continue;
1104
1105             float scale = 1.f/(S*(m < n ? std::sqrt((float)m) : 1.f));
1106             const int* leaf = (const int*)&posteriors[i*n];
1107             float* out_leaf = (float*)&posteriors[i*m];
1108
1109             for( j = 0; j < m; j++ )
1110             {
1111                 float val = 0;
1112                 if( m < n )
1113                 {
1114                     const uchar* csrow = csmatrix.ptr(j);
1115                     // Because -1's in the Bernoulli matrix are encoded as 0's,
1116                     // the real dot product value will be
1117                     // A - B, where A is the sum of leaf[j]'s for which csrow[j]==1 and
1118                     // B is the sum of leaf[j]'s for which csrow[j]==0.
1119                     // since A + B = S, then A - B = A - (S - A) = 2*A - S.
1120                     int A = 0;
1121                     for( k = 0; k < n; k++ )
1122                         A += leaf[k] & -(int)csrow[k];
1123                     val = (A*2 - S)*scale;
1124                 }
1125                 else
1126                     val = leaf[j]*scale;
1127                 dst[j] = val;
1128             }
1129
1130             // put the vector back (since it's shorter than the original, we can do it in-place)
1131             for( j = 0; j < m; j++ )
1132                 out_leaf[j] = dst[j];
1133         }
1134     }
1135     else if( compressionMethod == COMPRESSION_PCA )
1136     {
1137         // In case of PCA we do 3 passes over the data:
1138         //   first, we compute the mean vector
1139         //   second, we compute the covariation matrix
1140         //     then we do eigen decomposition of the matrix and construct the PCA
1141         //     projection matrix
1142         //   and on the third pass we actually do PCA compression.
1143
1144         int nonEmptyLeaves = 0;
1145         Mat_<double> _mean(1, n), _vec(1, n), _dvec(m, 1),
1146         _cov(n, n), _evals(n, 1), _evects(n, n);
1147         _mean = 0.;
1148         double* mean = &_mean(0,0);
1149         double* vec = &_vec(0,0);
1150         double* dvec = &_dvec(0,0);
1151
1152         for( i = 0; i < totalLeaves; i++ )
1153         {
1154             int S = sampleCounters[i];
1155             if( S == 0 )
1156                 continue;
1157             float invS = 1.f/S;
1158             const int* leaf = (const int*)&posteriors[0] + i*n;
1159             float* out_leaf = (float*)&posteriors[0] + i*n;
1160
1161             for( j = 0; j < n; j++ )
1162             {
1163                 float t = leaf[j]*invS;
1164                 out_leaf[j] = t;
1165                 mean[j] += t;
1166             }
1167             nonEmptyLeaves++;
1168         }
1169
1170         CV_Assert( nonEmptyLeaves >= ntrees );
1171         _mean *= 1./nonEmptyLeaves;
1172
1173         for( i = 0; i < totalLeaves; i++ )
1174         {
1175             int S = sampleCounters[i];
1176             if( S == 0 )
1177                 continue;
1178             const float* leaf = (const float*)&posteriors[0] + i*n;
1179             for( j = 0; j < n; j++ )
1180                 vec[j] = leaf[j] - mean[j];
1181             gemm(_vec, _vec, 1, _cov, 1, _cov, GEMM_1_T);
1182         }
1183
1184         _cov *= 1./nonEmptyLeaves;
1185         eigen(_cov, _evals, _evects);
1186         // use the first m eigenvectors (stored as rows of the eigenvector matrix)
1187         // as the projection matrix in PCA
1188         _evects = _evects(Range(0, m), Range::all());
1189
1190         for( i = 0; i < totalLeaves; i++ )
1191         {
1192             int S = sampleCounters[i];
1193             if( S == 0 )
1194                 continue;
1195             const float* leaf = (const float*)&posteriors[0] + i*n;
1196             float* out_leaf = (float*)&posteriors[0] + i*m;
1197
1198             for( j = 0; j < n; j++ )
1199                 vec[j] = leaf[j] - mean[j];
1200             gemm(_evects, _vec, 1, Mat(), 0, _dvec, GEMM_2_T);
1201
1202             for( j = 0; j < m; j++ )
1203                 out_leaf[j] = (float)dvec[j];
1204         }
1205     }
1206     else
1207         CV_Error( CV_StsBadArg,
1208                  "Unknown compression method; use COMPRESSION_RANDOM_PROJ or COMPRESSION_PCA" );
1209
1210     // and shrink the vector
1211     posteriors.resize(totalLeaves*m);
1212 #endif
1213 }
1214
1215 void FernClassifier::setVerbose(bool _verbose)
1216 {
1217     verbose = _verbose;
1218 }
1219
1220
1221 /****************************************************************************************\
1222 *                                  FernDescriptorMatcher                                 *
1223 \****************************************************************************************/
1224 FernDescriptorMatcher::Params::Params( int _nclasses, int _patchSize, int _signatureSize,
1225                                       int _nstructs, int _structSize, int _nviews, int _compressionMethod,
1226                                       const PatchGenerator& _patchGenerator ) :
1227 nclasses(_nclasses), patchSize(_patchSize), signatureSize(_signatureSize),
1228 nstructs(_nstructs), structSize(_structSize), nviews(_nviews),
1229 compressionMethod(_compressionMethod), patchGenerator(_patchGenerator)
1230 {}
1231
1232 FernDescriptorMatcher::Params::Params( const String& _filename )
1233 {
1234     filename = _filename;
1235 }
1236
1237 FernDescriptorMatcher::FernDescriptorMatcher( const Params& _params )
1238 {
1239     prevTrainCount = 0;
1240     params = _params;
1241     if( !params.filename.empty() )
1242     {
1243         classifier = makePtr<FernClassifier>();
1244         FileStorage fs(params.filename, FileStorage::READ);
1245         if( fs.isOpened() )
1246             classifier->read( fs.getFirstTopLevelNode() );
1247     }
1248 }
1249
1250 FernDescriptorMatcher::~FernDescriptorMatcher()
1251 {}
1252
1253 void FernDescriptorMatcher::clear()
1254 {
1255     GenericDescriptorMatcher::clear();
1256
1257     classifier.release();
1258     prevTrainCount = 0;
1259 }
1260
1261 void FernDescriptorMatcher::train()
1262 {
1263     if( !classifier || prevTrainCount < (int)trainPointCollection.keypointCount() )
1264     {
1265         assert( params.filename.empty() );
1266
1267         std::vector<std::vector<Point2f> > points( trainPointCollection.imageCount() );
1268         for( size_t imgIdx = 0; imgIdx < trainPointCollection.imageCount(); imgIdx++ )
1269             KeyPoint::convert( trainPointCollection.getKeypoints((int)imgIdx), points[imgIdx] );
1270
1271         classifier.reset(
1272             new FernClassifier( points, trainPointCollection.getImages(), std::vector<std::vector<int> >(), 0, // each points is a class
1273                                 params.patchSize, params.signatureSize, params.nstructs, params.structSize,
1274                                 params.nviews, params.compressionMethod, params.patchGenerator ));
1275     }
1276 }
1277
1278 bool FernDescriptorMatcher::isMaskSupported()
1279 {
1280     return false;
1281 }
1282
1283 void FernDescriptorMatcher::calcBestProbAndMatchIdx( const Mat& image, const Point2f& pt,
1284                                                     float& bestProb, int& bestMatchIdx, std::vector<float>& signature )
1285 {
1286     (*classifier)( image, pt, signature);
1287
1288     bestProb = -FLT_MAX;
1289     bestMatchIdx = -1;
1290     for( int ci = 0; ci < classifier->getClassCount(); ci++ )
1291     {
1292         if( signature[ci] > bestProb )
1293         {
1294             bestProb = signature[ci];
1295             bestMatchIdx = ci;
1296         }
1297     }
1298 }
1299
1300 void FernDescriptorMatcher::knnMatchImpl( InputArray _queryImage, std::vector<KeyPoint>& queryKeypoints,
1301                                          std::vector<std::vector<DMatch> >& matches, int knn,
1302                                          InputArrayOfArrays /*masks*/, bool /*compactResult*/ )
1303 {
1304     Mat queryImage = _queryImage.getMat();
1305
1306     train();
1307
1308     matches.resize( queryKeypoints.size() );
1309     std::vector<float> signature( (size_t)classifier->getClassCount() );
1310
1311     for( size_t queryIdx = 0; queryIdx < queryKeypoints.size(); queryIdx++ )
1312     {
1313         (*classifier)( queryImage, queryKeypoints[queryIdx].pt, signature);
1314
1315         for( int k = 0; k < knn; k++ )
1316         {
1317             DMatch bestMatch;
1318             size_t best_ci = 0;
1319             for( size_t ci = 0; ci < signature.size(); ci++ )
1320             {
1321                 if( -signature[ci] < bestMatch.distance )
1322                 {
1323                     int imgIdx = -1, trainIdx = -1;
1324                     trainPointCollection.getLocalIdx( (int)ci , imgIdx, trainIdx );
1325                     bestMatch = DMatch( (int)queryIdx, trainIdx, imgIdx, -signature[ci] );
1326                     best_ci = ci;
1327                 }
1328             }
1329
1330             if( bestMatch.trainIdx == -1 )
1331                 break;
1332             signature[best_ci] = -std::numeric_limits<float>::max();
1333             matches[queryIdx].push_back( bestMatch );
1334         }
1335     }
1336 }
1337
1338 void FernDescriptorMatcher::radiusMatchImpl( InputArray _queryImage, std::vector<KeyPoint>& queryKeypoints,
1339                                             std::vector<std::vector<DMatch> >& matches, float maxDistance,
1340                                             InputArrayOfArrays /*masks*/, bool /*compactResult*/ )
1341 {
1342     Mat queryImage = _queryImage.getMat();
1343     train();
1344     matches.resize( queryKeypoints.size() );
1345     std::vector<float> signature( (size_t)classifier->getClassCount() );
1346
1347     for( size_t i = 0; i < queryKeypoints.size(); i++ )
1348     {
1349         (*classifier)( queryImage, queryKeypoints[i].pt, signature);
1350
1351         for( int ci = 0; ci < classifier->getClassCount(); ci++ )
1352         {
1353             if( -signature[ci] < maxDistance )
1354             {
1355                 int imgIdx = -1, trainIdx = -1;
1356                 trainPointCollection.getLocalIdx( ci , imgIdx, trainIdx );
1357                 matches[i].push_back( DMatch( (int)i, trainIdx, imgIdx, -signature[ci] ) );
1358             }
1359         }
1360     }
1361 }
1362
1363 void FernDescriptorMatcher::read( const FileNode &fn )
1364 {
1365     params.nclasses = fn["nclasses"];
1366     params.patchSize = fn["patchSize"];
1367     params.signatureSize = fn["signatureSize"];
1368     params.nstructs = fn["nstructs"];
1369     params.structSize = fn["structSize"];
1370     params.nviews = fn["nviews"];
1371     params.compressionMethod = fn["compressionMethod"];
1372
1373     //classifier->read(fn);
1374 }
1375
1376 void FernDescriptorMatcher::write( FileStorage& fs ) const
1377 {
1378     fs << "nclasses" << params.nclasses;
1379     fs << "patchSize" << params.patchSize;
1380     fs << "signatureSize" << params.signatureSize;
1381     fs << "nstructs" << params.nstructs;
1382     fs << "structSize" << params.structSize;
1383     fs << "nviews" << params.nviews;
1384     fs << "compressionMethod" << params.compressionMethod;
1385
1386     //    classifier->write(fs);
1387 }
1388
1389 bool FernDescriptorMatcher::empty() const
1390 {
1391     return !classifier || classifier->empty();
1392 }
1393
1394 Ptr<GenericDescriptorMatcher> FernDescriptorMatcher::clone( bool emptyTrainData ) const
1395 {
1396     Ptr<FernDescriptorMatcher> matcher = makePtr<FernDescriptorMatcher>( params );
1397     if( !emptyTrainData )
1398     {
1399         CV_Error( CV_StsNotImplemented, "deep clone dunctionality is not implemented, because "
1400                  "FernClassifier has not copy constructor or clone method ");
1401
1402         //matcher->classifier;
1403         matcher->params = params;
1404         matcher->prevTrainCount = prevTrainCount;
1405         matcher->trainPointCollection = trainPointCollection;
1406     }
1407     return matcher;
1408 }
1409
1410 ////////////////////////////////////// Planar Object Detector ////////////////////////////////////
1411
1412 PlanarObjectDetector::PlanarObjectDetector()
1413 {
1414 }
1415
1416 PlanarObjectDetector::PlanarObjectDetector(const FileNode& node)
1417 {
1418     read(node);
1419 }
1420
1421 PlanarObjectDetector::PlanarObjectDetector(const std::vector<Mat>& pyr, int npoints,
1422                                            int patchSize, int nstructs, int structSize,
1423                                            int nviews, const LDetector& detector,
1424                                            const PatchGenerator& patchGenerator)
1425 {
1426     train(pyr, npoints, patchSize, nstructs,
1427           structSize, nviews, detector, patchGenerator);
1428 }
1429
1430 PlanarObjectDetector::~PlanarObjectDetector()
1431 {
1432 }
1433
1434 std::vector<KeyPoint> PlanarObjectDetector::getModelPoints() const
1435 {
1436     return modelPoints;
1437 }
1438
1439 void PlanarObjectDetector::train(const std::vector<Mat>& pyr, int npoints,
1440                                  int patchSize, int nstructs, int structSize,
1441                                  int nviews, const LDetector& detector,
1442                                  const PatchGenerator& patchGenerator)
1443 {
1444     modelROI = Rect(0, 0, pyr[0].cols, pyr[0].rows);
1445     ldetector = detector;
1446     ldetector.setVerbose(verbose);
1447     ldetector.getMostStable2D(pyr[0], modelPoints, npoints, patchGenerator);
1448
1449     npoints = (int)modelPoints.size();
1450     fernClassifier.setVerbose(verbose);
1451     fernClassifier.trainFromSingleView(pyr[0], modelPoints,
1452                                        patchSize, (int)modelPoints.size(), nstructs, structSize, nviews,
1453                                        FernClassifier::COMPRESSION_NONE, patchGenerator);
1454 }
1455
1456 void PlanarObjectDetector::train(const std::vector<Mat>& pyr, const std::vector<KeyPoint>& keypoints,
1457                                  int patchSize, int nstructs, int structSize,
1458                                  int nviews, const LDetector& detector,
1459                                  const PatchGenerator& patchGenerator)
1460 {
1461     modelROI = Rect(0, 0, pyr[0].cols, pyr[0].rows);
1462     ldetector = detector;
1463     ldetector.setVerbose(verbose);
1464     modelPoints.resize(keypoints.size());
1465     std::copy(keypoints.begin(), keypoints.end(), modelPoints.begin());
1466
1467     fernClassifier.setVerbose(verbose);
1468     fernClassifier.trainFromSingleView(pyr[0], modelPoints,
1469                                        patchSize, (int)modelPoints.size(), nstructs, structSize, nviews,
1470                                        FernClassifier::COMPRESSION_NONE, patchGenerator);
1471 }
1472
1473 void PlanarObjectDetector::read(const FileNode& node)
1474 {
1475     FileNodeIterator it = node["model-roi"].begin(), it_end;
1476     it >> modelROI.x >> modelROI.y >> modelROI.width >> modelROI.height;
1477     ldetector.read(node["detector"]);
1478     fernClassifier.read(node["fern-classifier"]);
1479     cv::read(node["model-points"], modelPoints);
1480     CV_Assert(modelPoints.size() == (size_t)fernClassifier.getClassCount());
1481 }
1482
1483
1484 void PlanarObjectDetector::write(FileStorage& fs, const String& objname) const
1485 {
1486     internal::WriteStructContext ws(fs, objname, CV_NODE_MAP);
1487
1488     {
1489         internal::WriteStructContext wsroi(fs, "model-roi", CV_NODE_SEQ + CV_NODE_FLOW);
1490         cv::write(fs, modelROI.x);
1491         cv::write(fs, modelROI.y);
1492         cv::write(fs, modelROI.width);
1493         cv::write(fs, modelROI.height);
1494     }
1495     ldetector.write(fs, "detector");
1496     cv::write(fs, "model-points", modelPoints);
1497     fernClassifier.write(fs, "fern-classifier");
1498 }
1499
1500
1501 bool PlanarObjectDetector::operator()(const Mat& image, Mat& H, std::vector<Point2f>& corners) const
1502 {
1503     std::vector<Mat> pyr;
1504     buildPyramid(image, pyr, ldetector.nOctaves - 1);
1505     std::vector<KeyPoint> keypoints;
1506     ldetector(pyr, keypoints);
1507
1508     return (*this)(pyr, keypoints, H, corners);
1509 }
1510
1511 bool PlanarObjectDetector::operator()(const std::vector<Mat>& pyr, const std::vector<KeyPoint>& keypoints,
1512                                       Mat& matH, std::vector<Point2f>& corners, std::vector<int>* pairs) const
1513 {
1514     int i, j, m = (int)modelPoints.size(), n = (int)keypoints.size();
1515     std::vector<int> bestMatches(m, -1);
1516     std::vector<float> maxLogProb(m, -FLT_MAX);
1517     std::vector<float> signature;
1518     std::vector<Point2f> fromPt, toPt;
1519
1520     for( i = 0; i < n; i++ )
1521     {
1522         KeyPoint kpt = keypoints[i];
1523         CV_Assert(0 <= kpt.octave && kpt.octave < (int)pyr.size());
1524         kpt.pt.x /= (float)(1 << kpt.octave);
1525         kpt.pt.y /= (float)(1 << kpt.octave);
1526         int k = fernClassifier(pyr[kpt.octave], kpt.pt, signature);
1527         if( k >= 0 && (bestMatches[k] < 0 || signature[k] > maxLogProb[k]) )
1528         {
1529             maxLogProb[k] = signature[k];
1530             bestMatches[k] = i;
1531         }
1532     }
1533
1534     if(pairs)
1535         pairs->resize(0);
1536
1537     for( i = 0; i < m; i++ )
1538         if( bestMatches[i] >= 0 )
1539         {
1540             fromPt.push_back(modelPoints[i].pt);
1541             toPt.push_back(keypoints[bestMatches[i]].pt);
1542         }
1543
1544     if( fromPt.size() < 4 )
1545         return false;
1546
1547     std::vector<uchar> mask;
1548     matH = findHomography(fromPt, toPt, RANSAC, 10, mask);
1549     if( matH.data )
1550     {
1551         const Mat_<double>& H = matH;
1552         corners.resize(4);
1553         for( i = 0; i < 4; i++ )
1554         {
1555             Point2f pt((float)(modelROI.x + (i == 0 || i == 3 ? 0 : modelROI.width)),
1556                        (float)(modelROI.y + (i <= 1 ? 0 : modelROI.height)));
1557             double w = 1./(H(2,0)*pt.x + H(2,1)*pt.y + H(2,2));
1558             corners[i] = Point2f((float)((H(0,0)*pt.x + H(0,1)*pt.y + H(0,2))*w),
1559                                  (float)((H(1,0)*pt.x + H(1,1)*pt.y + H(1,2))*w));
1560         }
1561     }
1562
1563     if( pairs )
1564     {
1565         for( i = j = 0; i < m; i++ )
1566             if( bestMatches[i] >= 0 && mask[j++] )
1567             {
1568                 pairs->push_back(i);
1569                 pairs->push_back(bestMatches[i]);
1570             }
1571     }
1572
1573     return matH.data != 0;
1574 }
1575
1576
1577 void PlanarObjectDetector::setVerbose(bool _verbose)
1578 {
1579     verbose = _verbose;
1580 }
1581
1582 }