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