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