#include "camodocal/chessboard/Chessboard.h"

#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "camodocal/chessboard/ChessboardQuad.h"
#include "camodocal/chessboard/Spline.h"

#define MAX_CONTOUR_APPROX  7

namespace camodocal
{

Chessboard::Chessboard(cv::Size boardSize, cv::Mat& image)
 : mBoardSize(boardSize)
 , mCornersFound(false)
{
    if (image.channels() == 1)
    {
        cv::cvtColor(image, mSketch, CV_GRAY2BGR);
        image.copyTo(mImage);
    }
    else
    {
        image.copyTo(mSketch);
        cv::cvtColor(image, mImage, CV_BGR2GRAY);
    }
}

void
Chessboard::findCorners(bool useOpenCV)
{
    mCornersFound = findChessboardCorners(mImage, mBoardSize, mCorners,
                                          CV_CALIB_CB_ADAPTIVE_THRESH +
                                          CV_CALIB_CB_NORMALIZE_IMAGE +
                                          CV_CALIB_CB_FILTER_QUADS +
                                          CV_CALIB_CB_FAST_CHECK,
                                          useOpenCV);

    if (mCornersFound)
    {
        // draw chessboard corners
        cv::drawChessboardCorners(mSketch, mBoardSize, mCorners, mCornersFound);
    }
}

const std::vector<cv::Point2f>&
Chessboard::getCorners(void) const
{
    return mCorners;
}

bool
Chessboard::cornersFound(void) const
{
    return mCornersFound;
}

const cv::Mat&
Chessboard::getImage(void) const
{
    return mImage;
}

const cv::Mat&
Chessboard::getSketch(void) const
{
    return mSketch;
}

bool
Chessboard::findChessboardCorners(const cv::Mat& image,
                                  const cv::Size& patternSize,
                                  std::vector<cv::Point2f>& corners,
                                  int flags, bool useOpenCV)
{
    if (useOpenCV)
    {
        return cv::findChessboardCorners(image, patternSize, corners, flags);
    }
    else
    {
        return findChessboardCornersImproved(image, patternSize, corners, flags);
    }
}

bool
Chessboard::findChessboardCornersImproved(const cv::Mat& image,
                                          const cv::Size& patternSize,
                                          std::vector<cv::Point2f>& corners,
                                          int flags)
{
    /************************************************************************************\
        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:
        ===============================================================

        The algorithms developed and implemented by Vezhnevets Vldimir
        aka Dead Moroz (vvp@graphics.cs.msu.ru)
        See http://graphics.cs.msu.su/en/research/calibration/opencv.html
        for detailed information.

        Reliability additions and modifications made by Philip Gruebele.
        <a href="mailto:pgruebele@cox.net">pgruebele@cox.net</a>

        Some improvements were made by:
        1) Martin Rufli - increased chance of correct corner matching
           Rufli, M., Scaramuzza, D., and Siegwart, R. (2008),
           Automatic Detection of Checkerboards on Blurred and Distorted Images,
           Proceedings of the IEEE/RSJ International Conference on
           Intelligent Robots and Systems (IROS 2008), Nice, France, September 2008.
        2) Lionel Heng - post-detection checks and better thresholding

    \************************************************************************************/

    //int bestDilation        = -1;
    const int minDilations    =  0;
    const int maxDilations    =  7;

    std::vector<ChessboardQuadPtr> outputQuadGroup;

    if (image.depth() != CV_8U || image.channels() == 2)
    {
        return false;
    }

    if (patternSize.width < 2 || patternSize.height < 2)
    {
        return false;
    }

    if (patternSize.width > 127 || patternSize.height > 127)
    {
        return false;
    }

    cv::Mat img = image;

    // Image histogram normalization and
    // BGR to Grayscale image conversion (if applicable)
    // MARTIN: Set to "false"
    if (image.channels() != 1 || (flags & CV_CALIB_CB_NORMALIZE_IMAGE))
    {
        cv::Mat norm_img(image.rows, image.cols, CV_8UC1);

        if (image.channels() != 1)
        {
            cv::cvtColor(image, norm_img, CV_BGR2GRAY);
            img = norm_img;
        }

        if (flags & CV_CALIB_CB_NORMALIZE_IMAGE)
        {
            cv::equalizeHist(image, norm_img);
            img = norm_img;
        }
    }

    if (flags & CV_CALIB_CB_FAST_CHECK)
    {
        if (!checkChessboard(img, patternSize))
        {
            return false;
        }
    }

    // PART 1: FIND LARGEST PATTERN
    //-----------------------------------------------------------------------
    // Checker patterns are tried to be found by dilating the background and
    // then applying a canny edge finder on the closed contours (checkers).
    // Try one dilation run, but if the pattern is not found, repeat until
    // max_dilations is reached.

    int prevSqrSize = 0;
    bool found = false;
    std::vector<ChessboardCornerPtr> outputCorners;

    for (int k = 0; k < 6; ++k)
    {
        for (int dilations = minDilations; dilations <= maxDilations; ++dilations)
        {
            if (found)
            {
                break;
            }

            cv::Mat thresh_img;

            // convert the input grayscale image to binary (black-n-white)
            if (flags & CV_CALIB_CB_ADAPTIVE_THRESH)
            {
                int blockSize = lround(prevSqrSize == 0 ?
                    std::min(img.cols,img.rows)*(k%2 == 0 ? 0.2 : 0.1): prevSqrSize*2)|1;

                // convert to binary
                cv::adaptiveThreshold(img, thresh_img, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, blockSize, (k/2)*5);
            }
            else
            {
                // empiric threshold level
                double mean = (cv::mean(img))[0];
                int thresh_level = lround(mean - 10);
                thresh_level = std::max(thresh_level, 10);

                cv::threshold(img, thresh_img, thresh_level, 255, CV_THRESH_BINARY);
            }

            // MARTIN's Code
            // Use both a rectangular and a cross kernel. In this way, a more
            // homogeneous dilation is performed, which is crucial for small,
            // distorted checkers. Use the CROSS kernel first, since its action
            // on the image is more subtle
            cv::Mat kernel1 = cv::getStructuringElement(CV_SHAPE_CROSS, cv::Size(3,3), cv::Point(1,1));
            cv::Mat kernel2 = cv::getStructuringElement(CV_SHAPE_RECT, cv::Size(3,3), cv::Point(1,1));

            if (dilations >= 1)
                cv::dilate(thresh_img, thresh_img, kernel1);
            if (dilations >= 2)
                cv::dilate(thresh_img, thresh_img, kernel2);
            if (dilations >= 3)
                cv::dilate(thresh_img, thresh_img, kernel1);
            if (dilations >= 4)
                cv::dilate(thresh_img, thresh_img, kernel2);
            if (dilations >= 5)
                cv::dilate(thresh_img, thresh_img, kernel1);
            if (dilations >= 6)
                cv::dilate(thresh_img, thresh_img, kernel2);

            // In order to find rectangles that go to the edge, we draw a white
            // line around the image edge. Otherwise FindContours will miss those
            // clipped rectangle contours. The border color will be the image mean,
            // because otherwise we risk screwing up filters like cvSmooth()
            cv::rectangle(thresh_img, cv::Point(0,0),
                          cv::Point(thresh_img.cols - 1, thresh_img.rows - 1),
                          CV_RGB(255,255,255), 3, 8);

            // Generate quadrangles in the following function
            std::vector<ChessboardQuadPtr> quads;

            generateQuads(quads, thresh_img, flags, dilations, true);
            if (quads.empty())
            {
                continue;
            }

            // The following function finds and assigns neighbor quads to every
            // quadrangle in the immediate vicinity fulfilling certain
            // prerequisites
            findQuadNeighbors(quads, dilations);

            // The connected quads will be organized in groups. The following loop
            // increases a "group_idx" identifier.
            // The function "findConnectedQuads assigns all connected quads
            // a unique group ID.
            // If more quadrangles were assigned to a given group (i.e. connected)
            // than are expected by the input variable "patternSize", the
            // function "cleanFoundConnectedQuads" erases the surplus
            // quadrangles by minimizing the convex hull of the remaining pattern.

            for (int group_idx = 0; ; ++group_idx)
            {
                std::vector<ChessboardQuadPtr> quadGroup;

                findConnectedQuads(quads, quadGroup, group_idx, dilations);

                if (quadGroup.empty())
                {
                    break;
                }

                cleanFoundConnectedQuads(quadGroup, patternSize);

                // The following function labels all corners of every quad
                // with a row and column entry.
                // "count" specifies the number of found quads in "quad_group"
                // with group identifier "group_idx"
                // The last parameter is set to "true", because this is the
                // first function call and some initializations need to be
                // made.
                labelQuadGroup(quadGroup, patternSize, true);

                found = checkQuadGroup(quadGroup, outputCorners, patternSize);

                float sumDist = 0;
                int total = 0;

                for (int i = 0; i < (int)outputCorners.size(); ++i)
                {
                    int ni = 0;
                    float avgi = outputCorners.at(i)->meanDist(ni);
                    sumDist += avgi * ni;
                    total += ni;
                }
                prevSqrSize = lround(sumDist / std::max(total, 1));

                if (found && !checkBoardMonotony(outputCorners, patternSize))
                {
                    found = false;
                }
            }
        }
    }

    if (!found)
    {
        return false;
    }
    else
    {
        corners.clear();
        corners.reserve(outputCorners.size());
        for (size_t i = 0; i < outputCorners.size(); ++i)
        {
            corners.push_back(outputCorners.at(i)->pt);
        }

        cv::cornerSubPix(image, corners, cv::Size(11, 11), cv::Size(-1,-1),
                         cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

        return true;
    }
}

//===========================================================================
// ERASE OVERHEAD
//===========================================================================
// If we found too many connected quads, remove those which probably do not
// belong.
void
Chessboard::cleanFoundConnectedQuads(std::vector<ChessboardQuadPtr>& quadGroup,
                                     cv::Size patternSize)
{
    cv::Point2f center(0.0f, 0.0f);

    // Number of quads this pattern should contain
    int count = ((patternSize.width + 1)*(patternSize.height + 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. Else go to the end
    // of the function
    if ((int)quadGroup.size() <= count)
    {
        return;
    }

    // Create an array of quadrangle centers
    std::vector<cv::Point2f> centers;
    centers.resize(quadGroup.size());

    for (size_t i = 0; i < quadGroup.size(); ++i)
    {
        cv::Point2f ci(0.0f, 0.0f);
        ChessboardQuadPtr& q = quadGroup[i];

        for (int j = 0; j < 4; ++j)
        {
            ci += q->corners[j]->pt;
        }

        ci *= 0.25f;

        // Centers(i), is the geometric center of quad(i)
        // Center, is the center of all found quads
        centers[i] = ci;

        center += ci;
    }

    center *= 1.0f / quadGroup.size();

    // If we 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
    while ((int)quadGroup.size() > count)
    {
        double minBoxArea = DBL_MAX;
        int minBoxAreaIndex = -1;

        // For each point, calculate box area without that point
        for (size_t skip = 0; skip < quadGroup.size(); ++skip)
        {
            // get bounding rectangle
            cv::Point2f temp = centers[skip];
            centers[skip] = center;

            std::vector<cv::Point2f> hull;
            cv::convexHull(centers, hull, true, true);
            centers[skip] = temp;

            double hull_area = fabs(cv::contourArea(hull));

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

        ChessboardQuadPtr& q0 = quadGroup[minBoxAreaIndex];

        // remove any references to this quad as a neighbor
        for (size_t i = 0; i < quadGroup.size(); ++i)
        {
            ChessboardQuadPtr& q = quadGroup.at(i);
            for (int j = 0; j < 4; ++j)
            {
                if (q->neighbors[j] == q0)
                {
                    q->neighbors[j].reset();
                    q->count--;
                    for (int k = 0; k < 4; ++k)
                    {
                        if (q0->neighbors[k] == q)
                        {
                            q0->neighbors[k].reset();
                            q0->count--;
                            break;
                        }
                    }
                    break;
                }
            }
        }
        // remove the quad
        quadGroup.at(minBoxAreaIndex) = quadGroup.back();
        centers.at(minBoxAreaIndex) = centers.back();
        quadGroup.pop_back();
        centers.pop_back();
    }
}

//===========================================================================
// FIND COONECTED QUADS
//===========================================================================
void
Chessboard::findConnectedQuads(std::vector<ChessboardQuadPtr>& quads,
                               std::vector<ChessboardQuadPtr>& group,
                               int group_idx, int dilation)
{
    ChessboardQuadPtr q;

    // Scan the array for a first unlabeled quad
    for (size_t i = 0; i < quads.size(); ++i)
    {
        ChessboardQuadPtr& quad = quads.at(i);

        if (quad->count > 0 && quad->group_idx < 0)
        {
            q = quad;
            break;
        }
    }

    if (q.get() == 0)
    {
        return;
    }

    // Recursively find a group of connected quads starting from the seed quad

    std::vector<ChessboardQuadPtr> stack;
    stack.push_back(q);

    group.push_back(q);
    q->group_idx = group_idx;

    while (!stack.empty())
    {
        q = stack.back();
        stack.pop_back();

        for (int i = 0; i < 4; ++i)
        {
            ChessboardQuadPtr& neighbor = q->neighbors[i];

            // If he neighbor exists and the neighbor has more than 0
            // neighbors and the neighbor has not been classified yet.
            if (neighbor.get() && neighbor->count > 0 && neighbor->group_idx < 0)
            {
                stack.push_back(neighbor);
                group.push_back(neighbor);
                neighbor->group_idx = group_idx;
            }
        }
    }
}

void
Chessboard::labelQuadGroup(std::vector<ChessboardQuadPtr>& quadGroup,
                           cv::Size patternSize, bool firstRun)
{
    // If this is the first function call, a seed quad needs to be selected
    if (firstRun)
    {
        // Search for the (first) quad with the maximum number of neighbors
        // (usually 4). This will be our starting point.
        int mark = -1;
        int maxNeighborCount = 0;
        for (size_t i = 0; i < quadGroup.size(); ++i)
        {
            ChessboardQuadPtr& q = quadGroup.at(i);
            if (q->count > maxNeighborCount)
            {
                mark = i;
                maxNeighborCount = q->count;

                if (maxNeighborCount == 4)
                {
                    break;
                }
            }
        }

        // Mark the starting quad's (per definition) upper left corner with
        //(0,0) and then proceed clockwise
        // The following labeling sequence enshures a "right coordinate system"
        ChessboardQuadPtr& q = quadGroup.at(mark);

        q->labeled = true;

        q->corners[0]->row = 0;
        q->corners[0]->column = 0;
        q->corners[1]->row = 0;
        q->corners[1]->column = 1;
        q->corners[2]->row = 1;
        q->corners[2]->column = 1;
        q->corners[3]->row = 1;
        q->corners[3]->column = 0;
    }


    // Mark all other corners with their respective row and column
    bool flagChanged = true;
    while (flagChanged)
    {
        // First reset the flag to "false"
        flagChanged = false;

        // Go through all quads top down is faster, since unlabeled quads will
        // be inserted at the end of the list
        for (int i = quadGroup.size() - 1; i >= 0; --i)
        {
            ChessboardQuadPtr& quad = quadGroup.at(i);

            // Check whether quad "i" has been labeled already
             if (!quad->labeled)
            {
                // Check its neighbors, whether some of them have been labeled
                // already
                for (int j = 0; j < 4; j++)
                {
                    // Check whether the neighbor exists (i.e. is not the NULL
                    // pointer)
                    if (quad->neighbors[j])
                    {
                        ChessboardQuadPtr& quadNeighbor = quad->neighbors[j];

                        // Only proceed, if neighbor "j" was labeled
                        if (quadNeighbor->labeled)
                        {
                            // For every quad it could happen to pass here
                            // multiple times. We therefore "break" later.
                            // Check whitch of the neighbors corners is
                            // connected to the current quad
                            int connectedNeighborCornerId = -1;
                            for (int k = 0; k < 4; ++k)
                            {
                                if (quadNeighbor->neighbors[k] == quad)
                                {
                                    connectedNeighborCornerId = k;

                                    // there is only one, therefore
                                    break;
                                }
                            }


                            // For the following calculations we need the row
                            // and column of the connected neighbor corner and
                            // all other corners of the connected quad "j",
                            // clockwise (CW)
                            ChessboardCornerPtr& conCorner        = quadNeighbor->corners[connectedNeighborCornerId];
                            ChessboardCornerPtr& conCornerCW1     = quadNeighbor->corners[(connectedNeighborCornerId+1)%4];
                            ChessboardCornerPtr& conCornerCW2     = quadNeighbor->corners[(connectedNeighborCornerId+2)%4];
                            ChessboardCornerPtr& conCornerCW3     = quadNeighbor->corners[(connectedNeighborCornerId+3)%4];

                            quad->corners[j]->row            =    conCorner->row;
                            quad->corners[j]->column        =    conCorner->column;
                            quad->corners[(j+1)%4]->row        =    conCorner->row - conCornerCW2->row + conCornerCW3->row;
                            quad->corners[(j+1)%4]->column    =    conCorner->column - conCornerCW2->column + conCornerCW3->column;
                            quad->corners[(j+2)%4]->row        =    conCorner->row + conCorner->row - conCornerCW2->row;
                            quad->corners[(j+2)%4]->column    =    conCorner->column + conCorner->column - conCornerCW2->column;
                            quad->corners[(j+3)%4]->row        =    conCorner->row - conCornerCW2->row + conCornerCW1->row;
                            quad->corners[(j+3)%4]->column    =    conCorner->column - conCornerCW2->column + conCornerCW1->column;

                            // Mark this quad as labeled
                            quad->labeled = true;

                            // Changes have taken place, set the flag
                            flagChanged = true;

                            // once is enough!
                            break;
                        }
                    }
                }
            }
        }
    }


    // All corners are marked with row and column
    // Record the minimal and maximal row and column indices
    // It is unlikely that more than 8bit checkers are used per dimension, if there are
    // an error would have been thrown at the beginning of "cvFindChessboardCorners2"
    int min_row        =  127;
    int max_row        = -127;
    int min_column    =  127;
    int max_column    = -127;

    for (int i = 0; i < (int)quadGroup.size(); ++i)
    {
        ChessboardQuadPtr& q = quadGroup.at(i);

        for (int j = 0; j < 4; ++j)
        {
            ChessboardCornerPtr& c = q->corners[j];

            if (c->row > max_row)
            {
                max_row = c->row;
            }
            if (c->row < min_row)
            {
                min_row = c->row;
            }
            if (c->column > max_column)
            {
                max_column = c->column;
            }
            if (c->column < min_column)
            {
                min_column = c->column;
            }
        }
    }

    // Label all internal corners with "needsNeighbor" = false
    // Label all external corners with "needsNeighbor" = true,
    // except if in a given dimension the pattern size is reached
    for (int i = min_row; i <= max_row; ++i)
    {
        for (int j = min_column; j <= max_column; ++j)
        {
            // A flag that indicates, whether a row/column combination is
            // executed multiple times
            bool flag = false;

            // Remember corner and quad
            int cornerID;
            int quadID;

            for (int k = 0; k < (int)quadGroup.size(); ++k)
            {
                ChessboardQuadPtr& q = quadGroup.at(k);

                for (int l = 0; l < 4; ++l)
                {
                    if ((q->corners[l]->row == i) && (q->corners[l]->column == j))
                    {
                        if (flag)
                        {
                            // Passed at least twice through here
                            q->corners[l]->needsNeighbor = false;
                            quadGroup[quadID]->corners[cornerID]->needsNeighbor = false;
                        }
                        else
                        {
                            // Mark with needs a neighbor, but note the
                            // address
                            q->corners[l]->needsNeighbor = true;
                            cornerID = l;
                            quadID = k;
                        }

                        // set the flag to true
                        flag = true;
                    }
                }
            }
        }
    }

    // Complete Linking:
    // sometimes not all corners were properly linked in "findQuadNeighbors",
    // but after labeling each corner with its respective row and column, it is
    // possible to match them anyway.
    for (int i = min_row; i <= max_row; ++i)
    {
        for (int j = min_column; j <= max_column; ++j)
        {
            // the following "number" indicates the number of corners which
            // correspond to the given (i,j) value
            // 1    is a border corner or a conrer which still needs a neighbor
            // 2    is a fully connected internal corner
            // >2    something went wrong during labeling, report a warning
            int number = 1;

            // remember corner and quad
            int cornerID;
            int quadID;

            for (int k = 0; k < (int)quadGroup.size(); ++k)
            {
                ChessboardQuadPtr& q = quadGroup.at(k);

                for (int l = 0; l < 4; ++l)
                {
                    if ((q->corners[l]->row == i) && (q->corners[l]->column == j))
                    {

                        if (number == 1)
                        {
                            // First corner, note its ID
                            cornerID = l;
                            quadID = k;
                        }

                        else if (number == 2)
                        {
                            // Second corner, check wheter this and the
                            // first one have equal coordinates, else
                            // interpolate
                            cv::Point2f delta = q->corners[l]->pt - quadGroup[quadID]->corners[cornerID]->pt;

                            if (delta.x != 0.0f || delta.y != 0.0f)
                            {
                                // Interpolate
                                q->corners[l]->pt -= delta * 0.5f;

                                quadGroup[quadID]->corners[cornerID]->pt += delta * 0.5f;
                            }
                        }
                        else if (number > 2)
                        {
                            // Something went wrong during row/column labeling
                            // Report a Warning
                            // ->Implemented in the function "mrWriteCorners"
                        }

                        // increase the number by one
                        ++number;
                    }
                }
            }
        }
    }


    // Bordercorners don't need any neighbors, if the pattern size in the
    // respective direction is reached
    // The only time we can make shure that the target pattern size is reached in a given
    // dimension, is when the larger side has reached the target size in the maximal
    // direction, or if the larger side is larger than the smaller target size and the
    // smaller side equals the smaller target size
    int largerDimPattern = std::max(patternSize.height,patternSize.width);
    int smallerDimPattern = std::min(patternSize.height,patternSize.width);
    bool flagSmallerDim1 = false;
    bool flagSmallerDim2 = false;

    if ((largerDimPattern + 1) == max_column - min_column)
    {
        flagSmallerDim1 = true;
        // We found out that in the column direction the target pattern size is reached
        // Therefore border column corners do not need a neighbor anymore
        // Go through all corners
        for (int k = 0; k < (int)quadGroup.size(); ++k)
        {
            ChessboardQuadPtr& q = quadGroup.at(k);

            for (int l = 0; l < 4; ++l)
            {
                ChessboardCornerPtr& c = q->corners[l];

                if (c->column == min_column || c->column == max_column)
                {
                    // Needs no neighbor anymore
                    c->needsNeighbor = false;
                }
            }
        }
    }

    if ((largerDimPattern + 1) == max_row - min_row)
    {
        flagSmallerDim2 = true;
        // We found out that in the column direction the target pattern size is reached
        // Therefore border column corners do not need a neighbor anymore
        // Go through all corners
        for (int k = 0; k < (int)quadGroup.size(); ++k)
        {
            ChessboardQuadPtr& q = quadGroup.at(k);

            for (int l = 0; l < 4; ++l)
            {
                ChessboardCornerPtr& c = q->corners[l];

                if (c->row == min_row || c->row == max_row)
                {
                    // Needs no neighbor anymore
                    c->needsNeighbor = false;
                }
            }
        }
    }


    // Check the two flags:
    //    -    If one is true and the other false, then the pattern target
    //        size was reached in in one direction -> We can check, whether the target
    //        pattern size is also reached in the other direction
    //  -    If both are set to true, then we deal with a square board -> do nothing
    //  -    If both are set to false -> There is a possibility that the larger side is
    //        larger than the smaller target size -> Check and if true, then check whether
    //        the other side has the same size as the smaller target size
    if ((flagSmallerDim1 == false && flagSmallerDim2 == true))
    {
        // Larger target pattern size is in row direction, check wheter smaller target
        // pattern size is reached in column direction
        if ((smallerDimPattern + 1) == max_column - min_column)
        {
            for (int k = 0; k < (int)quadGroup.size(); ++k)
            {
                ChessboardQuadPtr& q = quadGroup.at(k);

                for (int l = 0; l < 4; ++l)
                {
                    ChessboardCornerPtr& c = q->corners[l];

                    if (c->column == min_column || c->column == max_column)
                    {
                        // Needs no neighbor anymore
                        c->needsNeighbor = false;
                    }
                }
            }
        }
    }

    if ((flagSmallerDim1 == true && flagSmallerDim2 == false))
    {
        // Larger target pattern size is in column direction, check wheter smaller target
        // pattern size is reached in row direction
        if ((smallerDimPattern + 1) == max_row - min_row)
        {
            for (int k = 0; k < (int)quadGroup.size(); ++k)
            {
                ChessboardQuadPtr& q = quadGroup.at(k);

                for (int l = 0; l < 4; ++l)
                {
                    ChessboardCornerPtr& c = q->corners[l];

                    if (c->row == min_row || c->row == max_row)
                    {
                        // Needs no neighbor anymore
                        c->needsNeighbor = false;
                    }
                }
            }
        }
    }

    if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && smallerDimPattern + 1 < max_column - min_column)
    {
        // Larger target pattern size is in column direction, check wheter smaller target
        // pattern size is reached in row direction
        if ((smallerDimPattern + 1) == max_row - min_row)
        {
            for (int k = 0; k < (int)quadGroup.size(); ++k)
            {
                ChessboardQuadPtr& q = quadGroup.at(k);

                for (int l = 0; l < 4; ++l)
                {
                    ChessboardCornerPtr& c = q->corners[l];

                    if (c->row == min_row || c->row == max_row)
                    {
                        // Needs no neighbor anymore
                        c->needsNeighbor = false;
                    }
                }
            }
        }
    }

    if ((flagSmallerDim1 == false && flagSmallerDim2 == false) && smallerDimPattern + 1 < max_row - min_row)
    {
        // Larger target pattern size is in row direction, check wheter smaller target
        // pattern size is reached in column direction
        if ((smallerDimPattern + 1) == max_column - min_column)
        {
            for (int k = 0; k < (int)quadGroup.size(); ++k)
            {
                ChessboardQuadPtr& q = quadGroup.at(k);

                for (int l = 0; l < 4; ++l)
                {
                    ChessboardCornerPtr& c = q->corners[l];

                    if (c->column == min_column || c->column == max_column)
                    {
                        // Needs no neighbor anymore
                        c->needsNeighbor = false;
                    }
                }
            }
        }
    }
}

//===========================================================================
// GIVE A GROUP IDX
//===========================================================================
void
Chessboard::findQuadNeighbors(std::vector<ChessboardQuadPtr>& quads, int dilation)
{
    // Thresh dilation is used to counter the effect of dilation on the
    // distance between 2 neighboring corners. Since the distance below is
    // computed as its square, we do here the same. Additionally, we take the
    // conservative assumption that dilation was performed using the 3x3 CROSS
    // kernel, which coresponds to the 4-neighborhood.
    const float thresh_dilation = (float)(2*dilation+3)*(2*dilation+3)*2;    // the "*2" is for the x and y component
                                                                            // the "3" is for initial corner mismatch

    // Find quad neighbors
    for (size_t idx = 0; idx < quads.size(); ++idx)
    {
        ChessboardQuadPtr& curQuad = quads.at(idx);

        // Go through all quadrangles and label them in groups
        // For each corner of this quadrangle
        for (int i = 0; i < 4; ++i)
        {
            float minDist = FLT_MAX;
            int closestCornerIdx = -1;
            ChessboardQuadPtr closestQuad;

            if (curQuad->neighbors[i])
            {
                continue;
            }

            cv::Point2f pt = curQuad->corners[i]->pt;

            // Find the closest corner in all other quadrangles
            for (size_t k = 0; k < quads.size(); ++k)
            {
                if (k == idx)
                {
                    continue;
                }

                ChessboardQuadPtr& quad = quads.at(k);

                for (int j = 0; j < 4; ++j)
                {
                    // If it already has a neighbor
                    if (quad->neighbors[j])
                    {
                        continue;
                    }

                    cv::Point2f dp = pt - quad->corners[j]->pt;
                    float dist = dp.dot(dp);

                    // The following "if" checks, whether "dist" is the
                    // shortest so far and smaller than the smallest
                    // edge length of the current and target quads
                    if (dist < minDist &&
                        dist <= (curQuad->edge_len + thresh_dilation) &&
                        dist <= (quad->edge_len + thresh_dilation)   )
                    {
                        // Check whether conditions are fulfilled
                        if (matchCorners(curQuad, i, quad, j))
                        {
                            closestCornerIdx = j;
                            closestQuad = quad;
                            minDist = dist;
                        }
                    }
                }
            }

            // Have we found a matching corner point?
            if (closestCornerIdx >= 0 && minDist < FLT_MAX)
            {
                ChessboardCornerPtr closestCorner = closestQuad->corners[closestCornerIdx];

                // Make sure that the closest quad does not have the current
                // quad as neighbor already
                bool valid = true;
                for (int j = 0; j < 4; ++j)
                {
                    if (closestQuad->neighbors[j] == curQuad)
                    {
                        valid = false;
                        break;
                    }
                }
                if (!valid)
                {
                    continue;
                }

                // We've found one more corner - remember it
                closestCorner->pt = (pt + closestCorner->pt) * 0.5f;

                curQuad->count++;
                curQuad->neighbors[i] = closestQuad;
                curQuad->corners[i] = closestCorner;

                closestQuad->count++;
                closestQuad->neighbors[closestCornerIdx] = curQuad;
                closestQuad->corners[closestCornerIdx] = closestCorner;
            }
        }
    }
}



//===========================================================================
// AUGMENT PATTERN WITH ADDITIONAL QUADS
//===========================================================================
// The first part of the function is basically a copy of
// "findQuadNeighbors"
// The comparisons between two points and two lines could be computed in their
// own function
int
Chessboard::augmentBestRun(std::vector<ChessboardQuadPtr>& candidateQuads, int candidateDilation,
                           std::vector<ChessboardQuadPtr>& existingQuads, int existingDilation)
{
    // thresh dilation is used to counter the effect of dilation on the
    // distance between 2 neighboring corners. Since the distance below is
    // computed as its square, we do here the same. Additionally, we take the
    // conservative assumption that dilation was performed using the 3x3 CROSS
    // kernel, which coresponds to the 4-neighborhood.
    const float thresh_dilation = (2*candidateDilation+3)*(2*existingDilation+3)*2;    // the "*2" is for the x and y component

    // Search all old quads which have a neighbor that needs to be linked
    for (size_t idx = 0; idx < existingQuads.size(); ++idx)
    {
        ChessboardQuadPtr& curQuad = existingQuads.at(idx);

        // For each corner of this quadrangle
        for (int i = 0; i < 4; ++i)
        {
            float minDist = FLT_MAX;
            int closestCornerIdx = -1;
            ChessboardQuadPtr closestQuad;

            // If curQuad corner[i] is already linked, continue
            if (!curQuad->corners[i]->needsNeighbor)
            {
                continue;
            }

            cv::Point2f pt = curQuad->corners[i]->pt;

            // Look for a match in all candidateQuads' corners
            for (size_t k = 0; k < candidateQuads.size(); ++k)
            {
                ChessboardQuadPtr& candidateQuad = candidateQuads.at(k);

                // Only look at unlabeled new quads
                if (candidateQuad->labeled)
                {
                    continue;
                }

                for (int j = 0; j < 4; ++j)
                {
                    // Only proceed if they are less than dist away from each
                    // other
                    cv::Point2f dp = pt - candidateQuad->corners[j]->pt;
                    float dist = dp.dot(dp);

                    if ((dist < minDist) &&
                        dist <= (curQuad->edge_len + thresh_dilation) &&
                        dist <= (candidateQuad->edge_len + thresh_dilation))
                    {
                        if (matchCorners(curQuad, i, candidateQuad, j))
                        {
                            closestCornerIdx = j;
                            closestQuad = candidateQuad;
                            minDist = dist;
                        }
                    }
                }
            }

            // Have we found a matching corner point?
            if (closestCornerIdx >= 0 && minDist < FLT_MAX)
            {
                ChessboardCornerPtr closestCorner = closestQuad->corners[closestCornerIdx];
                closestCorner->pt = (pt + closestCorner->pt) * 0.5f;

                // We've found one more corner - remember it
                // ATTENTION: write the corner x and y coordinates separately,
                // else the crucial row/column entries will be overwritten !!!
                curQuad->corners[i]->pt = closestCorner->pt;
                curQuad->neighbors[i] = closestQuad;
                closestQuad->corners[closestCornerIdx]->pt = closestCorner->pt;

                // Label closest quad as labeled. In this way we exclude it
                // being considered again during the next loop iteration
                closestQuad->labeled = true;

                // We have a new member of the final pattern, copy it over
                ChessboardQuadPtr newQuad(new ChessboardQuad);
                newQuad->count        = 1;
                newQuad->edge_len    = closestQuad->edge_len;
                newQuad->group_idx    = curQuad->group_idx;    //the same as the current quad
                newQuad->labeled    = false;                //do it right afterwards

                curQuad->neighbors[i] = newQuad;

                // We only know one neighbor for sure
                newQuad->neighbors[closestCornerIdx] = curQuad;

                for (int j = 0; j < 4; j++)
                {
                    newQuad->corners[j].reset(new ChessboardCorner);
                    newQuad->corners[j]->pt = closestQuad->corners[j]->pt;
                }

                existingQuads.push_back(newQuad);

                // Start the function again
                return -1;
            }
        }
    }

    // Finished, don't start the function again
    return 1;
}



//===========================================================================
// GENERATE QUADRANGLES
//===========================================================================
void
Chessboard::generateQuads(std::vector<ChessboardQuadPtr>& quads,
                          cv::Mat& image, int flags,
                          int dilation, bool firstRun)
{
    // Empirical lower bound for the size of allowable quadrangles.
    // MARTIN, modified: Added "*0.1" in order to find smaller quads.
    int minSize = lround(image.cols * image.rows * .03 * 0.01 * 0.92 * 0.1);

    std::vector< std::vector<cv::Point> > contours;
    std::vector<cv::Vec4i> hierarchy;

    // Initialize contour retrieving routine
    cv::findContours(image, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);

    std::vector< std::vector<cv::Point> > quadContours;

    for (size_t i = 0; i < contours.size(); ++i)
    {
        // Reject contours with a too small perimeter and contours which are
        // completely surrounded by another contour
        // MARTIN: If this function is called during PART 1, then the parameter "first run"
        // is set to "true". This guarantees, that only "nice" squares are detected.
        // During PART 2, we allow the polygonal approximation function below to
        // approximate more freely, which can result in recognized "squares" that are in
        // reality multiple blurred and sticked together squares.
        std::vector<cv::Point>& contour = contours.at(i);

        if (hierarchy[i][3] == -1 || cv::contourArea(contour) < minSize)
        {
            continue;
        }

        int min_approx_level = 1, max_approx_level;
        if (firstRun)
        {
            max_approx_level = 3;
        }
        else
        {
            max_approx_level = MAX_CONTOUR_APPROX;
        }

        std::vector<cv::Point> approxContour;
        for (int approx_level = min_approx_level; approx_level <= max_approx_level; approx_level++)
        {
            cv::approxPolyDP(contour, approxContour, approx_level, true);

            if (approxContour.size() == 4)
            {
                break;
            }
        }

        // Reject non-quadrangles
        if (approxContour.size() == 4 && cv::isContourConvex(approxContour))
        {
            double p = cv::arcLength(approxContour, true);
            double area = cv::contourArea(approxContour);

            cv::Point pt[4];
            for (int i = 0; i < 4; i++)
            {
                pt[i] = approxContour[i];
            }

            cv::Point dp = pt[0] - pt[2];
            double d1 = sqrt(dp.dot(dp));

            dp = pt[1] - pt[3];
            double d2 = sqrt(dp.dot(dp));

            // PHILIPG: Only accept those quadrangles which are more
            // square than rectangular and which are big enough
            dp = pt[0] - pt[1];
            double d3 = sqrt(dp.dot(dp));
            dp = pt[1] - pt[2];
            double d4 = sqrt(dp.dot(dp));

            if (!(flags & CV_CALIB_CB_FILTER_QUADS) ||
                (d3*4 > d4 && d4*4 > d3 && d3*d4 < area*1.5 && area > minSize &&
                d1 >= 0.15 * p && d2 >= 0.15 * p))
            {
                quadContours.push_back(approxContour);
            }
        }
    }

    // Allocate quad & corner buffers
    quads.resize(quadContours.size());

    // Create array of quads structures
    for (size_t idx = 0; idx < quadContours.size(); ++idx)
    {
        ChessboardQuadPtr& q = quads.at(idx);
        std::vector<cv::Point>& contour = quadContours.at(idx);

        // Reset group ID
        q.reset(new ChessboardQuad);
        assert(contour.size() == 4);

        for (int i = 0; i < 4; ++i)
        {
            cv::Point2f pt = contour.at(i);
            ChessboardCornerPtr corner(new ChessboardCorner);
            corner->pt = pt;
            q->corners[i] = corner;
        }

        for (int i = 0; i < 4; ++i)
        {
            cv::Point2f dp = q->corners[i]->pt - q->corners[(i+1)&3]->pt;
            float d = dp.dot(dp);
            if (q->edge_len > d)
            {
                q->edge_len = d;
            }
        }
    }
}

bool
Chessboard::checkQuadGroup(std::vector<ChessboardQuadPtr>& quads,
                           std::vector<ChessboardCornerPtr>& corners,
                           cv::Size patternSize)
{
    // Initialize
    bool flagRow = false;
    bool flagColumn = false;
    int height = -1;
    int width = -1;

    // Compute the minimum and maximum row / column ID
    // (it is unlikely that more than 8bit checkers are used per dimension)
    int min_row    =  127;
    int max_row    = -127;
    int min_col    =  127;
    int max_col    = -127;

    for (size_t i = 0; i < quads.size(); ++i)
    {
        ChessboardQuadPtr& q = quads.at(i);

        for (int j = 0; j < 4; ++j)
        {
            ChessboardCornerPtr& c = q->corners[j];

            if (c->row > max_row)
            {
                max_row = c->row;
            }
            if (c->row < min_row)
            {
                min_row = c->row;
            }
            if (c->column > max_col)
            {
                max_col = c->column;
            }
            if (c->column < min_col)
            {
                min_col = c->column;
            }
        }
    }

    // If in a given direction the target pattern size is reached, we know exactly how
    // the checkerboard is oriented.
    // Else we need to prepare enough "dummy" corners for the worst case.
    for (size_t i = 0; i < quads.size(); ++i)
    {
        ChessboardQuadPtr& q = quads.at(i);

        for (int j = 0; j < 4; ++j)
        {
            ChessboardCornerPtr& c = q->corners[j];

            if (c->column == max_col && c->row != min_row && c->row != max_row && !c->needsNeighbor)
            {
                flagColumn = true;
            }
            if (c->row == max_row && c->column != min_col && c->column != max_col && !c->needsNeighbor)
            {
                flagRow = true;
            }
        }
    }

    if (flagColumn)
    {
        if (max_col - min_col == patternSize.width + 1)
        {
            width = patternSize.width;
            height = patternSize.height;
        }
        else
        {
            width = patternSize.height;
            height = patternSize.width;
        }
    }
    else if (flagRow)
    {
        if (max_row - min_row == patternSize.width + 1)
        {
            height = patternSize.width;
            width = patternSize.height;
        }
        else
        {
            height = patternSize.height;
            width = patternSize.width;
        }
    }
    else
    {
        // If target pattern size is not reached in at least one of the two
        // directions,  then we do not know where the remaining corners are
        // located. Account for this.
        width = std::max(patternSize.width, patternSize.height);
        height = std::max(patternSize.width, patternSize.height);
    }

    ++min_row;
    ++min_col;
    max_row = min_row + height - 1;
    max_col = min_col + width - 1;

    corners.clear();

    int linkedBorderCorners = 0;

    // Write the corners in increasing order to the output file
    for (int i = min_row; i <= max_row; ++i)
    {
        for (int j = min_col; j <= max_col; ++j)
        {
            // Reset the iterator
            int iter = 1;

            for (int k = 0; k < (int)quads.size(); ++k)
            {
                ChessboardQuadPtr& quad = quads.at(k);

                for (int l = 0; l < 4; ++l)
                {
                    ChessboardCornerPtr& c = quad->corners[l];

                    if (c->row == i && c->column == j)
                    {
                        bool boardEdge = false;
                        if (i == min_row || i == max_row ||
                            j == min_col || j == max_col)
                        {
                            boardEdge = true;
                        }

                        if ((iter == 1 && boardEdge) || (iter == 2 && !boardEdge))
                        {
                            // The respective row and column have been found
                            corners.push_back(quad->corners[l]);
                        }

                        if (iter == 2 && boardEdge)
                        {
                            ++linkedBorderCorners;
                        }

                        // If the iterator is larger than two, this means that more than
                        // two corners have the same row / column entries. Then some
                        // linking errors must have occured and we should not use the found
                        // pattern
                        if (iter > 2)
                        {
                            return false;
                        }

                        iter++;
                    }
                }
            }
        }
    }

    if ((int)corners.size() != patternSize.width * patternSize.height ||
        linkedBorderCorners < (patternSize.width * 2 + patternSize.height * 2 - 2) * 0.75f)
    {
        return false;
    }

    // check that no corners lie at image boundary
    float border = 5.0f;
    for (int i = 0; i < (int)corners.size(); ++i)
    {
        ChessboardCornerPtr& c = corners.at(i);

        if (c->pt.x < border || c->pt.x > mImage.cols - border ||
            c->pt.y < border || c->pt.y > mImage.rows - border)
        {
            return false;
        }
    }

    // check if we need to transpose the board
    if (width != patternSize.width)
    {
        std::swap(width, height);

        std::vector<ChessboardCornerPtr> outputCorners;
        outputCorners.resize(corners.size());

        for (int i = 0; i < height; ++i)
        {
            for (int j = 0; j < width; ++j)
            {
                outputCorners.at(i * width + j) = corners.at(j * height + i);
            }
        }

        corners = outputCorners;
    }

    // check if we need to revert the order in each row
    cv::Point2f p0 = corners.at(0)->pt;
    cv::Point2f p1 = corners.at(width-1)->pt;
    cv::Point2f p2 = corners.at(width)->pt;

    if ((p1 - p0).cross(p2 - p0) < 0.0f)
    {
        for (int i = 0; i < height; ++i)
        {
            for (int j = 0; j < width / 2; ++j)
            {
                std::swap(corners.at(i * width + j), corners.at(i * width + width - j - 1));
            }
        }
    }

    p0 = corners.at(0)->pt;
    p2 = corners.at(width)->pt;

    // check if we need to rotate the board
    if (p2.y < p0.y)
    {
        std::vector<ChessboardCornerPtr> outputCorners;
        outputCorners.resize(corners.size());

        for (int i = 0; i < height; ++i)
        {
            for (int j = 0; j < width; ++j)
            {
                outputCorners.at(i * width + j) = corners.at((height - i - 1) * width + width - j - 1);
            }
        }

        corners = outputCorners;
    }

    return true;
}

void
Chessboard::getQuadrangleHypotheses(const std::vector< std::vector<cv::Point> >& contours,
                                    std::vector< std::pair<float, int> >& quads,
                                    int classId) const
{
    const float minAspectRatio = 0.2f;
    const float maxAspectRatio = 5.0f;
    const float minBoxSize = 10.0f;

    for (size_t i = 0; i < contours.size(); ++i)
    {
        cv::RotatedRect box = cv::minAreaRect(contours.at(i));
        float boxSize = std::max(box.size.width, box.size.height);
        if (boxSize < minBoxSize)
        {
            continue;
        }

        float aspectRatio = box.size.width / std::max(box.size.height, 1.0f);

        if (aspectRatio < minAspectRatio || aspectRatio > maxAspectRatio)
        {
            continue;
        }

        quads.push_back(std::pair<float, int>(boxSize, classId));
    }
}

bool less_pred(const std::pair<float, int>& p1, const std::pair<float, int>& p2)
{
    return p1.first < p2.first;
}

void countClasses(const std::vector<std::pair<float, int> >& pairs, size_t idx1, size_t idx2, std::vector<int>& counts)
{
    counts.assign(2, 0);
    for (size_t i = idx1; i != idx2; ++i)
    {
        counts[pairs[i].second]++;
    }
}

bool
Chessboard::checkChessboard(const cv::Mat& image, cv::Size patternSize) const
{
    const int erosionCount = 1;
    const float blackLevel = 20.f;
    const float whiteLevel = 130.f;
    const float blackWhiteGap = 70.f;

    cv::Mat white = image.clone();

    cv::Mat black = image.clone();

    cv::erode(white, white, cv::Mat(), cv::Point(-1,-1), erosionCount);
    cv::dilate(black, black, cv::Mat(), cv::Point(-1,-1), erosionCount);

    cv::Mat thresh(image.rows, image.cols, CV_8UC1);

    bool result = false;
    for (float threshLevel = blackLevel; threshLevel < whiteLevel && !result; threshLevel += 20.0f)
    {
        cv::threshold(white, thresh, threshLevel + blackWhiteGap, 255, CV_THRESH_BINARY);

        std::vector< std::vector<cv::Point> > contours;
        std::vector<cv::Vec4i> hierarchy;

        // Initialize contour retrieving routine
        cv::findContours(thresh, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);

        std::vector<std::pair<float, int> > quads;
        getQuadrangleHypotheses(contours, quads, 1);

        cv::threshold(black, thresh, threshLevel, 255, CV_THRESH_BINARY_INV);

        cv::findContours(thresh, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
        getQuadrangleHypotheses(contours, quads, 0);

        const size_t min_quads_count = patternSize.width * patternSize.height / 2;
        std::sort(quads.begin(), quads.end(), less_pred);

        // now check if there are many hypotheses with similar sizes
        // do this by floodfill-style algorithm
        const float sizeRelDev = 0.4f;

        for (size_t i = 0; i < quads.size(); ++i)
        {
            size_t j = i + 1;
            for (; j < quads.size(); ++j)
            {
                if (quads[j].first / quads[i].first > 1.0f + sizeRelDev)
                {
                    break;
                }
            }

            if (j + 1 > min_quads_count + i)
            {
                // check the number of black and white squares
                std::vector<int> counts;
                countClasses(quads, i, j, counts);
                const int blackCount = lroundf(ceilf(patternSize.width / 2.0f) * ceilf(patternSize.height / 2.0f));
                const int whiteCount = lroundf(floorf(patternSize.width / 2.0f) * floorf(patternSize.height / 2.0f));
                if (counts[0] < blackCount * 0.75f ||
                    counts[1] < whiteCount * 0.75f)
                {
                    continue;
                }
                result = true;
                break;
            }
        }
    }

    return result;
}

bool
Chessboard::checkBoardMonotony(std::vector<ChessboardCornerPtr>& corners,
                               cv::Size patternSize)
{
    const float threshFactor = 0.2f;

    Spline splineXY, splineYX;
    splineXY.setLowBC(Spline::PARABOLIC_RUNOUT_BC);
    splineXY.setHighBC(Spline::PARABOLIC_RUNOUT_BC);
    splineYX.setLowBC(Spline::PARABOLIC_RUNOUT_BC);
    splineYX.setHighBC(Spline::PARABOLIC_RUNOUT_BC);

    // check if each row of corners approximates a cubic spline
    for (int i = 0; i < patternSize.height; ++i)
    {
        splineXY.clear();
        splineYX.clear();

        cv::Point2f p[3];
        p[0] = corners.at(i * patternSize.width)->pt;
        p[1] = corners.at(i * patternSize.width + patternSize.width / 2)->pt;
        p[2] = corners.at(i * patternSize.width + patternSize.width - 1)->pt;

        for (int j = 0; j < 3; ++j)
        {
            splineXY.addPoint(p[j].x, p[j].y);
            splineYX.addPoint(p[j].y, p[j].x);
        }

        for (int j = 1; j < patternSize.width - 1; ++j)
        {
            cv::Point2f& p_j = corners.at(i * patternSize.width + j)->pt;

            float thresh = std::numeric_limits<float>::max();

            // up-neighbor
            if (i > 0)
            {
                cv::Point2f& neighbor = corners.at((i - 1) * patternSize.width + j)->pt;
                thresh = fminf(thresh, cv::norm(neighbor - p_j));
            }
            // down-neighbor
            if (i < patternSize.height - 1)
            {
                cv::Point2f& neighbor = corners.at((i + 1) * patternSize.width + j)->pt;
                thresh = fminf(thresh, cv::norm(neighbor - p_j));
            }
            // left-neighbor
            {
                cv::Point2f& neighbor = corners.at(i * patternSize.width + j - 1)->pt;
                thresh = fminf(thresh, cv::norm(neighbor - p_j));
            }
            // right-neighbor
            {
                cv::Point2f& neighbor = corners.at(i * patternSize.width + j + 1)->pt;
                thresh = fminf(thresh, cv::norm(neighbor - p_j));
            }

            thresh *= threshFactor;

            if (fminf(fabsf(splineXY(p_j.x) - p_j.y), fabsf(splineYX(p_j.y) - p_j.x)) > thresh)
            {
                return false;
            }
        }
    }

    // check if each column of corners approximates a cubic spline
    for (int j = 0; j < patternSize.width; ++j)
    {
        splineXY.clear();
        splineYX.clear();

        cv::Point2f p[3];
        p[0] = corners.at(j)->pt;
        p[1] = corners.at(patternSize.height / 2 * patternSize.width + j)->pt;
        p[2] = corners.at((patternSize.height - 1) * patternSize.width + j)->pt;

        for (int i = 0; i < 3; ++i)
        {
            splineXY.addPoint(p[i].x, p[i].y);
            splineYX.addPoint(p[i].y, p[i].x);
        }

        for (int i = 1; i < patternSize.height - 1; ++i)
        {
            cv::Point2f& p_i = corners.at(i * patternSize.width + j)->pt;

            float thresh = std::numeric_limits<float>::max();

            // up-neighbor
            {
                cv::Point2f& neighbor = corners.at((i - 1) * patternSize.width + j)->pt;
                thresh = fminf(thresh, cv::norm(neighbor - p_i));
            }
            // down-neighbor
            {
                cv::Point2f& neighbor = corners.at((i + 1) * patternSize.width + j)->pt;
                thresh = fminf(thresh, cv::norm(neighbor - p_i));
            }
            // left-neighbor
            if (j > 0)
            {
                cv::Point2f& neighbor = corners.at(i * patternSize.width + j - 1)->pt;
                thresh = fminf(thresh, cv::norm(neighbor - p_i));
            }
            // right-neighbor
            if (j < patternSize.width - 1)
            {
                cv::Point2f& neighbor = corners.at(i * patternSize.width + j + 1)->pt;
                thresh = fminf(thresh, cv::norm(neighbor - p_i));
            }

            thresh *= threshFactor;

            if (fminf(fabsf(splineXY(p_i.x) - p_i.y), fabsf(splineYX(p_i.y) - p_i.x)) > thresh)
            {
                return false;
            }
        }
    }

    return true;
}

bool
Chessboard::matchCorners(ChessboardQuadPtr& quad1, int corner1,
                         ChessboardQuadPtr& quad2, int corner2) const
{
    // First Check everything from the viewpoint of the
    // current quad compute midpoints of "parallel" quad
    // sides 1
    float x1 = (quad1->corners[corner1]->pt.x + quad1->corners[(corner1+1)%4]->pt.x)/2;
    float y1 = (quad1->corners[corner1]->pt.y + quad1->corners[(corner1+1)%4]->pt.y)/2;
    float x2 = (quad1->corners[(corner1+2)%4]->pt.x + quad1->corners[(corner1+3)%4]->pt.x)/2;
    float y2 = (quad1->corners[(corner1+2)%4]->pt.y + quad1->corners[(corner1+3)%4]->pt.y)/2;
    // compute midpoints of "parallel" quad sides 2
    float x3 = (quad1->corners[corner1]->pt.x + quad1->corners[(corner1+3)%4]->pt.x)/2;
    float y3 = (quad1->corners[corner1]->pt.y + quad1->corners[(corner1+3)%4]->pt.y)/2;
    float x4 = (quad1->corners[(corner1+1)%4]->pt.x + quad1->corners[(corner1+2)%4]->pt.x)/2;
    float y4 = (quad1->corners[(corner1+1)%4]->pt.y + quad1->corners[(corner1+2)%4]->pt.y)/2;

    // MARTIN: Heuristic
    // For corner2 of quad2 to be considered,
    // it needs to be on the same side of the two lines as
    // corner1. This is given, if the cross product has
    // the same sign for both computations below:
    float a1 = x1 - x2;
    float b1 = y1 - y2;
    // the current corner
    float c11 = quad1->corners[corner1]->pt.x - x2;
    float d11 = quad1->corners[corner1]->pt.y - y2;
    // the candidate corner
    float c12 = quad2->corners[corner2]->pt.x - x2;
    float d12 = quad2->corners[corner2]->pt.y - y2;
    float sign11 = a1*d11 - c11*b1;
    float sign12 = a1*d12 - c12*b1;

    float a2 = x3 - x4;
    float b2 = y3 - y4;
    // the current corner
    float c21 = quad1->corners[corner1]->pt.x - x4;
    float d21 = quad1->corners[corner1]->pt.y - y4;
    // the candidate corner
    float c22 = quad2->corners[corner2]->pt.x - x4;
    float d22 = quad2->corners[corner2]->pt.y - y4;
    float sign21 = a2*d21 - c21*b2;
    float sign22 = a2*d22 - c22*b2;

    // Also make shure that two border quads of the same row or
    // column don't link. Check from the current corner's view,
    // whether the corner diagonal from the candidate corner
    // is also on the same side of the two lines as the current
    // corner and the candidate corner.
    float c13 = quad2->corners[(corner2+2)%4]->pt.x - x2;
    float d13 = quad2->corners[(corner2+2)%4]->pt.y - y2;
    float c23 = quad2->corners[(corner2+2)%4]->pt.x - x4;
    float d23 = quad2->corners[(corner2+2)%4]->pt.y - y4;
    float sign13 = a1*d13 - c13*b1;
    float sign23 = a2*d23 - c23*b2;


    // Second: Then check everything from the viewpoint of
    // the candidate quad. Compute midpoints of "parallel"
    // quad sides 1
    float u1 = (quad2->corners[corner2]->pt.x + quad2->corners[(corner2+1)%4]->pt.x)/2;
    float v1 = (quad2->corners[corner2]->pt.y + quad2->corners[(corner2+1)%4]->pt.y)/2;
    float u2 = (quad2->corners[(corner2+2)%4]->pt.x + quad2->corners[(corner2+3)%4]->pt.x)/2;
    float v2 = (quad2->corners[(corner2+2)%4]->pt.y + quad2->corners[(corner2+3)%4]->pt.y)/2;
    // compute midpoints of "parallel" quad sides 2
    float u3 = (quad2->corners[corner2]->pt.x + quad2->corners[(corner2+3)%4]->pt.x)/2;
    float v3 = (quad2->corners[corner2]->pt.y + quad2->corners[(corner2+3)%4]->pt.y)/2;
    float u4 = (quad2->corners[(corner2+1)%4]->pt.x + quad2->corners[(corner2+2)%4]->pt.x)/2;
    float v4 = (quad2->corners[(corner2+1)%4]->pt.y + quad2->corners[(corner2+2)%4]->pt.y)/2;

    // MARTIN: Heuristic
    // For corner2 of quad2 to be considered,
    // it needs to be on the same side of the two lines as
    // corner1. This is given, if the cross product has
    // the same sign for both computations below:
    float a3 = u1 - u2;
    float b3 = v1 - v2;
    // the current corner
    float c31 = quad1->corners[corner1]->pt.x - u2;
    float d31 = quad1->corners[corner1]->pt.y - v2;
    // the candidate corner
    float c32 = quad2->corners[corner2]->pt.x - u2;
    float d32 = quad2->corners[corner2]->pt.y - v2;
    float sign31 = a3*d31-c31*b3;
    float sign32 = a3*d32-c32*b3;

    float a4 = u3 - u4;
    float b4 = v3 - v4;
    // the current corner
    float c41 = quad1->corners[corner1]->pt.x - u4;
    float d41 = quad1->corners[corner1]->pt.y - v4;
    // the candidate corner
    float c42 = quad2->corners[corner2]->pt.x - u4;
    float d42 = quad2->corners[corner2]->pt.y - v4;
    float sign41 = a4*d41-c41*b4;
    float sign42 = a4*d42-c42*b4;

    // Also make sure that two border quads of the same row or
    // column don't link. Check from the candidate corner's view,
    // whether the corner diagonal from the current corner
    // is also on the same side of the two lines as the current
    // corner and the candidate corner.
    float c33 = quad1->corners[(corner1+2)%4]->pt.x - u2;
    float d33 = quad1->corners[(corner1+2)%4]->pt.y - v2;
    float c43 = quad1->corners[(corner1+2)%4]->pt.x - u4;
    float d43 = quad1->corners[(corner1+2)%4]->pt.y - v4;
    float sign33 = a3*d33-c33*b3;
    float sign43 = a4*d43-c43*b4;


    // This time we also need to make shure, that no quad
    // is linked to a quad of another dilation run which
    // may lie INSIDE it!!!
    // Third: Therefore check everything from the viewpoint
    // of the current quad compute midpoints of "parallel"
    // quad sides 1
    float x5 = quad1->corners[corner1]->pt.x;
    float y5 = quad1->corners[corner1]->pt.y;
    float x6 = quad1->corners[(corner1+1)%4]->pt.x;
    float y6 = quad1->corners[(corner1+1)%4]->pt.y;
    // compute midpoints of "parallel" quad sides 2
    float x7 = x5;
    float y7 = y5;
    float x8 = quad1->corners[(corner1+3)%4]->pt.x;
    float y8 = quad1->corners[(corner1+3)%4]->pt.y;

    // MARTIN: Heuristic
    // For corner2 of quad2 to be considered,
    // it needs to be on the other side of the two lines than
    // corner1. This is given, if the cross product has
    // a different sign for both computations below:
    float a5 = x6 - x5;
    float b5 = y6 - y5;
    // the current corner
    float c51 = quad1->corners[(corner1+2)%4]->pt.x - x5;
    float d51 = quad1->corners[(corner1+2)%4]->pt.y - y5;
    // the candidate corner
    float c52 = quad2->corners[corner2]->pt.x - x5;
    float d52 = quad2->corners[corner2]->pt.y - y5;
    float sign51 = a5*d51 - c51*b5;
    float sign52 = a5*d52 - c52*b5;

    float a6 = x8 - x7;
    float b6 = y8 - y7;
    // the current corner
    float c61 = quad1->corners[(corner1+2)%4]->pt.x - x7;
    float d61 = quad1->corners[(corner1+2)%4]->pt.y - y7;
    // the candidate corner
    float c62 = quad2->corners[corner2]->pt.x - x7;
    float d62 = quad2->corners[corner2]->pt.y - y7;
    float sign61 = a6*d61 - c61*b6;
    float sign62 = a6*d62 - c62*b6;


    // Fourth: Then check everything from the viewpoint of
    // the candidate quad compute midpoints of "parallel"
    // quad sides 1
    float u5 = quad2->corners[corner2]->pt.x;
    float v5 = quad2->corners[corner2]->pt.y;
    float u6 = quad2->corners[(corner2+1)%4]->pt.x;
    float v6 = quad2->corners[(corner2+1)%4]->pt.y;
    // compute midpoints of "parallel" quad sides 2
    float u7 = u5;
    float v7 = v5;
    float u8 = quad2->corners[(corner2+3)%4]->pt.x;
    float v8 = quad2->corners[(corner2+3)%4]->pt.y;

    // MARTIN: Heuristic
    // For corner2 of quad2 to be considered,
    // it needs to be on the other side of the two lines than
    // corner1. This is given, if the cross product has
    // a different sign for both computations below:
    float a7 = u6 - u5;
    float b7 = v6 - v5;
    // the current corner
    float c71 = quad1->corners[corner1]->pt.x - u5;
    float d71 = quad1->corners[corner1]->pt.y - v5;
    // the candidate corner
    float c72 = quad2->corners[(corner2+2)%4]->pt.x - u5;
    float d72 = quad2->corners[(corner2+2)%4]->pt.y - v5;
    float sign71 = a7*d71-c71*b7;
    float sign72 = a7*d72-c72*b7;

    float a8 = u8 - u7;
    float b8 = v8 - v7;
    // the current corner
    float c81 = quad1->corners[corner1]->pt.x - u7;
    float d81 = quad1->corners[corner1]->pt.y - v7;
    // the candidate corner
    float c82 = quad2->corners[(corner2+2)%4]->pt.x - u7;
    float d82 = quad2->corners[(corner2+2)%4]->pt.y - v7;
    float sign81 = a8*d81-c81*b8;
    float sign82 = a8*d82-c82*b8;

    // Check whether conditions are fulfilled
    if (((sign11 < 0 && sign12 < 0) || (sign11 > 0 && sign12 > 0))  &&
        ((sign21 < 0 && sign22 < 0) || (sign21 > 0 && sign22 > 0))  &&
        ((sign31 < 0 && sign32 < 0) || (sign31 > 0 && sign32 > 0))  &&
        ((sign41 < 0 && sign42 < 0) || (sign41 > 0 && sign42 > 0))  &&
        ((sign11 < 0 && sign13 < 0) || (sign11 > 0 && sign13 > 0))  &&
        ((sign21 < 0 && sign23 < 0) || (sign21 > 0 && sign23 > 0))  &&
        ((sign31 < 0 && sign33 < 0) || (sign31 > 0 && sign33 > 0))  &&
        ((sign41 < 0 && sign43 < 0) || (sign41 > 0 && sign43 > 0))  &&
        ((sign51 < 0 && sign52 > 0) || (sign51 > 0 && sign52 < 0))  &&
        ((sign61 < 0 && sign62 > 0) || (sign61 > 0 && sign62 < 0))  &&
        ((sign71 < 0 && sign72 > 0) || (sign71 > 0 && sign72 < 0))  &&
        ((sign81 < 0 && sign82 > 0) || (sign81 > 0 && sign82 < 0)))
    {
        return true;
    }
    else
    {
        return false;
    }
}

}
