modules/features2d/src/brisk.cpp

   1 /*********************************************************************
   2  * Software License Agreement (BSD License)
   3  *
   4  *  Copyright (C) 2011  The Autonomous Systems Lab (ASL), ETH Zurich,
   5  *                Stefan Leutenegger, Simon Lynen and Margarita Chli.
   6  *  Copyright (c) 2009, Willow Garage, Inc.
   7  *  All rights reserved.
   8  *
   9  *  Redistribution and use in source and binary forms, with or without
  10  *  modification, are permitted provided that the following conditions
  11  *  are met:
  12  *
  13  *   * Redistributions of source code must retain the above copyright
  14  *     notice, this list of conditions and the following disclaimer.
  15  *   * Redistributions in binary form must reproduce the above
  16  *     copyright notice, this list of conditions and the following
  17  *     disclaimer in the documentation and/or other materials provided
  18  *     with the distribution.
  19  *   * Neither the name of the Willow Garage nor the names of its
  20  *     contributors may be used to endorse or promote products derived
  21  *     from this software without specific prior written permission.
  22  *
  23  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  24  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  25  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  26  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  27  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  28  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  29  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  30  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  31  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  32  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  33  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  34  *  POSSIBILITY OF SUCH DAMAGE.
  35  *********************************************************************/
  36
  37 /*
  38  BRISK - Binary Robust Invariant Scalable Keypoints
  39  Reference implementation of
  40  [1] Stefan Leutenegger,Margarita Chli and Roland Siegwart, BRISK:
  41  Binary Robust Invariant Scalable Keypoints, in Proceedings of
  42  the IEEE International Conference on Computer Vision (ICCV2011).
  43  */
  44
  45 #include <opencv2/features2d.hpp>
  46 #include <opencv2/core.hpp>
  47 #include <opencv2/imgproc.hpp>
  48 #include <fstream>
  49 #include <stdlib.h>
  50
  51 #include "fast_score.hpp"
  52
  53 namespace cv
  54 {
  55
  56 // a layer in the Brisk detector pyramid
  57 class CV_EXPORTS BriskLayer
  58 {
  59 public:
  60   // constructor arguments
  61   struct CV_EXPORTS CommonParams
  62   {
  63     static const int HALFSAMPLE = 0;
  64     static const int TWOTHIRDSAMPLE = 1;
  65   };
  66   // construct a base layer
  67   BriskLayer(const cv::Mat& img, float scale = 1.0f, float offset = 0.0f);
  68   // derive a layer
  69   BriskLayer(const BriskLayer& layer, int mode);
  70
  71   // Fast/Agast without non-max suppression
  72   void
  73   getAgastPoints(int threshold, std::vector<cv::KeyPoint>& keypoints);
  74
  75   // get scores - attention, this is in layer coordinates, not scale=1 coordinates!
  76   inline int
  77   getAgastScore(int x, int y, int threshold) const;
  78   inline int
  79   getAgastScore_5_8(int x, int y, int threshold) const;
  80   inline int
  81   getAgastScore(float xf, float yf, int threshold, float scale = 1.0f) const;
  82
  83   // accessors
  84   inline const cv::Mat&
  85   img() const
  86   {
  87     return img_;
  88   }
  89   inline const cv::Mat&
  90   scores() const
  91   {
  92     return scores_;
  93   }
  94   inline float
  95   scale() const
  96   {
  97     return scale_;
  98   }
  99   inline float
 100   offset() const
 101   {
 102     return offset_;
 103   }
 104
 105   // half sampling
 106   static inline void
 107   halfsample(const cv::Mat& srcimg, cv::Mat& dstimg);
 108   // two third sampling
 109   static inline void
 110   twothirdsample(const cv::Mat& srcimg, cv::Mat& dstimg);
 111
 112 private:
 113   // access gray values (smoothed/interpolated)
 114   inline int
 115   value(const cv::Mat& mat, float xf, float yf, float scale) const;
 116   // the image
 117   cv::Mat img_;
 118   // its Fast scores
 119   cv::Mat_<uchar> scores_;
 120   // coordinate transformation
 121   float scale_;
 122   float offset_;
 123   // agast
 124   cv::Ptr<cv::FastFeatureDetector> fast_9_16_;
 125   int pixel_5_8_[25];
 126   int pixel_9_16_[25];
 127 };
 128
 129 class CV_EXPORTS BriskScaleSpace
 130 {
 131 public:
 132   // construct telling the octaves number:
 133   BriskScaleSpace(int _octaves = 3);
 134   ~BriskScaleSpace();
 135
 136   // construct the image pyramids
 137   void
 138   constructPyramid(const cv::Mat& image);
 139
 140   // get Keypoints
 141   void
 142   getKeypoints(const int _threshold, std::vector<cv::KeyPoint>& keypoints);
 143
 144 protected:
 145   // nonmax suppression:
 146   inline bool
 147   isMax2D(const int layer, const int x_layer, const int y_layer);
 148   // 1D (scale axis) refinement:
 149   inline float
 150   refine1D(const float s_05, const float s0, const float s05, float& max) const; // around octave
 151   inline float
 152   refine1D_1(const float s_05, const float s0, const float s05, float& max) const; // around intra
 153   inline float
 154   refine1D_2(const float s_05, const float s0, const float s05, float& max) const; // around octave 0 only
 155   // 2D maximum refinement:
 156   inline float
 157   subpixel2D(const int s_0_0, const int s_0_1, const int s_0_2, const int s_1_0, const int s_1_1, const int s_1_2,
 158              const int s_2_0, const int s_2_1, const int s_2_2, float& delta_x, float& delta_y) const;
 159   // 3D maximum refinement centered around (x_layer,y_layer)
 160   inline float
 161   refine3D(const int layer, const int x_layer, const int y_layer, float& x, float& y, float& scale, bool& ismax) const;
 162
 163   // interpolated score access with recalculation when needed:
 164   inline int
 165   getScoreAbove(const int layer, const int x_layer, const int y_layer) const;
 166   inline int
 167   getScoreBelow(const int layer, const int x_layer, const int y_layer) const;
 168
 169   // return the maximum of score patches above or below
 170   inline float
 171   getScoreMaxAbove(const int layer, const int x_layer, const int y_layer, const int threshold, bool& ismax,
 172                    float& dx, float& dy) const;
 173   inline float
 174   getScoreMaxBelow(const int layer, const int x_layer, const int y_layer, const int threshold, bool& ismax,
 175                    float& dx, float& dy) const;
 176
 177   // the image pyramids:
 178   int layers_;
 179   std::vector<BriskLayer> pyramid_;
 180
 181   // some constant parameters:
 182   static const float safetyFactor_;
 183   static const float basicSize_;
 184 };
 185
 186 const float BRISK::basicSize_ = 12.0f;
 187 const unsigned int BRISK::scales_ = 64;
 188 const float BRISK::scalerange_ = 30.f; // 40->4 Octaves - else, this needs to be adjusted...
 189 const unsigned int BRISK::n_rot_ = 1024; // discretization of the rotation look-up
 190
 191 const float BriskScaleSpace::safetyFactor_ = 1.0f;
 192 const float BriskScaleSpace::basicSize_ = 12.0f;
 193
 194 // constructors
 195 BRISK::BRISK(int thresh, int octaves_in, float patternScale)
 196 {
 197   threshold = thresh;
 198   octaves = octaves_in;
 199
 200   std::vector<float> rList;
 201   std::vector<int> nList;
 202
 203   // this is the standard pattern found to be suitable also
 204   rList.resize(5);
 205   nList.resize(5);
 206   const double f = 0.85 * patternScale;
 207
 208   rList[0] = (float)(f * 0.);
 209   rList[1] = (float)(f * 2.9);
 210   rList[2] = (float)(f * 4.9);
 211   rList[3] = (float)(f * 7.4);
 212   rList[4] = (float)(f * 10.8);
 213
 214   nList[0] = 1;
 215   nList[1] = 10;
 216   nList[2] = 14;
 217   nList[3] = 15;
 218   nList[4] = 20;
 219
 220   generateKernel(rList, nList, (float)(5.85 * patternScale), (float)(8.2 * patternScale));
 221
 222 }
 223 BRISK::BRISK(std::vector<float> &radiusList, std::vector<int> &numberList, float dMax, float dMin,
 224                                                    std::vector<int> indexChange)
 225 {
 226   generateKernel(radiusList, numberList, dMax, dMin, indexChange);
 227 }
 228
 229 void
 230 BRISK::generateKernel(std::vector<float> &radiusList, std::vector<int> &numberList, float dMax,
 231                                          float dMin, std::vector<int> indexChange)
 232 {
 233
 234   dMax_ = dMax;
 235   dMin_ = dMin;
 236
 237   // get the total number of points
 238   const int rings = (int)radiusList.size();
 239   CV_Assert(radiusList.size() != 0 && radiusList.size() == numberList.size());
 240   points_ = 0; // remember the total number of points
 241   for (int ring = 0; ring < rings; ring++)
 242   {
 243     points_ += numberList[ring];
 244   }
 245   // set up the patterns
 246   patternPoints_ = new BriskPatternPoint[points_ * scales_ * n_rot_];
 247   BriskPatternPoint* patternIterator = patternPoints_;
 248
 249   // define the scale discretization:
 250   static const float lb_scale = (float)(std::log(scalerange_) / std::log(2.0));
 251   static const float lb_scale_step = lb_scale / (scales_);
 252
 253   scaleList_ = new float[scales_];
 254   sizeList_ = new unsigned int[scales_];
 255
 256   const float sigma_scale = 1.3f;
 257
 258   for (unsigned int scale = 0; scale < scales_; ++scale)
 259   {
 260     scaleList_[scale] = (float)std::pow((double) 2.0, (double) (scale * lb_scale_step));
 261     sizeList_[scale] = 0;
 262
 263     // generate the pattern points look-up
 264     double alpha, theta;
 265     for (size_t rot = 0; rot < n_rot_; ++rot)
 266     {
 267       theta = double(rot) * 2 * CV_PI / double(n_rot_); // this is the rotation of the feature
 268       for (int ring = 0; ring < rings; ++ring)
 269       {
 270         for (int num = 0; num < numberList[ring]; ++num)
 271         {
 272           // the actual coordinates on the circle
 273           alpha = (double(num)) * 2 * CV_PI / double(numberList[ring]);
 274           patternIterator->x = (float)(scaleList_[scale] * radiusList[ring] * cos(alpha + theta)); // feature rotation plus angle of the point
 275           patternIterator->y = (float)(scaleList_[scale] * radiusList[ring] * sin(alpha + theta));
 276           // and the gaussian kernel sigma
 277           if (ring == 0)
 278           {
 279             patternIterator->sigma = sigma_scale * scaleList_[scale] * 0.5f;
 280           }
 281           else
 282           {
 283             patternIterator->sigma = (float)(sigma_scale * scaleList_[scale] * (double(radiusList[ring]))
 284                                      * sin(CV_PI / numberList[ring]));
 285           }
 286           // adapt the sizeList if necessary
 287           const unsigned int size = cvCeil(((scaleList_[scale] * radiusList[ring]) + patternIterator->sigma)) + 1;
 288           if (sizeList_[scale] < size)
 289           {
 290             sizeList_[scale] = size;
 291           }
 292
 293           // increment the iterator
 294           ++patternIterator;
 295         }
 296       }
 297     }
 298   }
 299
 300   // now also generate pairings
 301   shortPairs_ = new BriskShortPair[points_ * (points_ - 1) / 2];
 302   longPairs_ = new BriskLongPair[points_ * (points_ - 1) / 2];
 303   noShortPairs_ = 0;
 304   noLongPairs_ = 0;
 305
 306   // fill indexChange with 0..n if empty
 307   unsigned int indSize = (unsigned int)indexChange.size();
 308   if (indSize == 0)
 309   {
 310     indexChange.resize(points_ * (points_ - 1) / 2);
 311     indSize = (unsigned int)indexChange.size();
 312
 313     for (unsigned int i = 0; i < indSize; i++)
 314       indexChange[i] = i;
 315   }
 316   const float dMin_sq = dMin_ * dMin_;
 317   const float dMax_sq = dMax_ * dMax_;
 318   for (unsigned int i = 1; i < points_; i++)
 319   {
 320     for (unsigned int j = 0; j < i; j++)
 321     { //(find all the pairs)
 322       // point pair distance:
 323       const float dx = patternPoints_[j].x - patternPoints_[i].x;
 324       const float dy = patternPoints_[j].y - patternPoints_[i].y;
 325       const float norm_sq = (dx * dx + dy * dy);
 326       if (norm_sq > dMin_sq)
 327       {
 328         // save to long pairs
 329         BriskLongPair& longPair = longPairs_[noLongPairs_];
 330         longPair.weighted_dx = int((dx / (norm_sq)) * 2048.0 + 0.5);
 331         longPair.weighted_dy = int((dy / (norm_sq)) * 2048.0 + 0.5);
 332         longPair.i = i;
 333         longPair.j = j;
 334         ++noLongPairs_;
 335       }
 336       else if (norm_sq < dMax_sq)
 337       {
 338         // save to short pairs
 339         CV_Assert(noShortPairs_ < indSize);
 340         // make sure the user passes something sensible
 341         BriskShortPair& shortPair = shortPairs_[indexChange[noShortPairs_]];
 342         shortPair.j = j;
 343         shortPair.i = i;
 344         ++noShortPairs_;
 345       }
 346     }
 347   }
 348
 349   // no bits:
 350   strings_ = (int) ceil((float(noShortPairs_)) / 128.0) * 4 * 4;
 351 }
 352
 353 // simple alternative:
 354 inline int
 355 BRISK::smoothedIntensity(const cv::Mat& image, const cv::Mat& integral, const float key_x,
 356                                             const float key_y, const unsigned int scale, const unsigned int rot,
 357                                             const unsigned int point) const
 358 {
 359
 360   // get the float position
 361   const BriskPatternPoint& briskPoint = patternPoints_[scale * n_rot_ * points_ + rot * points_ + point];
 362   const float xf = briskPoint.x + key_x;
 363   const float yf = briskPoint.y + key_y;
 364   const int x = int(xf);
 365   const int y = int(yf);
 366   const int& imagecols = image.cols;
 367
 368   // get the sigma:
 369   const float sigma_half = briskPoint.sigma;
 370   const float area = 4.0f * sigma_half * sigma_half;
 371
 372   // calculate output:
 373   int ret_val;
 374   if (sigma_half < 0.5)
 375   {
 376     //interpolation multipliers:
 377     const int r_x = (int)((xf - x) * 1024);
 378     const int r_y = (int)((yf - y) * 1024);
 379     const int r_x_1 = (1024 - r_x);
 380     const int r_y_1 = (1024 - r_y);
 381     const uchar* ptr = &image.at<uchar>(y, x);
 382     size_t step = image.step;
 383     // just interpolate:
 384     ret_val = r_x_1 * r_y_1 * ptr[0] + r_x * r_y_1 * ptr[1] +
 385               r_x * r_y * ptr[step] + r_x_1 * r_y * ptr[step+1];
 386     return (ret_val + 512) / 1024;
 387   }
 388
 389   // this is the standard case (simple, not speed optimized yet):
 390
 391   // scaling:
 392   const int scaling = (int)(4194304.0 / area);
 393   const int scaling2 = int(float(scaling) * area / 1024.0);
 394
 395   // the integral image is larger:
 396   const int integralcols = imagecols + 1;
 397
 398   // calculate borders
 399   const float x_1 = xf - sigma_half;
 400   const float x1 = xf + sigma_half;
 401   const float y_1 = yf - sigma_half;
 402   const float y1 = yf + sigma_half;
 403
 404   const int x_left = int(x_1 + 0.5);
 405   const int y_top = int(y_1 + 0.5);
 406   const int x_right = int(x1 + 0.5);
 407   const int y_bottom = int(y1 + 0.5);
 408
 409   // overlap area - multiplication factors:
 410   const float r_x_1 = float(x_left) - x_1 + 0.5f;
 411   const float r_y_1 = float(y_top) - y_1 + 0.5f;
 412   const float r_x1 = x1 - float(x_right) + 0.5f;
 413   const float r_y1 = y1 - float(y_bottom) + 0.5f;
 414   const int dx = x_right - x_left - 1;
 415   const int dy = y_bottom - y_top - 1;
 416   const int A = (int)((r_x_1 * r_y_1) * scaling);
 417   const int B = (int)((r_x1 * r_y_1) * scaling);
 418   const int C = (int)((r_x1 * r_y1) * scaling);
 419   const int D = (int)((r_x_1 * r_y1) * scaling);
 420   const int r_x_1_i = (int)(r_x_1 * scaling);
 421   const int r_y_1_i = (int)(r_y_1 * scaling);
 422   const int r_x1_i = (int)(r_x1 * scaling);
 423   const int r_y1_i = (int)(r_y1 * scaling);
 424
 425   if (dx + dy > 2)
 426   {
 427     // now the calculation:
 428     uchar* ptr = image.data + x_left + imagecols * y_top;
 429     // first the corners:
 430     ret_val = A * int(*ptr);
 431     ptr += dx + 1;
 432     ret_val += B * int(*ptr);
 433     ptr += dy * imagecols + 1;
 434     ret_val += C * int(*ptr);
 435     ptr -= dx + 1;
 436     ret_val += D * int(*ptr);
 437
 438     // next the edges:
 439     int* ptr_integral = (int*) integral.data + x_left + integralcols * y_top + 1;
 440     // find a simple path through the different surface corners
 441     const int tmp1 = (*ptr_integral);
 442     ptr_integral += dx;
 443     const int tmp2 = (*ptr_integral);
 444     ptr_integral += integralcols;
 445     const int tmp3 = (*ptr_integral);
 446     ptr_integral++;
 447     const int tmp4 = (*ptr_integral);
 448     ptr_integral += dy * integralcols;
 449     const int tmp5 = (*ptr_integral);
 450     ptr_integral--;
 451     const int tmp6 = (*ptr_integral);
 452     ptr_integral += integralcols;
 453     const int tmp7 = (*ptr_integral);
 454     ptr_integral -= dx;
 455     const int tmp8 = (*ptr_integral);
 456     ptr_integral -= integralcols;
 457     const int tmp9 = (*ptr_integral);
 458     ptr_integral--;
 459     const int tmp10 = (*ptr_integral);
 460     ptr_integral -= dy * integralcols;
 461     const int tmp11 = (*ptr_integral);
 462     ptr_integral++;
 463     const int tmp12 = (*ptr_integral);
 464
 465     // assign the weighted surface integrals:
 466     const int upper = (tmp3 - tmp2 + tmp1 - tmp12) * r_y_1_i;
 467     const int middle = (tmp6 - tmp3 + tmp12 - tmp9) * scaling;
 468     const int left = (tmp9 - tmp12 + tmp11 - tmp10) * r_x_1_i;
 469     const int right = (tmp5 - tmp4 + tmp3 - tmp6) * r_x1_i;
 470     const int bottom = (tmp7 - tmp6 + tmp9 - tmp8) * r_y1_i;
 471
 472     return (ret_val + upper + middle + left + right + bottom + scaling2 / 2) / scaling2;
 473   }
 474
 475   // now the calculation:
 476   uchar* ptr = image.data + x_left + imagecols * y_top;
 477   // first row:
 478   ret_val = A * int(*ptr);
 479   ptr++;
 480   const uchar* end1 = ptr + dx;
 481   for (; ptr < end1; ptr++)
 482   {
 483     ret_val += r_y_1_i * int(*ptr);
 484   }
 485   ret_val += B * int(*ptr);
 486   // middle ones:
 487   ptr += imagecols - dx - 1;
 488   uchar* end_j = ptr + dy * imagecols;
 489   for (; ptr < end_j; ptr += imagecols - dx - 1)
 490   {
 491     ret_val += r_x_1_i * int(*ptr);
 492     ptr++;
 493     const uchar* end2 = ptr + dx;
 494     for (; ptr < end2; ptr++)
 495     {
 496       ret_val += int(*ptr) * scaling;
 497     }
 498     ret_val += r_x1_i * int(*ptr);
 499   }
 500   // last row:
 501   ret_val += D * int(*ptr);
 502   ptr++;
 503   const uchar* end3 = ptr + dx;
 504   for (; ptr < end3; ptr++)
 505   {
 506     ret_val += r_y1_i * int(*ptr);
 507   }
 508   ret_val += C * int(*ptr);
 509
 510   return (ret_val + scaling2 / 2) / scaling2;
 511 }
 512
 513 inline bool
 514 RoiPredicate(const float minX, const float minY, const float maxX, const float maxY, const KeyPoint& keyPt)
 515 {
 516   const Point2f& pt = keyPt.pt;
 517   return (pt.x < minX) || (pt.x >= maxX) || (pt.y < minY) || (pt.y >= maxY);
 518 }
 519
 520 // computes the descriptor
 521 void
 522 BRISK::operator()( InputArray _image, InputArray _mask, std::vector<KeyPoint>& keypoints,
 523                    OutputArray _descriptors, bool useProvidedKeypoints) const
 524 {
 525   bool doOrientation=true;
 526   if (useProvidedKeypoints)
 527     doOrientation = false;
 528
 529   // If the user specified cv::noArray(), this will yield false. Otherwise it will return true.
 530   bool doDescriptors = _descriptors.needed();
 531
 532   computeDescriptorsAndOrOrientation(_image, _mask, keypoints, _descriptors, doDescriptors, doOrientation,
 533                                        useProvidedKeypoints);
 534 }
 535
 536 void
 537 BRISK::computeDescriptorsAndOrOrientation(InputArray _image, InputArray _mask, std::vector<KeyPoint>& keypoints,
 538                                      OutputArray _descriptors, bool doDescriptors, bool doOrientation,
 539                                      bool useProvidedKeypoints) const
 540 {
 541   Mat image = _image.getMat(), mask = _mask.getMat();
 542   if( image.type() != CV_8UC1 )
 543       cvtColor(image, image, COLOR_BGR2GRAY);
 544
 545   if (!useProvidedKeypoints)
 546   {
 547     doOrientation = true;
 548     computeKeypointsNoOrientation(_image, _mask, keypoints);
 549   }
 550
 551   //Remove keypoints very close to the border
 552   size_t ksize = keypoints.size();
 553   std::vector<int> kscales; // remember the scale per keypoint
 554   kscales.resize(ksize);
 555   static const float log2 = 0.693147180559945f;
 556   static const float lb_scalerange = (float)(std::log(scalerange_) / (log2));
 557   std::vector<cv::KeyPoint>::iterator beginning = keypoints.begin();
 558   std::vector<int>::iterator beginningkscales = kscales.begin();
 559   static const float basicSize06 = basicSize_ * 0.6f;
 560   for (size_t k = 0; k < ksize; k++)
 561   {
 562     unsigned int scale;
 563       scale = std::max((int) (scales_ / lb_scalerange * (std::log(keypoints[k].size / (basicSize06)) / log2) + 0.5), 0);
 564       // saturate
 565       if (scale >= scales_)
 566         scale = scales_ - 1;
 567       kscales[k] = scale;
 568     const int border = sizeList_[scale];
 569     const int border_x = image.cols - border;
 570     const int border_y = image.rows - border;
 571     if (RoiPredicate((float)border, (float)border, (float)border_x, (float)border_y, keypoints[k]))
 572     {
 573       keypoints.erase(beginning + k);
 574       kscales.erase(beginningkscales + k);
 575       if (k == 0)
 576       {
 577         beginning = keypoints.begin();
 578         beginningkscales = kscales.begin();
 579       }
 580       ksize--;
 581       k--;
 582     }
 583   }
 584
 585   // first, calculate the integral image over the whole image:
 586   // current integral image
 587   cv::Mat _integral; // the integral image
 588   cv::integral(image, _integral);
 589
 590   int* _values = new int[points_]; // for temporary use
 591
 592   // resize the descriptors:
 593   cv::Mat descriptors;
 594   if (doDescriptors)
 595   {
 596     _descriptors.create((int)ksize, strings_, CV_8U);
 597     descriptors = _descriptors.getMat();
 598     descriptors.setTo(0);
 599   }
 600
 601   // now do the extraction for all keypoints:
 602
 603   // temporary variables containing gray values at sample points:
 604   int t1;
 605   int t2;
 606
 607   // the feature orientation
 608   uchar* ptr = descriptors.data;
 609   for (size_t k = 0; k < ksize; k++)
 610   {
 611     cv::KeyPoint& kp = keypoints[k];
 612     const int& scale = kscales[k];
 613     int* pvalues = _values;
 614     const float& x = kp.pt.x;
 615     const float& y = kp.pt.y;
 616
 617     if (doOrientation)
 618     {
 619         // get the gray values in the unrotated pattern
 620         for (unsigned int i = 0; i < points_; i++)
 621         {
 622           *(pvalues++) = smoothedIntensity(image, _integral, x, y, scale, 0, i);
 623         }
 624
 625         int direction0 = 0;
 626         int direction1 = 0;
 627         // now iterate through the long pairings
 628         const BriskLongPair* max = longPairs_ + noLongPairs_;
 629         for (BriskLongPair* iter = longPairs_; iter < max; ++iter)
 630         {
 631           t1 = *(_values + iter->i);
 632           t2 = *(_values + iter->j);
 633           const int delta_t = (t1 - t2);
 634           // update the direction:
 635           const int tmp0 = delta_t * (iter->weighted_dx) / 1024;
 636           const int tmp1 = delta_t * (iter->weighted_dy) / 1024;
 637           direction0 += tmp0;
 638           direction1 += tmp1;
 639         }
 640         kp.angle = (float)(atan2((float) direction1, (float) direction0) / CV_PI * 180.0);
 641         if (kp.angle < 0)
 642           kp.angle += 360.f;
 643     }
 644
 645     if (!doDescriptors)
 646       continue;
 647
 648     int theta;
 649     if (kp.angle==-1)
 650     {
 651         // don't compute the gradient direction, just assign a rotation of 0°
 652         theta = 0;
 653     }
 654     else
 655     {
 656         theta = (int) (n_rot_ * (kp.angle / (360.0)) + 0.5);
 657         if (theta < 0)
 658           theta += n_rot_;
 659         if (theta >= int(n_rot_))
 660           theta -= n_rot_;
 661     }
 662
 663     // now also extract the stuff for the actual direction:
 664     // let us compute the smoothed values
 665     int shifter = 0;
 666
 667     //unsigned int mean=0;
 668     pvalues = _values;
 669     // get the gray values in the rotated pattern
 670     for (unsigned int i = 0; i < points_; i++)
 671     {
 672       *(pvalues++) = smoothedIntensity(image, _integral, x, y, scale, theta, i);
 673     }
 674
 675     // now iterate through all the pairings
 676     unsigned int* ptr2 = (unsigned int*) ptr;
 677     const BriskShortPair* max = shortPairs_ + noShortPairs_;
 678     for (BriskShortPair* iter = shortPairs_; iter < max; ++iter)
 679     {
 680       t1 = *(_values + iter->i);
 681       t2 = *(_values + iter->j);
 682       if (t1 > t2)
 683       {
 684         *ptr2 |= ((1) << shifter);
 685
 686       } // else already initialized with zero
 687       // take care of the iterators:
 688       ++shifter;
 689       if (shifter == 32)
 690       {
 691         shifter = 0;
 692         ++ptr2;
 693       }
 694     }
 695
 696     ptr += strings_;
 697   }
 698
 699   // clean-up
 700   delete[] _values;
 701 }
 702
 703 int
 704 BRISK::descriptorSize() const
 705 {
 706   return strings_;
 707 }
 708
 709 int
 710 BRISK::descriptorType() const
 711 {
 712   return CV_8U;
 713 }
 714
 715 int
 716 BRISK::defaultNorm() const
 717 {
 718   return NORM_HAMMING;
 719 }
 720
 721 BRISK::~BRISK()
 722 {
 723   delete[] patternPoints_;
 724   delete[] shortPairs_;
 725   delete[] longPairs_;
 726   delete[] scaleList_;
 727   delete[] sizeList_;
 728 }
 729
 730 void
 731 BRISK::operator()(InputArray image, InputArray mask, std::vector<KeyPoint>& keypoints) const
 732 {
 733   computeKeypointsNoOrientation(image, mask, keypoints);
 734   computeDescriptorsAndOrOrientation(image, mask, keypoints, cv::noArray(), false, true, true);
 735 }
 736
 737 void
 738 BRISK::computeKeypointsNoOrientation(InputArray _image, InputArray _mask, std::vector<KeyPoint>& keypoints) const
 739 {
 740   Mat image = _image.getMat(), mask = _mask.getMat();
 741   if( image.type() != CV_8UC1 )
 742       cvtColor(_image, image, COLOR_BGR2GRAY);
 743
 744   BriskScaleSpace briskScaleSpace(octaves);
 745   briskScaleSpace.constructPyramid(image);
 746   briskScaleSpace.getKeypoints(threshold, keypoints);
 747
 748   // remove invalid points
 749   removeInvalidPoints(mask, keypoints);
 750 }
 751
 752
 753 void
 754 BRISK::detectImpl( InputArray image, std::vector<KeyPoint>& keypoints, InputArray mask) const
 755 {
 756     (*this)(image.getMat(), mask.getMat(), keypoints);
 757 }
 758
 759 void
 760     BRISK::computeImpl( InputArray image, std::vector<KeyPoint>& keypoints, OutputArray descriptors) const
 761 {
 762     (*this)(image, Mat(), keypoints, descriptors, true);
 763 }
 764
 765 // construct telling the octaves number:
 766 BriskScaleSpace::BriskScaleSpace(int _octaves)
 767 {
 768   if (_octaves == 0)
 769     layers_ = 1;
 770   else
 771     layers_ = 2 * _octaves;
 772 }
 773 BriskScaleSpace::~BriskScaleSpace()
 774 {
 775
 776 }
 777 // construct the image pyramids
 778 void
 779 BriskScaleSpace::constructPyramid(const cv::Mat& image)
 780 {
 781
 782   // set correct size:
 783   pyramid_.clear();
 784
 785   // fill the pyramid:
 786   pyramid_.push_back(BriskLayer(image.clone()));
 787   if (layers_ > 1)
 788   {
 789     pyramid_.push_back(BriskLayer(pyramid_.back(), BriskLayer::CommonParams::TWOTHIRDSAMPLE));
 790   }
 791   const int octaves2 = layers_;
 792
 793   for (uchar i = 2; i < octaves2; i += 2)
 794   {
 795     pyramid_.push_back(BriskLayer(pyramid_[i - 2], BriskLayer::CommonParams::HALFSAMPLE));
 796     pyramid_.push_back(BriskLayer(pyramid_[i - 1], BriskLayer::CommonParams::HALFSAMPLE));
 797   }
 798 }
 799
 800 void
 801 BriskScaleSpace::getKeypoints(const int threshold_, std::vector<cv::KeyPoint>& keypoints)
 802 {
 803   // make sure keypoints is empty
 804   keypoints.resize(0);
 805   keypoints.reserve(2000);
 806
 807   // assign thresholds
 808   int safeThreshold_ = (int)(threshold_ * safetyFactor_);
 809   std::vector<std::vector<cv::KeyPoint> > agastPoints;
 810   agastPoints.resize(layers_);
 811
 812   // go through the octaves and intra layers and calculate fast corner scores:
 813   for (int i = 0; i < layers_; i++)
 814   {
 815     // call OAST16_9 without nms
 816     BriskLayer& l = pyramid_[i];
 817     l.getAgastPoints(safeThreshold_, agastPoints[i]);
 818   }
 819
 820   if (layers_ == 1)
 821   {
 822     // just do a simple 2d subpixel refinement...
 823     const size_t num = agastPoints[0].size();
 824     for (size_t n = 0; n < num; n++)
 825     {
 826       const cv::Point2f& point = agastPoints.at(0)[n].pt;
 827       // first check if it is a maximum:
 828       if (!isMax2D(0, (int)point.x, (int)point.y))
 829         continue;
 830
 831       // let's do the subpixel and float scale refinement:
 832       BriskLayer& l = pyramid_[0];
 833       int s_0_0 = l.getAgastScore(point.x - 1, point.y - 1, 1);
 834       int s_1_0 = l.getAgastScore(point.x, point.y - 1, 1);
 835       int s_2_0 = l.getAgastScore(point.x + 1, point.y - 1, 1);
 836       int s_2_1 = l.getAgastScore(point.x + 1, point.y, 1);
 837       int s_1_1 = l.getAgastScore(point.x, point.y, 1);
 838       int s_0_1 = l.getAgastScore(point.x - 1, point.y, 1);
 839       int s_0_2 = l.getAgastScore(point.x - 1, point.y + 1, 1);
 840       int s_1_2 = l.getAgastScore(point.x, point.y + 1, 1);
 841       int s_2_2 = l.getAgastScore(point.x + 1, point.y + 1, 1);
 842       float delta_x, delta_y;
 843       float max = subpixel2D(s_0_0, s_0_1, s_0_2, s_1_0, s_1_1, s_1_2, s_2_0, s_2_1, s_2_2, delta_x, delta_y);
 844
 845       // store:
 846       keypoints.push_back(cv::KeyPoint(float(point.x) + delta_x, float(point.y) + delta_y, basicSize_, -1, max, 0));
 847
 848     }
 849
 850     return;
 851   }
 852
 853   float x, y, scale, score;
 854   for (int i = 0; i < layers_; i++)
 855   {
 856     BriskLayer& l = pyramid_[i];
 857     const size_t num = agastPoints[i].size();
 858     if (i == layers_ - 1)
 859     {
 860       for (size_t n = 0; n < num; n++)
 861       {
 862         const cv::Point2f& point = agastPoints.at(i)[n].pt;
 863         // consider only 2D maxima...
 864         if (!isMax2D(i, (int)point.x, (int)point.y))
 865           continue;
 866
 867         bool ismax;
 868         float dx, dy;
 869         getScoreMaxBelow(i, (int)point.x, (int)point.y, l.getAgastScore(point.x, point.y, safeThreshold_), ismax, dx, dy);
 870         if (!ismax)
 871           continue;
 872
 873         // get the patch on this layer:
 874         int s_0_0 = l.getAgastScore(point.x - 1, point.y - 1, 1);
 875         int s_1_0 = l.getAgastScore(point.x, point.y - 1, 1);
 876         int s_2_0 = l.getAgastScore(point.x + 1, point.y - 1, 1);
 877         int s_2_1 = l.getAgastScore(point.x + 1, point.y, 1);
 878         int s_1_1 = l.getAgastScore(point.x, point.y, 1);
 879         int s_0_1 = l.getAgastScore(point.x - 1, point.y, 1);
 880         int s_0_2 = l.getAgastScore(point.x - 1, point.y + 1, 1);
 881         int s_1_2 = l.getAgastScore(point.x, point.y + 1, 1);
 882         int s_2_2 = l.getAgastScore(point.x + 1, point.y + 1, 1);
 883         float delta_x, delta_y;
 884         float max = subpixel2D(s_0_0, s_0_1, s_0_2, s_1_0, s_1_1, s_1_2, s_2_0, s_2_1, s_2_2, delta_x, delta_y);
 885
 886         // store:
 887         keypoints.push_back(
 888             cv::KeyPoint((float(point.x) + delta_x) * l.scale() + l.offset(),
 889                          (float(point.y) + delta_y) * l.scale() + l.offset(), basicSize_ * l.scale(), -1, max, i));
 890       }
 891     }
 892     else
 893     {
 894       // not the last layer:
 895       for (size_t n = 0; n < num; n++)
 896       {
 897         const cv::Point2f& point = agastPoints.at(i)[n].pt;
 898
 899         // first check if it is a maximum:
 900         if (!isMax2D(i, (int)point.x, (int)point.y))
 901           continue;
 902
 903         // let's do the subpixel and float scale refinement:
 904         bool ismax=false;
 905         score = refine3D(i, (int)point.x, (int)point.y, x, y, scale, ismax);
 906         if (!ismax)
 907         {
 908           continue;
 909         }
 910
 911         // finally store the detected keypoint:
 912         if (score > float(threshold_))
 913         {
 914           keypoints.push_back(cv::KeyPoint(x, y, basicSize_ * scale, -1, score, i));
 915         }
 916       }
 917     }
 918   }
 919 }
 920
 921 // interpolated score access with recalculation when needed:
 922 inline int
 923 BriskScaleSpace::getScoreAbove(const int layer, const int x_layer, const int y_layer) const
 924 {
 925   CV_Assert(layer < layers_-1);
 926   const BriskLayer& l = pyramid_[layer + 1];
 927   if (layer % 2 == 0)
 928   { // octave
 929     const int sixths_x = 4 * x_layer - 1;
 930     const int x_above = sixths_x / 6;
 931     const int sixths_y = 4 * y_layer - 1;
 932     const int y_above = sixths_y / 6;
 933     const int r_x = (sixths_x % 6);
 934     const int r_x_1 = 6 - r_x;
 935     const int r_y = (sixths_y % 6);
 936     const int r_y_1 = 6 - r_y;
 937     uchar score = 0xFF
 938         & ((r_x_1 * r_y_1 * l.getAgastScore(x_above, y_above, 1) + r_x * r_y_1
 939                                                                    * l.getAgastScore(x_above + 1, y_above, 1)
 940             + r_x_1 * r_y * l.getAgastScore(x_above, y_above + 1, 1)
 941             + r_x * r_y * l.getAgastScore(x_above + 1, y_above + 1, 1) + 18)
 942            / 36);
 943
 944     return score;
 945   }
 946   else
 947   { // intra
 948     const int eighths_x = 6 * x_layer - 1;
 949     const int x_above = eighths_x / 8;
 950     const int eighths_y = 6 * y_layer - 1;
 951     const int y_above = eighths_y / 8;
 952     const int r_x = (eighths_x % 8);
 953     const int r_x_1 = 8 - r_x;
 954     const int r_y = (eighths_y % 8);
 955     const int r_y_1 = 8 - r_y;
 956     uchar score = 0xFF
 957         & ((r_x_1 * r_y_1 * l.getAgastScore(x_above, y_above, 1) + r_x * r_y_1
 958                                                                    * l.getAgastScore(x_above + 1, y_above, 1)
 959             + r_x_1 * r_y * l.getAgastScore(x_above, y_above + 1, 1)
 960             + r_x * r_y * l.getAgastScore(x_above + 1, y_above + 1, 1) + 32)
 961            / 64);
 962     return score;
 963   }
 964 }
 965 inline int
 966 BriskScaleSpace::getScoreBelow(const int layer, const int x_layer, const int y_layer) const
 967 {
 968   CV_Assert(layer);
 969   const BriskLayer& l = pyramid_[layer - 1];
 970   int sixth_x;
 971   int quarter_x;
 972   float xf;
 973   int sixth_y;
 974   int quarter_y;
 975   float yf;
 976
 977   // scaling:
 978   float offs;
 979   float area;
 980   int scaling;
 981   int scaling2;
 982
 983   if (layer % 2 == 0)
 984   { // octave
 985     sixth_x = 8 * x_layer + 1;
 986     xf = float(sixth_x) / 6.0f;
 987     sixth_y = 8 * y_layer + 1;
 988     yf = float(sixth_y) / 6.0f;
 989
 990     // scaling:
 991     offs = 2.0f / 3.0f;
 992     area = 4.0f * offs * offs;
 993     scaling = (int)(4194304.0 / area);
 994     scaling2 = (int)(float(scaling) * area);
 995   }
 996   else
 997   {
 998     quarter_x = 6 * x_layer + 1;
 999     xf = float(quarter_x) / 4.0f;
1000     quarter_y = 6 * y_layer + 1;
1001     yf = float(quarter_y) / 4.0f;
1002
1003     // scaling:
1004     offs = 3.0f / 4.0f;
1005     area = 4.0f * offs * offs;
1006     scaling = (int)(4194304.0 / area);
1007     scaling2 = (int)(float(scaling) * area);
1008   }
1009
1010   // calculate borders
1011   const float x_1 = xf - offs;
1012   const float x1 = xf + offs;
1013   const float y_1 = yf - offs;
1014   const float y1 = yf + offs;
1015
1016   const int x_left = int(x_1 + 0.5);
1017   const int y_top = int(y_1 + 0.5);
1018   const int x_right = int(x1 + 0.5);
1019   const int y_bottom = int(y1 + 0.5);
1020
1021   // overlap area - multiplication factors:
1022   const float r_x_1 = float(x_left) - x_1 + 0.5f;
1023   const float r_y_1 = float(y_top) - y_1 + 0.5f;
1024   const float r_x1 = x1 - float(x_right) + 0.5f;
1025   const float r_y1 = y1 - float(y_bottom) + 0.5f;
1026   const int dx = x_right - x_left - 1;
1027   const int dy = y_bottom - y_top - 1;
1028   const int A = (int)((r_x_1 * r_y_1) * scaling);
1029   const int B = (int)((r_x1 * r_y_1) * scaling);
1030   const int C = (int)((r_x1 * r_y1) * scaling);
1031   const int D = (int)((r_x_1 * r_y1) * scaling);
1032   const int r_x_1_i = (int)(r_x_1 * scaling);
1033   const int r_y_1_i = (int)(r_y_1 * scaling);
1034   const int r_x1_i = (int)(r_x1 * scaling);
1035   const int r_y1_i = (int)(r_y1 * scaling);
1036
1037   // first row:
1038   int ret_val = A * int(l.getAgastScore(x_left, y_top, 1));
1039   for (int X = 1; X <= dx; X++)
1040   {
1041     ret_val += r_y_1_i * int(l.getAgastScore(x_left + X, y_top, 1));
1042   }
1043   ret_val += B * int(l.getAgastScore(x_left + dx + 1, y_top, 1));
1044   // middle ones:
1045   for (int Y = 1; Y <= dy; Y++)
1046   {
1047     ret_val += r_x_1_i * int(l.getAgastScore(x_left, y_top + Y, 1));
1048
1049     for (int X = 1; X <= dx; X++)
1050     {
1051       ret_val += int(l.getAgastScore(x_left + X, y_top + Y, 1)) * scaling;
1052     }
1053     ret_val += r_x1_i * int(l.getAgastScore(x_left + dx + 1, y_top + Y, 1));
1054   }
1055   // last row:
1056   ret_val += D * int(l.getAgastScore(x_left, y_top + dy + 1, 1));
1057   for (int X = 1; X <= dx; X++)
1058   {
1059     ret_val += r_y1_i * int(l.getAgastScore(x_left + X, y_top + dy + 1, 1));
1060   }
1061   ret_val += C * int(l.getAgastScore(x_left + dx + 1, y_top + dy + 1, 1));
1062
1063   return ((ret_val + scaling2 / 2) / scaling2);
1064 }
1065
1066 inline bool
1067 BriskScaleSpace::isMax2D(const int layer, const int x_layer, const int y_layer)
1068 {
1069   const cv::Mat& scores = pyramid_[layer].scores();
1070   const int scorescols = scores.cols;
1071   uchar* data = scores.data + y_layer * scorescols + x_layer;
1072   // decision tree:
1073   const uchar center = (*data);
1074   data--;
1075   const uchar s_10 = *data;
1076   if (center < s_10)
1077     return false;
1078   data += 2;
1079   const uchar s10 = *data;
1080   if (center < s10)
1081     return false;
1082   data -= (scorescols + 1);
1083   const uchar s0_1 = *data;
1084   if (center < s0_1)
1085     return false;
1086   data += 2 * scorescols;
1087   const uchar s01 = *data;
1088   if (center < s01)
1089     return false;
1090   data--;
1091   const uchar s_11 = *data;
1092   if (center < s_11)
1093     return false;
1094   data += 2;
1095   const uchar s11 = *data;
1096   if (center < s11)
1097     return false;
1098   data -= 2 * scorescols;
1099   const uchar s1_1 = *data;
1100   if (center < s1_1)
1101     return false;
1102   data -= 2;
1103   const uchar s_1_1 = *data;
1104   if (center < s_1_1)
1105     return false;
1106
1107   // reject neighbor maxima
1108   std::vector<int> delta;
1109   // put together a list of 2d-offsets to where the maximum is also reached
1110   if (center == s_1_1)
1111   {
1112     delta.push_back(-1);
1113     delta.push_back(-1);
1114   }
1115   if (center == s0_1)
1116   {
1117     delta.push_back(0);
1118     delta.push_back(-1);
1119   }
1120   if (center == s1_1)
1121   {
1122     delta.push_back(1);
1123     delta.push_back(-1);
1124   }
1125   if (center == s_10)
1126   {
1127     delta.push_back(-1);
1128     delta.push_back(0);
1129   }
1130   if (center == s10)
1131   {
1132     delta.push_back(1);
1133     delta.push_back(0);
1134   }
1135   if (center == s_11)
1136   {
1137     delta.push_back(-1);
1138     delta.push_back(1);
1139   }
1140   if (center == s01)
1141   {
1142     delta.push_back(0);
1143     delta.push_back(1);
1144   }
1145   if (center == s11)
1146   {
1147     delta.push_back(1);
1148     delta.push_back(1);
1149   }
1150   const unsigned int deltasize = (unsigned int)delta.size();
1151   if (deltasize != 0)
1152   {
1153     // in this case, we have to analyze the situation more carefully:
1154     // the values are gaussian blurred and then we really decide
1155     data = scores.data + y_layer * scorescols + x_layer;
1156     int smoothedcenter = 4 * center + 2 * (s_10 + s10 + s0_1 + s01) + s_1_1 + s1_1 + s_11 + s11;
1157     for (unsigned int i = 0; i < deltasize; i += 2)
1158     {
1159       data = scores.data + (y_layer - 1 + delta[i + 1]) * scorescols + x_layer + delta[i] - 1;
1160       int othercenter = *data;
1161       data++;
1162       othercenter += 2 * (*data);
1163       data++;
1164       othercenter += *data;
1165       data += scorescols;
1166       othercenter += 2 * (*data);
1167       data--;
1168       othercenter += 4 * (*data);
1169       data--;
1170       othercenter += 2 * (*data);
1171       data += scorescols;
1172       othercenter += *data;
1173       data++;
1174       othercenter += 2 * (*data);
1175       data++;
1176       othercenter += *data;
1177       if (othercenter > smoothedcenter)
1178         return false;
1179     }
1180   }
1181   return true;
1182 }
1183
1184 // 3D maximum refinement centered around (x_layer,y_layer)
1185 inline float
1186 BriskScaleSpace::refine3D(const int layer, const int x_layer, const int y_layer, float& x, float& y, float& scale,
1187                           bool& ismax) const
1188 {
1189   ismax = true;
1190   const BriskLayer& thisLayer = pyramid_[layer];
1191   const int center = thisLayer.getAgastScore(x_layer, y_layer, 1);
1192
1193   // check and get above maximum:
1194   float delta_x_above = 0, delta_y_above = 0;
1195   float max_above = getScoreMaxAbove(layer, x_layer, y_layer, center, ismax, delta_x_above, delta_y_above);
1196
1197   if (!ismax)
1198     return 0.0f;
1199
1200   float max; // to be returned
1201
1202   if (layer % 2 == 0)
1203   { // on octave
1204     // treat the patch below:
1205     float delta_x_below, delta_y_below;
1206     float max_below_float;
1207     int max_below = 0;
1208     if (layer == 0)
1209     {
1210       // guess the lower intra octave...
1211       const BriskLayer& l = pyramid_[0];
1212       int s_0_0 = l.getAgastScore_5_8(x_layer - 1, y_layer - 1, 1);
1213       max_below = s_0_0;
1214       int s_1_0 = l.getAgastScore_5_8(x_layer, y_layer - 1, 1);
1215       max_below = std::max(s_1_0, max_below);
1216       int s_2_0 = l.getAgastScore_5_8(x_layer + 1, y_layer - 1, 1);
1217       max_below = std::max(s_2_0, max_below);
1218       int s_2_1 = l.getAgastScore_5_8(x_layer + 1, y_layer, 1);
1219       max_below = std::max(s_2_1, max_below);
1220       int s_1_1 = l.getAgastScore_5_8(x_layer, y_layer, 1);
1221       max_below = std::max(s_1_1, max_below);
1222       int s_0_1 = l.getAgastScore_5_8(x_layer - 1, y_layer, 1);
1223       max_below = std::max(s_0_1, max_below);
1224       int s_0_2 = l.getAgastScore_5_8(x_layer - 1, y_layer + 1, 1);
1225       max_below = std::max(s_0_2, max_below);
1226       int s_1_2 = l.getAgastScore_5_8(x_layer, y_layer + 1, 1);
1227       max_below = std::max(s_1_2, max_below);
1228       int s_2_2 = l.getAgastScore_5_8(x_layer + 1, y_layer + 1, 1);
1229       max_below = std::max(s_2_2, max_below);
1230
1231       max_below_float = subpixel2D(s_0_0, s_0_1, s_0_2, s_1_0, s_1_1, s_1_2, s_2_0, s_2_1, s_2_2, delta_x_below,
1232                                    delta_y_below);
1233       max_below_float = (float)max_below;
1234     }
1235     else
1236     {
1237       max_below_float = getScoreMaxBelow(layer, x_layer, y_layer, center, ismax, delta_x_below, delta_y_below);
1238       if (!ismax)
1239         return 0;
1240     }
1241
1242     // get the patch on this layer:
1243     int s_0_0 = thisLayer.getAgastScore(x_layer - 1, y_layer - 1, 1);
1244     int s_1_0 = thisLayer.getAgastScore(x_layer, y_layer - 1, 1);
1245     int s_2_0 = thisLayer.getAgastScore(x_layer + 1, y_layer - 1, 1);
1246     int s_2_1 = thisLayer.getAgastScore(x_layer + 1, y_layer, 1);
1247     int s_1_1 = thisLayer.getAgastScore(x_layer, y_layer, 1);
1248     int s_0_1 = thisLayer.getAgastScore(x_layer - 1, y_layer, 1);
1249     int s_0_2 = thisLayer.getAgastScore(x_layer - 1, y_layer + 1, 1);
1250     int s_1_2 = thisLayer.getAgastScore(x_layer, y_layer + 1, 1);
1251     int s_2_2 = thisLayer.getAgastScore(x_layer + 1, y_layer + 1, 1);
1252     float delta_x_layer, delta_y_layer;
1253     float max_layer = subpixel2D(s_0_0, s_0_1, s_0_2, s_1_0, s_1_1, s_1_2, s_2_0, s_2_1, s_2_2, delta_x_layer,
1254                                  delta_y_layer);
1255
1256     // calculate the relative scale (1D maximum):
1257     if (layer == 0)
1258     {
1259       scale = refine1D_2(max_below_float, std::max(float(center), max_layer), max_above, max);
1260     }
1261     else
1262       scale = refine1D(max_below_float, std::max(float(center), max_layer), max_above, max);
1263
1264     if (scale > 1.0)
1265     {
1266       // interpolate the position:
1267       const float r0 = (1.5f - scale) / .5f;
1268       const float r1 = 1.0f - r0;
1269       x = (r0 * delta_x_layer + r1 * delta_x_above + float(x_layer)) * thisLayer.scale() + thisLayer.offset();
1270       y = (r0 * delta_y_layer + r1 * delta_y_above + float(y_layer)) * thisLayer.scale() + thisLayer.offset();
1271     }
1272     else
1273     {
1274       if (layer == 0)
1275       {
1276         // interpolate the position:
1277         const float r0 = (scale - 0.5f) / 0.5f;
1278         const float r_1 = 1.0f - r0;
1279         x = r0 * delta_x_layer + r_1 * delta_x_below + float(x_layer);
1280         y = r0 * delta_y_layer + r_1 * delta_y_below + float(y_layer);
1281       }
1282       else
1283       {
1284         // interpolate the position:
1285         const float r0 = (scale - 0.75f) / 0.25f;
1286         const float r_1 = 1.0f - r0;
1287         x = (r0 * delta_x_layer + r_1 * delta_x_below + float(x_layer)) * thisLayer.scale() + thisLayer.offset();
1288         y = (r0 * delta_y_layer + r_1 * delta_y_below + float(y_layer)) * thisLayer.scale() + thisLayer.offset();
1289       }
1290     }
1291   }
1292   else
1293   {
1294     // on intra
1295     // check the patch below:
1296     float delta_x_below, delta_y_below;
1297     float max_below = getScoreMaxBelow(layer, x_layer, y_layer, center, ismax, delta_x_below, delta_y_below);
1298     if (!ismax)
1299       return 0.0f;
1300
1301     // get the patch on this layer:
1302     int s_0_0 = thisLayer.getAgastScore(x_layer - 1, y_layer - 1, 1);
1303     int s_1_0 = thisLayer.getAgastScore(x_layer, y_layer - 1, 1);
1304     int s_2_0 = thisLayer.getAgastScore(x_layer + 1, y_layer - 1, 1);
1305     int s_2_1 = thisLayer.getAgastScore(x_layer + 1, y_layer, 1);
1306     int s_1_1 = thisLayer.getAgastScore(x_layer, y_layer, 1);
1307     int s_0_1 = thisLayer.getAgastScore(x_layer - 1, y_layer, 1);
1308     int s_0_2 = thisLayer.getAgastScore(x_layer - 1, y_layer + 1, 1);
1309     int s_1_2 = thisLayer.getAgastScore(x_layer, y_layer + 1, 1);
1310     int s_2_2 = thisLayer.getAgastScore(x_layer + 1, y_layer + 1, 1);
1311     float delta_x_layer, delta_y_layer;
1312     float max_layer = subpixel2D(s_0_0, s_0_1, s_0_2, s_1_0, s_1_1, s_1_2, s_2_0, s_2_1, s_2_2, delta_x_layer,
1313                                  delta_y_layer);
1314
1315     // calculate the relative scale (1D maximum):
1316     scale = refine1D_1(max_below, std::max(float(center), max_layer), max_above, max);
1317     if (scale > 1.0)
1318     {
1319       // interpolate the position:
1320       const float r0 = 4.0f - scale * 3.0f;
1321       const float r1 = 1.0f - r0;
1322       x = (r0 * delta_x_layer + r1 * delta_x_above + float(x_layer)) * thisLayer.scale() + thisLayer.offset();
1323       y = (r0 * delta_y_layer + r1 * delta_y_above + float(y_layer)) * thisLayer.scale() + thisLayer.offset();
1324     }
1325     else
1326     {
1327       // interpolate the position:
1328       const float r0 = scale * 3.0f - 2.0f;
1329       const float r_1 = 1.0f - r0;
1330       x = (r0 * delta_x_layer + r_1 * delta_x_below + float(x_layer)) * thisLayer.scale() + thisLayer.offset();
1331       y = (r0 * delta_y_layer + r_1 * delta_y_below + float(y_layer)) * thisLayer.scale() + thisLayer.offset();
1332     }
1333   }
1334
1335   // calculate the absolute scale:
1336   scale *= thisLayer.scale();
1337
1338   // that's it, return the refined maximum:
1339   return max;
1340 }
1341
1342 // return the maximum of score patches above or below
1343 inline float
1344 BriskScaleSpace::getScoreMaxAbove(const int layer, const int x_layer, const int y_layer, const int threshold,
1345                                   bool& ismax, float& dx, float& dy) const
1346 {
1347
1348   ismax = false;
1349   // relevant floating point coordinates
1350   float x_1;
1351   float x1;
1352   float y_1;
1353   float y1;
1354
1355   // the layer above
1356   CV_Assert(layer + 1 < layers_);
1357   const BriskLayer& layerAbove = pyramid_[layer + 1];
1358
1359   if (layer % 2 == 0)
1360   {
1361     // octave
1362     x_1 = float(4 * (x_layer) - 1 - 2) / 6.0f;
1363     x1 = float(4 * (x_layer) - 1 + 2) / 6.0f;
1364     y_1 = float(4 * (y_layer) - 1 - 2) / 6.0f;
1365     y1 = float(4 * (y_layer) - 1 + 2) / 6.0f;
1366   }
1367   else
1368   {
1369     // intra
1370     x_1 = float(6 * (x_layer) - 1 - 3) / 8.0f;
1371     x1 = float(6 * (x_layer) - 1 + 3) / 8.0f;
1372     y_1 = float(6 * (y_layer) - 1 - 3) / 8.0f;
1373     y1 = float(6 * (y_layer) - 1 + 3) / 8.0f;
1374   }
1375
1376   // check the first row
1377   int max_x = (int)x_1 + 1;
1378   int max_y = (int)y_1 + 1;
1379   float tmp_max;
1380   float maxval = (float)layerAbove.getAgastScore(x_1, y_1, 1);
1381   if (maxval > threshold)
1382     return 0;
1383   for (int x = (int)x_1 + 1; x <= int(x1); x++)
1384   {
1385     tmp_max = (float)layerAbove.getAgastScore(float(x), y_1, 1);
1386     if (tmp_max > threshold)
1387       return 0;
1388     if (tmp_max > maxval)
1389     {
1390       maxval = tmp_max;
1391       max_x = x;
1392     }
1393   }
1394   tmp_max = (float)layerAbove.getAgastScore(x1, y_1, 1);
1395   if (tmp_max > threshold)
1396     return 0;
1397   if (tmp_max > maxval)
1398   {
1399     maxval = tmp_max;
1400     max_x = int(x1);
1401   }
1402
1403   // middle rows
1404   for (int y = (int)y_1 + 1; y <= int(y1); y++)
1405   {
1406     tmp_max = (float)layerAbove.getAgastScore(x_1, float(y), 1);
1407     if (tmp_max > threshold)
1408       return 0;
1409     if (tmp_max > maxval)
1410     {
1411       maxval = tmp_max;
1412       max_x = int(x_1 + 1);
1413       max_y = y;
1414     }
1415     for (int x = (int)x_1 + 1; x <= int(x1); x++)
1416     {
1417       tmp_max = (float)layerAbove.getAgastScore(x, y, 1);
1418       if (tmp_max > threshold)
1419         return 0;
1420       if (tmp_max > maxval)
1421       {
1422         maxval = tmp_max;
1423         max_x = x;
1424         max_y = y;
1425       }
1426     }
1427     tmp_max = (float)layerAbove.getAgastScore(x1, float(y), 1);
1428     if (tmp_max > threshold)
1429       return 0;
1430     if (tmp_max > maxval)
1431     {
1432       maxval = tmp_max;
1433       max_x = int(x1);
1434       max_y = y;
1435     }
1436   }
1437
1438   // bottom row
1439   tmp_max = (float)layerAbove.getAgastScore(x_1, y1, 1);
1440   if (tmp_max > maxval)
1441   {
1442     maxval = tmp_max;
1443     max_x = int(x_1 + 1);
1444     max_y = int(y1);
1445   }
1446   for (int x = (int)x_1 + 1; x <= int(x1); x++)
1447   {
1448     tmp_max = (float)layerAbove.getAgastScore(float(x), y1, 1);
1449     if (tmp_max > maxval)
1450     {
1451       maxval = tmp_max;
1452       max_x = x;
1453       max_y = int(y1);
1454     }
1455   }
1456   tmp_max = (float)layerAbove.getAgastScore(x1, y1, 1);
1457   if (tmp_max > maxval)
1458   {
1459     maxval = tmp_max;
1460     max_x = int(x1);
1461     max_y = int(y1);
1462   }
1463
1464   //find dx/dy:
1465   int s_0_0 = layerAbove.getAgastScore(max_x - 1, max_y - 1, 1);
1466   int s_1_0 = layerAbove.getAgastScore(max_x, max_y - 1, 1);
1467   int s_2_0 = layerAbove.getAgastScore(max_x + 1, max_y - 1, 1);
1468   int s_2_1 = layerAbove.getAgastScore(max_x + 1, max_y, 1);
1469   int s_1_1 = layerAbove.getAgastScore(max_x, max_y, 1);
1470   int s_0_1 = layerAbove.getAgastScore(max_x - 1, max_y, 1);
1471   int s_0_2 = layerAbove.getAgastScore(max_x - 1, max_y + 1, 1);
1472   int s_1_2 = layerAbove.getAgastScore(max_x, max_y + 1, 1);
1473   int s_2_2 = layerAbove.getAgastScore(max_x + 1, max_y + 1, 1);
1474   float dx_1, dy_1;
1475   float refined_max = subpixel2D(s_0_0, s_0_1, s_0_2, s_1_0, s_1_1, s_1_2, s_2_0, s_2_1, s_2_2, dx_1, dy_1);
1476
1477   // calculate dx/dy in above coordinates
1478   float real_x = float(max_x) + dx_1;
1479   float real_y = float(max_y) + dy_1;
1480   bool returnrefined = true;
1481   if (layer % 2 == 0)
1482   {
1483     dx = (real_x * 6.0f + 1.0f) / 4.0f - float(x_layer);
1484     dy = (real_y * 6.0f + 1.0f) / 4.0f - float(y_layer);
1485   }
1486   else
1487   {
1488     dx = (real_x * 8.0f + 1.0f) / 6.0f - float(x_layer);
1489     dy = (real_y * 8.0f + 1.0f) / 6.0f - float(y_layer);
1490   }
1491
1492   // saturate
1493   if (dx > 1.0f)
1494   {
1495     dx = 1.0f;
1496     returnrefined = false;
1497   }
1498   if (dx < -1.0f)
1499   {
1500     dx = -1.0f;
1501     returnrefined = false;
1502   }
1503   if (dy > 1.0f)
1504   {
1505     dy = 1.0f;
1506     returnrefined = false;
1507   }
1508   if (dy < -1.0f)
1509   {
1510     dy = -1.0f;
1511     returnrefined = false;
1512   }
1513
1514   // done and ok.
1515   ismax = true;
1516   if (returnrefined)
1517   {
1518     return std::max(refined_max, maxval);
1519   }
1520   return maxval;
1521 }
1522
1523 inline float
1524 BriskScaleSpace::getScoreMaxBelow(const int layer, const int x_layer, const int y_layer, const int threshold,
1525                                   bool& ismax, float& dx, float& dy) const
1526 {
1527   ismax = false;
1528
1529   // relevant floating point coordinates
1530   float x_1;
1531   float x1;
1532   float y_1;
1533   float y1;
1534
1535   if (layer % 2 == 0)
1536   {
1537     // octave
1538     x_1 = float(8 * (x_layer) + 1 - 4) / 6.0f;
1539     x1 = float(8 * (x_layer) + 1 + 4) / 6.0f;
1540     y_1 = float(8 * (y_layer) + 1 - 4) / 6.0f;
1541     y1 = float(8 * (y_layer) + 1 + 4) / 6.0f;
1542   }
1543   else
1544   {
1545     x_1 = float(6 * (x_layer) + 1 - 3) / 4.0f;
1546     x1 = float(6 * (x_layer) + 1 + 3) / 4.0f;
1547     y_1 = float(6 * (y_layer) + 1 - 3) / 4.0f;
1548     y1 = float(6 * (y_layer) + 1 + 3) / 4.0f;
1549   }
1550
1551   // the layer below
1552   CV_Assert(layer > 0);
1553   const BriskLayer& layerBelow = pyramid_[layer - 1];
1554
1555   // check the first row
1556   int max_x = (int)x_1 + 1;
1557   int max_y = (int)y_1 + 1;
1558   float tmp_max;
1559   float max = (float)layerBelow.getAgastScore(x_1, y_1, 1);
1560   if (max > threshold)
1561     return 0;
1562   for (int x = (int)x_1 + 1; x <= int(x1); x++)
1563   {
1564     tmp_max = (float)layerBelow.getAgastScore(float(x), y_1, 1);
1565     if (tmp_max > threshold)
1566       return 0;
1567     if (tmp_max > max)
1568     {
1569       max = tmp_max;
1570       max_x = x;
1571     }
1572   }
1573   tmp_max = (float)layerBelow.getAgastScore(x1, y_1, 1);
1574   if (tmp_max > threshold)
1575     return 0;
1576   if (tmp_max > max)
1577   {
1578     max = tmp_max;
1579     max_x = int(x1);
1580   }
1581
1582   // middle rows
1583   for (int y = (int)y_1 + 1; y <= int(y1); y++)
1584   {
1585     tmp_max = (float)layerBelow.getAgastScore(x_1, float(y), 1);
1586     if (tmp_max > threshold)
1587       return 0;
1588     if (tmp_max > max)
1589     {
1590       max = tmp_max;
1591       max_x = int(x_1 + 1);
1592       max_y = y;
1593     }
1594     for (int x = (int)x_1 + 1; x <= int(x1); x++)
1595     {
1596       tmp_max = (float)layerBelow.getAgastScore(x, y, 1);
1597       if (tmp_max > threshold)
1598         return 0;
1599       if (tmp_max == max)
1600       {
1601         const int t1 = 2
1602             * (layerBelow.getAgastScore(x - 1, y, 1) + layerBelow.getAgastScore(x + 1, y, 1)
1603                + layerBelow.getAgastScore(x, y + 1, 1) + layerBelow.getAgastScore(x, y - 1, 1))
1604                        + (layerBelow.getAgastScore(x + 1, y + 1, 1) + layerBelow.getAgastScore(x - 1, y + 1, 1)
1605                           + layerBelow.getAgastScore(x + 1, y - 1, 1) + layerBelow.getAgastScore(x - 1, y - 1, 1));
1606         const int t2 = 2
1607             * (layerBelow.getAgastScore(max_x - 1, max_y, 1) + layerBelow.getAgastScore(max_x + 1, max_y, 1)
1608                + layerBelow.getAgastScore(max_x, max_y + 1, 1) + layerBelow.getAgastScore(max_x, max_y - 1, 1))
1609                        + (layerBelow.getAgastScore(max_x + 1, max_y + 1, 1) + layerBelow.getAgastScore(max_x - 1,
1610                                                                                                        max_y + 1, 1)
1611                           + layerBelow.getAgastScore(max_x + 1, max_y - 1, 1)
1612                           + layerBelow.getAgastScore(max_x - 1, max_y - 1, 1));
1613         if (t1 > t2)
1614         {
1615           max_x = x;
1616           max_y = y;
1617         }
1618       }
1619       if (tmp_max > max)
1620       {
1621         max = tmp_max;
1622         max_x = x;
1623         max_y = y;
1624       }
1625     }
1626     tmp_max = (float)layerBelow.getAgastScore(x1, float(y), 1);
1627     if (tmp_max > threshold)
1628       return 0;
1629     if (tmp_max > max)
1630     {
1631       max = tmp_max;
1632       max_x = int(x1);
1633       max_y = y;
1634     }
1635   }
1636
1637   // bottom row
1638   tmp_max = (float)layerBelow.getAgastScore(x_1, y1, 1);
1639   if (tmp_max > max)
1640   {
1641     max = tmp_max;
1642     max_x = int(x_1 + 1);
1643     max_y = int(y1);
1644   }
1645   for (int x = (int)x_1 + 1; x <= int(x1); x++)
1646   {
1647     tmp_max = (float)layerBelow.getAgastScore(float(x), y1, 1);
1648     if (tmp_max > max)
1649     {
1650       max = tmp_max;
1651       max_x = x;
1652       max_y = int(y1);
1653     }
1654   }
1655   tmp_max = (float)layerBelow.getAgastScore(x1, y1, 1);
1656   if (tmp_max > max)
1657   {
1658     max = tmp_max;
1659     max_x = int(x1);
1660     max_y = int(y1);
1661   }
1662
1663   //find dx/dy:
1664   int s_0_0 = layerBelow.getAgastScore(max_x - 1, max_y - 1, 1);
1665   int s_1_0 = layerBelow.getAgastScore(max_x, max_y - 1, 1);
1666   int s_2_0 = layerBelow.getAgastScore(max_x + 1, max_y - 1, 1);
1667   int s_2_1 = layerBelow.getAgastScore(max_x + 1, max_y, 1);
1668   int s_1_1 = layerBelow.getAgastScore(max_x, max_y, 1);
1669   int s_0_1 = layerBelow.getAgastScore(max_x - 1, max_y, 1);
1670   int s_0_2 = layerBelow.getAgastScore(max_x - 1, max_y + 1, 1);
1671   int s_1_2 = layerBelow.getAgastScore(max_x, max_y + 1, 1);
1672   int s_2_2 = layerBelow.getAgastScore(max_x + 1, max_y + 1, 1);
1673   float dx_1, dy_1;
1674   float refined_max = subpixel2D(s_0_0, s_0_1, s_0_2, s_1_0, s_1_1, s_1_2, s_2_0, s_2_1, s_2_2, dx_1, dy_1);
1675
1676   // calculate dx/dy in above coordinates
1677   float real_x = float(max_x) + dx_1;
1678   float real_y = float(max_y) + dy_1;
1679   bool returnrefined = true;
1680   if (layer % 2 == 0)
1681   {
1682     dx = (float)((real_x * 6.0 + 1.0) / 8.0) - float(x_layer);
1683     dy = (float)((real_y * 6.0 + 1.0) / 8.0) - float(y_layer);
1684   }
1685   else
1686   {
1687     dx = (float)((real_x * 4.0 - 1.0) / 6.0) - float(x_layer);
1688     dy = (float)((real_y * 4.0 - 1.0) / 6.0) - float(y_layer);
1689   }
1690
1691   // saturate
1692   if (dx > 1.0)
1693   {
1694     dx = 1.0f;
1695     returnrefined = false;
1696   }
1697   if (dx < -1.0f)
1698   {
1699     dx = -1.0f;
1700     returnrefined = false;
1701   }
1702   if (dy > 1.0f)
1703   {
1704     dy = 1.0f;
1705     returnrefined = false;
1706   }
1707   if (dy < -1.0f)
1708   {
1709     dy = -1.0f;
1710     returnrefined = false;
1711   }
1712
1713   // done and ok.
1714   ismax = true;
1715   if (returnrefined)
1716   {
1717     return std::max(refined_max, max);
1718   }
1719   return max;
1720 }
1721
1722 inline float
1723 BriskScaleSpace::refine1D(const float s_05, const float s0, const float s05, float& max) const
1724 {
1725   int i_05 = int(1024.0 * s_05 + 0.5);
1726   int i0 = int(1024.0 * s0 + 0.5);
1727   int i05 = int(1024.0 * s05 + 0.5);
1728
1729   //   16.0000  -24.0000    8.0000
1730   //  -40.0000   54.0000  -14.0000
1731   //   24.0000  -27.0000    6.0000
1732
1733   int three_a = 16 * i_05 - 24 * i0 + 8 * i05;
1734   // second derivative must be negative:
1735   if (three_a >= 0)
1736   {
1737     if (s0 >= s_05 && s0 >= s05)
1738     {
1739       max = s0;
1740       return 1.0f;
1741     }
1742     if (s_05 >= s0 && s_05 >= s05)
1743     {
1744       max = s_05;
1745       return 0.75f;
1746     }
1747     if (s05 >= s0 && s05 >= s_05)
1748     {
1749       max = s05;
1750       return 1.5f;
1751     }
1752   }
1753
1754   int three_b = -40 * i_05 + 54 * i0 - 14 * i05;
1755   // calculate max location:
1756   float ret_val = -float(three_b) / float(2 * three_a);
1757   // saturate and return
1758   if (ret_val < 0.75)
1759     ret_val = 0.75;
1760   else if (ret_val > 1.5)
1761     ret_val = 1.5; // allow to be slightly off bounds ...?
1762   int three_c = +24 * i_05 - 27 * i0 + 6 * i05;
1763   max = float(three_c) + float(three_a) * ret_val * ret_val + float(three_b) * ret_val;
1764   max /= 3072.0f;
1765   return ret_val;
1766 }
1767
1768 inline float
1769 BriskScaleSpace::refine1D_1(const float s_05, const float s0, const float s05, float& max) const
1770 {
1771   int i_05 = int(1024.0 * s_05 + 0.5);
1772   int i0 = int(1024.0 * s0 + 0.5);
1773   int i05 = int(1024.0 * s05 + 0.5);
1774
1775   //  4.5000   -9.0000    4.5000
1776   //-10.5000   18.0000   -7.5000
1777   //  6.0000   -8.0000    3.0000
1778
1779   int two_a = 9 * i_05 - 18 * i0 + 9 * i05;
1780   // second derivative must be negative:
1781   if (two_a >= 0)
1782   {
1783     if (s0 >= s_05 && s0 >= s05)
1784     {
1785       max = s0;
1786       return 1.0f;
1787     }
1788     if (s_05 >= s0 && s_05 >= s05)
1789     {
1790       max = s_05;
1791       return 0.6666666666666666666666666667f;
1792     }
1793     if (s05 >= s0 && s05 >= s_05)
1794     {
1795       max = s05;
1796       return 1.3333333333333333333333333333f;
1797     }
1798   }
1799
1800   int two_b = -21 * i_05 + 36 * i0 - 15 * i05;
1801   // calculate max location:
1802   float ret_val = -float(two_b) / float(2 * two_a);
1803   // saturate and return
1804   if (ret_val < 0.6666666666666666666666666667f)
1805     ret_val = 0.666666666666666666666666667f;
1806   else if (ret_val > 1.33333333333333333333333333f)
1807     ret_val = 1.333333333333333333333333333f;
1808   int two_c = +12 * i_05 - 16 * i0 + 6 * i05;
1809   max = float(two_c) + float(two_a) * ret_val * ret_val + float(two_b) * ret_val;
1810   max /= 2048.0f;
1811   return ret_val;
1812 }
1813
1814 inline float
1815 BriskScaleSpace::refine1D_2(const float s_05, const float s0, const float s05, float& max) const
1816 {
1817   int i_05 = int(1024.0 * s_05 + 0.5);
1818   int i0 = int(1024.0 * s0 + 0.5);
1819   int i05 = int(1024.0 * s05 + 0.5);
1820
1821   //   18.0000  -30.0000   12.0000
1822   //  -45.0000   65.0000  -20.0000
1823   //   27.0000  -30.0000    8.0000
1824
1825   int a = 2 * i_05 - 4 * i0 + 2 * i05;
1826   // second derivative must be negative:
1827   if (a >= 0)
1828   {
1829     if (s0 >= s_05 && s0 >= s05)
1830     {
1831       max = s0;
1832       return 1.0f;
1833     }
1834     if (s_05 >= s0 && s_05 >= s05)
1835     {
1836       max = s_05;
1837       return 0.7f;
1838     }
1839     if (s05 >= s0 && s05 >= s_05)
1840     {
1841       max = s05;
1842       return 1.5f;
1843     }
1844   }
1845
1846   int b = -5 * i_05 + 8 * i0 - 3 * i05;
1847   // calculate max location:
1848   float ret_val = -float(b) / float(2 * a);
1849   // saturate and return
1850   if (ret_val < 0.7f)
1851     ret_val = 0.7f;
1852   else if (ret_val > 1.5f)
1853     ret_val = 1.5f; // allow to be slightly off bounds ...?
1854   int c = +3 * i_05 - 3 * i0 + 1 * i05;
1855   max = float(c) + float(a) * ret_val * ret_val + float(b) * ret_val;
1856   max /= 1024;
1857   return ret_val;
1858 }
1859
1860 inline float
1861 BriskScaleSpace::subpixel2D(const int s_0_0, const int s_0_1, const int s_0_2, const int s_1_0, const int s_1_1,
1862                             const int s_1_2, const int s_2_0, const int s_2_1, const int s_2_2, float& delta_x,
1863                             float& delta_y) const
1864 {
1865
1866   // the coefficients of the 2d quadratic function least-squares fit:
1867   int tmp1 = s_0_0 + s_0_2 - 2 * s_1_1 + s_2_0 + s_2_2;
1868   int coeff1 = 3 * (tmp1 + s_0_1 - ((s_1_0 + s_1_2) << 1) + s_2_1);
1869   int coeff2 = 3 * (tmp1 - ((s_0_1 + s_2_1) << 1) + s_1_0 + s_1_2);
1870   int tmp2 = s_0_2 - s_2_0;
1871   int tmp3 = (s_0_0 + tmp2 - s_2_2);
1872   int tmp4 = tmp3 - 2 * tmp2;
1873   int coeff3 = -3 * (tmp3 + s_0_1 - s_2_1);
1874   int coeff4 = -3 * (tmp4 + s_1_0 - s_1_2);
1875   int coeff5 = (s_0_0 - s_0_2 - s_2_0 + s_2_2) << 2;
1876   int coeff6 = -(s_0_0 + s_0_2 - ((s_1_0 + s_0_1 + s_1_2 + s_2_1) << 1) - 5 * s_1_1 + s_2_0 + s_2_2) << 1;
1877
1878   // 2nd derivative test:
1879   int H_det = 4 * coeff1 * coeff2 - coeff5 * coeff5;
1880
1881   if (H_det == 0)
1882   {
1883     delta_x = 0.0f;
1884     delta_y = 0.0f;
1885     return float(coeff6) / 18.0f;
1886   }
1887
1888   if (!(H_det > 0 && coeff1 < 0))
1889   {
1890     // The maximum must be at the one of the 4 patch corners.
1891     int tmp_max = coeff3 + coeff4 + coeff5;
1892     delta_x = 1.0f;
1893     delta_y = 1.0f;
1894
1895     int tmp = -coeff3 + coeff4 - coeff5;
1896     if (tmp > tmp_max)
1897     {
1898       tmp_max = tmp;
1899       delta_x = -1.0f;
1900       delta_y = 1.0f;
1901     }
1902     tmp = coeff3 - coeff4 - coeff5;
1903     if (tmp > tmp_max)
1904     {
1905       tmp_max = tmp;
1906       delta_x = 1.0f;
1907       delta_y = -1.0f;
1908     }
1909     tmp = -coeff3 - coeff4 + coeff5;
1910     if (tmp > tmp_max)
1911     {
1912       tmp_max = tmp;
1913       delta_x = -1.0f;
1914       delta_y = -1.0f;
1915     }
1916     return float(tmp_max + coeff1 + coeff2 + coeff6) / 18.0f;
1917   }
1918
1919   // this is hopefully the normal outcome of the Hessian test
1920   delta_x = float(2 * coeff2 * coeff3 - coeff4 * coeff5) / float(-H_det);
1921   delta_y = float(2 * coeff1 * coeff4 - coeff3 * coeff5) / float(-H_det);
1922   // TODO: this is not correct, but easy, so perform a real boundary maximum search:
1923   bool tx = false;
1924   bool tx_ = false;
1925   bool ty = false;
1926   bool ty_ = false;
1927   if (delta_x > 1.0)
1928     tx = true;
1929   else if (delta_x < -1.0)
1930     tx_ = true;
1931   if (delta_y > 1.0)
1932     ty = true;
1933   if (delta_y < -1.0)
1934     ty_ = true;
1935
1936   if (tx || tx_ || ty || ty_)
1937   {
1938     // get two candidates:
1939     float delta_x1 = 0.0f, delta_x2 = 0.0f, delta_y1 = 0.0f, delta_y2 = 0.0f;
1940     if (tx)
1941     {
1942       delta_x1 = 1.0f;
1943       delta_y1 = -float(coeff4 + coeff5) / float(2 * coeff2);
1944       if (delta_y1 > 1.0f)
1945         delta_y1 = 1.0f;
1946       else if (delta_y1 < -1.0f)
1947         delta_y1 = -1.0f;
1948     }
1949     else if (tx_)
1950     {
1951       delta_x1 = -1.0f;
1952       delta_y1 = -float(coeff4 - coeff5) / float(2 * coeff2);
1953       if (delta_y1 > 1.0f)
1954         delta_y1 = 1.0f;
1955       else if (delta_y1 < -1.0)
1956         delta_y1 = -1.0f;
1957     }
1958     if (ty)
1959     {
1960       delta_y2 = 1.0f;
1961       delta_x2 = -float(coeff3 + coeff5) / float(2 * coeff1);
1962       if (delta_x2 > 1.0f)
1963         delta_x2 = 1.0f;
1964       else if (delta_x2 < -1.0f)
1965         delta_x2 = -1.0f;
1966     }
1967     else if (ty_)
1968     {
1969       delta_y2 = -1.0f;
1970       delta_x2 = -float(coeff3 - coeff5) / float(2 * coeff1);
1971       if (delta_x2 > 1.0f)
1972         delta_x2 = 1.0f;
1973       else if (delta_x2 < -1.0f)
1974         delta_x2 = -1.0f;
1975     }
1976     // insert both options for evaluation which to pick
1977     float max1 = (coeff1 * delta_x1 * delta_x1 + coeff2 * delta_y1 * delta_y1 + coeff3 * delta_x1 + coeff4 * delta_y1
1978                   + coeff5 * delta_x1 * delta_y1 + coeff6)
1979                  / 18.0f;
1980     float max2 = (coeff1 * delta_x2 * delta_x2 + coeff2 * delta_y2 * delta_y2 + coeff3 * delta_x2 + coeff4 * delta_y2
1981                   + coeff5 * delta_x2 * delta_y2 + coeff6)
1982                  / 18.0f;
1983     if (max1 > max2)
1984     {
1985       delta_x = delta_x1;
1986       delta_y = delta_x1;
1987       return max1;
1988     }
1989     else
1990     {
1991       delta_x = delta_x2;
1992       delta_y = delta_x2;
1993       return max2;
1994     }
1995   }
1996
1997   // this is the case of the maximum inside the boundaries:
1998   return (coeff1 * delta_x * delta_x + coeff2 * delta_y * delta_y + coeff3 * delta_x + coeff4 * delta_y
1999           + coeff5 * delta_x * delta_y + coeff6)
2000          / 18.0f;
2001 }
2002
2003 // construct a layer
2004 BriskLayer::BriskLayer(const cv::Mat& img_in, float scale_in, float offset_in)
2005 {
2006   img_ = img_in;
2007   scores_ = cv::Mat_<uchar>::zeros(img_in.rows, img_in.cols);
2008   // attention: this means that the passed image reference must point to persistent memory
2009   scale_ = scale_in;
2010   offset_ = offset_in;
2011   // create an agast detector
2012   fast_9_16_ = makePtr<FastFeatureDetector>(1, true, FastFeatureDetector::TYPE_9_16);
2013   makeOffsets(pixel_5_8_, (int)img_.step, 8);
2014   makeOffsets(pixel_9_16_, (int)img_.step, 16);
2015 }
2016 // derive a layer
2017 BriskLayer::BriskLayer(const BriskLayer& layer, int mode)
2018 {
2019   if (mode == CommonParams::HALFSAMPLE)
2020   {
2021     img_.create(layer.img().rows / 2, layer.img().cols / 2, CV_8U);
2022     halfsample(layer.img(), img_);
2023     scale_ = layer.scale() * 2;
2024     offset_ = 0.5f * scale_ - 0.5f;
2025   }
2026   else
2027   {
2028     img_.create(2 * (layer.img().rows / 3), 2 * (layer.img().cols / 3), CV_8U);
2029     twothirdsample(layer.img(), img_);
2030     scale_ = layer.scale() * 1.5f;
2031     offset_ = 0.5f * scale_ - 0.5f;
2032   }
2033   scores_ = cv::Mat::zeros(img_.rows, img_.cols, CV_8U);
2034   fast_9_16_ = makePtr<FastFeatureDetector>(1, false, FastFeatureDetector::TYPE_9_16);
2035   makeOffsets(pixel_5_8_, (int)img_.step, 8);
2036   makeOffsets(pixel_9_16_, (int)img_.step, 16);
2037 }
2038
2039 // Fast/Agast
2040 // wraps the agast class
2041 void
2042 BriskLayer::getAgastPoints(int threshold, std::vector<KeyPoint>& keypoints)
2043 {
2044   fast_9_16_->set("threshold", threshold);
2045   fast_9_16_->detect(img_, keypoints);
2046
2047   // also write scores
2048   const size_t num = keypoints.size();
2049
2050   for (size_t i = 0; i < num; i++)
2051     scores_((int)keypoints[i].pt.y, (int)keypoints[i].pt.x) = saturate_cast<uchar>(keypoints[i].response);
2052 }
2053
2054 inline int
2055 BriskLayer::getAgastScore(int x, int y, int threshold) const
2056 {
2057   if (x < 3 || y < 3)
2058     return 0;
2059   if (x >= img_.cols - 3 || y >= img_.rows - 3)
2060     return 0;
2061   uchar& score = (uchar&)scores_(y, x);
2062   if (score > 2)
2063   {
2064     return score;
2065   }
2066   score = (uchar)cornerScore<16>(&img_.at<uchar>(y, x), pixel_9_16_, threshold - 1);
2067   if (score < threshold)
2068     score = 0;
2069   return score;
2070 }
2071
2072 inline int
2073 BriskLayer::getAgastScore_5_8(int x, int y, int threshold) const
2074 {
2075   if (x < 2 || y < 2)
2076     return 0;
2077   if (x >= img_.cols - 2 || y >= img_.rows - 2)
2078     return 0;
2079   int score = cornerScore<8>(&img_.at<uchar>(y, x), pixel_5_8_, threshold - 1);
2080   if (score < threshold)
2081     score = 0;
2082   return score;
2083 }
2084
2085 inline int
2086 BriskLayer::getAgastScore(float xf, float yf, int threshold_in, float scale_in) const
2087 {
2088   if (scale_in <= 1.0f)
2089   {
2090     // just do an interpolation inside the layer
2091     const int x = int(xf);
2092     const float rx1 = xf - float(x);
2093     const float rx = 1.0f - rx1;
2094     const int y = int(yf);
2095     const float ry1 = yf - float(y);
2096     const float ry = 1.0f - ry1;
2097
2098     return (uchar)(rx * ry * getAgastScore(x, y, threshold_in) + rx1 * ry * getAgastScore(x + 1, y, threshold_in)
2099            + rx * ry1 * getAgastScore(x, y + 1, threshold_in) + rx1 * ry1 * getAgastScore(x + 1, y + 1, threshold_in));
2100   }
2101   else
2102   {
2103     // this means we overlap area smoothing
2104     const float halfscale = scale_in / 2.0f;
2105     // get the scores first:
2106     for (int x = int(xf - halfscale); x <= int(xf + halfscale + 1.0f); x++)
2107     {
2108       for (int y = int(yf - halfscale); y <= int(yf + halfscale + 1.0f); y++)
2109       {
2110         getAgastScore(x, y, threshold_in);
2111       }
2112     }
2113     // get the smoothed value
2114     return value(scores_, xf, yf, scale_in);
2115   }
2116 }
2117
2118 // access gray values (smoothed/interpolated)
2119 inline int
2120 BriskLayer::value(const cv::Mat& mat, float xf, float yf, float scale_in) const
2121 {
2122   CV_Assert(!mat.empty());
2123   // get the position
2124   const int x = cvFloor(xf);
2125   const int y = cvFloor(yf);
2126   const cv::Mat& image = mat;
2127   const int& imagecols = image.cols;
2128
2129   // get the sigma_half:
2130   const float sigma_half = scale_in / 2;
2131   const float area = 4.0f * sigma_half * sigma_half;
2132   // calculate output:
2133   int ret_val;
2134   if (sigma_half < 0.5)
2135   {
2136     //interpolation multipliers:
2137     const int r_x = (int)((xf - x) * 1024);
2138     const int r_y = (int)((yf - y) * 1024);
2139     const int r_x_1 = (1024 - r_x);
2140     const int r_y_1 = (1024 - r_y);
2141     uchar* ptr = image.data + x + y * imagecols;
2142     // just interpolate:
2143     ret_val = (r_x_1 * r_y_1 * int(*ptr));
2144     ptr++;
2145     ret_val += (r_x * r_y_1 * int(*ptr));
2146     ptr += imagecols;
2147     ret_val += (r_x * r_y * int(*ptr));
2148     ptr--;
2149     ret_val += (r_x_1 * r_y * int(*ptr));
2150     return 0xFF & ((ret_val + 512) / 1024 / 1024);
2151   }
2152
2153   // this is the standard case (simple, not speed optimized yet):
2154
2155   // scaling:
2156   const int scaling = (int)(4194304.0f / area);
2157   const int scaling2 = (int)(float(scaling) * area / 1024.0f);
2158
2159   // calculate borders
2160   const float x_1 = xf - sigma_half;
2161   const float x1 = xf + sigma_half;
2162   const float y_1 = yf - sigma_half;
2163   const float y1 = yf + sigma_half;
2164
2165   const int x_left = int(x_1 + 0.5);
2166   const int y_top = int(y_1 + 0.5);
2167   const int x_right = int(x1 + 0.5);
2168   const int y_bottom = int(y1 + 0.5);
2169
2170   // overlap area - multiplication factors:
2171   const float r_x_1 = float(x_left) - x_1 + 0.5f;
2172   const float r_y_1 = float(y_top) - y_1 + 0.5f;
2173   const float r_x1 = x1 - float(x_right) + 0.5f;
2174   const float r_y1 = y1 - float(y_bottom) + 0.5f;
2175   const int dx = x_right - x_left - 1;
2176   const int dy = y_bottom - y_top - 1;
2177   const int A = (int)((r_x_1 * r_y_1) * scaling);
2178   const int B = (int)((r_x1 * r_y_1) * scaling);
2179   const int C = (int)((r_x1 * r_y1) * scaling);
2180   const int D = (int)((r_x_1 * r_y1) * scaling);
2181   const int r_x_1_i = (int)(r_x_1 * scaling);
2182   const int r_y_1_i = (int)(r_y_1 * scaling);
2183   const int r_x1_i = (int)(r_x1 * scaling);
2184   const int r_y1_i = (int)(r_y1 * scaling);
2185
2186   // now the calculation:
2187   uchar* ptr = image.data + x_left + imagecols * y_top;
2188   // first row:
2189   ret_val = A * int(*ptr);
2190   ptr++;
2191   const uchar* end1 = ptr + dx;
2192   for (; ptr < end1; ptr++)
2193   {
2194     ret_val += r_y_1_i * int(*ptr);
2195   }
2196   ret_val += B * int(*ptr);
2197   // middle ones:
2198   ptr += imagecols - dx - 1;
2199   uchar* end_j = ptr + dy * imagecols;
2200   for (; ptr < end_j; ptr += imagecols - dx - 1)
2201   {
2202     ret_val += r_x_1_i * int(*ptr);
2203     ptr++;
2204     const uchar* end2 = ptr + dx;
2205     for (; ptr < end2; ptr++)
2206     {
2207       ret_val += int(*ptr) * scaling;
2208     }
2209     ret_val += r_x1_i * int(*ptr);
2210   }
2211   // last row:
2212   ret_val += D * int(*ptr);
2213   ptr++;
2214   const uchar* end3 = ptr + dx;
2215   for (; ptr < end3; ptr++)
2216   {
2217     ret_val += r_y1_i * int(*ptr);
2218   }
2219   ret_val += C * int(*ptr);
2220
2221   return 0xFF & ((ret_val + scaling2 / 2) / scaling2 / 1024);
2222 }
2223
2224 // half sampling
2225 inline void
2226 BriskLayer::halfsample(const cv::Mat& srcimg, cv::Mat& dstimg)
2227 {
2228   // make sure the destination image is of the right size:
2229   CV_Assert(srcimg.cols / 2 == dstimg.cols);
2230   CV_Assert(srcimg.rows / 2 == dstimg.rows);
2231
2232   // handle non-SSE case
2233   resize(srcimg, dstimg, dstimg.size(), 0, 0, INTER_AREA);
2234 }
2235
2236 inline void
2237 BriskLayer::twothirdsample(const cv::Mat& srcimg, cv::Mat& dstimg)
2238 {
2239   // make sure the destination image is of the right size:
2240   CV_Assert((srcimg.cols / 3) * 2 == dstimg.cols);
2241   CV_Assert((srcimg.rows / 3) * 2 == dstimg.rows);
2242
2243   resize(srcimg, dstimg, dstimg.size(), 0, 0, INTER_AREA);
2244 }
2245
2246 }