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