001/**
002 * Copyright (c) 2011, The University of Southampton and the individual contributors.
003 * All rights reserved.
004 *
005 * Redistribution and use in source and binary forms, with or without modification,
006 * are permitted provided that the following conditions are met:
007 *
008 *   *  Redistributions of source code must retain the above copyright notice,
009 *      this list of conditions and the following disclaimer.
010 *
011 *   *  Redistributions in binary form must reproduce the above copyright notice,
012 *      this list of conditions and the following disclaimer in the documentation
013 *      and/or other materials provided with the distribution.
014 *
015 *   *  Neither the name of the University of Southampton nor the names of its
016 *      contributors may be used to endorse or promote products derived from this
017 *      software without specific prior written permission.
018 *
019 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
020 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
021 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
022 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
023 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
024 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
025 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
026 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
027 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
028 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
029 */
030package org.openimaj.image.feature.local.interest;
031
032import java.util.ArrayList;
033import java.util.List;
034
035import org.openimaj.algorithm.iterative.IterationState;
036import org.openimaj.image.FImage;
037import org.openimaj.image.processing.convolution.FImageConvolveSeparable;
038import org.openimaj.math.geometry.point.Point2d;
039import org.openimaj.math.geometry.point.Point2dImpl;
040import org.openimaj.math.matrix.PseudoInverse;
041import org.openimaj.util.function.Predicate;
042
043import Jama.Matrix;
044import net.jafama.FastMath;
045
046/**
047 * Refines detected corners (i.e. from {@link HarrisIPD} or other methods) to an
048 * optimised sub-pixel estimate. The method works by iteratively updating the
049 * sub-pixel position of a point based on the inverse of the local gradient
050 * auto-correlation matrix.
051 *
052 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
053 */
054public class SubPixelCorners {
055        private static final float GRAD_X_KERNEL[] = { -1.f, 0.f, 1.f };
056        private static final float GRAD_Y_KERNEL[] = { 0.f, 0.5f, 0.f };
057
058        private Predicate<IterationState> iter;
059        private int halfWidth;
060        private int halfHeight;
061        private int zeroZoneHalfWidth = -1;
062        private int zeroZoneHalfHeight = -1;
063
064        /**
065         * Construct with the given search window size and predicate to <b>stop</b> the
066         * iteration. No zeroing of weights is performed
067         *
068         * @param halfWidth
069         *            half the search window width (actual size is 2*halfWidth + 1,
070         *            centred on point)
071         * @param halfHeight
072         *            half the search window height (actual size is 2*halfHeight + 1,
073         *            centred on point)
074         * @param iter
075         *            the predicate to stop iteration
076         */
077        public SubPixelCorners(int halfWidth, int halfHeight, Predicate<IterationState> iter) {
078                this.halfWidth = halfWidth;
079                this.halfHeight = halfHeight;
080                this.iter = iter;
081        }
082
083        /**
084         * Construct with the given search window size, zeroed window and predicate to
085         * <b>stop</b> the iteration.
086         * <p>
087         * The zeroed window is a dead region in the middle of the search zone over
088         * which the summation over gradients is not done. It is used sometimes to avoid
089         * possible singularities of the autocorrelation matrix.
090         *
091         * @param halfWidth
092         *            half the search window width (actual size is 2*halfWidth + 1,
093         *            centred on point)
094         * @param halfHeight
095         *            half the search window height (actual size is 2*halfHeight + 1,
096         *            centred on point)
097         * @param zeroZoneHalfWidth
098         *            the half-width of the zeroed region in the weighting array
099         * @param zeroZoneHalfHeight
100         *            the half-height of the zeroed region in the weighting array
101         * @param iter
102         *            the predicate to stop iteration
103         */
104        public SubPixelCorners(int halfWidth, int halfHeight, int zeroZoneHalfWidth, int zeroZoneHalfHeight,
105                        Predicate<IterationState> iter)
106        {
107                this.halfWidth = halfWidth;
108                this.halfHeight = halfHeight;
109                this.zeroZoneHalfWidth = zeroZoneHalfWidth;
110                this.zeroZoneHalfHeight = zeroZoneHalfHeight;
111                this.iter = iter;
112        }
113
114        /**
115         * Find the sub-pixel estimated position of each corner
116         *
117         * @param src
118         *            the image
119         * @param corners
120         *            the initial corner positions
121         * @return the updated corners
122         */
123        public List<Point2dImpl> findSubPixCorners(FImage src, List<? extends Point2d> corners) {
124                final List<Point2dImpl> outCorners = new ArrayList<Point2dImpl>(corners.size());
125                final int windowWidth = halfWidth * 2 + 1;
126                final int windowHeight = halfHeight * 2 + 1;
127
128                if (corners.size() == 0)
129                        return outCorners;
130
131                final FImage weights = this.buildGaussianWeights(windowWidth, windowHeight);
132                final FImage gx = new FImage(windowWidth, windowHeight);
133                final FImage gy = new FImage(windowWidth, windowHeight);
134                final float[] buffer = new float[windowWidth + 2];
135
136                // note 2px padding as conv reduces size:
137                final FImage roi = new FImage(windowWidth + 2, windowHeight + 2);
138
139                // loop for all the points
140                for (int i = 0; i < corners.size(); i++) {
141                        final Point2d pt = corners.get(i);
142                        outCorners.add(this.findCornerSubPix(src, pt, roi, gx, gy, weights, buffer));
143                }
144
145                return outCorners;
146        }
147
148        /**
149         * Find the sub-pixel estimated position of a corner
150         *
151         * @param src
152         *            the image
153         * @param corner
154         *            the initial corner position
155         * @return the updated corner position
156         */
157        public Point2dImpl findSubPixCorner(FImage src, Point2d corner) {
158                final int windowWidth = halfWidth * 2 + 1;
159                final int windowHeight = halfHeight * 2 + 1;
160
161                final FImage weights = this.buildGaussianWeights(windowWidth, windowHeight);
162                final FImage gx = new FImage(windowWidth, windowHeight);
163                final FImage gy = new FImage(windowWidth, windowHeight);
164                final float[] buffer = new float[windowWidth + 2];
165
166                // note 2px padding as conv reduces size:
167                final FImage roi = new FImage(windowWidth + 2, windowHeight + 2);
168
169                return this.findCornerSubPix(src, corner, roi, gx, gy, weights, buffer);
170        }
171
172        /**
173         * Build the Gaussian weighting image
174         *
175         * @param width
176         *            the width
177         * @param height
178         *            the height
179         * @return
180         */
181        private FImage buildGaussianWeights(int width, int height) {
182                final FImage weights = new FImage(width, height);
183                final float[] weightsX = new float[width];
184
185                double coeff = 1. / (halfWidth * halfWidth);
186                for (int i = -halfWidth, k = 0; i <= halfWidth; i++, k++) {
187                        weightsX[k] = (float) Math.exp(-i * i * coeff);
188                }
189
190                float[] weightsY;
191                if (halfWidth == halfHeight) {
192                        weightsY = weightsX;
193                } else {
194                        weightsY = new float[height];
195                        coeff = 1. / (halfHeight * halfHeight);
196                        for (int i = -halfHeight, k = 0; i <= halfHeight; i++, k++) {
197                                weightsY[k] = (float) Math.exp(-i * i * coeff);
198                        }
199                }
200
201                for (int i = 0; i < height; i++) {
202                        for (int j = 0; j < width; j++) {
203                                weights.pixels[i][j] = weightsX[j] * weightsY[i];
204                        }
205                }
206
207                // if necessary, mask off the centre portion
208                if (zeroZoneHalfWidth >= 0 && zeroZoneHalfHeight >= 0 &&
209                                zeroZoneHalfWidth * 2 + 1 < width && zeroZoneHalfHeight * 2 + 1 < height)
210                {
211                        for (int i = halfHeight - zeroZoneHalfHeight; i <= halfHeight + zeroZoneHalfHeight; i++) {
212                                for (int j = halfWidth - zeroZoneHalfWidth; j <= halfWidth + zeroZoneHalfWidth; j++) {
213                                        weights.pixels[i][j] = 0;
214                                }
215                        }
216                }
217
218                return weights;
219        }
220
221        private Point2dImpl findCornerSubPix(FImage src, Point2d pt, FImage roi, FImage gx, FImage gy, FImage weights,
222                        final float[] buffer)
223        {
224                final IterationState state = new IterationState();
225                Point2dImpl current = new Point2dImpl(pt);
226
227                while (!iter.test(state)) {
228                        // get window around pt
229                        src.extractCentreSubPix(current, roi);
230
231                        // calc derivatives
232                        FImageConvolveSeparable.fastConvolve3(roi, gx, GRAD_X_KERNEL, GRAD_Y_KERNEL, buffer);
233                        FImageConvolveSeparable.fastConvolve3(roi, gy, GRAD_Y_KERNEL, GRAD_X_KERNEL, buffer);
234
235                        double a = 0, b = 0, c = 0, bb1 = 0, bb2 = 0;
236                        final int win_w = weights.width;
237                        final int win_h = weights.height;
238
239                        // process gradient
240                        for (int i = 0; i < win_h; i++) {
241                                final double py = i - halfHeight;
242
243                                for (int j = 0; j < win_w; j++) {
244                                        final double m = weights.pixels[i][j];
245                                        final double tgx = gx.pixels[i][j];
246                                        final double tgy = gy.pixels[i][j];
247                                        final double gxx = tgx * tgx * m;
248                                        final double gxy = tgx * tgy * m;
249                                        final double gyy = tgy * tgy * m;
250                                        final double px = j - halfWidth;
251
252                                        a += gxx;
253                                        b += gxy;
254                                        c += gyy;
255
256                                        bb1 += gxx * px + gxy * py;
257                                        bb2 += gxy * px + gyy * py;
258                                }
259                        }
260
261                        final Matrix m = new Matrix(new double[][] { { a, b }, { b, c } });
262                        final Matrix mInv = PseudoInverse.pseudoInverse(m);
263                        final Point2dImpl cI2 = new Point2dImpl();
264                        cI2.x = (float) (current.x + mInv.get(0, 0) * bb1 + mInv.get(0, 1) * bb2);
265                        cI2.y = (float) (current.y + mInv.get(1, 0) * bb1 + mInv.get(1, 1) * bb2);
266
267                        state.epsilon = FastMath.sqrt((cI2.x - current.x) * (cI2.x - current.x) + (cI2.y - current.y)
268                                        * (cI2.y - current.y));
269                        state.iteration++;
270                        current = cI2;
271                }
272
273                // if new point is too far from initial, it means poor convergence.
274                // return initial point as the result
275                if (Math.abs(current.x - pt.getX()) > halfWidth || Math.abs(current.y - pt.getY()) > halfHeight) {
276                        return new Point2dImpl(pt);
277                }
278
279                // otherwise return the updated point
280                return current;
281        }
282}