/**
 * Copyright (c) 2011, The University of Southampton and the individual contributors.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 *   *  Redistributions of source code must retain the above copyright notice,
 *      this list of conditions and the following disclaimer.
 *
 *   *  Redistributions in binary form must reproduce the above copyright notice,
 *      this list of conditions and the following disclaimer in the documentation
 *      and/or other materials provided with the distribution.
 *
 *   *  Neither the name of the University of Southampton nor the names of its
 *      contributors may be used to endorse or promote products derived from this
 *      software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
package org.openimaj.image.camera.calibration;

import gnu.trove.map.hash.TIntIntHashMap;
import gnu.trove.map.hash.TObjectIntHashMap;

import java.io.IOException;
import java.util.ArrayDeque;
import java.util.ArrayList;
import java.util.Collections;
import java.util.Deque;
import java.util.EnumSet;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import org.apache.log4j.Logger;
import org.openimaj.algorithm.iterative.MinEpsilonOrMaxIterations;
import org.openimaj.image.FImage;
import org.openimaj.image.MBFImage;
import org.openimaj.image.analyser.ImageAnalyser;
import org.openimaj.image.colour.RGBColour;
import org.openimaj.image.contour.Contour;
import org.openimaj.image.contour.SuzukiContourProcessor;
import org.openimaj.image.feature.local.interest.SubPixelCorners;
import org.openimaj.image.processing.algorithm.EqualisationProcessor;
import org.openimaj.image.processing.algorithm.MaxFilter;
import org.openimaj.image.processing.algorithm.MeanCenter;
import org.openimaj.image.processing.threshold.AdaptiveLocalThresholdMean;
import org.openimaj.math.geometry.point.Point2d;
import org.openimaj.math.geometry.point.Point2dImpl;
import org.openimaj.math.geometry.point.PointList;
import org.openimaj.math.geometry.shape.Circle;
import org.openimaj.math.geometry.shape.Polygon;
import org.openimaj.math.geometry.shape.Rectangle;
import org.openimaj.util.pair.FloatIntPair;
import org.openimaj.video.VideoDisplay;
import org.openimaj.video.VideoDisplayAdapter;
import org.openimaj.video.capture.VideoCapture;

/**
 * This is a port/reworking of the OpenCV chessboard corner finder algorithm to
 * OpenIMAJ. The key image-processing and geometry algorithms use the OpenIMAJ
 * equivalents, but the code to extract the board and assess the correctness is
 * purely based on a (slightly sanitised) port of the original OpenCV code.
 * <p>
 * This is improved variant of chessboard corner detection algorithm that uses a
 * graph of connected quads. It is based on the code contributed by Vladimir
 * Vezhnevets and Philip Gruebele. Here is the copyright notice from the
 * original Vladimir's code:
 * <p>
 * The algorithms developed and implemented by Vezhnevets Vldimir aka Dead Moroz
 * (vvp@graphics.cs.msu.ru) See <a
 * href="http://graphics.cs.msu.su/en/research/calibration/opencv.html"
 * >http://graphics.cs.msu.su/en/research/calibration/opencv.html</a> for
 * detailed information.
 * <p>
 * Reliability additions and modifications made by Philip Gruebele.
 * <p>
 * Some further improvements for detection of partially occluded boards at
 * non-ideal lighting conditions have been made by Alex Bovyrin and Kurt
 * Kolonige.
 *
 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
 */
public class ChessboardCornerFinder implements ImageAnalyser<FImage> {
        private static final class Corner extends Point2dImpl {
                int row; // Board row index
                int count; // Number of neighbor corners
                Corner[] neighbours = new Corner[4];

                public Corner(Point2d point2d) {
                        super(point2d);
                }

                FloatIntPair meanDist()
                {
                        float sum = 0;
                        int n = 0;
                        for (int i = 0; i < 4; i++)
                        {
                                if (neighbours[i] != null)
                                {
                                        final float dx = neighbours[i].x - x;
                                        final float dy = neighbours[i].y - y;
                                        sum += Math.sqrt(dx * dx + dy * dy);
                                        n++;
                                }
                        }
                        return new FloatIntPair(sum / Math.max(n, 1), n);
                }
        }

        private static final class Quad {
                /**
                 * The group that the quad belongs
                 */
                int groupIdx = -1;

                /**
                 * The corners
                 */
                Corner[] corners = new Corner[4];

                /**
                 * Minimum edge length in pixels
                 */
                float minEdgeLength;

                /**
                 * Actual number of neighbours
                 */
                int count;

                /**
                 * The neighbours
                 */
                Quad[] neighbours = new Quad[4];

                /**
                 * has the quad been sorted?
                 */
                boolean ordered;
                /**
                 * The row position
                 */
                int row;
                /**
                 * The column position
                 */
                int col;
        }

        private static final Logger logger = Logger.getLogger(ChessboardCornerFinder.class);

        private static final int MIN_DILATIONS = 0;
        private static final int MAX_DILATIONS = 7;
        private static final SubPixelCorners SUB_PIX_CORNERS = new SubPixelCorners(2, 2,
                        new MinEpsilonOrMaxIterations(0.1, 15));

        /**
         * Should filtering be enabled
         */
        boolean filterQuads = true;

        /**
         * Minimum area of quad if filtering
         */
        private double minSize = 25;

        /**
         * Maximum approximation distance
         */
        private int maxApproxLevel = 7;

        /**
         * Pre-process with histogram equalisation
         */
        boolean histogramEqualise = false;

        /**
         * Should mean adaptive thresholding be used?
         */
        boolean adaptiveThreshold = false;

        /**
         * Set if a fast check for the pattern be performed to bail early
         */
        final FastChessboardDetector fastDetector;

        /**
         * Number of blocks across the pattern
         */
        private int patternWidth;

        /**
         * Number of blocks down the pattern
         */
        private int patternHeight;

        /**
         * Was the complete pattern found?
         */
        boolean found = false;

        /**
         * The final corners
         */
        private List<Point2dImpl> outCorners = new ArrayList<Point2dImpl>();

        /**
         * Options for controlling how the corner finder works
         *
         * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
         *
         */
        public static enum Options {
                /**
                 * Apply filtering to remove unlikely quads from detection
                 */
                FILTER_QUADS,
                /**
                 * Pre-process the image by performing histogram equalisation
                 */
                HISTOGRAM_EQUALISE,
                /**
                 * Perform adaptive (mean local) thresholding, rather than global
                 */
                ADAPTIVE_THRESHOLD,
                /**
                 * Perform the fast check to detect a pattern and bail early
                 */
                FAST_CHECK;
        }

        /**
         * Construct with the given pattern size and options set.
         *
         * @param patternWidth
         *            the pattern width
         * @param patternHeight
         *            the pattern height
         * @param opts
         *            the options
         */
        public ChessboardCornerFinder(int patternWidth, int patternHeight, Options... opts)
        {
                this.patternWidth = patternWidth;
                this.patternHeight = patternHeight;

                final EnumSet<Options> es = EnumSet.noneOf(Options.class);
                for (final Options o : opts)
                        es.add(o);

                this.filterQuads = es.contains(Options.FILTER_QUADS);
                this.histogramEqualise = es.contains(Options.HISTOGRAM_EQUALISE);
                this.adaptiveThreshold = es.contains(Options.ADAPTIVE_THRESHOLD);

                if (es.contains(Options.FAST_CHECK))
                        fastDetector = new FastChessboardDetector(patternWidth, patternHeight);
                else
                        fastDetector = null;
        }

        @Override
        public void analyseImage(FImage image) {
                // reset state:
                this.found = false;
                this.outCorners.clear();

                if (histogramEqualise)
                        image = image.process(new EqualisationProcessor());

                int prevSqrSize = 0;

                if (fastDetector != null) {
                        fastDetector.analyseImage(image);

                        if (!fastDetector.chessboardDetected()) {
                                return;
                        }
                }

                for (int k = 0; k < 6; k++) {
                        for (int dilations = MIN_DILATIONS; dilations < MAX_DILATIONS; dilations++) {
                                if (found)
                                        break;

                                FImage threshImg;
                                if (adaptiveThreshold) {
                                        final int blockSize = (int) (Math.round(prevSqrSize == 0 ?
                                                        Math.min(image.width, image.height) * (k % 2 == 0 ? 0.2 : 0.1) : prevSqrSize * 2) | 1);

                                        // adaptive mean thresholding
                                        threshImg = image.process(new AdaptiveLocalThresholdMean(blockSize, (k / 2) * 5));
                                        if (dilations > 0) {
                                                MaxFilter.filter(threshImg, dilations - 1);
                                        }
                                } else {
                                        threshImg = image.clone().threshold(MeanCenter.patchMean(image.pixels) - 10f / 255f);
                                        MaxFilter.filter(threshImg, dilations);
                                }

                                // draw a border to allow us to find quads that go off the edge
                                // of the image
                                threshImg.drawShape(new Rectangle(1, 1, threshImg.width - 3, threshImg.height - 3), 3, 1f);

                                final List<Quad> quads = extractQuads(threshImg);

                                // not found...
                                if (quads.size() <= 0)
                                        continue;

                                findQuadNeighbours(quads);

                                for (int groupIdx = 0;; groupIdx++)
                                {
                                        final List<Quad> quadGroup = findConnectedQuads(quads, groupIdx);
                                        int count = quadGroup.size();

                                        final int icount = count;
                                        if (count == 0)
                                                break;

                                        // order the quad corners globally
                                        // maybe delete or add some
                                        logger.trace("Starting ordering of inner quads");
                                        count = orderFoundConnectedQuads(quadGroup, quads);
                                        logger.trace(String.format("Orig count: %d  After ordering: %d", icount, count));

                                        if (count == 0)
                                                continue; // haven't found inner quads

                                        // If count is more than it should be, this will remove
                                        // those quads which cause maximum deviation from a nice
                                        // square pattern.
                                        count = cleanFoundConnectedQuads(quadGroup);
                                        logger.trace(String.format("Connected group: %d  orig count: %d cleaned: %d", groupIdx, icount,
                                                        count));

                                        final List<Corner> cornerGroup = new ArrayList<Corner>();
                                        count = checkQuadGroup(quadGroup, cornerGroup);
                                        logger.trace(String.format("Connected group: %d  count: %d  cleaned: %d", groupIdx, icount, count));

                                        int n = count > 0 ? patternWidth * patternHeight : -count;
                                        n = Math.min(n, patternWidth * patternHeight);
                                        float sumDist = 0;
                                        int total = 0;

                                        for (int i = 0; i < n; i++)
                                        {
                                                final FloatIntPair pair = cornerGroup.get(i).meanDist();
                                                final float avgi = pair.first;
                                                final int ni = pair.second;
                                                sumDist += avgi * ni;
                                                total += ni;
                                        }
                                        prevSqrSize = Math.round(sumDist / Math.max(total, 1));

                                        if (count > 0 || (outCorners.size() > 0 && -count > outCorners.size()))
                                        {
                                                // copy corners to output array
                                                outCorners.clear();
                                                for (int i = 0; i < n; i++)
                                                        outCorners.add(new Point2dImpl(cornerGroup.get(i)));

                                                if (count == patternWidth * patternHeight && checkBoardMonotony())
                                                {
                                                        found = true;
                                                        break;
                                                }
                                        }
                                } // grp
                        } // dilations
                } // k

                if (found)
                        found = checkBoardMonotony();

                // check that none of the found corners is too close to the image
                // boundary
                if (found)
                {
                        final int BORDER = 8;
                        int k;
                        for (k = 0; k < patternWidth * patternHeight; k++)
                        {
                                if (outCorners.get(k).x <= BORDER || outCorners.get(k).x > image.width - BORDER ||
                                                outCorners.get(k).y <= BORDER || outCorners.get(k).y > image.height - BORDER)
                                        break;
                        }

                        found = k == patternWidth * patternHeight;
                }

                if (found && patternHeight % 2 == 0 && patternWidth % 2 == 0)
                {
                        final int lastRow = (patternHeight - 1) * patternWidth;
                        final double dy0 = outCorners.get(lastRow).y - outCorners.get(0).y;
                        if (dy0 < 0)
                        {
                                int i;
                                final int n = patternWidth * patternHeight;
                                for (i = 0; i < n / 2; i++)
                                {
                                        Collections.swap(outCorners, i, n - i - 1);
                                }
                        }
                }

                if (found) {
                        outCorners = SUB_PIX_CORNERS.findSubPixCorners(image, outCorners);
                }
        }

        /**
         * Returns corners in clockwise order corners don't necessarily start at
         * same position on quad (e.g., top left corner)
         *
         * @param threshImg
         *            the binary image
         * @return the extracted Quads
         */
        private List<Quad> extractQuads(final FImage threshImg) {
                // if filtering is enabled, we try to guess the board contour containing
                // all the relevant quads
                final TObjectIntHashMap<Contour> counters = new TObjectIntHashMap<Contour>();
                // cornerList is the list of valid quads (prior to
                // filtering out those with the wrong parent if applicable)
                final List<Corner[]> cornerList = new ArrayList<Corner[]>();
                final Map<Corner[], Contour> parentMapping = new HashMap<Corner[], Contour>();
                Contour board = null;

                // extract contours
                final Contour contours = SuzukiContourProcessor.findContours(threshImg);

                for (final Contour c : contours.contourIterable()) {
                        final Rectangle bounds = c.calculateRegularBoundingBox();

                        // skip small regions
                        if (!(c.isHole() && bounds.width * bounds.height >= minSize))
                                continue;

                        // try and make an approximated polygon with 4 vertices
                        Polygon p = null;
                        for (int approxLevel = 1; approxLevel <= maxApproxLevel; approxLevel++) {
                                p = c.reduceVertices(approxLevel);
                                if (p.nVertices() == 4)
                                        break;
                        }

                        // test polygon for correctness
                        if (p != null && p.nVertices() == 4 && p.isConvex() && p.hasNoInnerPolygons()) {
                                final double perimeter = p.calculatePerimeter();
                                final double area = p.calculateArea();

                                final Corner[] pt = new Corner[4];
                                for (int i = 0; i < 4; i++)
                                        pt[i] = new Corner(p.points.get(i));

                                float dx = pt[0].x - pt[2].x;
                                float dy = pt[0].y - pt[2].y;
                                final double d1 = Math.sqrt(dx * dx + dy * dy);

                                dx = pt[1].x - pt[3].x;
                                dy = pt[1].y - pt[3].y;
                                final double d2 = Math.sqrt(dx * dx + dy * dy);

                                dx = pt[0].x - pt[1].x;
                                dy = pt[0].y - pt[1].y;
                                final double d3 = Math.sqrt(dx * dx + dy * dy);
                                dx = pt[1].x - pt[2].x;
                                dy = pt[1].y - pt[2].y;
                                final double d4 = Math.sqrt(dx * dx + dy * dy);

                                if (!filterQuads
                                                ||
                                                (d3 * 4 > d4 && d4 * 4 > d3 && d3 * d4 < area * 1.5 && area > minSize && d1 >= 0.15 * perimeter && d2 >= 0.15 * perimeter))
                                {
                                        final Contour parent = c.parent;
                                        counters.adjustOrPutValue(parent, 1, 1);

                                        // basic idea is that the board should have more holes in it
                                        // than anything else in the scene
                                        if (board == null || counters.get(board) < counters.get(parent))
                                                board = parent;

                                        cornerList.add(pt);
                                        parentMapping.put(pt, c.parent);
                                }
                        }
                }

                final List<Quad> quads = new ArrayList<Quad>();
                for (int i = 0; i < cornerList.size(); i++) {
                        final Corner[] pts = cornerList.get(i);

                        // reject regions that don't belong to the predicted board
                        if (filterQuads && parentMapping.get(pts) != board)
                                continue;

                        final Quad q = new Quad();
                        q.corners = pts;

                        q.minEdgeLength = Float.MAX_VALUE;
                        for (int j = 0; j < 4; j++) {
                                final float dx = pts[j].x - pts[(j + 1) & 3].x;
                                final float dy = pts[j].y - pts[(j + 1) & 3].y;
                                final float d = dx * dx + dy * dy;
                                if (q.minEdgeLength > d)
                                        q.minEdgeLength = d;
                        }

                        quads.add(q);
                }
                return quads;
        }

        /**
         * Find the neighbouring quads for each quad
         *
         * @param quads
         *            the quads
         */
        private void findQuadNeighbours(List<Quad> quads)
        {
                final int quadCount = quads.size();
                final float threshScale = 1.f;

                // find quad neighbours
                for (int idx = 0; idx < quads.size(); idx++)
                {
                        final Quad curQuad = quads.get(idx);

                        // choose the points of the current quadrangle that are close to
                        // some points of the other quadrangles
                        // (it can happen for split corners (due to dilation) of the
                        // checker board). Search only in other quadrangles!

                        // for each corner of this quadrangle
                        for (int i = 0; i < 4; i++)
                        {
                                float minDist = Float.MAX_VALUE;
                                int closestCornerIdx = -1;
                                Quad closestQuad = null;
                                Corner closestCorner = null;

                                if (curQuad.neighbours[i] != null)
                                        continue;

                                final Corner pt = curQuad.corners[i];

                                float dx;
                                float dy;
                                float dist;
                                // find the closest corner in all other quadrangles
                                for (int k = 0; k < quadCount; k++)
                                {
                                        if (k == idx)
                                                continue;

                                        for (int j = 0; j < 4; j++)
                                        {
                                                if (quads.get(k).neighbours[j] != null)
                                                        continue;

                                                dx = pt.x - quads.get(k).corners[j].x;
                                                dy = pt.y - quads.get(k).corners[j].y;
                                                dist = dx * dx + dy * dy;

                                                if (dist < minDist &&
                                                                dist <= curQuad.minEdgeLength * threshScale &&
                                                                dist <= quads.get(k).minEdgeLength * threshScale)
                                                {
                                                        // check edge lengths, make sure they're compatible
                                                        // edges that are different by more than 1:4 are
                                                        // rejected
                                                        final float ediff = curQuad.minEdgeLength - quads.get(k).minEdgeLength;
                                                        if (ediff > 32 * curQuad.minEdgeLength ||
                                                                        ediff > 32 * quads.get(k).minEdgeLength)
                                                        {
                                                                logger.trace("Incompatible edge lengths");
                                                                continue;
                                                        }
                                                        closestCornerIdx = j;
                                                        closestQuad = quads.get(k);
                                                        minDist = dist;
                                                }
                                        }
                                }

                                // we found a matching corner point?
                                if (closestCornerIdx >= 0 && minDist < Float.MAX_VALUE)
                                {
                                        // If another point from our current quad is closer to the
                                        // found corner
                                        // than the current one, then we don't count this one after
                                        // all.
                                        // This is necessary to support small squares where
                                        // otherwise the wrong
                                        // corner will get matched to closest_quad;
                                        closestCorner = closestQuad.corners[closestCornerIdx];

                                        int j;
                                        for (j = 0; j < 4; j++)
                                        {
                                                if (curQuad.neighbours[j] == closestQuad)
                                                        break;

                                                dx = closestCorner.x - curQuad.corners[j].x;
                                                dy = closestCorner.y - curQuad.corners[j].y;

                                                if (dx * dx + dy * dy < minDist)
                                                        break;
                                        }

                                        if (j < 4 || curQuad.count >= 4 || closestQuad.count >= 4)
                                                continue;

                                        // Check that each corner is a neighbor of different quads
                                        for (j = 0; j < closestQuad.count; j++)
                                        {
                                                if (closestQuad.neighbours[j] == curQuad)
                                                        break;
                                        }
                                        if (j < closestQuad.count)
                                                continue;

                                        // check whether the closest corner to closestCorner
                                        // is different from curQuad.corners[i].pt
                                        int k;
                                        for (k = 0; k < quadCount; k++)
                                        {
                                                final Quad q = quads.get(k);
                                                if (k == idx || q == closestQuad)
                                                        continue;

                                                for (j = 0; j < 4; j++)
                                                        if (q.neighbours[j] == null)
                                                        {
                                                                dx = closestCorner.x - q.corners[j].x;
                                                                dy = closestCorner.y - q.corners[j].y;
                                                                dist = dx * dx + dy * dy;
                                                                if (dist < minDist)
                                                                        break;
                                                        }
                                                if (j < 4)
                                                        break;
                                        }

                                        if (k < quadCount)
                                                continue;

                                        closestCorner.x = (pt.x + closestCorner.x) * 0.5f;
                                        closestCorner.y = (pt.y + closestCorner.y) * 0.5f;

                                        // We've found one more corner - remember it
                                        curQuad.count++;
                                        curQuad.neighbours[i] = closestQuad;
                                        curQuad.corners[i] = closestCorner;

                                        closestQuad.count++;
                                        closestQuad.neighbours[closestCornerIdx] = curQuad;
                                }
                        }
                }
        }

        /**
         * Was a chessboard found in the last call to {@link #analyseImage(FImage)}?
         *
         * @return true if found; false otherwise
         */
        public boolean isFound() {
                return found;
        }

        /**
         * Get any corners detected by the last call to
         * {@link #analyseImage(FImage)}.
         *
         * @return the corners
         */
        public List<Point2dImpl> getCorners() {
                return this.outCorners;
        }

        /**
         * Find groups of connected quads. This searches for the first un-labelled
         * quad and then finds the connected ones.
         *
         * @param quads
         *            the quads
         * @param groupIdx
         *            the group index
         * @return the quads belonging to the group
         */
        private List<Quad> findConnectedQuads(List<Quad> quads, int groupIdx)
        {
                final Deque<Quad> stack = new ArrayDeque<Quad>();
                int i;
                final int quadCount = quads.size();
                final List<Quad> outGroup = new ArrayList<Quad>();

                // Scan the array for a first unlabeled quad
                for (i = 0; i < quadCount; i++)
                {
                        if (quads.get(i).count > 0 && quads.get(i).groupIdx < 0)
                                break;
                }

                // Recursively find a group of connected quads starting from the seed
                // quad[i]
                if (i < quadCount)
                {
                        Quad q = quads.get(i);
                        stack.push(q);
                        outGroup.add(q);
                        q.groupIdx = groupIdx;
                        q.ordered = false;

                        while (stack.size() > 0)
                        {
                                q = stack.pop();
                                for (i = 0; i < 4; i++)
                                {
                                        final Quad neighbour = q.neighbours[i];
                                        if (neighbour != null && neighbour.count > 0 && neighbour.groupIdx < 0)
                                        {
                                                stack.push(neighbour);
                                                outGroup.add(neighbour);
                                                neighbour.groupIdx = groupIdx;
                                                neighbour.ordered = false;
                                        }
                                }
                        }
                }

                return outGroup;
        }

        /**
         * order a group of connected quads order of corners: 0 is top left
         * clockwise from there note: "top left" is nominal, depends on initial
         * ordering of starting quad but all other quads are ordered consistently
         *
         * can change the number of quads in the group can add quads, so we need to
         * have quad/corner arrays passed in
         */
        private int orderFoundConnectedQuads(List<Quad> quads, List<Quad> allQuads)
        {
                final Deque<Quad> stack = new ArrayDeque<Quad>();

                int quadCount = quads.size();

                // first find an interior quad
                Quad start = null;
                for (int i = 0; i < quadCount; i++)
                {
                        if (quads.get(i).count == 4)
                        {
                                start = quads.get(i);
                                break;
                        }
                }

                if (start == null)
                        return 0; // no 4-connected quad

                // start with first one, assign rows/cols
                int rowMin = 0, colMin = 0, rowMax = 0, colMax = 0;

                final TIntIntHashMap colHist = new TIntIntHashMap();
                final TIntIntHashMap rowHist = new TIntIntHashMap();

                stack.push(start);
                start.row = 0;
                start.col = 0;
                start.ordered = true;

                // Recursively order the quads so that all position numbers (e.g.,
                // 0,1,2,3) are in the at the same relative corner (e.g., lower right).

                while (stack.size() > 0)
                {
                        final Quad q = stack.pop();
                        int col = q.col;
                        int row = q.row;
                        colHist.adjustOrPutValue(col, 1, 1);
                        rowHist.adjustOrPutValue(row, 1, 1);

                        // check min/max
                        if (row > rowMax)
                                rowMax = row;
                        if (row < rowMin)
                                rowMin = row;
                        if (col > colMax)
                                colMax = col;
                        if (col < colMin)
                                colMin = col;

                        for (int i = 0; i < 4; i++)
                        {
                                final Quad neighbour = q.neighbours[i];
                                switch (i) // adjust col, row for this quad
                                { // start at top left, go clockwise
                                case 0:
                                        row--;
                                        col--;
                                        break;
                                case 1:
                                        col += 2;
                                        break;
                                case 2:
                                        row += 2;
                                        break;
                                case 3:
                                        col -= 2;
                                        break;
                                }

                                // just do inside quads
                                if (neighbour != null && neighbour.ordered == false && neighbour.count == 4)
                                {
                                        orderQuad(neighbour, q.corners[i], (i + 2) % 4); // set in
                                        // order
                                        neighbour.ordered = true;
                                        neighbour.row = row;
                                        neighbour.col = col;
                                        stack.push(neighbour);
                                }
                        }
                }

                // analyze inner quad structure
                int w = patternWidth - 1;
                int h = patternHeight - 1;
                final int drow = rowMax - rowMin + 1;
                final int dcol = colMax - colMin + 1;

                // normalize pattern and found quad indices
                if ((w > h && dcol < drow) ||
                                (w < h && drow < dcol))
                {
                        h = patternWidth - 1;
                        w = patternHeight - 1;
                }

                logger.trace(String.format("Size: %dx%d  Pattern: %dx%d", dcol, drow, w, h));

                // check if there are enough inner quads
                if (dcol < w || drow < h) // found enough inner quads?
                {
                        logger.trace("Too few inner quad rows/cols");
                        return 0; // no, return
                }

                // check edges of inner quads
                // if there is an outer quad missing, fill it in
                // first order all inner quads
                int found = 0;
                for (int i = 0; i < quadCount; i++)
                {
                        if (quads.get(i).count == 4)
                        { // ok, look at neighbours
                                int col = quads.get(i).col;
                                int row = quads.get(i).row;
                                for (int j = 0; j < 4; j++)
                                {
                                        switch (j) // adjust col, row for this quad
                                        { // start at top left, go clockwise
                                        case 0:
                                                row--;
                                                col--;
                                                break;
                                        case 1:
                                                col += 2;
                                                break;
                                        case 2:
                                                row += 2;
                                                break;
                                        case 3:
                                                col -= 2;
                                                break;
                                        }
                                        final Quad neighbour = quads.get(i).neighbours[j];
                                        if (neighbour != null && !neighbour.ordered && // is it an
                                                        // inner
                                                        // quad?
                                                        col <= colMax && col >= colMin &&
                                                        row <= rowMax && row >= rowMin)
                                        {
                                                // if so, set in order
                                                logger.trace(String.format("Adding inner: col: %d  row: %d", col, row));
                                                found++;
                                                orderQuad(neighbour, quads.get(i).corners[j], (j + 2) % 4);
                                                neighbour.ordered = true;
                                                neighbour.row = row;
                                                neighbour.col = col;
                                        }
                                }
                        }
                }

                // if we have found inner quads, add corresponding outer quads,
                // which are missing
                if (found > 0)
                {
                        logger.trace(String.format("Found %d inner quads not connected to outer quads, repairing", found));
                        for (int i = 0; i < quadCount; i++)
                        {
                                if (quads.get(i).count < 4 && quads.get(i).ordered)
                                {
                                        final int added = addOuterQuad(quads.get(i), quads, allQuads);
                                        quadCount += added;
                                }
                        }
                }

                // final trimming of outer quads
                if (dcol == w && drow == h) // found correct inner quads
                {
                        logger.trace("Inner bounds ok, check outer quads");
                        int rcount = quadCount;
                        for (int i = quadCount - 1; i >= 0; i--) // eliminate any quad not
                                // connected to
                                // an ordered quad
                        {
                                if (quads.get(i).ordered == false)
                                {
                                        boolean outer = false;
                                        for (int j = 0; j < 4; j++) // any neighbours that are
                                                // ordered?
                                        {
                                                if (quads.get(i).neighbours[j] != null && quads.get(i).neighbours[j].ordered)
                                                        outer = true;
                                        }
                                        if (!outer) // not an outer quad, eliminate
                                        {
                                                logger.trace(String.format("Removing quad %d", i));
                                                removeQuadFromGroup(quads, quads.get(i));
                                                rcount--;
                                        }
                                }

                        }
                        return rcount;
                }

                return 0;
        }

        /**
         * put quad into correct order, where <code>corner</code> has value
         * <code<common</code>
         *
         * @param quad
         *            the quad to sort
         * @param corner
         *            the corner
         * @param common
         *            the common vertex
         */
        private void orderQuad(Quad quad, Corner corner, int common)
        {
                // find the corner
                int tc;
                for (tc = 0; tc < 4; tc++)
                        if (quad.corners[tc].x == corner.x &&
                        quad.corners[tc].y == corner.y)
                                break;

                // set corner order
                // shift
                while (tc != common)
                {
                        // shift by one
                        final Corner tempc = quad.corners[3];
                        final Quad tempq = quad.neighbours[3];
                        for (int i = 3; i > 0; i--)
                        {
                                quad.corners[i] = quad.corners[i - 1];
                                quad.neighbours[i] = quad.neighbours[i - 1];
                        }
                        quad.corners[0] = tempc;
                        quad.neighbours[0] = tempq;
                        tc++;
                        tc = tc % 4;
                }
        }

        /*
         * add an outer quad
         *
         * looks for the neighbor of <code>quad</code> that isn't present, tries to
         * add it in. <code>quad</code> is ordered
         *
         * @param quad the quad
         *
         * @param quads all quad group
         *
         * @param allQuads all the quads
         *
         * @return
         */
        private int addOuterQuad(final Quad quad, List<Quad> quads, List<Quad> allQuads)
        {
                int added = 0;
                for (int i = 0; i < 4; i++) // find no-neighbor corners
                {
                        if (quad.neighbours[i] == null) // ok, create and add neighbor
                        {
                                final int j = (i + 2) % 4;
                                logger.trace("Adding quad as neighbor 2");
                                final Quad q = new Quad();
                                allQuads.add(q);
                                added++;
                                quads.add(q);

                                // set neighbor and group id
                                quad.neighbours[i] = q;
                                quad.count += 1;
                                q.neighbours[j] = quad;
                                q.groupIdx = quad.groupIdx;
                                q.count = 1; // number of neighbours
                                q.ordered = false;
                                q.minEdgeLength = quad.minEdgeLength;

                                // make corners of new quad
                                // same as neighbor quad, but offset
                                Corner pt = quad.corners[i];
                                final float dx = pt.x - quad.corners[j].x;
                                final float dy = pt.y - quad.corners[j].y;
                                for (int k = 0; k < 4; k++)
                                {
                                        final Corner corner = new Corner(pt);
                                        pt = quad.corners[k];
                                        q.corners[k] = corner;
                                        corner.x += dx;
                                        corner.y += dy;
                                }
                                // have to set exact corner
                                q.corners[j] = quad.corners[i];

                                // now find other neighbor and add it, if possible
                                if (quad.neighbours[(i + 3) % 4] != null &&
                                                quad.neighbours[(i + 3) % 4].ordered &&
                                                quad.neighbours[(i + 3) % 4].neighbours[i] != null &&
                                                quad.neighbours[(i + 3) % 4].neighbours[i].ordered)
                                {
                                        final Quad qn = quad.neighbours[(i + 3) % 4].neighbours[i];
                                        q.count = 2;
                                        q.neighbours[(j + 1) % 4] = qn;
                                        qn.neighbours[(i + 1) % 4] = q;
                                        qn.count += 1;
                                        // have to set exact corner
                                        q.corners[(j + 1) % 4] = qn.corners[(i + 1) % 4];
                                }
                        }
                }
                return added;
        }

        /**
         * remove quad from quad group
         */
        private void removeQuadFromGroup(final List<Quad> quads, Quad q0)
        {
                final int count = quads.size();
                // remove any references to this quad as a neighbor
                for (int i = 0; i < count; i++)
                {
                        final Quad q = quads.get(i);
                        for (int j = 0; j < 4; j++)
                        {
                                if (q.neighbours[j] == q0)
                                {
                                        q.neighbours[j] = null;
                                        q.count--;
                                        for (int k = 0; k < 4; k++)
                                                if (q0.neighbours[k] == q)
                                                {
                                                        q0.neighbours[k] = null;
                                                        q0.count--;
                                                        break;
                                                }
                                        break;
                                }
                        }
                }

                quads.remove(q0);
        }

        /**
         * if we found too many connect quads, remove those which probably do not
         * belong.
         *
         * @param quadGroup
         *            the group of quads
         * @return the new group size
         */
        private int cleanFoundConnectedQuads(List<Quad> quadGroup)
        {
                int quadCount = quadGroup.size();
                final Point2dImpl center = new Point2dImpl();

                // number of quads this pattern should contain
                final int count = ((patternWidth + 1) * (patternHeight + 1) + 1) / 2;

                // if we have more quadrangles than we should,
                // try to eliminate duplicates or ones which don't belong to the pattern
                // rectangle...
                if (quadCount <= count)
                        return quadCount;

                // create an array of quadrangle centers
                final List<Point2dImpl> centers = new ArrayList<Point2dImpl>(quadCount);

                for (int i = 0; i < quadCount; i++)
                {
                        final Point2dImpl ci = new Point2dImpl();
                        final Quad q = quadGroup.get(i);

                        for (int j = 0; j < 4; j++)
                        {
                                final Point2dImpl pt = q.corners[j];
                                ci.x += pt.x;
                                ci.y += pt.y;
                        }

                        ci.x *= 0.25f;
                        ci.y *= 0.25f;

                        centers.add(ci);
                        center.x += ci.x;
                        center.y += ci.y;
                }
                center.x /= quadCount;
                center.y /= quadCount;

                // If we still have more quadrangles than we should,
                // we try to eliminate bad ones based on minimizing the bounding box.
                // We iteratively remove the point which reduces the size of
                // the bounding box of the blobs the most
                // (since we want the rectangle to be as small as possible)
                // remove the quadrange that causes the biggest reduction
                // in pattern size until we have the correct number
                for (; quadCount > count; quadCount--)
                {
                        double minBoxArea = Double.MAX_VALUE;
                        int minBoxAreaIndex = -1;
                        Quad q0, q;

                        // For each point, calculate box area without that point
                        for (int skip = 0; skip < quadCount; skip++)
                        {
                                // get bounding rectangle
                                final Point2dImpl temp = centers.get(skip); // temporarily make
                                // index 'skip' the
                                // same as
                                centers.set(skip, center); // pattern center (so it is not
                                // counted for convex hull)
                                final PointList pl = new PointList(centers);
                                final Polygon hull = pl.calculateConvexHull();
                                centers.set(skip, temp);
                                final double hullArea = hull.calculateArea();

                                // remember smallest box area
                                if (hullArea < minBoxArea)
                                {
                                        minBoxArea = hullArea;
                                        minBoxAreaIndex = skip;
                                }
                        }

                        q0 = quadGroup.get(minBoxAreaIndex);

                        // remove any references to this quad as a neighbor
                        for (int i = 0; i < quadCount; i++)
                        {
                                q = quadGroup.get(i);
                                for (int j = 0; j < 4; j++)
                                {
                                        if (q.neighbours[j] == q0)
                                        {
                                                q.neighbours[j] = null;
                                                q.count--;
                                                for (int k = 0; k < 4; k++)
                                                        if (q0.neighbours[k] == q)
                                                        {
                                                                q0.neighbours[k] = null;
                                                                q0.count--;
                                                                break;
                                                        }
                                                break;
                                        }
                                }
                        }

                        // remove the quad
                        quadCount--;
                        quadGroup.remove(minBoxAreaIndex);
                        centers.remove(minBoxAreaIndex);
                }

                return quadCount;
        }

        /**
         *
         */
        private int checkQuadGroup(List<Quad> quadGroup, List<Corner> outCorners)
        {
                final int ROW1 = 1000000;
                final int ROW2 = 2000000;
                final int ROW_ = 3000000;
                int result = 0;
                final int quadCount = quadGroup.size();
                int cornerCount = 0;
                final Corner[] corners = new Corner[quadCount * 4];

                int width = 0, height = 0;
                final int[] hist = { 0, 0, 0, 0, 0 };
                Corner first = null, first2 = null, right, cur, below, c;

                try {
                        // build dual graph, which vertices are internal quad corners
                        // and two vertices are connected iff they lie on the same quad edge
                        for (int i = 0; i < quadCount; i++)
                        {
                                final Quad q = quadGroup.get(i);

                                for (int j = 0; j < 4; j++)
                                {
                                        if (q.neighbours[j] != null)
                                        {
                                                final Corner a = q.corners[j], b = q.corners[(j + 1) & 3];
                                                // mark internal corners that belong to:
                                                // - a quad with a single neighbor - with ROW1,
                                                // - a quad with two neighbours - with ROW2
                                                // make the rest of internal corners with ROW_
                                                final int rowFlag = q.count == 1 ? ROW1 : q.count == 2 ? ROW2 : ROW_;

                                                if (a.row == 0)
                                                {
                                                        corners[cornerCount++] = a;
                                                        a.row = rowFlag;
                                                }
                                                else if (a.row > rowFlag)
                                                        a.row = rowFlag;

                                                if (q.neighbours[(j + 1) & 3] != null)
                                                {
                                                        if (a.count >= 4 || b.count >= 4)
                                                                throw new Exception();
                                                        for (int k = 0; k < 4; k++)
                                                        {
                                                                if (a.neighbours[k] == b)
                                                                        throw new Exception();
                                                                if (b.neighbours[k] == a)
                                                                        throw new Exception();
                                                        }
                                                        a.neighbours[a.count++] = b;
                                                        b.neighbours[b.count++] = a;
                                                }
                                        }
                                }
                        }

                        if (cornerCount != patternWidth * patternHeight)
                                throw new Exception();

                        for (int i = 0; i < cornerCount; i++)
                        {
                                final int n = corners[i].count;
                                assert (0 <= n && n <= 4);
                                hist[n]++;
                                if (first == null && n == 2)
                                {
                                        if (corners[i].row == ROW1)
                                                first = corners[i];
                                        else if (first2 == null && corners[i].row == ROW2)
                                                first2 = corners[i];
                                }
                        }

                        // start with a corner that belongs to a quad with a signle
                        // neighbor.
                        // if we do not have such, start with a corner of a quad with two
                        // neighbours.
                        if (first == null)
                                first = first2;

                        if (first == null || hist[0] != 0 || hist[1] != 0 || hist[2] != 4 ||
                                        hist[3] != (patternWidth + patternHeight) * 2 - 8)
                                throw new Exception();

                        cur = first;
                        right = below = null;
                        outCorners.add(cur);

                        for (int k = 0; k < 4; k++)
                        {
                                c = cur.neighbours[k];
                                if (c != null)
                                {
                                        if (right == null)
                                                right = c;
                                        else if (below == null)
                                                below = c;
                                }
                        }

                        if (right == null || (right.count != 2 && right.count != 3) ||
                                        below == null || (below.count != 2 && below.count != 3))
                                throw new Exception();

                        cur.row = 0;
                        first = below; // remember the first corner in the next row
                        // find and store the first row (or column)
                        while (true)
                        {
                                right.row = 0;
                                outCorners.add(right);

                                if (right.count == 2)
                                        break;
                                if (right.count != 3 || outCorners.size() >= Math.max(patternWidth, patternHeight))
                                        throw new Exception();
                                cur = right;

                                for (int k = 0; k < 4; k++)
                                {
                                        c = cur.neighbours[k];
                                        if (c != null && c.row > 0)
                                        {
                                                int kk;
                                                for (kk = 0; kk < 4; kk++)
                                                {
                                                        if (c.neighbours[kk] == below)
                                                                break;
                                                }
                                                if (kk < 4)
                                                        below = c;
                                                else
                                                        right = c;
                                        }
                                }
                        }

                        width = outCorners.size();
                        if (width == patternWidth)
                                height = patternHeight;
                        else if (width == patternHeight)
                                height = patternWidth;
                        else
                                throw new Exception();

                        // find and store all the other rows
                        for (int i = 1;; i++)
                        {
                                if (first == null)
                                        break;
                                cur = first;
                                first = null;
                                int j;
                                for (j = 0;; j++)
                                {
                                        cur.row = i;
                                        outCorners.add(cur);
                                        if (cur.count == 2 + (i < height - 1 ? 1 : 0) && j > 0)
                                                break;

                                        right = null;

                                        // find a neighbor that has not been processed yet
                                        // and that has a neighbor from the previous row
                                        for (int k = 0; k < 4; k++)
                                        {
                                                c = cur.neighbours[k];
                                                if (c != null && c.row > i)
                                                {
                                                        int kk;
                                                        for (kk = 0; kk < 4; kk++)
                                                        {
                                                                if (c.neighbours[kk] != null && c.neighbours[kk].row == i - 1)
                                                                        break;
                                                        }
                                                        if (kk < 4)
                                                        {
                                                                right = c;
                                                                if (j > 0)
                                                                        break;
                                                        }
                                                        else if (j == 0)
                                                                first = c;
                                                }
                                        }
                                        if (right == null)
                                                throw new Exception();
                                        cur = right;
                                }

                                if (j != width - 1)
                                        throw new Exception();
                        }

                        if (outCorners.size() != cornerCount)
                                throw new Exception();

                        // check if we need to transpose the board
                        if (width != patternWidth)
                        {
                                final int t = width;
                                width = height;
                                height = t;

                                for (int i = 0; i < cornerCount; i++)
                                        corners[i] = outCorners.get(i);

                                for (int i = 0; i < height; i++)
                                        for (int j = 0; j < width; j++)
                                                outCorners.set(i * width + j, corners[j * height + i]);
                        }

                        // check if we need to revert the order in each row
                        {
                                final Point2dImpl p0 = outCorners.get(0), p1 = outCorners.get(patternWidth - 1), p2 = outCorners
                                                .get(patternWidth);
                                if ((p1.x - p0.x) * (p2.y - p1.y) - (p1.y - p0.y) * (p2.x - p1.x) < 0)
                                {
                                        if (width % 2 == 0)
                                        {
                                                for (int i = 0; i < height; i++)
                                                        for (int j = 0; j < width / 2; j++)
                                                                Collections.swap(outCorners, i * width + j, i * width + width - j - 1);
                                        }
                                        else
                                        {
                                                for (int j = 0; j < width; j++)
                                                        for (int i = 0; i < height / 2; i++)
                                                                Collections.swap(outCorners, i * width + j, (height - i - 1) * width + j);
                                        }
                                }
                        }

                        result = cornerCount;

                } catch (final Exception ex) {
                        // ignore
                }

                if (result <= 0)
                {
                        cornerCount = Math.min(cornerCount, patternWidth * patternHeight);
                        outCorners.clear();
                        for (int i = 0; i < cornerCount; i++)
                                outCorners.add(corners[i]);
                        result = -cornerCount;

                        if (result == -patternWidth * patternHeight)
                                result = -result;
                }

                return result;
        }

        /**
         * Checks that each board row and column is pretty much monotonous curve:
         *
         * It analyzes each row and each column of the chessboard as following:
         *
         * for each corner c lying between end points in the same row/column it
         * checks that the point projection to the line segment (a,b) is lying
         * between projections of the neighbor corners in the same row/column.
         *
         * This function has been created as temporary workaround for the bug in
         * current implementation of cvFindChessboardCornes that produces absolutely
         * unordered sets of corners.
         *
         * @return true if the board is good; false otherwise.
         */
        private boolean checkBoardMonotony()
        {
                int i, j, k;

                for (k = 0; k < 2; k++)
                {
                        for (i = 0; i < (k == 0 ? patternHeight : patternWidth); i++)
                        {
                                final Point2dImpl a = k == 0 ? outCorners.get(i * patternWidth) : outCorners.get(i);
                                final Point2dImpl b = k == 0 ? outCorners.get((i + 1) * patternWidth - 1) :
                                        outCorners.get((patternHeight - 1) * patternWidth + i);
                                float prevt = 0;
                                final float dx0 = b.x - a.x, dy0 = b.y - a.y;
                                if (Math.abs(dx0) + Math.abs(dy0) < Float.MIN_VALUE)
                                        return false;
                                for (j = 1; j < (k == 0 ? patternWidth : patternHeight) - 1; j++)
                                {
                                        final Point2dImpl c = k == 0 ? outCorners.get(i * patternWidth + j) :
                                                outCorners.get(j * patternWidth + i);
                                        final float t = ((c.x - a.x) * dx0 + (c.y - a.y) * dy0) / (dx0 * dx0 + dy0 * dy0);
                                        if (t < prevt || t > 1)
                                                return false;
                                        prevt = t;
                                }
                        }
                }

                return true;
        }

        /**
         * Draw the predicted chessboard corners from the last call to
         * {@link #analyseImage(FImage)} on the given image.
         *
         * @param image
         *            the image to draw on
         */
        public void drawChessboardCorners(MBFImage image) {
                drawChessboardCorners(image, patternWidth, patternHeight, outCorners, found);
        }

        /**
         * Draw the given chessboard corners from on the given image.
         *
         * @param image
         *            the image to draw on
         * @param patternWidth
         *            the chessboard pattern width
         * @param patternHeight
         *            the chessboard pattern height
         * @param corners
         *            the corners
         * @param found
         *            true if the corners were found
         */
        public static void drawChessboardCorners(MBFImage image, int patternWidth, int patternHeight,
                        List<? extends Point2d> corners, boolean found)
        {
                final int radius = 4;

                if (!found) {
                        final Float[] color = RGBColour.RGB(0, 0, 255);

                        for (int i = 0; i < corners.size(); i++)
                        {
                                final Point2d pt = corners.get(i);
                                image.drawLine(new Point2dImpl(pt.getX() - radius, pt.getY() - radius),
                                                new Point2dImpl(pt.getX() + radius, pt.getY() + radius), 1, color);
                                image.drawLine(new Point2dImpl(pt.getX() - radius, pt.getY() + radius),
                                                new Point2dImpl(pt.getX() + radius, pt.getY() - radius), 1, color);
                                image.drawShape(new Circle(pt, radius), 1, color);
                        }
                } else {
                        Point2d prevPt = new Point2dImpl();
                        final Float[][] lineColours =
                                {
                                        RGBColour.RGB(0, 0, 255),
                                        RGBColour.RGB(0, 128, 255),
                                        RGBColour.RGB(0, 200, 200),
                                        RGBColour.RGB(0, 255, 0),
                                        RGBColour.RGB(200, 200, 0),
                                        RGBColour.RGB(255, 0, 0),
                                        RGBColour.RGB(255, 0, 255)
                                };

                        for (int y = 0, i = 0; y < patternHeight; y++) {
                                final Float[] color = lineColours[y % lineColours.length];

                                for (int x = 0; x < patternWidth; x++, i++) {
                                        final Point2d pt = corners.get(i);

                                        if (i != 0) {
                                                image.drawLine(prevPt, pt, 1, color);
                                        }

                                        image.drawLine(new Point2dImpl(pt.getX() - radius, pt.getY() - radius),
                                                        new Point2dImpl(pt.getX() + radius, pt.getY() + radius), 1, color);
                                        image.drawLine(new Point2dImpl(pt.getX() - radius, pt.getY() + radius),
                                                        new Point2dImpl(pt.getX() + radius, pt.getY() - radius), 1, color);
                                        image.drawShape(new Circle(pt, radius), 1, color);

                                        prevPt = pt;
                                }
                        }
                }
        }

        /**
         * Simple test program
         *
         * @param args
         * @throws IOException
         */
        public static void main(String[] args) throws IOException {
                final ChessboardCornerFinder fcc = new ChessboardCornerFinder(9, 6,
                                Options.FILTER_QUADS, Options.FAST_CHECK, Options.ADAPTIVE_THRESHOLD);
                VideoDisplay.createVideoDisplay(new VideoCapture(640, 480)).addVideoListener(new VideoDisplayAdapter<MBFImage>()
                                {
                        @Override
                        public void beforeUpdate(MBFImage frame) {
                                fcc.analyseImage(frame.flatten());
                                fcc.drawChessboardCorners(frame);
                        }
                                });
        }
}