001/*******************************************************************************
002 * This software is provided as a supplement to the authors' textbooks on digital
003 * image processing published by Springer-Verlag in various languages and editions.
004 * Permission to use and distribute this software is granted under the BSD 2-Clause
005 * "Simplified" License (see http://opensource.org/licenses/BSD-2-Clause).
006 * Copyright (c) 2006-2023 Wilhelm Burger, Mark J. Burge. All rights reserved.
007 * Visit https://imagingbook.com for additional details.
008 ******************************************************************************/
009
010package imagingbook.common.sift;
011
012import ij.process.FloatProcessor;
013import imagingbook.common.math.Arithmetic;
014import imagingbook.common.math.Matrix;
015import imagingbook.common.sift.scalespace.DogOctave;
016import imagingbook.common.sift.scalespace.DogScaleSpace;
017import imagingbook.common.sift.scalespace.GaussianScaleSpace;
018import imagingbook.common.sift.scalespace.ScaleLevel;
019
020import java.util.ArrayList;
021import java.util.Collections;
022import java.util.List;
023
024import static imagingbook.common.math.Arithmetic.mod;
025import static imagingbook.common.math.Arithmetic.sqr;
026
027/**
028 * <p>
029 * This class implements the detection of SIFT features from images, as described in [1]. See Ch. 25 of [2] for more
030 * details.
031 * </p>
032 * <p>
033 * Currently only images of type {@link FloatProcessor} are supported. The input image is normalized to values in [0,1]
034 * before SIFT detection starts. A large set of parameters can be specified (see {@link SiftParameters}).
035 * </p>
036 * <p>
037 * [1] D. G. Lowe. Distinctive image features from scale-invariant keypoints. International Journal of Computer Vision
038 * 60, 91–110 (2004). <br> [2] W. Burger, M.J. Burge, <em>Digital Image Processing &ndash; An Algorithmic
039 * Introduction</em>, 3rd ed, Springer (2022).
040 * </p>
041 *
042 * @author WB
043 * @version 2022/11/20
044 * @see SiftParameters
045 */
046public class SiftDetector {
047        
048        /**
049         * Types of 3D neighborhoods used in min/max detection 
050         */
051        public enum NeighborhoodType3D {
052                NH8(8), NH10(10), NH18(18), NH26(26);
053                private final int size;
054                private NeighborhoodType3D(int size) {
055                        this.size = size;
056                }
057        }
058
059        static private final double PI2 = 2 * Math.PI;
060        
061        private final SiftParameters params;
062        private final int nhSize;
063        private final GaussianScaleSpace G;
064        private final DogScaleSpace D;
065
066        // Constructors ------------------------------------ 
067
068        /**
069         * Constructor using default parameters.
070         * @param fp the input image
071         * @see #SiftDetector(FloatProcessor, SiftParameters)
072         */
073        public SiftDetector(FloatProcessor fp) {
074                this(fp, new SiftParameters()); // uses default parameters
075        }
076
077        /**
078         * <p>
079         * Constructor using specific parameters. The input image is normalized to values in [0,1], the minimum pixel value
080         * being mapped to 0 and the maximum value to 1. An exception is thrown if the supplied image is "flat", i.e.,
081         * contains only a single pixel value. A large set of parameters can be specified (see {@link SiftParameters}). The
082         * constructor sets up the complete Gaussian and DoG scale spaces but does not perform feature detection itself,
083         * which is done by calling {@link #getSiftFeatures()}.
084         * </p>
085         *
086         * @param fp the input image
087         * @param params an instance of {@link SiftParameters}
088         * @see SiftParameters
089         */
090        public SiftDetector(FloatProcessor fp, SiftParameters params) {
091                this.params = params;
092                this.nhSize = params.nhType.size;
093                if (!normalizeTo01(fp))
094                        throw new IllegalArgumentException("could not normalize input image");
095                this.G = new GaussianScaleSpace(fp, params.P, params.Q, params.sigmaS, params.sigma0, -1, params.Q + 1);
096                this.D = new DogScaleSpace(G);
097        }
098        
099        private boolean normalizeTo01(FloatProcessor fp) {
100                float[] a = (float[])fp.getPixels();
101                float minVal = Matrix.min(a);
102                float maxVal = Matrix.max(a);
103                float diff = maxVal - minVal;
104                if (Arithmetic.isZero(diff)) {
105                        return false;   // only one pixel value
106                }
107                float scale = 1.0f / diff;
108                for (int i = 0; i < a.length; i++) {
109                        a[i] = (a[i] - minVal) * scale; 
110                }
111                return true;
112        }
113        
114        // --------------------------------------------------
115        
116        /**
117         * Calculates and returns a list of SIFT descriptors.
118         * 
119         * @return a list of extracted SIFT descriptors
120         */
121        public List<SiftDescriptor> getSiftFeatures() {
122                List<KeyPoint> keyPoints = getKeyPoints();
123                List<SiftDescriptor> siftDescriptors = new ArrayList<SiftDescriptor>();
124                for (KeyPoint kp : keyPoints) {
125                        for (double phi_d : getDominantOrientations(kp)) {
126                                SiftDescriptor sd = makeSiftDescriptor(kp, phi_d);
127                                if (sd != null) {
128                                        siftDescriptors.add(sd);
129                                }
130                        }
131                }
132                return siftDescriptors;
133        }
134
135        /**
136         * Returns the {@link GaussianScaleSpace} for this SIFT detector instance. Mainly intended for debugging and
137         * testing.
138         *
139         * @return the {@link GaussianScaleSpace} for this SIFT detector
140         */
141        public GaussianScaleSpace getGaussianScaleSpace() {
142                return this.G;
143        }
144
145        /**
146         * Returns the {@link DogScaleSpace} for this SIFT detector instance. Mainly intended for debugging and testing.
147         *
148         * @return the {@link DogScaleSpace} for this SIFT detector
149         */
150        public DogScaleSpace getDogScaleSpace() {
151                return this.D;
152        }
153        
154        // --------------------------------------------------
155
156        /**
157         * Only used for debugging: produces key points with orientation histograms attached for display purposes.
158         *
159         * @param keypoints the sequence of original key points
160         * @return a sequence of enhanced key points
161         */
162        @SuppressWarnings("unused")
163        private  List<KeyPoint> makeRichKeypoints(List<KeyPoint> keypoints) {
164                List<KeyPoint> richKeyPoints = new ArrayList<KeyPoint>();
165                for (KeyPoint kp : keypoints) {
166                        float[] oh = getOrientationHistogram(kp);
167                        smoothCircular(oh,params.nSmooth);
168                        kp.orientation_histogram = oh;  // TODO: remove, for testing only!!
169                        List<Double> peakOrientations = findPeakOrientationIndices(oh);
170                        for (double km : peakOrientations) {
171                                //for (int i=0; i<Math.min(1, peakOrientations.length); i++) {  // use only 1 descriptor!
172                                float phi = (float) (km * 2 * Math.PI / oh.length);     // 0 <= phi < 2 PI. Should be in range +/-PI?
173                                KeyPoint rkp = kp.clone();
174                                rkp.orientation = phi;
175                                richKeyPoints.add(rkp);
176                        }
177                }
178                return richKeyPoints;
179        }
180
181        /**
182         * Used for debugging/illustrations only!
183         * 
184         * @param oh orientation histogram
185         * @return list of histogram indices for the peak orientations
186         */
187        private List<Double> findPeakOrientationIndices(float[] oh) {
188                int nb = oh.length;
189                List<Double> orientIndexes = new ArrayList<Double>(nb);
190                // find the maximum entry in the orientation histogram 'oh':
191                float maxh = oh[0];
192                for (int k = 1; k < nb; k++) {
193                        if (oh[k] > maxh)
194                                maxh = oh[k];
195                }
196
197                if (maxh > 0.01f) { // ascertain minimum (non-zero) gradient energy
198                        // collect all peaks > 80% of the maximum entry in 'oh'
199                        float minh = maxh * (float) params.tDomOr;
200                        for (int k = 0; k < nb; k++) { // hp ~ hc ~ ha
201                                // angles[k] = Float.NaN;
202                                float hc = oh[k]; // center value
203                                if (oh[k] > minh) { // value is min. 80% of global peak
204                                        float hp = oh[(k - 1 + nb) % nb]; // previous histogram
205                                        // value
206                                        float hn = oh[(k + 1) % nb]; // next histogram value
207                                        if (hc > hp && hc > hn) { // check if 'hc' is a local peak
208                                                // interpolate orientation by a quadratic function (parabola):
209                                                double delta = interpolateQuadratic(hp, hc, hn);
210                                                double k_max = (k + delta + nb) % nb; 
211                                                // interpolated bin index, 0 <= km < nPhi:
212                                                // double phi_max = k_max * 2 * Math.PI / nb; // 0 <=
213                                                // phi < 2 PI. Should be in range +/-PI?
214                                                orientIndexes.add(k_max);
215                                        }
216                                }
217                        }
218                }
219                return orientIndexes;
220        }
221
222        private List<KeyPoint> getKeyPoints() {
223                List<KeyPoint> keyPts = new ArrayList<KeyPoint>();
224                final int P = params.P;
225                final int K = params.Q;
226                for (int p = 0; p <= P-1; p++) {        // for every octave p
227                        for (int q = 0; q <= K-1; q++) {        // for every scale level q
228                                List<KeyPoint> extrema = findExtrema(p, q);
229                                for (KeyPoint e : extrema) {
230                                        KeyPoint c = refineKeyPosition(D, e);
231                                        if (c != null) {
232                                                keyPts.add(c);
233                                        }
234                                }
235                        }
236                }
237                Collections.sort(keyPts);       // always sort (by decreasing score)
238                return keyPts;
239        }
240
241        private List<KeyPoint> findExtrema(int p, int q) {
242                final float tMag = (float) params.tMag;
243                final float tExtrm = (float) params.tExtrm;
244                final DogOctave Dp = D.getOctave(p);
245                final ScaleLevel Dpq = D.getScaleLevel(p, q);
246                final int M = Dpq.getWidth();
247                final int N = Dpq.getHeight();
248                
249                List<KeyPoint> E = new ArrayList<KeyPoint>();
250                float scale = (float)D.getAbsoluteScale(p, q);  //D.getScaleIndexFloat(p, q); needed?
251
252                final float[][][] nh = new float[3][3][3];                      // 3x3x3 neighborhood [q][u][v]
253                for (int u = 1; u <= M - 2; u++) {
254                        float x_real = (float) D.getRealX(p, u);                // for display purposes only
255                        for (int v = 1; v <= N - 2; v++) {
256                                float y_real = (float) D.getRealY(p, v);        // for display purposes only
257                                float mag = Math.abs(Dpq.getValue(u, v));
258                                if (mag > tMag) {
259                                        Dp.getNeighborhood(q, u, v, nh);                // CHANGE to use D not Dp!!
260                                        if (isExtremum(nh, tExtrm)) {
261                                                KeyPoint e = new KeyPoint(p, q, u, v, u, v, x_real, y_real, scale, mag);
262                                                E.add(e);
263                                        }
264                                }
265                        }
266                }
267                return E;
268        }
269
270        private KeyPoint refineKeyPosition(DogScaleSpace D, KeyPoint k) { 
271                final int p = k.p;
272                final int q = k.q;
273                int u = k.u;
274                int v = k.v;
275                
276                final DogOctave Dp = D.getOctave(p);
277                final double rhoMax = params.rhoMax;
278                final double tPeak = params.tPeak;
279                final int nRefine = params.nRefine;
280
281                final float[][][] nh = new float[3][3][3];      // 3x3x3 neighborhood nh[q][u][v]
282                final float[] grad       = new float[3];                // gradient (dx, dy, ds)        
283                final float[] d          = new float[3];                // relocation displacement (dx, dy, ds)
284                final float[][] hess = new float[3][3];         // 3D Hessian matrix:
285                                                                                                        // | dxx dxy dxs |
286                                                                                                        // | dxy dyy dys |
287                                                                                                        // | dxs dys dss |
288                
289                final float aMax = (float) (sqr(rhoMax + 1) / rhoMax);
290                KeyPoint kr = null;                                                     // refined keypoint
291                boolean done = false;
292                int n = 1;
293                while (!done && n <= nRefine && Dp.isInside(q, u, v)) {
294                        Dp.getNeighborhood(q, u, v, nh);        // TODO: CHANGE to use D instead of Dp!
295                        gradient(nh, grad);                                     // result stored in grad
296                        hessian(nh, hess);                                      // result stored in hess
297                        final double detH = Matrix.determinant3x3(hess);
298                        if (Arithmetic.isZero(detH)) {  // Hessian matrix has zero determinant?
299                                done = true;    // ignore this point and finish
300                        }
301                        else {
302                                final float dxx = hess[0][0];   // extract 2x2 Hessian for calculating curvature ratio below
303                                final float dxy = hess[0][1];
304                                final float dyy = hess[1][1];
305//                              final float[][] invHess = Matrix.inverse3x3(hess);      // deprecated
306                                final float[][] invHess = Matrix.inverse(hess);         // alternative (numerically stable)
307                                Matrix.multiplyD(invHess, grad, d);     // d <-- invHess . grad
308                                Matrix.multiplyD(-1, d);                        // d <-- - d
309                                final float xx = d[0];  // = x'
310                                final float yy = d[1];  // = y'
311
312                                if (Math.abs(xx) < 0.5f && Math.abs(yy) < 0.5f) {       // stay at this lattice point
313                                        done = true;
314                                        final float Dpeak = nh[1][1][1] + 0.5f * (grad[0]*xx + grad[1]*yy + grad[2]*d[2]);
315                                        final float detHxy = dxx * dyy - dxy * dxy;
316                                        if (Math.abs(Dpeak) > tPeak && detHxy > 0) { // check peak magnitude
317                                                final float a = sqr(dxx + dyy) / detHxy;
318                                                if (a <= aMax) {                                        // check curvature ratio (edgeness)
319                                                        // new, refined keypoint:
320                                                        float x = u + xx;
321                                                        float y = v + yy;
322                                                        float x_real = (float) D.getRealX(p, x);
323                                                        float y_real = (float) D.getRealY(p, y);
324                                                        float scale  = (float) D.getAbsoluteScale(p, q);
325                                                        kr = new KeyPoint(k.p, k.q, k.u, k.v, x, y, x_real, y_real, scale, k.magnitude);
326                                                }
327                                        }
328                                }
329                                else { // large displacement, move to a neighboring DoG cell at same level q by at most one unit
330                                        u = u + Math.min(1, Math.max(-1, Math.round(xx)));      // move u by max +/-1
331                                        v = v + Math.min(1, Math.max(-1, Math.round(yy)));      // move v by max +/-1
332                                        // Note: we don't move along the scale axis!
333                                }
334                        }
335                        n = n + 1;
336                }
337                return kr;
338        }
339
340        private boolean isExtremum(float[][][] nh, float tExtrm) {
341                return isLocalMin(nh, tExtrm) || isLocalMax(nh, tExtrm);
342        }
343
344        private boolean isLocalMin(final float[][][] neighborhood, float tExtrm) {
345                final float c = neighborhood[1][1][1] + tExtrm; // center value + threshold
346                if (c >= 0) return false;       // local minimum must have a negative value
347                // check 8 neighbors in scale plane q
348                if (c >= neighborhood[1][0][0]) return false;
349                if (c >= neighborhood[1][1][0]) return false;
350                if (c >= neighborhood[1][2][0]) return false;
351                if (c >= neighborhood[1][0][1]) return false;
352                if (c >= neighborhood[1][2][1]) return false;
353                if (c >= neighborhood[1][0][2]) return false;
354                if (c >= neighborhood[1][1][2]) return false;
355                if (c >= neighborhood[1][2][2]) return false;
356                if (nhSize >= 10) {
357                        if (c >= neighborhood[0][1][1]) return false;
358                        if (c >= neighborhood[2][1][1]) return false;
359                        if (nhSize >= 18) {
360                                // check 4 more neighbors in scale plane q-1
361                                if (c >= neighborhood[0][0][1]) return false;
362                                if (c >= neighborhood[0][2][1]) return false;
363                                if (c >= neighborhood[0][1][0]) return false;
364                                if (c >= neighborhood[0][1][2]) return false;
365                                // check 4 more neighbors in scale plane q+1
366                                if (c >= neighborhood[2][0][1]) return false;
367                                if (c >= neighborhood[2][2][1]) return false;
368                                if (c >= neighborhood[2][1][0]) return false;
369                                if (c >= neighborhood[2][1][2]) return false;
370                                // optionally check 8 remaining neighbors (corners of cube):
371                                if (nhSize >= 26) {
372                                        if (c >= neighborhood[0][0][0]) return false;
373                                        if (c >= neighborhood[0][2][0]) return false;
374                                        if (c >= neighborhood[0][0][2]) return false;
375                                        if (c >= neighborhood[0][2][2]) return false;
376                                        if (c >= neighborhood[2][0][0]) return false;
377                                        if (c >= neighborhood[2][2][0]) return false;
378                                        if (c >= neighborhood[2][0][2]) return false;
379                                        if (c >= neighborhood[2][2][2]) return false;
380                                }
381                        }
382                }
383                return true;
384        }
385
386        private boolean isLocalMax(final float[][][] neighborhood, float tExtrm) {
387                final float c = neighborhood[1][1][1] - tExtrm;         // center value - threshold
388                if (c <= 0) return false;       // local maximum must have a positive value
389                // check 8 neighbors in scale plane q
390                if (c <= neighborhood[1][0][0]) return false;
391                if (c <= neighborhood[1][1][0]) return false;
392                if (c <= neighborhood[1][2][0]) return false;
393                if (c <= neighborhood[1][0][1]) return false;
394                if (c <= neighborhood[1][2][1]) return false;
395                if (c <= neighborhood[1][0][2]) return false;
396                if (c <= neighborhood[1][1][2]) return false;
397                if (c <= neighborhood[1][2][2]) return false;
398                if (nhSize >= 10) {
399                        if (c <= neighborhood[0][1][1]) return false;
400                        if (c <= neighborhood[2][1][1]) return false;
401                        if (nhSize >= 18) {
402                                // check 4 more neighbors in scale plane q-1
403                                if (c <= neighborhood[0][0][1]) return false;
404                                if (c <= neighborhood[0][2][1]) return false;
405                                if (c <= neighborhood[0][1][0]) return false;
406                                if (c <= neighborhood[0][1][2]) return false;
407                                // check 4 more neighbors in scale plane q+1
408                                if (c <= neighborhood[2][0][1]) return false;
409                                if (c <= neighborhood[2][2][1]) return false;
410                                if (c <= neighborhood[2][1][0]) return false;
411                                if (c <= neighborhood[2][1][2]) return false;
412                                // optionally check 8 remaining neighbors (corners of cube):
413                                if (nhSize >= 26) {
414                                        if (c <= neighborhood[0][0][0]) return false;
415                                        if (c <= neighborhood[0][2][0]) return false;
416                                        if (c <= neighborhood[0][0][2]) return false;
417                                        if (c <= neighborhood[0][2][2]) return false;
418                                        if (c <= neighborhood[2][0][0]) return false;
419                                        if (c <= neighborhood[2][2][0]) return false;
420                                        if (c <= neighborhood[2][0][2]) return false;
421                                        if (c <= neighborhood[2][2][2]) return false;
422                                }
423                        }
424                }
425                return true;
426        }
427
428        /*
429         * Calculates the 3-dimensional gradient vector for the 3x3x3 neighborhood
430         * nh[s][x][y]. The result is stored in the supplied vector grad, to which a reference is 
431         * returned.
432         */
433        private float[] gradient(final float[][][] nh, final float[] grad) {
434                // Note: factor 0.5f not needed, kept for clarity.
435                grad[0] = 0.5f * (nh[1][2][1] - nh[1][0][1]);   // = dx
436                grad[1] = 0.5f * (nh[1][1][2] - nh[1][1][0]);   // = dy
437                grad[2] = 0.5f * (nh[2][1][1] - nh[0][1][1]);   // = ds
438                return grad;
439        }
440
441        /* 
442         * Calculates the 3x3 Hessian matrix for the 3x3x3 neighborhood nh[s][i][j],
443         * with scale index s and spatial indices i, j.
444         * The result is stored in the supplied array hess, to which a reference is 
445         * returned.
446         */
447        private float[][] hessian(final float[][][] nh, final float[][] hess) {
448                final float nh_111 = 2 * nh[1][1][1];
449                final float dxx = nh[1][0][1] - nh_111 + nh[1][2][1];
450                final float dyy = nh[1][1][0] - nh_111 + nh[1][1][2];
451                final float dss = nh[0][1][1] - nh_111 + nh[2][1][1];
452
453                final float dxy = (nh[1][2][2] - nh[1][0][2] - nh[1][2][0] + nh[1][0][0]) * 0.25f;
454                final float dxs = (nh[2][2][1] - nh[2][0][1] - nh[0][2][1] + nh[0][0][1]) * 0.25f;
455                final float dys = (nh[2][1][2] - nh[2][1][0] - nh[0][1][2] + nh[0][1][0]) * 0.25f;
456
457                hess[0][0] = dxx;  hess[0][1] = dxy;  hess[0][2] = dxs;
458                hess[1][0] = dxy;  hess[1][1] = dyy;  hess[1][2] = dys;
459                hess[2][0] = dxs;  hess[2][1] = dys;  hess[2][2] = dss;
460                return hess;
461        }
462
463        /*
464         * Returns a list of orientations (angles) for the keypoint c.
465         */
466        private List<Double> getDominantOrientations(KeyPoint c) {
467                float[] h_phi = getOrientationHistogram(c);
468                smoothCircular(h_phi, params.nSmooth);
469                return findPeakOrientations(h_phi);
470        }
471
472        // Smoothes the array A in-place (i.e., destructively) in 'iterations' passes.
473        private void smoothCircular(float[] X, int n_iter) {
474                final float[] H = { 0.25f, 0.5f, 0.25f }; // filter kernel
475                final int n = X.length;
476                for (int i = 1; i <= n_iter; i++) {
477                        float s = X[0];
478                        float p = X[n - 1];
479                        for (int j = 0; j <= n - 2; j++) {
480                                float c = X[j];
481                                X[j] = H[0] * p + H[1] * X[j] + H[2] * X[j + 1];
482                                p = c;
483                        }
484                        X[n - 1] = H[0] * p + H[1] * X[n - 1] + H[2] * s;
485                }
486        }
487
488        /* 
489         * Extracts the peaks in the orientation histogram 'h_phi'
490         * and returns the corresponding angles in a (possibly empty) list. 
491         */
492        private List<Double> findPeakOrientations(float[] h_phi) {
493                int n = h_phi.length;
494                List<Double> angles = new ArrayList<Double>(n);
495                // find the maximum entry in the orientation histogram 'h_phi'
496                float h_max = h_phi[0];
497                for (int k = 1; k < n; k++) {
498                        if (h_phi[k] > h_max)
499                                h_max = h_phi[k];
500                }
501                if (h_max > 0.01f) {    // ascertain minimum (non-zero) gradient energy 
502                        // collect all peaks > 80% of the maximum entry in 'oh'
503                        float h_min = h_max * 0.8f;
504                        for (int k = 0; k < n; k++) {   // hp ~ hc ~ ha
505                                float hc = h_phi[k];                                            // center value
506                                if (hc > h_min) {               // value is min. 80% of global peak
507                                        float hp = h_phi[(k - 1 + n) % n];      // previous histogram value
508                                        float hn = h_phi[(k + 1) % n];          // next histogram value
509                                        if (hc > hp && hc > hn) {                       // check if 'hc' is a local peak
510                                                // interpolate orientation by a quadratic function (parabola):
511                                                double delta = interpolateQuadratic(hp, hc, hn);
512                                                double k_max = (k + delta + n) % n;     // interpolated bin index, 0 <= km < nPhi
513                                                double phi_max = k_max * 2 * Math.PI / n;       // 0 <= phi < 2 PI. Should be in range +/-PI?
514                                                angles.add(phi_max);
515                                        }
516                                }
517                        }
518                }
519                return angles;
520        }
521
522        private float[] getOrientationHistogram(KeyPoint c) {
523                final int n_phi = params.nOrient;
524                final int K = params.Q;
525
526                ScaleLevel Gpq = G.getScaleLevel(c.p, c.q);
527                final float[] h_phi = new float[n_phi]; // automatically initialized to zero
528                final int M = Gpq.getWidth(), N = Gpq.getHeight();
529                final double x = c.x, y = c.y;
530
531                final double sigma_w = 1.5 * params.sigma0 * Math.pow(2,(double)c.q/K);
532                final double sigma_w22 = 2 * sigma_w * sigma_w;
533                final double r_w = Math.max(1, 2.5 * sigma_w);
534                final double r_w2 = r_w * r_w;
535
536                final int u_min = Math.max((int)Math.floor(x - r_w), 1);
537                final int u_max = Math.min((int)Math.ceil(x + r_w), M-2);
538                final int v_min = Math.max((int)Math.floor(y - r_w), 1);
539                final int v_max = Math.min((int)Math.ceil(y + r_w), N-2);
540
541                double[] gradPol = new double[2]; // gradient magnitude/orientation
542
543                for (int u = u_min; u <= u_max; u++) {
544                        double dx = u - x;                                      // distance to feature's center
545                        for (int v = v_min; v <= v_max; v++) {
546                                double dy = v - y;
547                                double r2 = dx * dx + dy * dy;  // squared distance from center
548                                if (r2 < r_w2) {                                // inside limiting circle
549                                        Gpq.getGradientPolar(u, v, gradPol);
550                                        double E = gradPol[0], phi = gradPol[1];                                // gradient magnitude/orientation
551                                        double wG = Math.exp(-(dx*dx + dy*dy)/sigma_w22);               // Gaussian weight
552                                        double z = E * wG;
553                                        double k_phi = n_phi * phi / (2 * Math.PI);                             // continuous histogram bin index
554                                        double alpha = k_phi - Math.floor(k_phi);                               // weight alpha
555                                        int k_0 = Math.floorMod((int)Math.floor(k_phi), n_phi); // lower histogram index
556                                        int k_1 = Math.floorMod(k_0 + 1, n_phi);                                                        // upper histogram index
557                                        h_phi[k_0] = (float) (h_phi[k_0] + (1 - alpha) * z);    // distribute z into bins k_0, k_1
558                                        h_phi[k_1] = (float) (h_phi[k_1] + alpha * z);
559                                }
560                        }
561                }
562                return h_phi;
563        }
564
565        private SiftDescriptor makeSiftDescriptor(KeyPoint c, double phi_d) {
566                final int p = c.p;
567                final int q = c.q;
568                final double x = c.x;
569                final double y = c.y;
570                final double mag = c.magnitude;
571
572                ScaleLevel Gpq = G.getScaleLevel(p, q);
573                final int M = Gpq.getWidth(), N = Gpq.getHeight(), K = G.getQ();
574
575                double sigma_q = G.getSigma_0() * Math.pow(2, (double) q / K);  // = Gpq.getAbsoluteScale() ?? decimated scale
576                double w_d = params.sDesc * sigma_q;            // descriptor width
577                double sigma_d = 0.25 * w_d;    // width of Gaussian weighting function
578                double sigma_d2 = 2 * sigma_d * sigma_d;
579                double r_d = 2.5 * sigma_d;             // limiting radius
580                double r_d2 = r_d * r_d;                // squared limiting radius
581
582                double sc = 1.0/w_d;                                    // scale to canonical frame
583                double sin_phi_d = Math.sin(-phi_d);    // precalculated sine
584                double cos_phi_d = Math.cos(-phi_d);    // precalculated cosine
585
586                int u_min = Math.max((int)Math.floor(x - r_d), 1);
587                int u_max = Math.min((int)Math.ceil(x + r_d), M - 2);
588
589                int v_min = Math.max((int)Math.floor(y - r_d), 1);
590                int v_max = Math.min((int)Math.ceil(y + r_d), N - 2);
591
592                // create the 3D orientation histogram, initialize to zero:
593                final int n_Spat = params.nSpat;
594                final int n_Angl = params.nAngl;
595                double[][][] h_grad = new double[n_Spat][n_Spat][n_Angl];
596                double[] gmo = new double[2];   // gradient magnitude/orientation
597
598                for (int u = u_min; u <= u_max; u++) {
599                        double dx = u - x;
600                        for (int v = v_min; v <= v_max; v++) {
601                                double dy = v - y;
602                                double r2 = sqr(dx) + sqr(dy);  // squared distance from center
603                                if (r2 < r_d2) {                                        // inside limiting circle
604                                        // map to the canonical coordinate frame, ii,jj \in [-1/2, +1/2]:
605                                        double uu = sc * (cos_phi_d * dx - sin_phi_d * dy);
606                                        double vv = sc * (sin_phi_d * dx + cos_phi_d * dy);
607                                        // calculate the gradient of Gaussian scale level (p,q) at position (u,v):
608                                        Gpq.getGradientPolar(u, v, gmo);
609                                        double E = gmo[0];                      // gradient magnitude
610                                        double phi = gmo[1];            // gradient orientation
611                                        double phi_norm = mod(phi - phi_d, PI2);// normalized gradient orientation      
612                                        double w_G = Math.exp(-r2/sigma_d2);    // Gaussian weight
613                                        double z = E * w_G;                                             // quantity to accumulate
614                                        updateGradientHistogram(h_grad, uu, vv, phi_norm, z);
615                                }
616                        }
617                }
618                int[] f_int = makeFeatureVector(h_grad);
619                double sigma_pq = G.getAbsoluteScale(p, q);
620                double x_real = G.getRealX(p, x);
621                double y_real = G.getRealY(p, y);
622                return new SiftDescriptor(x_real, y_real, sigma_pq, p, mag, phi_d, f_int);
623        }
624
625        private void updateGradientHistogram(double[][][] h_grad, double uu, double vv, double phi_norm, double z) {
626                final int n_Spat = params.nSpat;
627                final int n_Angl = params.nAngl;
628
629                final double ii = n_Spat * uu + 0.5 * (n_Spat - 1);     // continuous spatial histogram index i'
630                final double jj = n_Spat * vv + 0.5 * (n_Spat - 1);     // continuous spatial histogram index j'
631                final double kk = phi_norm * (n_Angl / PI2);            // continuous orientation histogram index k'
632
633                final int i0 = (int) Math.floor(ii);
634                final int i1 = i0 + 1;
635                
636                final int j0 = (int) Math.floor(jj);
637                final int j1 = j0 + 1;
638                
639                final int k0 = Math.floorMod((int) Math.floor(kk), n_Angl);
640                final int k1 = (k0 + 1) % n_Angl;                       // k0 >= 0
641                
642                final double alpha0 = 1.0 - (ii - i0);
643                final double alpha1 = 1.0 - alpha0;
644
645                final double beta0 = 1.0 - (jj - j0);
646                final double beta1 = 1.0 - beta0;
647
648                final double gamma0 = 1.0 - (kk - Math.floor(kk));
649                final double gamma1 = 1.0 - gamma0;
650
651                final int[] I = {i0, i1};       // index arrays used in loops below
652                final int[] J = {j0, j1};
653                final int[] K = {k0, k1};
654
655                final double[] A = {alpha0, alpha1};
656                final double[] B = {beta0, beta1};
657                final double[] C = {gamma0, gamma1};
658
659                // distribute z over 8 adjacent spatial/angular bins:
660                for (int a = 0; a <= 1; a++) {
661                        int i = I[a];
662                        if (i >= 0 && i < n_Spat) {
663                                double wa = A[a];
664                                for (int b = 0; b <= 1; b++) {
665                                        int j = J[b];
666                                        if (j >= 0 && j < n_Spat) {
667                                                double wb = B[b];
668                                                for (int c = 0; c <= 1; c++) {
669                                                        int k = K[c];
670                                                        double wc = C[c];
671                                                        h_grad[i][j][k] = h_grad[i][j][k] + z * wa * wb * wc;
672
673                                                }
674                                        }
675                                }
676                        }
677                }
678        }
679
680        private int[] makeFeatureVector(double[][][] h_grad) {
681                final int n_Spat = params.nSpat;
682                final int n_Angl = params.nAngl;
683                float[] f = new float[n_Spat * n_Spat * n_Angl];
684                // flatten 3D histogram to a 1D vector
685                // the histogram is vectorized such that k (phi) is the fastest
686                // varying index, followed by j and i (which is the slowest index).
687                int m = 0;
688                for (int i = 0; i < n_Spat; i++) {
689                        for (int j = 0; j < n_Spat; j++) {
690                                for (int k = 0; k < n_Angl; k++) {
691                                        f[m] = (float) h_grad[i][j][k];
692                                        m = m + 1;
693                                }
694                        }
695                }
696                normalize(f);
697                clipPeaks(f, (float) params.tFclip);    
698                normalize(f);
699                return mapToIntegers(f, (float) params.sFscale);
700        }
701
702        private void normalize(float[] x) {
703                final double norm = Matrix.normL2(x);
704                if (norm > Arithmetic.EPSILON_FLOAT) {
705                        final float s = (float) (1.0 / norm);
706                        for (int i = 0; i < x.length; i++) {
707                                x[i] = s * x[i];
708                        }
709                }
710        }
711
712        private void clipPeaks(float[] x, float xmax) {
713                for (int i = 0; i<x.length; i++) {
714                        if (x[i] > xmax) {
715                                x[i] = xmax;
716                        }
717                }
718        }
719
720        private int[] mapToIntegers(float[] x, float s) {
721                int[] ivec = new int[x.length];
722                for (int i = 0; i < x.length; i++) {
723                        ivec[i] = Math.round(s * x[i]);
724                }
725                return ivec;
726        }
727
728        //  auxiliary methods -------------------------
729
730        /*
731         * Calculate the extremal position from 3 discrete function values 
732         * at x = -1, 0, +1: f(-1) = 'y1', f(0) = 'y2', f(+1) = 'y3'.
733         */
734        private float interpolateQuadratic(float y1, float y2, float y3) {
735                float a = (y1 - 2*y2 + y3) / 2;
736                if (Math.abs(a) < 0.00001f) {
737                        throw new IllegalArgumentException("quadratic interpolation failed " 
738                                        + a + " " + y1 + " " + y2 + " " + y3);
739                }
740                float b = (y3 - y1) / 2;
741//              float c = y2;
742                float x_extrm = -b / (2*a);             // extremal position
743//              float y_extrm = a*x*x + b*x + c;        // extremal value (not needed)
744                return x_extrm; // x is in [-1,+1]
745        }
746
747}