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.camera.calibration;
031
032import gnu.trove.map.hash.TIntIntHashMap;
033import gnu.trove.map.hash.TObjectIntHashMap;
034
035import java.io.IOException;
036import java.util.ArrayDeque;
037import java.util.ArrayList;
038import java.util.Collections;
039import java.util.Deque;
040import java.util.EnumSet;
041import java.util.HashMap;
042import java.util.List;
043import java.util.Map;
044
045import org.apache.log4j.Logger;
046import org.openimaj.algorithm.iterative.MinEpsilonOrMaxIterations;
047import org.openimaj.image.FImage;
048import org.openimaj.image.MBFImage;
049import org.openimaj.image.analyser.ImageAnalyser;
050import org.openimaj.image.colour.RGBColour;
051import org.openimaj.image.contour.Contour;
052import org.openimaj.image.contour.SuzukiContourProcessor;
053import org.openimaj.image.feature.local.interest.SubPixelCorners;
054import org.openimaj.image.processing.algorithm.EqualisationProcessor;
055import org.openimaj.image.processing.algorithm.MaxFilter;
056import org.openimaj.image.processing.algorithm.MeanCenter;
057import org.openimaj.image.processing.threshold.AdaptiveLocalThresholdMean;
058import org.openimaj.math.geometry.point.Point2d;
059import org.openimaj.math.geometry.point.Point2dImpl;
060import org.openimaj.math.geometry.point.PointList;
061import org.openimaj.math.geometry.shape.Circle;
062import org.openimaj.math.geometry.shape.Polygon;
063import org.openimaj.math.geometry.shape.Rectangle;
064import org.openimaj.util.pair.FloatIntPair;
065import org.openimaj.video.VideoDisplay;
066import org.openimaj.video.VideoDisplayAdapter;
067import org.openimaj.video.capture.VideoCapture;
068
069/**
070 * This is a port/reworking of the OpenCV chessboard corner finder algorithm to
071 * OpenIMAJ. The key image-processing and geometry algorithms use the OpenIMAJ
072 * equivalents, but the code to extract the board and assess the correctness is
073 * purely based on a (slightly sanitised) port of the original OpenCV code.
074 * <p>
075 * This is improved variant of chessboard corner detection algorithm that uses a
076 * graph of connected quads. It is based on the code contributed by Vladimir
077 * Vezhnevets and Philip Gruebele. Here is the copyright notice from the
078 * original Vladimir's code:
079 * <p>
080 * The algorithms developed and implemented by Vezhnevets Vldimir aka Dead Moroz
081 * (vvp@graphics.cs.msu.ru) See <a
082 * href="http://graphics.cs.msu.su/en/research/calibration/opencv.html"
083 * >http://graphics.cs.msu.su/en/research/calibration/opencv.html</a> for
084 * detailed information.
085 * <p>
086 * Reliability additions and modifications made by Philip Gruebele.
087 * <p>
088 * Some further improvements for detection of partially occluded boards at
089 * non-ideal lighting conditions have been made by Alex Bovyrin and Kurt
090 * Kolonige.
091 *
092 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
093 */
094public class ChessboardCornerFinder implements ImageAnalyser<FImage> {
095        private static final class Corner extends Point2dImpl {
096                int row; // Board row index
097                int count; // Number of neighbor corners
098                Corner[] neighbours = new Corner[4];
099
100                public Corner(Point2d point2d) {
101                        super(point2d);
102                }
103
104                FloatIntPair meanDist()
105                {
106                        float sum = 0;
107                        int n = 0;
108                        for (int i = 0; i < 4; i++)
109                        {
110                                if (neighbours[i] != null)
111                                {
112                                        final float dx = neighbours[i].x - x;
113                                        final float dy = neighbours[i].y - y;
114                                        sum += Math.sqrt(dx * dx + dy * dy);
115                                        n++;
116                                }
117                        }
118                        return new FloatIntPair(sum / Math.max(n, 1), n);
119                }
120        }
121
122        private static final class Quad {
123                /**
124                 * The group that the quad belongs
125                 */
126                int groupIdx = -1;
127
128                /**
129                 * The corners
130                 */
131                Corner[] corners = new Corner[4];
132
133                /**
134                 * Minimum edge length in pixels
135                 */
136                float minEdgeLength;
137
138                /**
139                 * Actual number of neighbours
140                 */
141                int count;
142
143                /**
144                 * The neighbours
145                 */
146                Quad[] neighbours = new Quad[4];
147
148                /**
149                 * has the quad been sorted?
150                 */
151                boolean ordered;
152                /**
153                 * The row position
154                 */
155                int row;
156                /**
157                 * The column position
158                 */
159                int col;
160        }
161
162        private static final Logger logger = Logger.getLogger(ChessboardCornerFinder.class);
163
164        private static final int MIN_DILATIONS = 0;
165        private static final int MAX_DILATIONS = 7;
166        private static final SubPixelCorners SUB_PIX_CORNERS = new SubPixelCorners(2, 2,
167                        new MinEpsilonOrMaxIterations(0.1, 15));
168
169        /**
170         * Should filtering be enabled
171         */
172        boolean filterQuads = true;
173
174        /**
175         * Minimum area of quad if filtering
176         */
177        private double minSize = 25;
178
179        /**
180         * Maximum approximation distance
181         */
182        private int maxApproxLevel = 7;
183
184        /**
185         * Pre-process with histogram equalisation
186         */
187        boolean histogramEqualise = false;
188
189        /**
190         * Should mean adaptive thresholding be used?
191         */
192        boolean adaptiveThreshold = false;
193
194        /**
195         * Set if a fast check for the pattern be performed to bail early
196         */
197        final FastChessboardDetector fastDetector;
198
199        /**
200         * Number of blocks across the pattern
201         */
202        private int patternWidth;
203
204        /**
205         * Number of blocks down the pattern
206         */
207        private int patternHeight;
208
209        /**
210         * Was the complete pattern found?
211         */
212        boolean found = false;
213
214        /**
215         * The final corners
216         */
217        private List<Point2dImpl> outCorners = new ArrayList<Point2dImpl>();
218
219        /**
220         * Options for controlling how the corner finder works
221         *
222         * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
223         *
224         */
225        public static enum Options {
226                /**
227                 * Apply filtering to remove unlikely quads from detection
228                 */
229                FILTER_QUADS,
230                /**
231                 * Pre-process the image by performing histogram equalisation
232                 */
233                HISTOGRAM_EQUALISE,
234                /**
235                 * Perform adaptive (mean local) thresholding, rather than global
236                 */
237                ADAPTIVE_THRESHOLD,
238                /**
239                 * Perform the fast check to detect a pattern and bail early
240                 */
241                FAST_CHECK;
242        }
243
244        /**
245         * Construct with the given pattern size and options set.
246         *
247         * @param patternWidth
248         *            the pattern width
249         * @param patternHeight
250         *            the pattern height
251         * @param opts
252         *            the options
253         */
254        public ChessboardCornerFinder(int patternWidth, int patternHeight, Options... opts)
255        {
256                this.patternWidth = patternWidth;
257                this.patternHeight = patternHeight;
258
259                final EnumSet<Options> es = EnumSet.noneOf(Options.class);
260                for (final Options o : opts)
261                        es.add(o);
262
263                this.filterQuads = es.contains(Options.FILTER_QUADS);
264                this.histogramEqualise = es.contains(Options.HISTOGRAM_EQUALISE);
265                this.adaptiveThreshold = es.contains(Options.ADAPTIVE_THRESHOLD);
266
267                if (es.contains(Options.FAST_CHECK))
268                        fastDetector = new FastChessboardDetector(patternWidth, patternHeight);
269                else
270                        fastDetector = null;
271        }
272
273        @Override
274        public void analyseImage(FImage image) {
275                // reset state:
276                this.found = false;
277                this.outCorners.clear();
278
279                if (histogramEqualise)
280                        image = image.process(new EqualisationProcessor());
281
282                int prevSqrSize = 0;
283
284                if (fastDetector != null) {
285                        fastDetector.analyseImage(image);
286
287                        if (!fastDetector.chessboardDetected()) {
288                                return;
289                        }
290                }
291
292                for (int k = 0; k < 6; k++) {
293                        for (int dilations = MIN_DILATIONS; dilations < MAX_DILATIONS; dilations++) {
294                                if (found)
295                                        break;
296
297                                FImage threshImg;
298                                if (adaptiveThreshold) {
299                                        final int blockSize = (int) (Math.round(prevSqrSize == 0 ?
300                                                        Math.min(image.width, image.height) * (k % 2 == 0 ? 0.2 : 0.1) : prevSqrSize * 2) | 1);
301
302                                        // adaptive mean thresholding
303                                        threshImg = image.process(new AdaptiveLocalThresholdMean(blockSize, (k / 2) * 5));
304                                        if (dilations > 0) {
305                                                MaxFilter.filter(threshImg, dilations - 1);
306                                        }
307                                } else {
308                                        threshImg = image.clone().threshold(MeanCenter.patchMean(image.pixels) - 10f / 255f);
309                                        MaxFilter.filter(threshImg, dilations);
310                                }
311
312                                // draw a border to allow us to find quads that go off the edge
313                                // of the image
314                                threshImg.drawShape(new Rectangle(1, 1, threshImg.width - 3, threshImg.height - 3), 3, 1f);
315
316                                final List<Quad> quads = extractQuads(threshImg);
317
318                                // not found...
319                                if (quads.size() <= 0)
320                                        continue;
321
322                                findQuadNeighbours(quads);
323
324                                for (int groupIdx = 0;; groupIdx++)
325                                {
326                                        final List<Quad> quadGroup = findConnectedQuads(quads, groupIdx);
327                                        int count = quadGroup.size();
328
329                                        final int icount = count;
330                                        if (count == 0)
331                                                break;
332
333                                        // order the quad corners globally
334                                        // maybe delete or add some
335                                        logger.trace("Starting ordering of inner quads");
336                                        count = orderFoundConnectedQuads(quadGroup, quads);
337                                        logger.trace(String.format("Orig count: %d  After ordering: %d", icount, count));
338
339                                        if (count == 0)
340                                                continue; // haven't found inner quads
341
342                                        // If count is more than it should be, this will remove
343                                        // those quads which cause maximum deviation from a nice
344                                        // square pattern.
345                                        count = cleanFoundConnectedQuads(quadGroup);
346                                        logger.trace(String.format("Connected group: %d  orig count: %d cleaned: %d", groupIdx, icount,
347                                                        count));
348
349                                        final List<Corner> cornerGroup = new ArrayList<Corner>();
350                                        count = checkQuadGroup(quadGroup, cornerGroup);
351                                        logger.trace(String.format("Connected group: %d  count: %d  cleaned: %d", groupIdx, icount, count));
352
353                                        int n = count > 0 ? patternWidth * patternHeight : -count;
354                                        n = Math.min(n, patternWidth * patternHeight);
355                                        float sumDist = 0;
356                                        int total = 0;
357
358                                        for (int i = 0; i < n; i++)
359                                        {
360                                                final FloatIntPair pair = cornerGroup.get(i).meanDist();
361                                                final float avgi = pair.first;
362                                                final int ni = pair.second;
363                                                sumDist += avgi * ni;
364                                                total += ni;
365                                        }
366                                        prevSqrSize = Math.round(sumDist / Math.max(total, 1));
367
368                                        if (count > 0 || (outCorners.size() > 0 && -count > outCorners.size()))
369                                        {
370                                                // copy corners to output array
371                                                outCorners.clear();
372                                                for (int i = 0; i < n; i++)
373                                                        outCorners.add(new Point2dImpl(cornerGroup.get(i)));
374
375                                                if (count == patternWidth * patternHeight && checkBoardMonotony())
376                                                {
377                                                        found = true;
378                                                        break;
379                                                }
380                                        }
381                                } // grp
382                        } // dilations
383                } // k
384
385                if (found)
386                        found = checkBoardMonotony();
387
388                // check that none of the found corners is too close to the image
389                // boundary
390                if (found)
391                {
392                        final int BORDER = 8;
393                        int k;
394                        for (k = 0; k < patternWidth * patternHeight; k++)
395                        {
396                                if (outCorners.get(k).x <= BORDER || outCorners.get(k).x > image.width - BORDER ||
397                                                outCorners.get(k).y <= BORDER || outCorners.get(k).y > image.height - BORDER)
398                                        break;
399                        }
400
401                        found = k == patternWidth * patternHeight;
402                }
403
404                if (found && patternHeight % 2 == 0 && patternWidth % 2 == 0)
405                {
406                        final int lastRow = (patternHeight - 1) * patternWidth;
407                        final double dy0 = outCorners.get(lastRow).y - outCorners.get(0).y;
408                        if (dy0 < 0)
409                        {
410                                int i;
411                                final int n = patternWidth * patternHeight;
412                                for (i = 0; i < n / 2; i++)
413                                {
414                                        Collections.swap(outCorners, i, n - i - 1);
415                                }
416                        }
417                }
418
419                if (found) {
420                        outCorners = SUB_PIX_CORNERS.findSubPixCorners(image, outCorners);
421                }
422        }
423
424        /**
425         * Returns corners in clockwise order corners don't necessarily start at
426         * same position on quad (e.g., top left corner)
427         *
428         * @param threshImg
429         *            the binary image
430         * @return the extracted Quads
431         */
432        private List<Quad> extractQuads(final FImage threshImg) {
433                // if filtering is enabled, we try to guess the board contour containing
434                // all the relevant quads
435                final TObjectIntHashMap<Contour> counters = new TObjectIntHashMap<Contour>();
436                // cornerList is the list of valid quads (prior to
437                // filtering out those with the wrong parent if applicable)
438                final List<Corner[]> cornerList = new ArrayList<Corner[]>();
439                final Map<Corner[], Contour> parentMapping = new HashMap<Corner[], Contour>();
440                Contour board = null;
441
442                // extract contours
443                final Contour contours = SuzukiContourProcessor.findContours(threshImg);
444
445                for (final Contour c : contours.contourIterable()) {
446                        final Rectangle bounds = c.calculateRegularBoundingBox();
447
448                        // skip small regions
449                        if (!(c.isHole() && bounds.width * bounds.height >= minSize))
450                                continue;
451
452                        // try and make an approximated polygon with 4 vertices
453                        Polygon p = null;
454                        for (int approxLevel = 1; approxLevel <= maxApproxLevel; approxLevel++) {
455                                p = c.reduceVertices(approxLevel);
456                                if (p.nVertices() == 4)
457                                        break;
458                        }
459
460                        // test polygon for correctness
461                        if (p != null && p.nVertices() == 4 && p.isConvex() && p.hasNoInnerPolygons()) {
462                                final double perimeter = p.calculatePerimeter();
463                                final double area = p.calculateArea();
464
465                                final Corner[] pt = new Corner[4];
466                                for (int i = 0; i < 4; i++)
467                                        pt[i] = new Corner(p.points.get(i));
468
469                                float dx = pt[0].x - pt[2].x;
470                                float dy = pt[0].y - pt[2].y;
471                                final double d1 = Math.sqrt(dx * dx + dy * dy);
472
473                                dx = pt[1].x - pt[3].x;
474                                dy = pt[1].y - pt[3].y;
475                                final double d2 = Math.sqrt(dx * dx + dy * dy);
476
477                                dx = pt[0].x - pt[1].x;
478                                dy = pt[0].y - pt[1].y;
479                                final double d3 = Math.sqrt(dx * dx + dy * dy);
480                                dx = pt[1].x - pt[2].x;
481                                dy = pt[1].y - pt[2].y;
482                                final double d4 = Math.sqrt(dx * dx + dy * dy);
483
484                                if (!filterQuads
485                                                ||
486                                                (d3 * 4 > d4 && d4 * 4 > d3 && d3 * d4 < area * 1.5 && area > minSize && d1 >= 0.15 * perimeter && d2 >= 0.15 * perimeter))
487                                {
488                                        final Contour parent = c.parent;
489                                        counters.adjustOrPutValue(parent, 1, 1);
490
491                                        // basic idea is that the board should have more holes in it
492                                        // than anything else in the scene
493                                        if (board == null || counters.get(board) < counters.get(parent))
494                                                board = parent;
495
496                                        cornerList.add(pt);
497                                        parentMapping.put(pt, c.parent);
498                                }
499                        }
500                }
501
502                final List<Quad> quads = new ArrayList<Quad>();
503                for (int i = 0; i < cornerList.size(); i++) {
504                        final Corner[] pts = cornerList.get(i);
505
506                        // reject regions that don't belong to the predicted board
507                        if (filterQuads && parentMapping.get(pts) != board)
508                                continue;
509
510                        final Quad q = new Quad();
511                        q.corners = pts;
512
513                        q.minEdgeLength = Float.MAX_VALUE;
514                        for (int j = 0; j < 4; j++) {
515                                final float dx = pts[j].x - pts[(j + 1) & 3].x;
516                                final float dy = pts[j].y - pts[(j + 1) & 3].y;
517                                final float d = dx * dx + dy * dy;
518                                if (q.minEdgeLength > d)
519                                        q.minEdgeLength = d;
520                        }
521
522                        quads.add(q);
523                }
524                return quads;
525        }
526
527        /**
528         * Find the neighbouring quads for each quad
529         *
530         * @param quads
531         *            the quads
532         */
533        private void findQuadNeighbours(List<Quad> quads)
534        {
535                final int quadCount = quads.size();
536                final float threshScale = 1.f;
537
538                // find quad neighbours
539                for (int idx = 0; idx < quads.size(); idx++)
540                {
541                        final Quad curQuad = quads.get(idx);
542
543                        // choose the points of the current quadrangle that are close to
544                        // some points of the other quadrangles
545                        // (it can happen for split corners (due to dilation) of the
546                        // checker board). Search only in other quadrangles!
547
548                        // for each corner of this quadrangle
549                        for (int i = 0; i < 4; i++)
550                        {
551                                float minDist = Float.MAX_VALUE;
552                                int closestCornerIdx = -1;
553                                Quad closestQuad = null;
554                                Corner closestCorner = null;
555
556                                if (curQuad.neighbours[i] != null)
557                                        continue;
558
559                                final Corner pt = curQuad.corners[i];
560
561                                float dx;
562                                float dy;
563                                float dist;
564                                // find the closest corner in all other quadrangles
565                                for (int k = 0; k < quadCount; k++)
566                                {
567                                        if (k == idx)
568                                                continue;
569
570                                        for (int j = 0; j < 4; j++)
571                                        {
572                                                if (quads.get(k).neighbours[j] != null)
573                                                        continue;
574
575                                                dx = pt.x - quads.get(k).corners[j].x;
576                                                dy = pt.y - quads.get(k).corners[j].y;
577                                                dist = dx * dx + dy * dy;
578
579                                                if (dist < minDist &&
580                                                                dist <= curQuad.minEdgeLength * threshScale &&
581                                                                dist <= quads.get(k).minEdgeLength * threshScale)
582                                                {
583                                                        // check edge lengths, make sure they're compatible
584                                                        // edges that are different by more than 1:4 are
585                                                        // rejected
586                                                        final float ediff = curQuad.minEdgeLength - quads.get(k).minEdgeLength;
587                                                        if (ediff > 32 * curQuad.minEdgeLength ||
588                                                                        ediff > 32 * quads.get(k).minEdgeLength)
589                                                        {
590                                                                logger.trace("Incompatible edge lengths");
591                                                                continue;
592                                                        }
593                                                        closestCornerIdx = j;
594                                                        closestQuad = quads.get(k);
595                                                        minDist = dist;
596                                                }
597                                        }
598                                }
599
600                                // we found a matching corner point?
601                                if (closestCornerIdx >= 0 && minDist < Float.MAX_VALUE)
602                                {
603                                        // If another point from our current quad is closer to the
604                                        // found corner
605                                        // than the current one, then we don't count this one after
606                                        // all.
607                                        // This is necessary to support small squares where
608                                        // otherwise the wrong
609                                        // corner will get matched to closest_quad;
610                                        closestCorner = closestQuad.corners[closestCornerIdx];
611
612                                        int j;
613                                        for (j = 0; j < 4; j++)
614                                        {
615                                                if (curQuad.neighbours[j] == closestQuad)
616                                                        break;
617
618                                                dx = closestCorner.x - curQuad.corners[j].x;
619                                                dy = closestCorner.y - curQuad.corners[j].y;
620
621                                                if (dx * dx + dy * dy < minDist)
622                                                        break;
623                                        }
624
625                                        if (j < 4 || curQuad.count >= 4 || closestQuad.count >= 4)
626                                                continue;
627
628                                        // Check that each corner is a neighbor of different quads
629                                        for (j = 0; j < closestQuad.count; j++)
630                                        {
631                                                if (closestQuad.neighbours[j] == curQuad)
632                                                        break;
633                                        }
634                                        if (j < closestQuad.count)
635                                                continue;
636
637                                        // check whether the closest corner to closestCorner
638                                        // is different from curQuad.corners[i].pt
639                                        int k;
640                                        for (k = 0; k < quadCount; k++)
641                                        {
642                                                final Quad q = quads.get(k);
643                                                if (k == idx || q == closestQuad)
644                                                        continue;
645
646                                                for (j = 0; j < 4; j++)
647                                                        if (q.neighbours[j] == null)
648                                                        {
649                                                                dx = closestCorner.x - q.corners[j].x;
650                                                                dy = closestCorner.y - q.corners[j].y;
651                                                                dist = dx * dx + dy * dy;
652                                                                if (dist < minDist)
653                                                                        break;
654                                                        }
655                                                if (j < 4)
656                                                        break;
657                                        }
658
659                                        if (k < quadCount)
660                                                continue;
661
662                                        closestCorner.x = (pt.x + closestCorner.x) * 0.5f;
663                                        closestCorner.y = (pt.y + closestCorner.y) * 0.5f;
664
665                                        // We've found one more corner - remember it
666                                        curQuad.count++;
667                                        curQuad.neighbours[i] = closestQuad;
668                                        curQuad.corners[i] = closestCorner;
669
670                                        closestQuad.count++;
671                                        closestQuad.neighbours[closestCornerIdx] = curQuad;
672                                }
673                        }
674                }
675        }
676
677        /**
678         * Was a chessboard found in the last call to {@link #analyseImage(FImage)}?
679         *
680         * @return true if found; false otherwise
681         */
682        public boolean isFound() {
683                return found;
684        }
685
686        /**
687         * Get any corners detected by the last call to
688         * {@link #analyseImage(FImage)}.
689         *
690         * @return the corners
691         */
692        public List<Point2dImpl> getCorners() {
693                return this.outCorners;
694        }
695
696        /**
697         * Find groups of connected quads. This searches for the first un-labelled
698         * quad and then finds the connected ones.
699         *
700         * @param quads
701         *            the quads
702         * @param groupIdx
703         *            the group index
704         * @return the quads belonging to the group
705         */
706        private List<Quad> findConnectedQuads(List<Quad> quads, int groupIdx)
707        {
708                final Deque<Quad> stack = new ArrayDeque<Quad>();
709                int i;
710                final int quadCount = quads.size();
711                final List<Quad> outGroup = new ArrayList<Quad>();
712
713                // Scan the array for a first unlabeled quad
714                for (i = 0; i < quadCount; i++)
715                {
716                        if (quads.get(i).count > 0 && quads.get(i).groupIdx < 0)
717                                break;
718                }
719
720                // Recursively find a group of connected quads starting from the seed
721                // quad[i]
722                if (i < quadCount)
723                {
724                        Quad q = quads.get(i);
725                        stack.push(q);
726                        outGroup.add(q);
727                        q.groupIdx = groupIdx;
728                        q.ordered = false;
729
730                        while (stack.size() > 0)
731                        {
732                                q = stack.pop();
733                                for (i = 0; i < 4; i++)
734                                {
735                                        final Quad neighbour = q.neighbours[i];
736                                        if (neighbour != null && neighbour.count > 0 && neighbour.groupIdx < 0)
737                                        {
738                                                stack.push(neighbour);
739                                                outGroup.add(neighbour);
740                                                neighbour.groupIdx = groupIdx;
741                                                neighbour.ordered = false;
742                                        }
743                                }
744                        }
745                }
746
747                return outGroup;
748        }
749
750        /**
751         * order a group of connected quads order of corners: 0 is top left
752         * clockwise from there note: "top left" is nominal, depends on initial
753         * ordering of starting quad but all other quads are ordered consistently
754         *
755         * can change the number of quads in the group can add quads, so we need to
756         * have quad/corner arrays passed in
757         */
758        private int orderFoundConnectedQuads(List<Quad> quads, List<Quad> allQuads)
759        {
760                final Deque<Quad> stack = new ArrayDeque<Quad>();
761
762                int quadCount = quads.size();
763
764                // first find an interior quad
765                Quad start = null;
766                for (int i = 0; i < quadCount; i++)
767                {
768                        if (quads.get(i).count == 4)
769                        {
770                                start = quads.get(i);
771                                break;
772                        }
773                }
774
775                if (start == null)
776                        return 0; // no 4-connected quad
777
778                // start with first one, assign rows/cols
779                int rowMin = 0, colMin = 0, rowMax = 0, colMax = 0;
780
781                final TIntIntHashMap colHist = new TIntIntHashMap();
782                final TIntIntHashMap rowHist = new TIntIntHashMap();
783
784                stack.push(start);
785                start.row = 0;
786                start.col = 0;
787                start.ordered = true;
788
789                // Recursively order the quads so that all position numbers (e.g.,
790                // 0,1,2,3) are in the at the same relative corner (e.g., lower right).
791
792                while (stack.size() > 0)
793                {
794                        final Quad q = stack.pop();
795                        int col = q.col;
796                        int row = q.row;
797                        colHist.adjustOrPutValue(col, 1, 1);
798                        rowHist.adjustOrPutValue(row, 1, 1);
799
800                        // check min/max
801                        if (row > rowMax)
802                                rowMax = row;
803                        if (row < rowMin)
804                                rowMin = row;
805                        if (col > colMax)
806                                colMax = col;
807                        if (col < colMin)
808                                colMin = col;
809
810                        for (int i = 0; i < 4; i++)
811                        {
812                                final Quad neighbour = q.neighbours[i];
813                                switch (i) // adjust col, row for this quad
814                                { // start at top left, go clockwise
815                                case 0:
816                                        row--;
817                                        col--;
818                                        break;
819                                case 1:
820                                        col += 2;
821                                        break;
822                                case 2:
823                                        row += 2;
824                                        break;
825                                case 3:
826                                        col -= 2;
827                                        break;
828                                }
829
830                                // just do inside quads
831                                if (neighbour != null && neighbour.ordered == false && neighbour.count == 4)
832                                {
833                                        orderQuad(neighbour, q.corners[i], (i + 2) % 4); // set in
834                                        // order
835                                        neighbour.ordered = true;
836                                        neighbour.row = row;
837                                        neighbour.col = col;
838                                        stack.push(neighbour);
839                                }
840                        }
841                }
842
843                // analyze inner quad structure
844                int w = patternWidth - 1;
845                int h = patternHeight - 1;
846                final int drow = rowMax - rowMin + 1;
847                final int dcol = colMax - colMin + 1;
848
849                // normalize pattern and found quad indices
850                if ((w > h && dcol < drow) ||
851                                (w < h && drow < dcol))
852                {
853                        h = patternWidth - 1;
854                        w = patternHeight - 1;
855                }
856
857                logger.trace(String.format("Size: %dx%d  Pattern: %dx%d", dcol, drow, w, h));
858
859                // check if there are enough inner quads
860                if (dcol < w || drow < h) // found enough inner quads?
861                {
862                        logger.trace("Too few inner quad rows/cols");
863                        return 0; // no, return
864                }
865
866                // check edges of inner quads
867                // if there is an outer quad missing, fill it in
868                // first order all inner quads
869                int found = 0;
870                for (int i = 0; i < quadCount; i++)
871                {
872                        if (quads.get(i).count == 4)
873                        { // ok, look at neighbours
874                                int col = quads.get(i).col;
875                                int row = quads.get(i).row;
876                                for (int j = 0; j < 4; j++)
877                                {
878                                        switch (j) // adjust col, row for this quad
879                                        { // start at top left, go clockwise
880                                        case 0:
881                                                row--;
882                                                col--;
883                                                break;
884                                        case 1:
885                                                col += 2;
886                                                break;
887                                        case 2:
888                                                row += 2;
889                                                break;
890                                        case 3:
891                                                col -= 2;
892                                                break;
893                                        }
894                                        final Quad neighbour = quads.get(i).neighbours[j];
895                                        if (neighbour != null && !neighbour.ordered && // is it an
896                                                        // inner
897                                                        // quad?
898                                                        col <= colMax && col >= colMin &&
899                                                        row <= rowMax && row >= rowMin)
900                                        {
901                                                // if so, set in order
902                                                logger.trace(String.format("Adding inner: col: %d  row: %d", col, row));
903                                                found++;
904                                                orderQuad(neighbour, quads.get(i).corners[j], (j + 2) % 4);
905                                                neighbour.ordered = true;
906                                                neighbour.row = row;
907                                                neighbour.col = col;
908                                        }
909                                }
910                        }
911                }
912
913                // if we have found inner quads, add corresponding outer quads,
914                // which are missing
915                if (found > 0)
916                {
917                        logger.trace(String.format("Found %d inner quads not connected to outer quads, repairing", found));
918                        for (int i = 0; i < quadCount; i++)
919                        {
920                                if (quads.get(i).count < 4 && quads.get(i).ordered)
921                                {
922                                        final int added = addOuterQuad(quads.get(i), quads, allQuads);
923                                        quadCount += added;
924                                }
925                        }
926                }
927
928                // final trimming of outer quads
929                if (dcol == w && drow == h) // found correct inner quads
930                {
931                        logger.trace("Inner bounds ok, check outer quads");
932                        int rcount = quadCount;
933                        for (int i = quadCount - 1; i >= 0; i--) // eliminate any quad not
934                                // connected to
935                                // an ordered quad
936                        {
937                                if (quads.get(i).ordered == false)
938                                {
939                                        boolean outer = false;
940                                        for (int j = 0; j < 4; j++) // any neighbours that are
941                                                // ordered?
942                                        {
943                                                if (quads.get(i).neighbours[j] != null && quads.get(i).neighbours[j].ordered)
944                                                        outer = true;
945                                        }
946                                        if (!outer) // not an outer quad, eliminate
947                                        {
948                                                logger.trace(String.format("Removing quad %d", i));
949                                                removeQuadFromGroup(quads, quads.get(i));
950                                                rcount--;
951                                        }
952                                }
953
954                        }
955                        return rcount;
956                }
957
958                return 0;
959        }
960
961        /**
962         * put quad into correct order, where <code>corner</code> has value
963         * <code<common</code>
964         *
965         * @param quad
966         *            the quad to sort
967         * @param corner
968         *            the corner
969         * @param common
970         *            the common vertex
971         */
972        private void orderQuad(Quad quad, Corner corner, int common)
973        {
974                // find the corner
975                int tc;
976                for (tc = 0; tc < 4; tc++)
977                        if (quad.corners[tc].x == corner.x &&
978                        quad.corners[tc].y == corner.y)
979                                break;
980
981                // set corner order
982                // shift
983                while (tc != common)
984                {
985                        // shift by one
986                        final Corner tempc = quad.corners[3];
987                        final Quad tempq = quad.neighbours[3];
988                        for (int i = 3; i > 0; i--)
989                        {
990                                quad.corners[i] = quad.corners[i - 1];
991                                quad.neighbours[i] = quad.neighbours[i - 1];
992                        }
993                        quad.corners[0] = tempc;
994                        quad.neighbours[0] = tempq;
995                        tc++;
996                        tc = tc % 4;
997                }
998        }
999
1000        /*
1001         * add an outer quad
1002         *
1003         * looks for the neighbor of <code>quad</code> that isn't present, tries to
1004         * add it in. <code>quad</code> is ordered
1005         *
1006         * @param quad the quad
1007         *
1008         * @param quads all quad group
1009         *
1010         * @param allQuads all the quads
1011         *
1012         * @return
1013         */
1014        private int addOuterQuad(final Quad quad, List<Quad> quads, List<Quad> allQuads)
1015        {
1016                int added = 0;
1017                for (int i = 0; i < 4; i++) // find no-neighbor corners
1018                {
1019                        if (quad.neighbours[i] == null) // ok, create and add neighbor
1020                        {
1021                                final int j = (i + 2) % 4;
1022                                logger.trace("Adding quad as neighbor 2");
1023                                final Quad q = new Quad();
1024                                allQuads.add(q);
1025                                added++;
1026                                quads.add(q);
1027
1028                                // set neighbor and group id
1029                                quad.neighbours[i] = q;
1030                                quad.count += 1;
1031                                q.neighbours[j] = quad;
1032                                q.groupIdx = quad.groupIdx;
1033                                q.count = 1; // number of neighbours
1034                                q.ordered = false;
1035                                q.minEdgeLength = quad.minEdgeLength;
1036
1037                                // make corners of new quad
1038                                // same as neighbor quad, but offset
1039                                Corner pt = quad.corners[i];
1040                                final float dx = pt.x - quad.corners[j].x;
1041                                final float dy = pt.y - quad.corners[j].y;
1042                                for (int k = 0; k < 4; k++)
1043                                {
1044                                        final Corner corner = new Corner(pt);
1045                                        pt = quad.corners[k];
1046                                        q.corners[k] = corner;
1047                                        corner.x += dx;
1048                                        corner.y += dy;
1049                                }
1050                                // have to set exact corner
1051                                q.corners[j] = quad.corners[i];
1052
1053                                // now find other neighbor and add it, if possible
1054                                if (quad.neighbours[(i + 3) % 4] != null &&
1055                                                quad.neighbours[(i + 3) % 4].ordered &&
1056                                                quad.neighbours[(i + 3) % 4].neighbours[i] != null &&
1057                                                quad.neighbours[(i + 3) % 4].neighbours[i].ordered)
1058                                {
1059                                        final Quad qn = quad.neighbours[(i + 3) % 4].neighbours[i];
1060                                        q.count = 2;
1061                                        q.neighbours[(j + 1) % 4] = qn;
1062                                        qn.neighbours[(i + 1) % 4] = q;
1063                                        qn.count += 1;
1064                                        // have to set exact corner
1065                                        q.corners[(j + 1) % 4] = qn.corners[(i + 1) % 4];
1066                                }
1067                        }
1068                }
1069                return added;
1070        }
1071
1072        /**
1073         * remove quad from quad group
1074         */
1075        private void removeQuadFromGroup(final List<Quad> quads, Quad q0)
1076        {
1077                final int count = quads.size();
1078                // remove any references to this quad as a neighbor
1079                for (int i = 0; i < count; i++)
1080                {
1081                        final Quad q = quads.get(i);
1082                        for (int j = 0; j < 4; j++)
1083                        {
1084                                if (q.neighbours[j] == q0)
1085                                {
1086                                        q.neighbours[j] = null;
1087                                        q.count--;
1088                                        for (int k = 0; k < 4; k++)
1089                                                if (q0.neighbours[k] == q)
1090                                                {
1091                                                        q0.neighbours[k] = null;
1092                                                        q0.count--;
1093                                                        break;
1094                                                }
1095                                        break;
1096                                }
1097                        }
1098                }
1099
1100                quads.remove(q0);
1101        }
1102
1103        /**
1104         * if we found too many connect quads, remove those which probably do not
1105         * belong.
1106         *
1107         * @param quadGroup
1108         *            the group of quads
1109         * @return the new group size
1110         */
1111        private int cleanFoundConnectedQuads(List<Quad> quadGroup)
1112        {
1113                int quadCount = quadGroup.size();
1114                final Point2dImpl center = new Point2dImpl();
1115
1116                // number of quads this pattern should contain
1117                final int count = ((patternWidth + 1) * (patternHeight + 1) + 1) / 2;
1118
1119                // if we have more quadrangles than we should,
1120                // try to eliminate duplicates or ones which don't belong to the pattern
1121                // rectangle...
1122                if (quadCount <= count)
1123                        return quadCount;
1124
1125                // create an array of quadrangle centers
1126                final List<Point2dImpl> centers = new ArrayList<Point2dImpl>(quadCount);
1127
1128                for (int i = 0; i < quadCount; i++)
1129                {
1130                        final Point2dImpl ci = new Point2dImpl();
1131                        final Quad q = quadGroup.get(i);
1132
1133                        for (int j = 0; j < 4; j++)
1134                        {
1135                                final Point2dImpl pt = q.corners[j];
1136                                ci.x += pt.x;
1137                                ci.y += pt.y;
1138                        }
1139
1140                        ci.x *= 0.25f;
1141                        ci.y *= 0.25f;
1142
1143                        centers.add(ci);
1144                        center.x += ci.x;
1145                        center.y += ci.y;
1146                }
1147                center.x /= quadCount;
1148                center.y /= quadCount;
1149
1150                // If we still have more quadrangles than we should,
1151                // we try to eliminate bad ones based on minimizing the bounding box.
1152                // We iteratively remove the point which reduces the size of
1153                // the bounding box of the blobs the most
1154                // (since we want the rectangle to be as small as possible)
1155                // remove the quadrange that causes the biggest reduction
1156                // in pattern size until we have the correct number
1157                for (; quadCount > count; quadCount--)
1158                {
1159                        double minBoxArea = Double.MAX_VALUE;
1160                        int minBoxAreaIndex = -1;
1161                        Quad q0, q;
1162
1163                        // For each point, calculate box area without that point
1164                        for (int skip = 0; skip < quadCount; skip++)
1165                        {
1166                                // get bounding rectangle
1167                                final Point2dImpl temp = centers.get(skip); // temporarily make
1168                                // index 'skip' the
1169                                // same as
1170                                centers.set(skip, center); // pattern center (so it is not
1171                                // counted for convex hull)
1172                                final PointList pl = new PointList(centers);
1173                                final Polygon hull = pl.calculateConvexHull();
1174                                centers.set(skip, temp);
1175                                final double hullArea = hull.calculateArea();
1176
1177                                // remember smallest box area
1178                                if (hullArea < minBoxArea)
1179                                {
1180                                        minBoxArea = hullArea;
1181                                        minBoxAreaIndex = skip;
1182                                }
1183                        }
1184
1185                        q0 = quadGroup.get(minBoxAreaIndex);
1186
1187                        // remove any references to this quad as a neighbor
1188                        for (int i = 0; i < quadCount; i++)
1189                        {
1190                                q = quadGroup.get(i);
1191                                for (int j = 0; j < 4; j++)
1192                                {
1193                                        if (q.neighbours[j] == q0)
1194                                        {
1195                                                q.neighbours[j] = null;
1196                                                q.count--;
1197                                                for (int k = 0; k < 4; k++)
1198                                                        if (q0.neighbours[k] == q)
1199                                                        {
1200                                                                q0.neighbours[k] = null;
1201                                                                q0.count--;
1202                                                                break;
1203                                                        }
1204                                                break;
1205                                        }
1206                                }
1207                        }
1208
1209                        // remove the quad
1210                        quadCount--;
1211                        quadGroup.remove(minBoxAreaIndex);
1212                        centers.remove(minBoxAreaIndex);
1213                }
1214
1215                return quadCount;
1216        }
1217
1218        /**
1219         *
1220         */
1221        private int checkQuadGroup(List<Quad> quadGroup, List<Corner> outCorners)
1222        {
1223                final int ROW1 = 1000000;
1224                final int ROW2 = 2000000;
1225                final int ROW_ = 3000000;
1226                int result = 0;
1227                final int quadCount = quadGroup.size();
1228                int cornerCount = 0;
1229                final Corner[] corners = new Corner[quadCount * 4];
1230
1231                int width = 0, height = 0;
1232                final int[] hist = { 0, 0, 0, 0, 0 };
1233                Corner first = null, first2 = null, right, cur, below, c;
1234
1235                try {
1236                        // build dual graph, which vertices are internal quad corners
1237                        // and two vertices are connected iff they lie on the same quad edge
1238                        for (int i = 0; i < quadCount; i++)
1239                        {
1240                                final Quad q = quadGroup.get(i);
1241
1242                                for (int j = 0; j < 4; j++)
1243                                {
1244                                        if (q.neighbours[j] != null)
1245                                        {
1246                                                final Corner a = q.corners[j], b = q.corners[(j + 1) & 3];
1247                                                // mark internal corners that belong to:
1248                                                // - a quad with a single neighbor - with ROW1,
1249                                                // - a quad with two neighbours - with ROW2
1250                                                // make the rest of internal corners with ROW_
1251                                                final int rowFlag = q.count == 1 ? ROW1 : q.count == 2 ? ROW2 : ROW_;
1252
1253                                                if (a.row == 0)
1254                                                {
1255                                                        corners[cornerCount++] = a;
1256                                                        a.row = rowFlag;
1257                                                }
1258                                                else if (a.row > rowFlag)
1259                                                        a.row = rowFlag;
1260
1261                                                if (q.neighbours[(j + 1) & 3] != null)
1262                                                {
1263                                                        if (a.count >= 4 || b.count >= 4)
1264                                                                throw new Exception();
1265                                                        for (int k = 0; k < 4; k++)
1266                                                        {
1267                                                                if (a.neighbours[k] == b)
1268                                                                        throw new Exception();
1269                                                                if (b.neighbours[k] == a)
1270                                                                        throw new Exception();
1271                                                        }
1272                                                        a.neighbours[a.count++] = b;
1273                                                        b.neighbours[b.count++] = a;
1274                                                }
1275                                        }
1276                                }
1277                        }
1278
1279                        if (cornerCount != patternWidth * patternHeight)
1280                                throw new Exception();
1281
1282                        for (int i = 0; i < cornerCount; i++)
1283                        {
1284                                final int n = corners[i].count;
1285                                assert (0 <= n && n <= 4);
1286                                hist[n]++;
1287                                if (first == null && n == 2)
1288                                {
1289                                        if (corners[i].row == ROW1)
1290                                                first = corners[i];
1291                                        else if (first2 == null && corners[i].row == ROW2)
1292                                                first2 = corners[i];
1293                                }
1294                        }
1295
1296                        // start with a corner that belongs to a quad with a signle
1297                        // neighbor.
1298                        // if we do not have such, start with a corner of a quad with two
1299                        // neighbours.
1300                        if (first == null)
1301                                first = first2;
1302
1303                        if (first == null || hist[0] != 0 || hist[1] != 0 || hist[2] != 4 ||
1304                                        hist[3] != (patternWidth + patternHeight) * 2 - 8)
1305                                throw new Exception();
1306
1307                        cur = first;
1308                        right = below = null;
1309                        outCorners.add(cur);
1310
1311                        for (int k = 0; k < 4; k++)
1312                        {
1313                                c = cur.neighbours[k];
1314                                if (c != null)
1315                                {
1316                                        if (right == null)
1317                                                right = c;
1318                                        else if (below == null)
1319                                                below = c;
1320                                }
1321                        }
1322
1323                        if (right == null || (right.count != 2 && right.count != 3) ||
1324                                        below == null || (below.count != 2 && below.count != 3))
1325                                throw new Exception();
1326
1327                        cur.row = 0;
1328                        first = below; // remember the first corner in the next row
1329                        // find and store the first row (or column)
1330                        while (true)
1331                        {
1332                                right.row = 0;
1333                                outCorners.add(right);
1334
1335                                if (right.count == 2)
1336                                        break;
1337                                if (right.count != 3 || outCorners.size() >= Math.max(patternWidth, patternHeight))
1338                                        throw new Exception();
1339                                cur = right;
1340
1341                                for (int k = 0; k < 4; k++)
1342                                {
1343                                        c = cur.neighbours[k];
1344                                        if (c != null && c.row > 0)
1345                                        {
1346                                                int kk;
1347                                                for (kk = 0; kk < 4; kk++)
1348                                                {
1349                                                        if (c.neighbours[kk] == below)
1350                                                                break;
1351                                                }
1352                                                if (kk < 4)
1353                                                        below = c;
1354                                                else
1355                                                        right = c;
1356                                        }
1357                                }
1358                        }
1359
1360                        width = outCorners.size();
1361                        if (width == patternWidth)
1362                                height = patternHeight;
1363                        else if (width == patternHeight)
1364                                height = patternWidth;
1365                        else
1366                                throw new Exception();
1367
1368                        // find and store all the other rows
1369                        for (int i = 1;; i++)
1370                        {
1371                                if (first == null)
1372                                        break;
1373                                cur = first;
1374                                first = null;
1375                                int j;
1376                                for (j = 0;; j++)
1377                                {
1378                                        cur.row = i;
1379                                        outCorners.add(cur);
1380                                        if (cur.count == 2 + (i < height - 1 ? 1 : 0) && j > 0)
1381                                                break;
1382
1383                                        right = null;
1384
1385                                        // find a neighbor that has not been processed yet
1386                                        // and that has a neighbor from the previous row
1387                                        for (int k = 0; k < 4; k++)
1388                                        {
1389                                                c = cur.neighbours[k];
1390                                                if (c != null && c.row > i)
1391                                                {
1392                                                        int kk;
1393                                                        for (kk = 0; kk < 4; kk++)
1394                                                        {
1395                                                                if (c.neighbours[kk] != null && c.neighbours[kk].row == i - 1)
1396                                                                        break;
1397                                                        }
1398                                                        if (kk < 4)
1399                                                        {
1400                                                                right = c;
1401                                                                if (j > 0)
1402                                                                        break;
1403                                                        }
1404                                                        else if (j == 0)
1405                                                                first = c;
1406                                                }
1407                                        }
1408                                        if (right == null)
1409                                                throw new Exception();
1410                                        cur = right;
1411                                }
1412
1413                                if (j != width - 1)
1414                                        throw new Exception();
1415                        }
1416
1417                        if (outCorners.size() != cornerCount)
1418                                throw new Exception();
1419
1420                        // check if we need to transpose the board
1421                        if (width != patternWidth)
1422                        {
1423                                final int t = width;
1424                                width = height;
1425                                height = t;
1426
1427                                for (int i = 0; i < cornerCount; i++)
1428                                        corners[i] = outCorners.get(i);
1429
1430                                for (int i = 0; i < height; i++)
1431                                        for (int j = 0; j < width; j++)
1432                                                outCorners.set(i * width + j, corners[j * height + i]);
1433                        }
1434
1435                        // check if we need to revert the order in each row
1436                        {
1437                                final Point2dImpl p0 = outCorners.get(0), p1 = outCorners.get(patternWidth - 1), p2 = outCorners
1438                                                .get(patternWidth);
1439                                if ((p1.x - p0.x) * (p2.y - p1.y) - (p1.y - p0.y) * (p2.x - p1.x) < 0)
1440                                {
1441                                        if (width % 2 == 0)
1442                                        {
1443                                                for (int i = 0; i < height; i++)
1444                                                        for (int j = 0; j < width / 2; j++)
1445                                                                Collections.swap(outCorners, i * width + j, i * width + width - j - 1);
1446                                        }
1447                                        else
1448                                        {
1449                                                for (int j = 0; j < width; j++)
1450                                                        for (int i = 0; i < height / 2; i++)
1451                                                                Collections.swap(outCorners, i * width + j, (height - i - 1) * width + j);
1452                                        }
1453                                }
1454                        }
1455
1456                        result = cornerCount;
1457
1458                } catch (final Exception ex) {
1459                        // ignore
1460                }
1461
1462                if (result <= 0)
1463                {
1464                        cornerCount = Math.min(cornerCount, patternWidth * patternHeight);
1465                        outCorners.clear();
1466                        for (int i = 0; i < cornerCount; i++)
1467                                outCorners.add(corners[i]);
1468                        result = -cornerCount;
1469
1470                        if (result == -patternWidth * patternHeight)
1471                                result = -result;
1472                }
1473
1474                return result;
1475        }
1476
1477        /**
1478         * Checks that each board row and column is pretty much monotonous curve:
1479         *
1480         * It analyzes each row and each column of the chessboard as following:
1481         *
1482         * for each corner c lying between end points in the same row/column it
1483         * checks that the point projection to the line segment (a,b) is lying
1484         * between projections of the neighbor corners in the same row/column.
1485         *
1486         * This function has been created as temporary workaround for the bug in
1487         * current implementation of cvFindChessboardCornes that produces absolutely
1488         * unordered sets of corners.
1489         *
1490         * @return true if the board is good; false otherwise.
1491         */
1492        private boolean checkBoardMonotony()
1493        {
1494                int i, j, k;
1495
1496                for (k = 0; k < 2; k++)
1497                {
1498                        for (i = 0; i < (k == 0 ? patternHeight : patternWidth); i++)
1499                        {
1500                                final Point2dImpl a = k == 0 ? outCorners.get(i * patternWidth) : outCorners.get(i);
1501                                final Point2dImpl b = k == 0 ? outCorners.get((i + 1) * patternWidth - 1) :
1502                                        outCorners.get((patternHeight - 1) * patternWidth + i);
1503                                float prevt = 0;
1504                                final float dx0 = b.x - a.x, dy0 = b.y - a.y;
1505                                if (Math.abs(dx0) + Math.abs(dy0) < Float.MIN_VALUE)
1506                                        return false;
1507                                for (j = 1; j < (k == 0 ? patternWidth : patternHeight) - 1; j++)
1508                                {
1509                                        final Point2dImpl c = k == 0 ? outCorners.get(i * patternWidth + j) :
1510                                                outCorners.get(j * patternWidth + i);
1511                                        final float t = ((c.x - a.x) * dx0 + (c.y - a.y) * dy0) / (dx0 * dx0 + dy0 * dy0);
1512                                        if (t < prevt || t > 1)
1513                                                return false;
1514                                        prevt = t;
1515                                }
1516                        }
1517                }
1518
1519                return true;
1520        }
1521
1522        /**
1523         * Draw the predicted chessboard corners from the last call to
1524         * {@link #analyseImage(FImage)} on the given image.
1525         *
1526         * @param image
1527         *            the image to draw on
1528         */
1529        public void drawChessboardCorners(MBFImage image) {
1530                drawChessboardCorners(image, patternWidth, patternHeight, outCorners, found);
1531        }
1532
1533        /**
1534         * Draw the given chessboard corners from on the given image.
1535         *
1536         * @param image
1537         *            the image to draw on
1538         * @param patternWidth
1539         *            the chessboard pattern width
1540         * @param patternHeight
1541         *            the chessboard pattern height
1542         * @param corners
1543         *            the corners
1544         * @param found
1545         *            true if the corners were found
1546         */
1547        public static void drawChessboardCorners(MBFImage image, int patternWidth, int patternHeight,
1548                        List<? extends Point2d> corners, boolean found)
1549        {
1550                final int radius = 4;
1551
1552                if (!found) {
1553                        final Float[] color = RGBColour.RGB(0, 0, 255);
1554
1555                        for (int i = 0; i < corners.size(); i++)
1556                        {
1557                                final Point2d pt = corners.get(i);
1558                                image.drawLine(new Point2dImpl(pt.getX() - radius, pt.getY() - radius),
1559                                                new Point2dImpl(pt.getX() + radius, pt.getY() + radius), 1, color);
1560                                image.drawLine(new Point2dImpl(pt.getX() - radius, pt.getY() + radius),
1561                                                new Point2dImpl(pt.getX() + radius, pt.getY() - radius), 1, color);
1562                                image.drawShape(new Circle(pt, radius), 1, color);
1563                        }
1564                } else {
1565                        Point2d prevPt = new Point2dImpl();
1566                        final Float[][] lineColours =
1567                                {
1568                                        RGBColour.RGB(0, 0, 255),
1569                                        RGBColour.RGB(0, 128, 255),
1570                                        RGBColour.RGB(0, 200, 200),
1571                                        RGBColour.RGB(0, 255, 0),
1572                                        RGBColour.RGB(200, 200, 0),
1573                                        RGBColour.RGB(255, 0, 0),
1574                                        RGBColour.RGB(255, 0, 255)
1575                                };
1576
1577                        for (int y = 0, i = 0; y < patternHeight; y++) {
1578                                final Float[] color = lineColours[y % lineColours.length];
1579
1580                                for (int x = 0; x < patternWidth; x++, i++) {
1581                                        final Point2d pt = corners.get(i);
1582
1583                                        if (i != 0) {
1584                                                image.drawLine(prevPt, pt, 1, color);
1585                                        }
1586
1587                                        image.drawLine(new Point2dImpl(pt.getX() - radius, pt.getY() - radius),
1588                                                        new Point2dImpl(pt.getX() + radius, pt.getY() + radius), 1, color);
1589                                        image.drawLine(new Point2dImpl(pt.getX() - radius, pt.getY() + radius),
1590                                                        new Point2dImpl(pt.getX() + radius, pt.getY() - radius), 1, color);
1591                                        image.drawShape(new Circle(pt, radius), 1, color);
1592
1593                                        prevPt = pt;
1594                                }
1595                        }
1596                }
1597        }
1598
1599        /**
1600         * Simple test program
1601         *
1602         * @param args
1603         * @throws IOException
1604         */
1605        public static void main(String[] args) throws IOException {
1606                final ChessboardCornerFinder fcc = new ChessboardCornerFinder(9, 6,
1607                                Options.FILTER_QUADS, Options.FAST_CHECK, Options.ADAPTIVE_THRESHOLD);
1608                VideoDisplay.createVideoDisplay(new VideoCapture(640, 480)).addVideoListener(new VideoDisplayAdapter<MBFImage>()
1609                                {
1610                        @Override
1611                        public void beforeUpdate(MBFImage frame) {
1612                                fcc.analyseImage(frame.flatten());
1613                                fcc.drawChessboardCorners(frame);
1614                        }
1615                                });
1616        }
1617}