chessboard.cpp

// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.

#include "precomp.hpp"
#include "opencv2/flann.hpp"
#include "chessboard.hpp"
#include "math.h"

//#define CV_DETECTORS_CHESSBOARD_DEBUG
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
#include <opencv2/highgui.hpp>
static cv::Mat debug_image;
#endif

using namespace std;
namespace cv {
namespace details {

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
// magic numbers used for chessboard corner detection
/////////////////////////////////////////////////////////////////////////////
static const float CORNERS_SEARCH = 0.5F;                       // percentage of the edge length to the next corner used to find new corners
static const float MAX_ANGLE = float(48.0/180.0*CV_PI);          // max angle between line segments supposed to be straight
static const float MIN_COS_ANGLE = float(cos(35.0/180*CV_PI));   // min cos angle between board edges
static const float MIN_RESPONSE_RATIO = 0.1F;
static const float ELLIPSE_WIDTH = 0.35F;                       // width of the search ellipse in percentage of its length
static const float RAD2DEG = float(180.0/CV_PI);
static const int MAX_SYMMETRY_ERRORS = 5;                       // maximal number of failures during point symmetry test (filtering out lines)
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////

// some helper methods
static bool isPointOnLine(cv::Point2f l1,cv::Point2f l2,cv::Point2f pt,float min_angle);
static int testPointSymmetry(const cv::Mat& mat,cv::Point2f pt,float dist,float max_error);
static float calcSubpixel(const float &x_l,const float &x,const float &x_r);
static float calcSubPos(const float &x_l,const float &x,const float &x_r);
static void polyfit(const Mat& src_x, const Mat& src_y, Mat& dst, int order);
static float calcSignedDistance(const cv::Vec2f &n,const cv::Point2f &a,const cv::Point2f &pt);
static void normalizePoints1D(cv::InputArray _points,cv::OutputArray _T,cv::OutputArray _new_points);
static cv::Mat findHomography1D(cv::InputArray _src,cv::InputArray _dst);

void normalizePoints1D(cv::InputArray _points,cv::OutputArray _T,cv::OutputArray _new_points)
{
    cv::Mat points = _points.getMat();
    if(points.cols > 1 && points.rows == 1)
        points = points.reshape(1,points.cols);
    CV_CheckChannelsEQ(points.channels(), 1, "points must have only one channel");

    // calc centroid
    double centroid= cv::mean(points)[0];

    // shift origin to centroid
    cv::Mat new_points = points-centroid;

    // calc mean distance
    double mean_dist = cv::mean(cv::abs(new_points))[0];
    if(mean_dist<= DBL_EPSILON)
        CV_Error(Error::StsBadArg, "all given points are identical");
    double scale = 1.0/mean_dist;


    // generate transformation
    cv::Matx22d Tx(
        scale, -scale*centroid,
        0,     1
    );
    Mat(Tx, false).copyTo(_T);

    // calc normalized points;
    _new_points.create(points.rows,1,points.type());
    new_points = _new_points.getMat();
    switch(points.type())
    {
    case CV_32FC1:
        for(int i=0;i < points.rows;++i)
        {
            cv::Vec2d p(points.at<float>(i), 1.0);
            p = Tx*p;
            new_points.at<float>(i) = float(p(0)/p(1));
        }
        break;
    case CV_64FC1:
        for(int i=0;i < points.rows;++i)
        {
            cv::Vec2d p(points.at<double>(i), 1.0);
            p = Tx*p;
            new_points.at<double>(i) = p(0)/p(1);
        }
        break;
    default:
        CV_Error(Error::StsUnsupportedFormat, "unsupported point type");
    }
}

cv::Mat findHomography1D(cv::InputArray _src,cv::InputArray _dst)
{
    // check inputs
    cv::Mat src = _src.getMat();
    cv::Mat dst = _dst.getMat();
    if(src.cols > 1 && src.rows == 1)
        src = src.reshape(1,src.cols);
    if(dst.cols > 1 && dst.rows == 1)
        dst = dst.reshape(1,dst.cols);
    CV_CheckEQ(src.rows, dst.rows, "size mismatch");
    CV_CheckChannelsEQ(src.channels(), 1, "data with only one channel are supported");
    CV_CheckChannelsEQ(dst.channels(), 1, "data with only one channel are supported");
    CV_CheckTypeEQ(src.type(), dst.type(), "src and dst must have the same type");
    CV_Check(src.rows, src.rows >= 3,"at least three point pairs are needed");

    // normalize points
    cv::Mat src_T,dst_T, src_n,dst_n;
    normalizePoints1D(src,src_T,src_n);
    normalizePoints1D(dst,dst_T,dst_n);

    int count = src_n.rows;
    cv::Mat A = cv::Mat::zeros(count,3,CV_64FC1);
    cv::Mat b = cv::Mat::zeros(count,1,CV_64FC1);

    // fill A;b and perform singular value decomposition
    // it is assumed that w is one for both cooridnates
    // h22 is kept to 1
    switch(src_n.type())
    {
    case CV_32FC1:
        for(int i=0;i<count;++i)
        {
            double s = src_n.at<float>(i);
            double d = dst_n.at<float>(i);
            A.at<double>(i,0) = s;
            A.at<double>(i,1) = 1.0;
            A.at<double>(i,2) = -s*d;
            b.at<double>(i) = d;
        }
        break;
    case CV_64FC1:
        for(int i=0;i<count;++i)
        {
            double s = src_n.at<double>(i);
            double d = dst_n.at<double>(i);
            A.at<double>(i,0) = s;
            A.at<double>(i,1) = 1.0;
            A.at<double>(i,2) = -s*d;
            b.at<double>(i) = d;
        }
        break;
    default:
        CV_Error(Error::StsUnsupportedFormat,"unsupported type");
    }

    cv::Mat u,d,vt;
    cv::SVD::compute(A,d,u,vt);
    cv::Mat b_ = u.t()*b;

    cv::Mat y(b_.rows,1,CV_64FC1);
    for(int i=0;i<b_.rows;++i)
        y.at<double>(i) = b_.at<double>(i)/d.at<double>(i);

    cv::Mat x = vt.t()*y;
    cv::Matx22d H_(x.at<double>(0), x.at<double>(1), x.at<double>(2), 1.0);

    // denormalize
    Mat H = dst_T.inv()*Mat(H_, false)*src_T;

    // enforce frobeniusnorm of one
    double scale = cv::norm(H);
    CV_Assert(fabs(scale) > DBL_EPSILON);
    scale = 1.0 / scale;
    return H*scale;
}
void polyfit(const Mat& src_x, const Mat& src_y, Mat& dst, int order)
{
    int npoints = src_x.checkVector(1);
    int nypoints = src_y.checkVector(1);
    CV_Assert(npoints == nypoints && npoints >= order+1);
    Mat_<double> srcX(src_x), srcY(src_y);
    Mat_<double> A = Mat_<double>::ones(npoints,order + 1);
    // build A matrix
    for (int y = 0; y < npoints; ++y)
    {
        for (int x = 1; x < A.cols; ++x)
            A.at<double>(y,x) = srcX.at<double>(y)*A.at<double>(y,x-1);
    }
    cv::Mat w;
    solve(A,srcY,w,DECOMP_SVD);
    w.convertTo(dst, ((src_x.depth() == CV_64F || src_y.depth() == CV_64F) ? CV_64F : CV_32F));
}

float calcSignedDistance(const cv::Vec2f &n,const cv::Point2f &a,const cv::Point2f &pt)
{
    cv::Vec3f v1(n[0],n[1],0);
    cv::Vec3f v2(pt.x-a.x,pt.y-a.y,0);
    return v1.cross(v2)[2];
}

bool isPointOnLine(cv::Point2f l1,cv::Point2f l2,cv::Point2f pt,float min_angle)
{
    cv::Vec2f vec1(l1-pt);
    cv::Vec2f vec2(pt-l2);
    if(vec1.dot(vec2) < min_angle*cv::norm(vec1)*cv::norm(vec2))
        return false;
    return true;
}

// returns how many tests fails out of 10
int testPointSymmetry(const cv::Mat& mat,cv::Point2f pt,float dist,float max_error)
{
    cv::Rect image_rect(int(0.5*dist),int(0.5*dist),int(mat.cols-0.5*dist),int(mat.rows-0.5*dist));
    cv::Size size(int(0.5*dist),int(0.5*dist));
    int count = 0;
    cv::Mat patch1,patch2;
    cv::Point2f center1,center2;
    for (int angle_i = 0; angle_i < 10; angle_i++)
    {
        double angle = angle_i * (CV_PI * 0.1);
        cv::Point2f n(float(cos(angle)),float(-sin(angle)));
        center1 = pt+dist*n;
        if(!image_rect.contains(center1))
            return false;
        center2 = pt-dist*n;
        if(!image_rect.contains(center2))
            return false;
        cv::getRectSubPix(mat,size,center1,patch1);
        cv::getRectSubPix(mat,size,center2,patch2);
        if(fabs(cv::mean(patch1)[0]-cv::mean(patch2)[0]) > max_error)
            ++count;
    }
    return count;
}

float calcSubpixel(const float &x_l,const float &x,const float &x_r)
{
    // prevent zero values
    if(x_l <= 0)
        return 0;
    if(x <= 0)
        return 0;
    if(x_r <= 0)
        return 0;
    const float l0 = float(std::log(x_l+1e-6));
    const float l1 = float(std::log(x+1e-6));
    const float l2 = float(std::log(x_r+1e-6));
    float delta = l2-l1-l1+l0;
    if(!delta) // this happens if all values are identical
        return 0;
    delta = (l0-l2)/(delta+delta);
    return delta;
}

float calcSubPos(const float &x_l,const float &x,const float &x_r)
{
    float val = 2.0F *(x_l-2.0F*x+x_r);
    if(val == 0.0F)
        return 0.0F;
    val = (x_l-x_r)/val;
    if(val > 1.0F)
        return 1.0F;
    if(val < -1.0F)
        return -1.0F;
    return val;
}

FastX::FastX(const Parameters &para)
{
    reconfigure(para);
}

void FastX::reconfigure(const Parameters &para)
{
    CV_Check(para.min_scale, para.min_scale >= 0 && para.min_scale <= para.max_scale, "invalid scale");
    parameters = para;
}

// rotates the image around its center
void FastX::rotate(float angle,const cv::Mat &img,cv::Size size,cv::Mat &out)const
{
    if(angle == 0)
    {
        out = img;
        return;
    }
    else
    {
        cv::Mat_<double> m = cv::getRotationMatrix2D(cv::Point2f(float(img.cols*0.5),float(img.rows*0.5)),float(angle/CV_PI*180),1);
        m.at<double>(0,2) += 0.5*(size.width-img.cols);
        m.at<double>(1,2) += 0.5*(size.height-img.rows);
        cv::warpAffine(img,out,m,size);
    }
}

void FastX::calcFeatureMap(const Mat &images,Mat& out)const
{
    if(images.empty())
        CV_Error(Error::StsBadArg,"no rotation images");
    int type = images.type(), depth = CV_MAT_DEPTH(type);
    CV_CheckType(type,depth == CV_8U,
            "Only 8-bit grayscale or color images are supported");
    if(!images.isContinuous())
        CV_Error(Error::StsBadArg,"image must be continuous");

    float signal,noise,rating;
    int count1;
    unsigned char val1,val2,val3;
    const unsigned char* wrap_around;
    const unsigned char* pend;
    const unsigned char* pimages = images.data;
    const int channels = images.channels();
    if(channels < 4)
        CV_Error(Error::StsBadArg,"images must have at least four channels");

    // for each pixel
    out = cv::Mat::zeros(images.rows,images.cols,CV_32FC1);
    const float *pout_end = reinterpret_cast<const float*>(out.dataend);
    for(float *pout=out.ptr<float>(0,0);pout != pout_end;++pout)
    {
        //reset values
        rating = 0.0; count1 = 0;
        noise = 255; signal = 0;

        //calc rating
        pend = pimages+channels;
        val1 = *(pend-1);                       // wrap around (last value)
        wrap_around = pimages++;                // store for wrap around (first value)
        val2 = *wrap_around;                    // first value
        for(;pimages != pend;++pimages)
        {
            val3 = *pimages;
            if(val1 <= val2)
            {
                if(val3 < val2) // maxima
                {
                    if(signal < val2)
                        signal = val2;
                    ++count1;
                }
            }
            else if(val1 > val2 && val3 >= val2) // minima
            {
                if(noise > val2)
                    noise = val2;
                ++count1;
            }
            val1 = val2;
            val2 = val3;
        }
        // wrap around
        if(val1 <= val2) // maxima
        {
            if(*wrap_around < val2)
            {
                if(signal < val2)
                    signal = val2;
                ++count1;
            }
        }
        else if(val1 > val2 && *wrap_around >= val2) // minima
        {
            if(noise > val2)
                noise = val2;
            ++count1;
        }

        // store rating
        if(count1 == parameters.branches)
        {
            rating = signal-noise;
            *pout = rating*rating; //store rating in the feature map
        }
    }
}

std::vector<std::vector<float> > FastX::calcAngles(const std::vector<cv::Mat> &rotated_images,std::vector<cv::KeyPoint> &keypoints)const
{
    // validate rotated_images
    if(rotated_images.empty())
        CV_Error(Error::StsBadArg,"no rotated images");
    std::vector<cv::Mat>::const_iterator iter = rotated_images.begin();
    for(;iter != rotated_images.end();++iter)
    {
        if(iter->empty())
            CV_Error(Error::StsBadArg,"empty rotated images");
        if(iter->channels() < 4)
            CV_Error(Error::StsBadArg,"rotated images must have at least four channels");
    }

    // assuming all elements of the same channel
    const int channels = rotated_images.front().channels();
    int channels_1 = channels-1;
    float resolution = float(CV_PI/channels);

    float angle;
    float val1,val2,val3,wrap_around;
    const unsigned char *pimages1,*pimages2,*pimages3,*pimages4;
    std::vector<std::vector<float> > angles;
    angles.resize(keypoints.size());
    float scale = float(parameters.super_resolution)+1.0F;

    // for each keypoint
    std::vector<cv::KeyPoint>::iterator pt_iter = keypoints.begin();
    for(int id=0;pt_iter != keypoints.end();++pt_iter,++id)
    {
        int scale_id = pt_iter->octave - parameters.min_scale;
        if(scale_id>= int(rotated_images.size()) ||scale_id < 0)
            CV_Error(Error::StsBadArg,"no rotated image for requested keypoint octave");
        const cv::Mat &s_rotated_images = rotated_images[scale_id];

        float x2 = pt_iter->pt.x*scale;
        float y2 = pt_iter->pt.y*scale;
        int row = int(y2);
        int col = int(x2);
        x2 -= col;
        y2 -= row;
        float x1 = 1.0F-x2; float y1 = 1.0F-y2;
        float a = x1*y1; float b = x2*y1; float c = x1*y2; float d = x2*y2;
        pimages1 = s_rotated_images.ptr<unsigned char>(row,col);
        pimages2 = s_rotated_images.ptr<unsigned char>(row,col+1);
        pimages3 = s_rotated_images.ptr<unsigned char>(row+1,col);
        pimages4 = s_rotated_images.ptr<unsigned char>(row+1,col+1);
        std::vector<float> &angles_i = angles[id];

        //calc rating
        val1 = a**(pimages1+channels_1)+b**(pimages2+channels_1)+
            c**(pimages3+channels_1)+d**(pimages4+channels_1);        // wrap around (last value)
        wrap_around = a**(pimages1++)+b**(pimages2++)+c**(pimages3++)+d**(pimages4++); // first value
        val2 = wrap_around;                                                             // first value
        for(int i=0;i<channels-1;++pimages1,++pimages2,++pimages3,++pimages4,++i)
        {
            val3 = a**(pimages1)+b**(pimages2)+c**(pimages3)+d**(pimages4);
            if(val1 <= val2)
            {
                if(val3 < val2)
                {
                    angle = float((calcSubPos(val1,val2,val3)+i)*resolution);
                    if(angle < 0)
                        angle += float(CV_PI);
                    else if(angle > CV_PI)
                        angle -= float(CV_PI);
                    angles_i.push_back(angle);
                    pt_iter->angle = 360.0F-angle*RAD2DEG;
                }
            }
            else if(val1 > val2 && val3 >= val2)
            {
                angle = float((calcSubPos(val1,val2,val3)+i)*resolution);
                if(angle < 0)
                    angle += float(CV_PI);
                else if(angle > CV_PI)
                    angle -= float(CV_PI);
                angles_i.push_back(-angle);
                pt_iter->angle = 360.0F-angle*RAD2DEG;
            }
            val1 = val2;
            val2 = val3;
        }
        // wrap around
        if(val1 <= val2)
        {
            if(wrap_around< val2)
            {
                angle = float((calcSubPos(val1,val2,wrap_around)+channels-1)*resolution);
                if(angle < 0)
                    angle += float(CV_PI);
                else if(angle > CV_PI)
                    angle -= float(CV_PI);
                angles_i.push_back(angle);
                pt_iter->angle = 360.0F-angle*RAD2DEG;
            }
        }
        else if(val1 > val2 && wrap_around >= val2)
        {
            angle = float((calcSubPos(val1,val2,wrap_around)+channels-1)*resolution);
            if(angle < 0)
                angle += float(CV_PI);
            else if(angle > CV_PI)
                angle -= float(CV_PI);
            angles_i.push_back(-angle);
            pt_iter->angle = 360.0F-angle*RAD2DEG;
        }
    }
    return angles;
}

void FastX::findKeyPoints(const std::vector<cv::Mat> &feature_maps, std::vector<KeyPoint>& keypoints,const Mat& _mask) const
{
    //TODO check that all feature_maps have the same size
    int num_scales = parameters.max_scale-parameters.min_scale;
    CV_CheckGE(int(feature_maps.size()), num_scales, "missing feature maps");
    if (!_mask.empty())
    {
        CV_CheckTypeEQ(_mask.type(), CV_8UC1, "wrong mask type");
        CV_CheckEQ(_mask.size(), feature_maps.front().size(),"wrong mask type or size");
    }
    keypoints.clear();

    cv::Mat mask;
    if(!_mask.empty())
        mask = _mask;
    else
        mask = cv::Mat::ones(feature_maps.front().size(),CV_8UC1);

    int super_res = int(parameters.super_resolution);
    int super_scale = super_res+1;
    float super_comp = 0.25F*super_res;

    // for each scale
    float strength = parameters.strength;
    std::vector<int> windows;
    cv::Point pt,pt2;
    double min,max;
    cv::Mat src;
    for(int scale=parameters.max_scale;scale>=parameters.min_scale;--scale)
    {
        int window_size = (1 << (scale + super_res)) + 1;
        float window_size2 = 0.5F*window_size;
        float window_size4 = 0.25F*window_size;
        int window_size2i = cvRound(window_size2);

        const cv::Mat &feature_map = feature_maps[scale-parameters.min_scale];
        int y = ((feature_map.rows)/window_size)-6;
        int x = ((feature_map.cols)/window_size)-6;
        for(int row=5;row<y;++row)
        {
            for(int col=5;col<x;++col)
            {
                Rect rect(col*window_size,row*window_size,window_size,window_size);
                src = feature_map(rect);
                cv::minMaxLoc(src,&min,&max,NULL,&pt);
                if(min == max || max < strength)
                    continue;

                cv::Point pos(pt.x+rect.x,pt.y+rect.y);
                if(mask.at<unsigned char>(pos.y,pos.x) == 0)
                    continue;

                Rect rect2(int(pos.x-window_size2),int(pos.y-window_size2),window_size,window_size);
                src = feature_map(rect2);
                cv::minMaxLoc(src,NULL,NULL,NULL,&pt2);
                if(pos.x == pt2.x+rect2.x && pos.y == pt2.y+rect2.y)
                {
                    // the point is the best one on the current scale
                    // check all larger scales if there is a stronger one
                    double max2;
                    int scale2= scale-1;
                    //parameters.min_scale;
                    for(;scale2>=parameters.min_scale;--scale2)
                    {
                        cv::minMaxLoc(feature_maps[scale2-parameters.min_scale](rect),NULL,&max2,NULL,NULL);
                        if(max2 > max)
                            break;
                    }
                    if(scale2<parameters.min_scale && pos.x+1 < feature_map.cols && pos.y+1 < feature_map.rows)
                    {
                        float sub_x = float(calcSubpixel(feature_map.at<float>(pos.y,pos.x-1),
                                feature_map.at<float>(pos.y,pos.x),
                                feature_map.at<float>(pos.y,pos.x+1)));
                        float sub_y = float(calcSubpixel(feature_map.at<float>(pos.y-1,pos.x),
                                feature_map.at<float>(pos.y,pos.x),
                                feature_map.at<float>(pos.y+1,pos.x)));
                        cv::KeyPoint kpt(sub_x+pos.x,sub_y+pos.y,float(window_size),0.F,float(max),scale);
                        int x2 = std::max(0,int(kpt.pt.x-window_size4));
                        int y2 = std::max(0,int(kpt.pt.y-window_size4));
                        int w = std::min(int(mask.cols-x2),window_size2i);
                        int h = std::min(int(mask.rows-y2),window_size2i);
                        mask(cv::Rect(x2,y2,w,h)) = 0.0;
                        if(super_scale != 1)
                        {
                            kpt.pt.x /= super_scale;
                            kpt.pt.y /= super_scale;
                            kpt.pt.x -= super_comp;
                            kpt.pt.y -= super_comp;
                            kpt.size /= super_scale;
                        }
                        keypoints.push_back(kpt);
                    }
                }
            }
        }
    }
}

void FastX::detectAndCompute(cv::InputArray image,cv::InputArray mask,std::vector<cv::KeyPoint>& keypoints,
        cv::OutputArray _descriptors,bool useProvidedKeyPoints)
{
    useProvidedKeyPoints = false;
    detectImpl(image.getMat(),keypoints,mask.getMat());
    if(!_descriptors.needed())
        return;

    // generate descriptors based on their position
    _descriptors.create(int(keypoints.size()),2,CV_32FC1);
    cv::Mat descriptors = _descriptors.getMat();
    std::vector<cv::KeyPoint>::const_iterator iter = keypoints.begin();
    for(int row=0;iter != keypoints.end();++iter,++row)
    {
        descriptors.at<float>(row,0) = iter->pt.x;
        descriptors.at<float>(row,1) = iter->pt.y;
    }
    if(!useProvidedKeyPoints)        // suppress compiler warning
        return;
    return;
}

void FastX::detectImpl(const cv::Mat& _gray_image,
        std::vector<cv::Mat> &rotated_images,
        std::vector<cv::Mat> &feature_maps,
        const cv::Mat &_mask)const
{
    if(!_mask.empty())
        CV_Error(Error::StsBadSize, "Mask is not supported");
    CV_CheckTypeEQ(_gray_image.type(), CV_8UC1, "Unsupported image type");

    // up-sample if needed
    cv::Mat gray_image;
    int super_res = int(parameters.super_resolution);
    if(super_res)
        cv::resize(_gray_image,gray_image,cv::Size(),2,2);
    else
        gray_image = _gray_image;

    //for each scale
    int num_scales = parameters.max_scale-parameters.min_scale+1;
    rotated_images.resize(num_scales);
    feature_maps.resize(num_scales);
    parallel_for_(Range(parameters.min_scale,parameters.max_scale+1),[&](const Range& range){
        for(int scale=range.start;scale < range.end;++scale)
        {
            // calc images
            // for each angle step
            int scale_id = scale-parameters.min_scale;
            cv::Mat rotated,filtered_h,filtered_v;
            int diag = int(sqrt(gray_image.rows*gray_image.rows+gray_image.cols*gray_image.cols));
            cv::Size size(diag,diag);
            int num = int(0.5001*CV_PI/parameters.resolution);
            std::vector<cv::Mat> images;
            images.resize(2*num);
            int scale_size = int(1+pow(2.0,scale+1+super_res));
            int scale_size2 = int((scale_size/10)*2+1);
            for(int i=0;i<num;++i)
            {
                float angle = parameters.resolution*i;
                rotate(-angle,gray_image,size,rotated);
                cv::blur(rotated,filtered_h,cv::Size(scale_size,scale_size2));
                cv::blur(rotated,filtered_v,cv::Size(scale_size2,scale_size));

                // rotate filtered images back
                rotate(angle,filtered_h,gray_image.size(),images[i]);
                rotate(angle,filtered_v,gray_image.size(),images[i+num]);
            }
            cv::merge(images,rotated_images[scale_id]);

            // calc feature map
            calcFeatureMap(rotated_images[scale_id],feature_maps[scale_id]);
            // filter feature map to improve impulse responses
            if(parameters.filter)
            {
                cv::Mat high,low;
                cv::blur(feature_maps[scale_id],low,cv::Size(scale_size,scale_size));
                int scale2 = int((scale_size/6))*2+1;
                cv::blur(feature_maps[scale_id],high,cv::Size(scale2,scale2));
                feature_maps[scale_id] = high-0.8*low;
            }
        }
    });
}

void FastX::detectImpl(const cv::Mat& image,std::vector<cv::KeyPoint>& keypoints,std::vector<cv::Mat> &feature_maps,const cv::Mat &mask)const
{
    std::vector<cv::Mat> rotated_images;
    detectImpl(image,rotated_images,feature_maps,mask);
    findKeyPoints(feature_maps,keypoints,mask);
}

void FastX::detectImpl(InputArray image, std::vector<KeyPoint>& keypoints, InputArray mask)const
{
    std::vector<cv::Mat> feature_maps;
    detectImpl(image.getMat(),keypoints,feature_maps,mask.getMat());
}

void FastX::detectImpl(const Mat& src, std::vector<KeyPoint>& keypoints, const Mat& mask)const
{
    std::vector<cv::Mat> feature_maps;
    detectImpl(src,keypoints,feature_maps,mask);
}


Ellipse::Ellipse():
    angle(0),
    cosf(0),
    sinf(0)
{
}

Ellipse::Ellipse(const cv::Point2f &_center, const cv::Size2f &_axes, float _angle):
    center(_center),
    axes(_axes),
    angle(_angle),
    cosf(cos(-_angle)),
    sinf(sin(-_angle))
{
}

Ellipse::Ellipse(const Ellipse &other)
{
    center = other.center;
    axes= other.axes;
    angle= other.angle;
    cosf = other.cosf;
    sinf = other.sinf;
}

const cv::Size2f &Ellipse::getAxes()const
{
    return axes;
}

cv::Point2f Ellipse::getCenter()const
{
    return center;
}

void Ellipse::draw(cv::InputOutputArray img,const cv::Scalar &color)const
{
    cv::ellipse(img,center,axes,360-angle/CV_PI*180,0,360,color);
}

bool Ellipse::contains(const cv::Point2f &pt)const
{
    cv::Point2f ptc = pt-center;
    float x = cosf*ptc.x+sinf*ptc.y;
    float y = -sinf*ptc.x+cosf*ptc.y;
    if(x*x/(axes.width*axes.width)+y*y/(axes.height*axes.height) <= 1.0)
        return true;
    return false;
}


// returns false if the angle from the line pt1-pt2 to the line pt3-pt4 is negative
static bool checkOrientation(const cv::Point2f &pt1,const cv::Point2f &pt2,
        const cv::Point2f &pt3,const cv::Point2f &pt4)
{
    cv::Point3f p1(pt2.x-pt1.x,pt2.y-pt1.y,0);
    cv::Point3f p2(pt4.x-pt3.x,pt4.y-pt3.y,0);
    return p1.cross(p2).z > 0;
}

static bool sortKeyPoint(const cv::KeyPoint &pt1,const cv::KeyPoint &pt2)
{
    // used as comparison function for partial sort
    // the keypoints with the best score should be first
    return pt1.response > pt2.response;
}

cv::Mat Chessboard::getObjectPoints(const cv::Size &pattern_size,float cell_size)
{
    cv::Mat result(pattern_size.width*pattern_size.height,1,CV_32FC3);
    for(int row=0;row < pattern_size.height;++row)
    {
        for(int col=0;col< pattern_size.width;++col)
        {
            cv::Point3f &pt = *result.ptr<cv::Point3f>(row*pattern_size.width+col);
            pt.x = cell_size*col;
            pt.y = cell_size*row;
            pt.z = 0;
        }
    }
    return result;
}

bool Chessboard::Board::Cell::empty()const
{
    // check if one of its corners has NaN
    if(top_left->x != top_left->x || top_left->y != top_left->y)
        return true;
    if(top_right->x != top_right->x || top_right->y != top_right->y)
        return true;
    if(bottom_right->x != bottom_right->x || bottom_right->y != bottom_right->y)
        return true;
    if(bottom_left->x != bottom_left->x || bottom_left->y != bottom_left->y)
        return true;
    return false;
}

int Chessboard::Board::Cell::getRow()const
{
    int row = 0;
    Cell const* temp = this;
    for(;temp->top;temp=temp->top,++row);
    return row;
}

int Chessboard::Board::Cell::getCol()const
{
    int col = 0;
    Cell const* temp = this;
    for(;temp->left;temp=temp->left,++col);
    return col;
}

Chessboard::Board::Cell::Cell() :
    top_left(NULL), top_right(NULL), bottom_right(NULL), bottom_left(NULL),
    left(NULL), top(NULL), right(NULL), bottom(NULL),black(false)
{}

Chessboard::Board::PointIter::PointIter(Cell *_cell,CornerIndex _corner_index):
    corner_index(_corner_index),
    cell(_cell)
{
}

Chessboard::Board::PointIter::PointIter(const PointIter &other)
{
    this->operator=(other);
}

void Chessboard::Board::PointIter::operator=(const PointIter &other)
{
    corner_index = other.corner_index;
    cell = other.cell;
}

Chessboard::Board::Cell* Chessboard::Board::PointIter::getCell()
{
    return cell;
}

bool Chessboard::Board::PointIter::valid()const
{
    return cell != NULL;
}

bool Chessboard::Board::PointIter::isNaN()const
{
    const cv::Point2f *pt = operator*();
    if(pt->x != pt->x || pt->y != pt->y)        // NaN check
        return true;
    return false;
}

bool Chessboard::Board::PointIter::checkCorner()const
{
    if(!cell->empty())
        return true;
    // test all other cells
    switch(corner_index)
    {
    case BOTTOM_LEFT:
        if(cell->left)
        {
            if(!cell->left->empty())
                return true;
            if(cell->left->bottom && !cell->left->bottom->empty())
                return true;
        }
        if(cell->bottom)
        {
            if(!cell->bottom->empty())
                return true;
            if(cell->bottom->left && !cell->bottom->left->empty())
                return true;
        }
        break;
    case TOP_LEFT:
        if(cell->left)
        {
            if(!cell->left->empty())
                return true;
            if(cell->left->top && !cell->left->top->empty())
                return true;
        }
        if(cell->top)
        {
            if(!cell->top->empty())
                return true;
            if(cell->top->left && !cell->top->left->empty())
                return true;
        }
        break;
    case TOP_RIGHT:
        if(cell->right)
        {
            if(!cell->right->empty())
                return true;
            if(cell->right->top && !cell->right->top->empty())
                return true;
        }
        if(cell->top)
        {
            if(!cell->top->empty())
                return true;
            if(cell->top->right && !cell->top->right->empty())
                return true;
        }
        break;
    case BOTTOM_RIGHT:
        if(cell->right)
        {
            if(!cell->right->empty())
                return true;
            if(cell->right->bottom && !cell->right->bottom->empty())
                return true;
        }
        if(cell->bottom)
        {
            if(!cell->bottom->empty())
                return true;
            if(cell->bottom->right && !cell->bottom->right->empty())
                return true;
        }
        break;
    default:
        CV_Assert(false);
    }
    return false;
}


bool Chessboard::Board::PointIter::left(bool check_empty)
{
    switch(corner_index)
    {
    case BOTTOM_LEFT:
        if(cell->left && (!check_empty || !cell->left->empty()))
            cell = cell->left;
        else if(check_empty && cell->bottom && cell->bottom->left && !cell->bottom->left->empty())
        {
            cell = cell->bottom->left;
            corner_index = TOP_LEFT;
        }
        else
            return false;
        break;
    case TOP_LEFT:
        if(cell->left && (!check_empty || !cell->left->empty()))
            cell = cell->left;
        else if(check_empty && cell->top && cell->top->left && !cell->top->left->empty())
        {
            cell = cell->top->left;
            corner_index = BOTTOM_LEFT;
        }
        else
            return false;
        break;
    case TOP_RIGHT:
        corner_index = TOP_LEFT;
        break;
    case BOTTOM_RIGHT:
        corner_index = BOTTOM_LEFT;
        break;
    default:
        CV_Assert(false);
    }
    return true;
}

bool Chessboard::Board::PointIter::top(bool check_empty)

{
    switch(corner_index)
    {
    case TOP_RIGHT:
        if(cell->top && (!check_empty || !cell->top->empty()))
            cell = cell->top;
        else if(check_empty && cell->right && cell->right->top&& !cell->right->top->empty())
        {
            cell = cell->right->top;
            corner_index = TOP_LEFT;
        }
        else
            return false;
        break;
    case TOP_LEFT:
        if(cell->top && (!check_empty || !cell->top->empty()))
            cell = cell->top;
        else if(check_empty && cell->left && cell->left->top&& !cell->left->top->empty())
        {
            cell = cell->left->top;
            corner_index = TOP_RIGHT;
        }
        else
            return false;
        break;
    case BOTTOM_LEFT:
        corner_index = TOP_LEFT;
        break;
    case BOTTOM_RIGHT:
        corner_index = TOP_RIGHT;
        break;
    default:
        CV_Assert(false);
    }
    return true;
}

bool Chessboard::Board::PointIter::right(bool check_empty)
{
    switch(corner_index)
    {
    case TOP_RIGHT:
        if(cell->right && (!check_empty || !cell->right->empty()))
            cell = cell->right;
        else if(check_empty && cell->top && cell->top->right && !cell->top->right->empty())
        {
            cell = cell->top->right;
            corner_index = BOTTOM_RIGHT;
        }
        else
            return false;
        break;
    case BOTTOM_RIGHT:
        if(cell->right && (!check_empty || !cell->right->empty()))
            cell = cell->right;
        else if(check_empty && cell->bottom && cell->bottom->right && !cell->bottom->right->empty())
        {
            cell = cell->bottom->right;
            corner_index = TOP_RIGHT;
        }
        else
            return false;
        break;
    case TOP_LEFT:
        corner_index = TOP_RIGHT;
        break;
    case BOTTOM_LEFT:
        corner_index = BOTTOM_RIGHT;
        break;
    default:
        CV_Assert(false);
    }
    return true;
}

bool Chessboard::Board::PointIter::bottom(bool check_empty)
{
    switch(corner_index)
    {
    case BOTTOM_LEFT:
        if(cell->bottom && (!check_empty || !cell->bottom->empty()))
            cell = cell->bottom;
        else if(check_empty && cell->left && cell->left->bottom && !cell->left->bottom->empty())
        {
            cell = cell->left->bottom;
            corner_index = BOTTOM_RIGHT;
        }
        else
            return false;
        break;
    case BOTTOM_RIGHT:
        if(cell->bottom && (!check_empty || !cell->bottom->empty()))
            cell = cell->bottom;
        else if(check_empty && cell->right && cell->right->bottom && !cell->right->bottom->empty())
        {
            cell = cell->right->bottom;
            corner_index = BOTTOM_LEFT;
        }
        else
            return false;
        break;
    case TOP_LEFT:
        corner_index = BOTTOM_LEFT;
        break;
    case TOP_RIGHT:
        corner_index = BOTTOM_RIGHT;
        break;
    default:
        CV_Assert(false);
    }
    return true;
}


const cv::Point2f* Chessboard::Board::PointIter::operator*()const
{
    switch(corner_index)
    {
    case TOP_LEFT:
        return cell->top_left;
    case TOP_RIGHT:
        return cell->top_right;
    case BOTTOM_RIGHT:
        return cell->bottom_right;
    case BOTTOM_LEFT:
        return cell->bottom_left;
    }
    CV_Assert(false);
}

const cv::Point2f* Chessboard::Board::PointIter::operator->()const
{
    return operator*();
}

cv::Point2f* Chessboard::Board::PointIter::operator*()
{
    const cv::Point2f *pt = const_cast<const PointIter*>(this)->operator*();
    return const_cast<cv::Point2f*>(pt);
}

cv::Point2f* Chessboard::Board::PointIter::operator->()
{
    return operator*();
}

Chessboard::Board::Board(float _white_angle,float _black_angle):
    top_left(NULL),
    rows(0),
    cols(0),
    white_angle(_white_angle),
    black_angle(_black_angle)
{
}


Chessboard::Board::Board(const Chessboard::Board &other):
    top_left(NULL),
    rows(0),
    cols(0)
{
    *this = other;
}

Chessboard::Board::Board(const cv::Size &size, const std::vector<cv::Point2f> &points,float _white_angle,float _black_angle):
    top_left(NULL),
    rows(0),
    cols(0),
    white_angle(_white_angle),
    black_angle(_black_angle)
{
    if(size.width*size.height != int(points.size()))
        CV_Error(Error::StsBadArg,"size mismatch");
    if(size.width < 3 || size.height < 3)
        CV_Error(Error::StsBadArg,"at least 3 rows and cols are needed to initialize the board");

    // init board with 3x3
    // TODO write function speeding up the copying
    cv::Mat data = cv::Mat(points).reshape(2,size.height);
    cv::Mat temp;
    data(cv::Rect(0,0,3,3)).copyTo(temp);
    std::vector<cv::Point2f> ipoints = temp.reshape(2,1);
    if(!init(ipoints))
        return;

    // add all cols
    for(int col=3 ; col< data.cols;++col)
    {
        data(cv::Rect(col,0,1,3)).copyTo(temp);
        ipoints = temp.reshape(2,1);
        addColumnRight(ipoints);
    }

    // add all rows
    for(int row=3; row < data.rows;++row)
    {
        data(cv::Rect(0,row,cols,1)).copyTo(temp);
        ipoints = temp.reshape(2,1);
        addRowBottom(ipoints);
    }
}

Chessboard::Board::~Board()
{
    clear();
}

std::vector<cv::Point2f> Chessboard::Board::getCellCenters()const
{
    int icols = int(colCount());
    int irows = int(rowCount());
    if(icols < 3 || irows < 3)
        throw std::runtime_error("getCellCenters: Chessboard must be at least consist of 3 rows and cols to calcualte the cell centers");

    std::vector<cv::Point2f> points;
    cv::Matx33d H(estimateHomography(DUMMY_FIELD_SIZE));
    cv::Vec3d pt1,pt2;
    pt1[2] = 1;
    for(int row = 0;row < irows;++row)
    {
        pt1[1] = (0.5+row)*DUMMY_FIELD_SIZE;
        for(int col= 0;col< icols;++col)
        {
            pt1[0] = (0.5+col)*DUMMY_FIELD_SIZE;
            pt2 = H*pt1;
            points.push_back(cv::Point2f(float(pt2[0]/pt2[2]),float(pt2[1]/pt2[2])));
        }
    }
    return points;
}

void Chessboard::Board::draw(cv::InputArray m,cv::OutputArray out,cv::InputArray _H)const
{
    cv::Mat H = _H.getMat();
    if(H.empty())
        H = estimateHomography();
    cv::Mat image = m.getMat().clone();
    if(image.type() == CV_32FC1)
    {
        double maxVal,minVal;
        cv::minMaxLoc(image, &minVal, &maxVal);
        double scale = 255.0/(maxVal-minVal);
        image.convertTo(image,CV_8UC1,scale,-scale*minVal);
        cv::applyColorMap(image,image,cv::COLORMAP_JET);
    }

    // draw all points and search areas
    std::vector<cv::Point2f> points = getCorners();
    std::vector<cv::Point2f>::const_iterator iter1 = points.begin();
    int icols = int(colCount());
    int irows = int(rowCount());
    int count=0;
    for(int row=0;row<irows;++row)
    {
        for(int col=0;col<icols;++col,++iter1)
        {
            if(iter1->x != iter1->x)    // NaN check
            {
                // draw search ellipse
                Ellipse ellipse = estimateSearchArea(H,row,col,0.4F);
                ellipse.draw(image,cv::Scalar::all(200));
            }
            else
            {
                cv::circle(image,*iter1,4,cv::Scalar(count*20,count*20,count*20,255),-1);
                ++count;
            }
        }
    }

    // draw field colors
    for(int row=0;row<irows-1;++row)
    {
        for(int col=0;col<icols-1;++col)
        {
            const Cell *cell = getCell(row,col);
            cv::Point2f center = *cell->top_left+*cell->top_right+*cell->bottom_left+*cell->bottom_right;
            center.x /=4;
            center.y /=4;
            int size = 4;
            if(row==0&&col==0)
                size=8;
            if(row==0&&col==1)
                size=7;
            if(cell->black)
                cv::circle(image,center,size,cv::Scalar::all(255),-1);
            else
                cv::circle(image,center,size,cv::Scalar(0,0,10,255),-1);
        }
    }

    out.create(image.rows,image.cols,image.type());
    image.copyTo(out.getMat());
}

bool Chessboard::Board::estimatePose(const cv::Size2f &real_size,cv::InputArray _K,cv::OutputArray rvec,cv::OutputArray tvec)const
{
    cv::Mat K = _K.getMat();
    CV_CheckTypeEQ(K.type(), CV_64FC1, "wrong K type");
    CV_CheckEQ(K.size(), Size(3, 3), "wrong K size");
    if(isEmpty())
        return false;

    int icols = int(colCount());
    int irows = int(rowCount());
    float field_width = real_size.width/(icols+1);
    float field_height= real_size.height/(irows+1);
    // the center of the board is placed at (0,0,1)
    int offset_x = int(-(icols-1)*field_width*0.5F);
    int offset_y = int(-(irows-1)*field_width*0.5F);

    std::vector<cv::Point2f> image_points;
    std::vector<cv::Point3f> object_points;
    std::vector<cv::Point2f> corners_temp = getCorners(true);
    std::vector<cv::Point2f>::const_iterator iter = corners_temp.begin();
    for(int row = 0;row < irows;++row)
    {
        for(int col= 0;col<icols;++col,++iter)
        {
            if(iter == corners_temp.end())
                CV_Error(Error::StsInternal,"internal error");
            if(iter->x != iter->x)      // NaN check
                continue;
            image_points.push_back(*iter);
            object_points.push_back(cv::Point3f(field_width*col-offset_x,field_height*row-offset_y,1.0));
        }
    }
    return cv::solvePnP(object_points,image_points,K,cv::Mat(),rvec,tvec);//,cv::SOLVEPNP_P3P);
}

float Chessboard::Board::getBlackAngle()const
{
    return black_angle;
}

float Chessboard::Board::getWhiteAngle()const
{
    return white_angle;
}

void Chessboard::Board::swap(Chessboard::Board &other)
{
    corners.swap(other.corners);
    cells.swap(other.cells);
    std::swap(rows,other.rows);
    std::swap(cols,other.cols);
    std::swap(top_left,other.top_left);
    std::swap(white_angle,other.white_angle);
    std::swap(black_angle,other.black_angle);
}

Chessboard::Board& Chessboard::Board::operator=(const Chessboard::Board &other)
{
    if(this == &other)
        return *this;
    clear();
    rows = other.rows;
    cols = other.cols;
    white_angle = other.white_angle;
    black_angle = other.black_angle;
    cells.reserve(other.cells.size());
    corners.reserve(other.corners.size());

    //copy all points and generate mapping
    std::map<cv::Point2f*,cv::Point2f*> point_point_mapping;
    point_point_mapping[NULL] = NULL;
    std::vector<cv::Point2f*>::const_iterator iter = other.corners.begin();
    for(;iter != other.corners.end();++iter)
    {
        cv::Point2f *pt = new cv::Point2f(**iter);
        point_point_mapping[*iter] = pt;
        corners.push_back(pt);
    }

    //copy all cells using mapping
    std::map<Cell*,Cell*> cell_cell_mapping;
    std::vector<Cell*>::const_iterator iter2 = other.cells.begin();
    for(;iter2 != other.cells.end();++iter2)
    {
        Cell *cell = new Cell;
        cell->top_left = point_point_mapping[(*iter2)->top_left];
        cell->top_right= point_point_mapping[(*iter2)->top_right];
        cell->bottom_right= point_point_mapping[(*iter2)->bottom_right];
        cell->bottom_left = point_point_mapping[(*iter2)->bottom_left];
        cell->black = (*iter2)->black;
        cell_cell_mapping[*iter2] = cell;
        cells.push_back(cell);
    }

    //set cell connections using mapping
    cell_cell_mapping[NULL] = NULL;
    iter2 = other.cells.begin();
    std::vector<Cell*>::iterator iter3 = cells.begin();
    for(;iter2 != other.cells.end();++iter2,++iter3)
    {
        (*iter3)->left = cell_cell_mapping[(*iter2)->left];
        (*iter3)->top = cell_cell_mapping[(*iter2)->top];
        (*iter3)->right = cell_cell_mapping[(*iter2)->right];
        (*iter3)->bottom= cell_cell_mapping[(*iter2)->bottom];
    }
    top_left = cell_cell_mapping[other.top_left];
    return *this;
}

void Chessboard::Board::normalizeOrientation(bool bblack)
{
    // fix ordering
    cv::Point2f y = getCorner(0,1)-getCorner(2,1);
    cv::Point2f x = getCorner(1,2)-getCorner(1,0);
    cv::Point3f y3d(y.x,y.y,0);
    cv::Point3f x3d(x.x,x.y,0);
    if(x3d.cross(y3d).z > 0)
        flipHorizontal();

    //normalize orientation so that first element is black or white
    const Cell* cell = getCell(0,0);
    if(cell->black != bblack && colCount()%2 != 0)
        rotateLeft();
    else if(cell->black != bblack && rowCount()%2 != 0)
    {
        rotateLeft();
        rotateLeft();
    }

    //find closest point to top left image corner
    //in case of symmetric checkerboard
    if(colCount() == rowCount())
    {
        PointIter iter_top_right(top_left,TOP_RIGHT);
        while(iter_top_right.right());
        PointIter iter_bottom_right(iter_top_right);
        while(iter_bottom_right.bottom());
        PointIter iter_bottom_left(top_left,BOTTOM_LEFT);
        while(iter_bottom_left.bottom());
        // check if one of the cell is empty and do not normalize if so
        if(top_left->empty() || iter_top_right.getCell()->empty() ||
                iter_bottom_left.getCell()->empty() || iter_bottom_right.getCell()->empty())
            return;

        float d1 = pow(top_left->top_left->x,2)+pow(top_left->top_left->y,2);
        float d2 = pow((*iter_top_right)->x,2)+pow((*iter_top_right)->y,2);
        float d3 = pow((*iter_bottom_left)->x,2)+pow((*iter_bottom_left)->y,2);
        float d4 = pow((*iter_bottom_right)->x,2)+pow((*iter_bottom_right)->y,2);
        if(d2 <= d1 && d2 <= d3 && d2 <= d4) // top left is top right
            rotateLeft();
        else if(d3 <= d1 && d3 <= d2 && d3 <= d4) // top left is bottom left
            rotateRight();
        else if(d4 <= d1 && d4 <= d2 && d4 <= d3)      // top left is bottom right
        {
            rotateLeft();
            rotateLeft();
        }
    }
}

void Chessboard::Board::rotateRight()
{
    PointIter p_iter(top_left,BOTTOM_LEFT);
    while(p_iter.bottom());

    std::vector<Cell*>::iterator iter = cells.begin();
    for(;iter != cells.end();++iter)
    {
        Cell *temp = (*iter)->bottom;
        (*iter)->bottom = (*iter)->right;
        (*iter)->right= (*iter)->top;
        (*iter)->top= (*iter)->left;
        (*iter)->left = temp;

        cv::Point2f *ptemp = (*iter)->bottom_left;
        (*iter)->bottom_left= (*iter)->bottom_right;
        (*iter)->bottom_right= (*iter)->top_right;
        (*iter)->top_right= (*iter)->top_left;
        (*iter)->top_left= ptemp;
    }
    int temp = rows;
    rows = cols;
    cols = temp;
    top_left = p_iter.getCell();
}


void Chessboard::Board::rotateLeft()
{
    PointIter p_iter(top_left,TOP_RIGHT);
    while(p_iter.right());

    std::vector<Cell*>::iterator iter = cells.begin();
    for(;iter != cells.end();++iter)
    {
        Cell *temp = (*iter)->top;
        (*iter)->top = (*iter)->right;
        (*iter)->right= (*iter)->bottom;
        (*iter)->bottom= (*iter)->left;
        (*iter)->left = temp;

        cv::Point2f *ptemp = (*iter)->top_left;
        (*iter)->top_left = (*iter)->top_right;
        (*iter)->top_right= (*iter)->bottom_right;
        (*iter)->bottom_right = (*iter)->bottom_left;
        (*iter)->bottom_left = ptemp;
    }
    int temp = rows;
    rows = cols;
    cols = temp;
    top_left = p_iter.getCell();
}

void Chessboard::Board::flipHorizontal()
{
    PointIter p_iter(top_left,TOP_RIGHT);
    while(p_iter.right());

    std::vector<Cell*>::iterator iter = cells.begin();
    for(;iter != cells.end();++iter)
    {
        Cell *temp = (*iter)->right;
        (*iter)->right= (*iter)->left;
        (*iter)->left = temp;

        cv::Point2f *ptemp = (*iter)->top_left;
        (*iter)->top_left = (*iter)->top_right;
        (*iter)->top_right = ptemp;

        ptemp = (*iter)->bottom_left;
        (*iter)->bottom_left = (*iter)->bottom_right;
        (*iter)->bottom_right = ptemp;
    }
    top_left = p_iter.getCell();
}

void Chessboard::Board::flipVertical()
{
    PointIter p_iter(top_left,BOTTOM_LEFT);
    while(p_iter.bottom());

    std::vector<Cell*>::iterator iter = cells.begin();
    for(;iter != cells.end();++iter)
    {
        Cell *temp = (*iter)->top;
        (*iter)->top= (*iter)->bottom;
        (*iter)->bottom = temp;

        cv::Point2f *ptemp = (*iter)->top_left;
        (*iter)->top_left = (*iter)->bottom_left;
        (*iter)->bottom_left = ptemp;

        ptemp = (*iter)->top_right;
        (*iter)->top_right = (*iter)->bottom_right;
        (*iter)->bottom_right = ptemp;
    }
    top_left = p_iter.getCell();
}

// returns the best found score
// if NaN is returned for a point no point at all was found
// if 0 is returned the point lies outside of the ellipse
float Chessboard::Board::findMaxPoint(cv::flann::Index &index,const cv::Mat &data,const Ellipse &ellipse,float white_angle,float black_angle,cv::Point2f &point)
{
    // flann data type enriched with angles (third column)
    CV_CheckType(data.type(), CV_32FC1, "type of flann data is not supported");
    CV_CheckEQ(data.cols, 4, "4-cols flann data is expected");

    std::vector<float> query,dists;
    std::vector<int> indices;
    query.resize(2);
    point = ellipse.getCenter();
    query[0] = point.x;
    query[1] = point.y;
    index.knnSearch(query,indices,dists,4,cv::flann::SearchParams(64));
    std::vector<int>::const_iterator iter = indices.begin();
    float best_score = -std::numeric_limits<float>::max();
    point.x = std::numeric_limits<float>::quiet_NaN();
    point.y = std::numeric_limits<float>::quiet_NaN();
    for(;iter != indices.end();++iter)
    {
        const float *val = data.ptr<float>(*iter);
        const float &response = *(val+3);
        if(response < best_score)
            continue;
        const float &a0 = *(val+2);
        float a1 = std::fabs(a0-white_angle);
        float a2 = std::fabs(a0-black_angle);
        if(a1 > CV_PI*0.5)
            a1 = std::fabs(float(a1-CV_PI));
        if(a2> CV_PI*0.5)
            a2 = std::fabs(float(a2-CV_PI));
        if(a1  < MAX_ANGLE || a2 < MAX_ANGLE )
        {
            cv::Point2f pt(val[0], val[1]);
            if(point.x != point.x)       // NaN check
                point = pt;
            if(best_score < response && ellipse.contains(pt))
            {
                best_score = response;
                point = pt;
            }
        }
    }
    if(best_score == -std::numeric_limits<float>::max())
        return 0;
    else
        return best_score;
}

void Chessboard::Board::clear()
{
    top_left = NULL; rows = 0; cols = 0;
    std::vector<Cell*>::iterator iter = cells.begin();
    for(;iter != cells.end();++iter)
        delete *iter;
    cells.clear();
    std::vector<cv::Point2f*>::iterator iter2 = corners.begin();
    for(;iter2 != corners.end();++iter2)
        delete *iter2;
    corners.clear();
}

// p0 p1 p2
// p3 p4 p5
// p6 p7 p8
bool Chessboard::Board::init(const std::vector<cv::Point2f> points)
{
    clear();
    if(points.size() != 9)
        CV_Error(Error::StsBadArg,"exact nine points are expected to initialize the board");

    // generate cells
    corners.resize(9);
    for(int i=0;i < 9;++i)
        corners[i] = new cv::Point2f(points[i]);
    cells.resize(4);
    for(int i=0;i<4;++i)
        cells[i] = new Cell();

    //cell 0
    cells[0]->top_left = corners[0];
    cells[0]->top_right = corners[1];
    cells[0]->bottom_right = corners[4];
    cells[0]->bottom_left = corners[3];
    cells[0]->right = cells[1];
    cells[0]->bottom = cells[2];

    //cell 1
    cells[1]->top_left = corners[1];
    cells[1]->top_right = corners[2];
    cells[1]->bottom_right = corners[5];
    cells[1]->bottom_left = corners[4];
    cells[1]->left = cells[0];
    cells[1]->bottom = cells[3];

    //cell 2
    cells[2]->top_left = corners[3];
    cells[2]->top_right = corners[4];
    cells[2]->bottom_right = corners[7];
    cells[2]->bottom_left = corners[6];
    cells[2]->top = cells[0];
    cells[2]->right = cells[3];

    //cell 3
    cells[3]->top_left = corners[4];
    cells[3]->top_right = corners[5];
    cells[3]->bottom_right = corners[8];
    cells[3]->bottom_left = corners[7];
    cells[3]->top = cells[1];
    cells[3]->left= cells[2];

    top_left = cells.front();
    rows = 3;
    cols = 3;

    // set inital cell colors
    Point2f pt1 = *(cells[0]->top_right)-*(cells[0]->bottom_left);
    pt1 /= cv::norm(pt1);
    cv::Point2f pt2(cos(white_angle),-sin(white_angle));
    cv::Point2f pt3(cos(black_angle),-sin(black_angle));
    if(fabs(pt1.dot(pt2)) < fabs(pt1.dot(pt3)))
    {
        cells[0]->black = false;
        cells[1]->black = true;
        cells[2]->black = true;
        cells[3]->black = false;
    }
    else
    {
        cells[0]->black = true;
        cells[1]->black = false;
        cells[2]->black = false;
        cells[3]->black = true;
    }
    return true;
}

//TODO magic number
bool Chessboard::Board::estimatePoint(const cv::Point2f &p0,const cv::Point2f &p1,const cv::Point2f &p2, cv::Point2f &p3)
{
    // use cross ration to find new point
    if(p0 == p1 || p0 == p2 || p1 == p2)
        return false;
    cv::Point2f p01 = p1-p0;
    cv::Point2f p12 = p2-p1;
    float a = float(cv::norm(p01));
    float b = float(cv::norm(p12));
    float t = (0.75F*a-0.25F*b);
    if(t <= 0)
        return false;
    float c = 0.25F*b*(a+b)/t;
    if(c < 0.1F)
        return false;
    p01 = p01/a;
    p12 = p12/b;
    // check angle between p01 and p12 < 25°
    if(p01.dot(p12) < 0.9)
        return false;
    // calc mean
    // p12 = (p01+p12)*0.5;
    // p3 = p2+p12*c;
    p3 = p2+p12*c;

    // compensate radial distortion by fitting polynom
    std::vector<double> x,y;
    x.resize(3,0); y.resize(3,0);
    x[1] = b;
    x[2] = b+a;
    y[2] = calcSignedDistance(-p12,p2,p0);
    cv::Mat dst;
    polyfit(cv::Mat(x),cv::Mat(y),dst,2);
    double d = dst.at<double>(0)-dst.at<double>(1)*c+dst.at<double>(2)*c*c;
    cv::Vec3f v1(p12.x,p12.y,0);
    cv::Vec3f v2(0,0,1);
    cv::Vec3f v3 = v1.cross(v2);
    cv::Point2f n2(v3[0],v3[1]);
    p3 += d*n2;
    return true;
}

bool Chessboard::Board::estimatePoint(const cv::Point2f &p0,const cv::Point2f &p1,const cv::Point2f &p2, const cv::Point2f &p3, cv::Point2f &p4)
{
    // use 1D homography to find fith point minimizing square error
    if(p0 == p1 || p0 == p2 || p0 == p3 || p1 == p2 || p1 == p3 || p2 == p3 )
        return false;
    static const cv::Mat src = (cv::Mat_<double>(1,4) << 0,10,20,30);
    cv::Point2f p01 = p1-p0;
    cv::Point2f p02 = p2-p0;
    cv::Point2f p03 = p3-p0;
    float a = float(cv::norm(p01));
    float b = float(cv::norm(p02));
    float c = float(cv::norm(p03));
    cv::Mat dst = (cv::Mat_<double>(1,4) << 0,a,b,c);
    cv::Mat h = findHomography1D(src,dst);
    float d = float((h.at<double>(0,0)*40+h.at<double>(0,1))/(h.at<double>(1,0)*40+h.at<double>(1,1)));
    cv::Point2f p12 = p2-p1;
    cv::Point2f p23 = p3-p2;
    p01 = p01/a;
    p12 = p12/cv::norm(p12);
    p23 = p23/cv::norm(p23);
    p4 = p3+(d-c)*p23;

    // compensate radial distortion by fitting polynom
    std::vector<double> x,y;
    x.resize(4,0); y.resize(4,0);
    x[1] = c-b;
    x[2] = c-a;
    x[3] = c;
    y[2] = calcSignedDistance(-p23,p3,p1);
    y[3] = calcSignedDistance(-p23,p3,p0);
    polyfit(cv::Mat(x),cv::Mat(y),dst,2);
    d = d-c;
    double e = dst.at<double>(0)-dst.at<double>(1)*fabs(d)+dst.at<double>(2)*d*d;
    cv::Vec3f v1(p23.x,p23.y,0);
    cv::Vec3f v2(0,0,1);
    cv::Vec3f v3 = v1.cross(v2);
    cv::Point2f n2(v3[0],v3[1]);
    p4 += e*n2;
    return true;
}

// H is describing the transformation from dummy to reality
Ellipse Chessboard::Board::estimateSearchArea(cv::Mat _H,int row, int col,float p,int field_size)
{
    cv::Matx31d point1,point2,center;
    center(0) = (1+col)*field_size;
    center(1) = (1+row)*field_size;
    center(2) = 1.0;
    point1(0) = center(0)-p*field_size;
    point1(1) = center(1);
    point1(2) = center(2);
    point2(0) = center(0);
    point2(1) = center(1)-p*field_size;
    point2(2) = center(2);

    cv::Matx33d H(_H);
    point1 = H*point1;
    point2 = H*point2;
    center = H*center;
    cv::Point2f pt(float(center(0)/center(2)),float(center(1)/center(2)));
    cv::Point2f pt1(float(point1(0)/point1(2)),float(point1(1)/point1(2)));
    cv::Point2f pt2(float(point2(0)/point2(2)),float(point2(1)/point2(2)));

    cv::Point2f p01(pt1-pt);
    cv::Point2f p02(pt2-pt);
    float norm1 = float(cv::norm(p01));
    float norm2 = float(cv::norm(p02));
    float angle = float(acos(p01.dot(p02)/norm1/norm2));
    cv::Size2f axes(norm1,norm2);
    return Ellipse(pt,axes,angle);
}

bool Chessboard::Board::estimateSearchArea(const cv::Point2f &p1,const cv::Point2f &p2,const cv::Point2f &p3,float p,Ellipse &ellipse,const cv::Point2f *p0)
{
    cv::Point2f p4,n;
    if(p0)
    {
        // use 1D homography
        if(!estimatePoint(*p0,p1,p2,p3,p4))
            return false;
        n = p4-*p0;
    }
    else
    {
        // use cross ratio
        if(!estimatePoint(p1,p2,p3,p4))
            return false;
        n = p4-p1;
    }
    float norm = float(cv::norm(n));
    n = n/norm;
    float angle = acos(n.x);
    if(n.y > 0)
        angle = float(2.0F*CV_PI-angle);
    n = p4-p3;
    norm = float(cv::norm(n));
    double delta = std::max(3.0F,p*norm);
    ellipse = Ellipse(p4,cv::Size(int(delta),int(std::max(2.0,delta*ELLIPSE_WIDTH))),angle);
    return true;
}

bool Chessboard::Board::checkRowColumn(const std::vector<cv::Point2f> &points)
{
    if(points.size() < 4)
    {
        if(points.size() == 3)
            return true;
        else
            return false;
    }
    std::vector<cv::Point2f>::const_iterator iter = points.begin();
    std::vector<cv::Point2f>::const_iterator iter2 = iter+1;
    std::vector<cv::Point2f>::const_iterator iter3 = iter2+1;
    std::vector<cv::Point2f>::const_iterator iter4 = iter3+1;
    Ellipse ellipse;
    if(!estimateSearchArea(*iter4,*iter3,*iter2,CORNERS_SEARCH*3,ellipse))
        return false;
    if(!ellipse.contains(*iter))
        return false;

    std::vector<cv::Point2f>::const_iterator iter5 = iter4+1;
    for(;iter5 != points.end();++iter5)
    {
        if(!estimateSearchArea(*iter2,*iter3,*iter4,CORNERS_SEARCH,ellipse,&(*iter)))
            return false;
        if(!ellipse.contains(*iter5))
            return false;
        iter = iter2;
        iter2 = iter3;
        iter3 = iter4;
        iter4 = iter5;
    }
    return true;
}

cv::Point2f &Chessboard::Board::getCorner(int _row,int _col)
{
    int _rows = int(rowCount());
    int _cols = int(colCount());
    if(_row >= _rows || _col >= _cols)
        CV_Error(Error::StsBadArg,"out of bound");
    if(_row == 0)
    {
        PointIter iter(top_left,TOP_LEFT);
        int count = 0;
        do
        {
            if(count == _col)
                return *(*iter);
            ++count;
        }while(iter.right());
    }
    else
    {
        Cell *row_start = top_left;
        int count = 1;
        do
        {
            if(count == _row)
            {
                PointIter iter(row_start,BOTTOM_LEFT);
                int count2 = 0;
                do
                {
                    if(count2 == _col)
                        return *(*iter);
                    ++count2;
                }while(iter.right());
            }
            ++count;
            row_start = row_start->bottom;
        }while(_row);
    }
    CV_Error(Error::StsInternal,"cannot find corner");
    // return *top_left->top_left; // never reached
}

bool Chessboard::Board::isCellBlack(int row,int col)const
{
    return getCell(row,col)->black;
}

bool Chessboard::Board::isCellEmpty(int row,int col)
{
    return getCell(row,col)->empty();
}

Chessboard::Board::Cell* Chessboard::Board::getCell(int row,int col)
{
    const Cell *cell = const_cast<const Board*>(this)->getCell(row,col);
    return const_cast<Cell*>(cell);
}

const Chessboard::Board::Cell* Chessboard::Board::getCell(int row,int col)const
{
    if(row > rows-1 || row < 0 || col > cols-1 || col < 0)
        CV_Error(Error::StsBadArg,"out of bound");
    PointIter p_iter(top_left,BOTTOM_RIGHT);
    for(int i=0; i< row; p_iter.bottom(),++i);
    for(int i=0; i< col; p_iter.right(),++i);
    return p_iter.getCell();
}


bool Chessboard::Board::isEmpty()const
{
    return cells.empty();
}

size_t Chessboard::Board::colCount()const
{
    return cols;
}

size_t Chessboard::Board::rowCount()const
{
    return rows;
}

cv::Size Chessboard::Board::getSize()const
{
    return cv::Size(int(colCount()),int(rowCount()));
}

void Chessboard::Board::drawEllipses(const std::vector<Ellipse> &ellipses)
{
    // currently there is no global image find way to store global image
    // without polluting namespace
    if(ellipses.empty())
        return;     //avoid compiler warning
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
    cv::Mat img;
    draw(debug_image,img);
    std::vector<Ellipse>::iterator iter;
    for(;iter != ellipses.end();++iter)
        iter->draw(img);
    cv::imshow("chessboard",img);
    cv::waitKey(-1);
#endif
}


void Chessboard::Board::growLeft()
{
    if(isEmpty())
        CV_Error(Error::StsInternal,"Board is empty");
    PointIter iter(top_left,TOP_LEFT);
    std::vector<cv::Point2f> points;
    cv::Point2f pt;
    do
    {
        PointIter iter2(iter);
        cv::Point2f *p0 = *iter2;
        iter2.right();
        cv::Point2f *p1 = *iter2;
        iter2.right();
        cv::Point2f *p2 = *iter2;
        if(iter2.right())
            estimatePoint(**iter2,*p2,*p1,*p0,pt);
        else
            estimatePoint(*p2,*p1,*p0,pt);
        points.push_back(pt);
    }
    while(iter.bottom());
    addColumnLeft(points);
}

bool Chessboard::Board::growLeft(const cv::Mat &map,cv::flann::Index &flann_index)
{
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
    std::vector<Ellipse> ellipses;
#endif
    if(isEmpty())
        CV_Error(Error::StsInternal,"growLeft: Board is empty");
    PointIter iter(top_left,TOP_LEFT);
    std::vector<cv::Point2f> points;
    int count = 0;
    Ellipse ellipse;
    cv::Point2f pt;
    do
    {
        PointIter iter2(iter);
        cv::Point2f *p0 = *iter2;
        iter2.right();
        cv::Point2f *p1 = *iter2;
        iter2.right();
        cv::Point2f *p2 = *iter2;
        cv::Point2f *p3 = NULL;
        if(iter2.right())
            p3 = *iter2;
        if(!estimateSearchArea(*p2,*p1,*p0,CORNERS_SEARCH,ellipse,p3))
            return false;
        float result = findMaxPoint(flann_index,map,ellipse,white_angle,black_angle,pt);
        if(pt == *p0)
        {
            ++count;
            points.push_back(ellipse.getCenter());
        }
        else if(result != 0)
        {
            points.push_back(pt);
            if(result < 0)
                ++count;
        }
        else
        {
            ++count;
            if(pt.x != pt.x)    // NaN check
                points.push_back(ellipse.getCenter());
            else
                points.push_back(pt);
        }
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
        ellipses.push_back(ellipse);
#endif
    }
    while(iter.bottom());
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
    drawEllipses(ellipses);
#endif
    if(points.size()-count <= 2)
        return false;
    if(count > points.size()*0.5 || !checkRowColumn(points))
        return false;
    addColumnLeft(points);
    return true;
}

void Chessboard::Board::growTop()
{
    if(isEmpty())
        CV_Error(Error::StsInternal,"Board is empty");
    PointIter iter(top_left,TOP_LEFT);
    std::vector<cv::Point2f> points;
    cv::Point2f pt;
    do
    {
        PointIter iter2(iter);
        cv::Point2f *p0 = *iter2;
        iter2.bottom();
        cv::Point2f *p1 = *iter2;
        iter2.bottom();
        cv::Point2f *p2 = *iter2;
        if(iter2.bottom())
            estimatePoint(**iter2,*p2,*p1,*p0,pt);
        else
            estimatePoint(*p2,*p1,*p0,pt);
        points.push_back(pt);
    }
    while(iter.right());
    addRowTop(points);
}

bool Chessboard::Board::growTop(const cv::Mat &map,cv::flann::Index &flann_index)
{
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
    std::vector<Ellipse> ellipses;
#endif
    if(isEmpty())
        CV_Error(Error::StsInternal,"Board is empty");

    PointIter iter(top_left,TOP_LEFT);
    std::vector<cv::Point2f> points;
    int count = 0;
    Ellipse ellipse;
    cv::Point2f pt;
    do
    {
        PointIter iter2(iter);
        cv::Point2f *p0 = *iter2;
        iter2.bottom();
        cv::Point2f *p1 = *iter2;
        iter2.bottom();
        cv::Point2f *p2 = *iter2;
        cv::Point2f *p3 = NULL;
        if(iter2.bottom())
            p3 = *iter2;
        if(!estimateSearchArea(*p2,*p1,*p0,CORNERS_SEARCH,ellipse,p3))
            return false;
        float result = findMaxPoint(flann_index,map,ellipse,white_angle,black_angle,pt);
        if(pt == *p0)
        {
            ++count;
            points.push_back(ellipse.getCenter());
        }
        else if(result != 0)
        {
            points.push_back(pt);
            if(result < 0)
                ++count;
        }
        else
        {
            ++count;
            if(pt.x != pt.x)    // NaN check
                points.push_back(ellipse.getCenter());
            else
                points.push_back(pt);
        }
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
        ellipses.push_back(ellipse);
#endif
    }
    while(iter.right());
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
    drawEllipses(ellipses);
#endif
    if(count > points.size()*0.5 || !checkRowColumn(points))
        return false;
    addRowTop(points);
    return true;
}

void Chessboard::Board::growRight()
{
    if(isEmpty())
        CV_Error(Error::StsInternal,"Board is empty");
    PointIter iter(top_left,TOP_RIGHT);
    while(iter.right());
    std::vector<cv::Point2f> points;
    cv::Point2f pt;
    do
    {
        PointIter iter2(iter);
        cv::Point2f *p0 = *iter2;
        iter2.left();
        cv::Point2f *p1 = *iter2;
        iter2.left();
        cv::Point2f *p2 = *iter2;
        if(iter2.left())
            estimatePoint(**iter2,*p2,*p1,*p0,pt);
        else
            estimatePoint(*p2,*p1,*p0,pt);
        points.push_back(pt);
    }
    while(iter.bottom());
    addColumnRight(points);
}

bool Chessboard::Board::growRight(const cv::Mat &map,cv::flann::Index &flann_index)
{
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
    std::vector<Ellipse> ellipses;
#endif
    if(isEmpty())
        CV_Error(Error::StsInternal,"Board is empty");

    PointIter iter(top_left,TOP_RIGHT);
    while(iter.right());
    std::vector<cv::Point2f> points;
    cv::Point2f pt;
    Ellipse ellipse;
    int count = 0;
    do
    {
        PointIter iter2(iter);
        cv::Point2f *p0 = *iter2;
        iter2.left();
        cv::Point2f *p1 = *iter2;
        iter2.left();
        cv::Point2f *p2 = *iter2;
        cv::Point2f *p3 = NULL;
        if(iter2.left())
            p3 = *iter2;
        if(!estimateSearchArea(*p2,*p1,*p0,CORNERS_SEARCH,ellipse,p3))
            return false;
        float result = findMaxPoint(flann_index,map,ellipse,white_angle,black_angle,pt);
        if(pt == *p0)
        {
            ++count;
            points.push_back(ellipse.getCenter());
        }
        else if(result != 0)
        {
            points.push_back(pt);
            if(result < 0)
                ++count;
        }
        else
        {
            ++count;
            if(pt.x != pt.x)     // NaN check
                points.push_back(ellipse.getCenter());
            else
                points.push_back(pt);
        }
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
        ellipses.push_back(ellipse);
#endif
    }
    while(iter.bottom());
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
    drawEllipses(ellipses);
#endif
    if(count > points.size()*0.5 || !checkRowColumn(points))
        return false;
    addColumnRight(points);
    return true;
}

void Chessboard::Board::growBottom()
{
    if(isEmpty())
        CV_Error(Error::StsInternal,"Board is empty");

    PointIter iter(top_left,BOTTOM_LEFT);
    while(iter.bottom());
    std::vector<cv::Point2f> points;
    cv::Point2f pt;
    do
    {
        PointIter iter2(iter);
        cv::Point2f *p0 = *iter2;
        iter2.top();
        cv::Point2f *p1 = *iter2;
        iter2.top();
        cv::Point2f *p2 = *iter2;
        if(iter2.top())
            estimatePoint(**iter2,*p2,*p1,*p0,pt);
        else
            estimatePoint(*p2,*p1,*p0,pt);
        points.push_back(pt);
    }
    while(iter.right());
    addRowBottom(points);
}

bool Chessboard::Board::growBottom(const cv::Mat &map,cv::flann::Index &flann_index)
{
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
    std::vector<Ellipse> ellipses;
#endif
    if(isEmpty())
        CV_Error(Error::StsInternal,"Board is empty");

    PointIter iter(top_left,BOTTOM_LEFT);
    while(iter.bottom());
    std::vector<cv::Point2f> points;
    cv::Point2f pt;
    Ellipse ellipse;
    int count = 0;
    do
    {
        PointIter iter2(iter);
        cv::Point2f *p0 = *iter2;
        iter2.top();
        cv::Point2f *p1 = *iter2;
        iter2.top();
        cv::Point2f *p2 = *iter2;
        cv::Point2f *p3 = NULL;
        if(iter2.top())
            p3 = *iter2;
        if(!estimateSearchArea(*p2,*p1,*p0,CORNERS_SEARCH,ellipse,p3))
            return false;
        float result = findMaxPoint(flann_index,map,ellipse,white_angle,black_angle,pt);
        if(pt == *p0)
        {
            ++count;
            points.push_back(ellipse.getCenter());
        }
        else if(result != 0)
        {
            points.push_back(pt);
            if(result < 0)
                ++count;
        }
        else
        {
            ++count;
            if(pt.x != pt.x)     // NaN check
                points.push_back(ellipse.getCenter());
            else
                points.push_back(pt);
        }
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
        ellipses.push_back(ellipse);
#endif
    }
    while(iter.right());
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
    drawEllipses(ellipses);
#endif
    if(count > points.size()*0.5 || !checkRowColumn(points))
        return false;
    addRowBottom(points);
    return true;
}

void Chessboard::Board::addColumnLeft(const std::vector<cv::Point2f> &points)
{
    if(points.empty() || points.size() != rowCount())
        CV_Error(Error::StsBadArg,"wrong number of points");

    int offset = int(cells.size());
    cells.resize(offset+points.size()-1);
    for(int i = offset;i < (int) cells.size();++i)
        cells[i] = new Cell();
    corners.push_back(new cv::Point2f(points.front()));

    Cell *cell = top_left;
    std::vector<cv::Point2f>::const_iterator iter = points.begin()+1;
    for(int pos=offset;iter != points.end();++iter,cell = cell->bottom,++pos)
    {
        cell->left = cells[pos];
        cells[pos]->black = !cell->black;
        if(pos != offset)
            cells[pos]->top = cells[pos-1];
        cells[pos]->right = cell;
        if(pos +1 < (int)cells.size())
            cells[pos]->bottom= cells[pos+1];
        cells[pos]->top_left = corners.back();
        corners.push_back(new cv::Point2f(*iter));
        cells[pos]->bottom_left = corners.back();
        cells[pos]->top_right=cell->top_left;
        cells[pos]->bottom_right=cell->bottom_left;
    }
    top_left = cells[offset];
    ++cols;
}

void Chessboard::Board::addRowTop(const std::vector<cv::Point2f> &points)
{
    if(points.empty() || points.size() != colCount())
        CV_Error(Error::StsBadArg,"wrong number of points");

    int offset = int(cells.size());
    cells.resize(offset+points.size()-1);
    for(int i = offset;i < (int) cells.size();++i)
        cells[i] = new Cell();
    corners.push_back(new cv::Point2f(points.front()));

    Cell *cell = top_left;
    std::vector<cv::Point2f>::const_iterator iter = points.begin()+1;
    for(int pos=offset;iter != points.end();++iter,cell = cell->right,++pos)
    {
        cell->top = cells[pos];
        cells[pos]->black = !cell->black;
        if(pos != offset)
            cells[pos]->left= cells[pos-1];
        cells[pos]->bottom= cell;
        if(pos +1 <(int) cells.size())
            cells[pos]->right= cells[pos+1];

        cells[pos]->top_left = corners.back();
        corners.push_back(new cv::Point2f(*iter));
        cells[pos]->top_right = corners.back();
        cells[pos]->bottom_left = cell->top_left;
        cells[pos]->bottom_right = cell->top_right;
    }
    top_left = cells[offset];
    ++rows;
}

void Chessboard::Board::addColumnRight(const std::vector<cv::Point2f> &points)
{
    if(points.empty() || points.size() != rowCount())
        CV_Error(Error::StsBadArg,"wrong number of points");

    int offset = int(cells.size());
    cells.resize(offset+points.size()-1);
    for(int i = offset;i < (int) cells.size();++i)
        cells[i] = new Cell();
    corners.push_back(new cv::Point2f(points.front()));

    Cell *cell = top_left;
    for(;cell->right;cell = cell->right);
    std::vector<cv::Point2f>::const_iterator iter = points.begin()+1;
    for(int pos=offset;iter != points.end();++iter,cell = cell->bottom,++pos)
    {
        cell->right = cells[pos];
        cells[pos]->black = !cell->black;
        if(pos != offset)
            cells[pos]->top= cells[pos-1];
        cells[pos]->left = cell;
        if(pos +1 <(int) cells.size())
            cells[pos]->bottom= cells[pos+1];

        cells[pos]->top_right = corners.back();
        corners.push_back(new cv::Point2f(*iter));
        cells[pos]->bottom_right = corners.back();
        cells[pos]->top_left =cell->top_right;
        cells[pos]->bottom_left =cell->bottom_right;
    }
    ++cols;
}

void Chessboard::Board::addRowBottom(const std::vector<cv::Point2f> &points)
{
    if(points.empty() || points.size() != colCount())
        CV_Error(Error::StsBadArg,"wrong number of points");

    int offset = int(cells.size());
    cells.resize(offset+points.size()-1);
    for(int i = offset;i < (int) cells.size();++i)
        cells[i] = new Cell();
    corners.push_back(new cv::Point2f(points.front()));

    Cell *cell = top_left;
    for(;cell->bottom;cell = cell->bottom);
    std::vector<cv::Point2f>::const_iterator iter = points.begin()+1;
    for(int pos=offset;iter != points.end();++iter,cell = cell->right,++pos)
    {
        cell->bottom = cells[pos];
        cells[pos]->black = !cell->black;
        if(pos != offset)
            cells[pos]->left = cells[pos-1];
        cells[pos]->top = cell;
        if(pos +1 < (int)cells.size())
            cells[pos]->right= cells[pos+1];

        cells[pos]->bottom_left = corners.back();
        corners.push_back(new cv::Point2f(*iter));
        cells[pos]->bottom_right = corners.back();
        cells[pos]->top_left = cell->bottom_left;
        cells[pos]->top_right = cell->bottom_right;
    }
    ++rows;
}

bool Chessboard::Board::checkUnique()const
{
    std::vector<cv::Point2f> points = getCorners(false);
    std::vector<cv::Point2f>::const_iterator iter = points.begin();
    for(;iter != points.end();++iter)
    {
        std::vector<cv::Point2f>::const_iterator iter2 = iter+1;
        for(;iter2 != points.end();++iter2)
        {
            if(*iter == *iter2)
                return false;
        }
    }
    return true;
}

int Chessboard::Board::validateCorners(const cv::Mat &data,cv::flann::Index &flann_index,const cv::Mat &h,float min_response)
{
    // TODO check input
    if(isEmpty() || h.empty())
        return 0;
    int count = 0; int icol = 0;
    // first row
    PointIter iter(top_left,TOP_LEFT);
    cv::Point2f point;
    do
    {
        if((*iter)->x == (*iter)->x)
            ++count;
        else
        {
            Ellipse ellipse = estimateSearchArea(h,0,icol,0.4F);
            float result = findMaxPoint(flann_index,data,ellipse,white_angle,black_angle,point);
            if(fabs(result) >= min_response)
            {
                ++count;
                **iter = point;
            }
        }
        ++icol;
    }while(iter.right());

    // all other rows
    int irow = 1;
    Cell *row = top_left;
    do
    {
        PointIter iter2(row,BOTTOM_LEFT);
        icol = 0;
        do
        {
            if((*iter2)->x == (*iter2)->x)
                ++count;
            else
            {
                Ellipse ellipse = estimateSearchArea(h,irow,icol,0.4F);
                if(min_response <= findMaxPoint(flann_index,data,ellipse,white_angle,black_angle,point))
                {
                    ++count;
                    **iter2 = point;
                }
            }
            ++icol;
        }while(iter2.right());
        row = row->bottom;
        ++irow;
    }while(row);

    // check that there are no points with the same coordinate
    std::vector<cv::Point2f> points = getCorners(false);
    std::vector<cv::Point2f>::const_iterator iter1 = points.begin();
    for(;iter1 != points.end();++iter1)
    {
        // we do not have to check for NaN because of getCorners(flase)
        std::vector<cv::Point2f>::const_iterator iter2 = iter1+1;
        for(;iter2 != points.end();++iter2)
            if(*iter1 == *iter2)
                return -1;  // one corner is there twice -> not valid configuration
    }
    return count;
}

bool Chessboard::Board::validateContour()const
{
    std::vector<cv::Point2f> contour = getContour();
    if(contour.size() != 4)
    {
        return false;
    }
    cv::Point2f n1 = contour[1]-contour[0];
    cv::Point2f n2 = contour[2]-contour[1];
    cv::Point2f n3 = contour[3]-contour[2];
    cv::Point2f n4 = contour[0]-contour[3];
    n1 = n1/cv::norm(n1);
    n2 = n2/cv::norm(n2);
    n3 = n3/cv::norm(n3);
    n4 = n4/cv::norm(n4);
    // a > b => cos(a) < cos(b)
    if(fabs(n1.dot(n2)) > MIN_COS_ANGLE||
            fabs(n2.dot(n3)) > MIN_COS_ANGLE||
            fabs(n3.dot(n4)) > MIN_COS_ANGLE||
            fabs(n4.dot(n1)) > MIN_COS_ANGLE)
        return false;
    return true;
}

std::vector<cv::Point2f> Chessboard::Board::getContour()const
{
    std::vector<cv::Point2f> points;
    if(isEmpty())
        return points;

    //find start cell part of the contour
    Cell* start_cell = NULL;
    PointIter iter(top_left,TOP_LEFT);
    do
    {
        PointIter iter2(iter);
        do
        {
            if(!iter2.getCell()->empty())
            {
                start_cell = iter2.getCell();
                iter = iter2;
                break;
            }
        }while(iter2.right());
    }while(!start_cell && iter.bottom());
    if(start_cell == NULL)
        return points;

    // trace contour
    const cv::Point2f *start_pt = *iter;
    int mode = 2; int last = -1;
    do
    {
        PointIter current_iter(iter);
        switch(mode)
        {
        case 1: // top
            if(iter.top(true))
            {
                if(last != 1)
                    points.push_back(**current_iter);
                mode = 4;
                last = 1;
                break;
            }
            /* fallthrough */
        case 2: // right
            if(iter.right(true))
            {
                if(last != 2)
                    points.push_back(**current_iter);
                mode = 1;
                last = 2;
                break;
            }
            /* fallthrough */
        case 3: // bottom
            if(iter.bottom(true))
            {
                if(last != 3)
                    points.push_back(**current_iter);
                mode = 2;
                last = 3;
                break;
            }
            /* fallthrough */
        case 4: // left
            if(iter.left(true))
            {
                if(last != 4)
                    points.push_back(**current_iter);
                mode = 3;
                last = 4;
                break;
            }
            mode = 1;
            break;
        default:
            CV_Error(Error::StsInternal,"cannot retrieve contour");
        }
    }while(*iter != start_pt);
    return points;
}


cv::Mat Chessboard::Board::estimateHomography(cv::Rect rect,int field_size)const
{
    int _rows = int(rowCount());
    int _cols = int(colCount());
    if(_rows < 3  || _cols < 3)
        return cv::Mat();
    if(rect.width <= 0)
        rect.width= _cols;
    if(rect.height <= 0)
        rect.height= _rows;

    int col_end = std::min(rect.x+rect.width,_cols);
    int row_end = std::min(rect.y+rect.height,_rows);
    std::vector<cv::Point2f> points = getCorners(true);

    // build src and dst
    std::vector<cv::Point2f> src,dst;
    for(int row =rect.y;row < row_end;++row)
    {
        for(int col=rect.x;col <col_end;++col)
        {
            const cv::Point2f &pt = points[row*_rows+col];
            if(pt.x != pt.x)    // NaN check
                continue;
            src.push_back(cv::Point2f(float(field_size)*(col+1),float(field_size)*(row+1)));
            dst.push_back(pt);
        }
    }
    if(dst.size() < 4)
        return cv::Mat();
    return cv::findHomography(src, dst,cv::LMEDS);
}

cv::Mat Chessboard::Board::estimateHomography(int field_size)const
{
    int _rows = int(rowCount());
    int _cols = int(colCount());
    if(_rows < 3  || _cols < 3)
        return cv::Mat();
    std::vector<cv::Point2f> src,dst;
    std::vector<cv::Point2f> points = getCorners(true);
    std::vector<cv::Point2f>::const_iterator iter = points.begin();
    for(int row =0;row < _rows;++row)
    {
        for(int col=0;col <_cols;++col,++iter)
        {
            const cv::Point2f &pt = *iter;
            if(pt.x == pt.x)
            {
                src.push_back(cv::Point2f(float(field_size)*(col+1),float(field_size)*(row+1)));
                dst.push_back(pt);
            }
        }
    }
    if(dst.size() < 4)
        return cv::Mat();
    return cv::findHomography(src, dst);
}

bool Chessboard::Board::findNextPoint(cv::flann::Index &index,const cv::Mat &data,
        const cv::Point2f &pt1,const cv::Point2f &pt2, const cv::Point2f &pt3,
        float white_angle,float black_angle,float min_response,cv::Point2f &point)
{
    Ellipse ellipse;
    if(!estimateSearchArea(pt1,pt2,pt3,0.4F,ellipse))
        return false;
    if(min_response > fabs(findMaxPoint(index,data,ellipse,white_angle,black_angle,point)))
        return false;
    return true;
}

int Chessboard::Board::grow(const cv::Mat &map,cv::flann::Index &flann_index)
{
    if(isEmpty())
        CV_Error(Error::StsInternal,"Board is empty");
    bool bleft = true;
    bool btop = true;
    bool bright = true;
    bool bbottom= true;
    int count = 0;
    do
    {
        // grow to the left
        if(bleft)
        {
            bleft = growLeft(map,flann_index);
            if(bleft)
                ++count;
        }
        if(btop)
        {
            btop= growTop(map,flann_index);
            if(btop)
                ++count;
        }
        if(bright)
        {
            bright= growRight(map,flann_index);
            if(bright)
                ++count;
        }
        if(bbottom)
        {
            bbottom= growBottom(map,flann_index);
            if(bbottom)
                ++count;
        }
    }while(bleft || btop || bright || bbottom );
    return count;
}

std::map<int,int> Chessboard::Board::getMapping()const
{
    std::map<int,int> map;
    std::vector<cv::Point2f> points = getCorners();
    std::vector<cv::Point2f>::iterator iter = points.begin();
    for(int idx1=0,idx2=0;iter != points.end();++iter,++idx1)
    {
        if(iter->x != iter->x)  // NaN check
            continue;
        map[idx1] = idx2++;
    }
    return map;
}

std::vector<cv::Point2f> Chessboard::Board::getCorners(bool ball)const
{
    std::vector<cv::Point2f> points;
    if(isEmpty())
        return points;

    // first row
    PointIter iter(top_left,TOP_LEFT);
    do
    {
        if(ball || !iter.isNaN())
            points.push_back(*(*iter));
    }while(iter.right());

    // all other rows
    Cell *row = top_left;
    do
    {
        PointIter iter2(row,BOTTOM_LEFT);
        do
        {
            if(ball || !iter2.isNaN())
                points.push_back(*(*iter2));
        }while(iter2.right());
        row = row->bottom;
    }while(row);
    return points;
}

std::vector<cv::KeyPoint> Chessboard::Board::getKeyPoints(bool ball)const
{
    std::vector<cv::KeyPoint> keypoints;
    std::vector<cv::Point2f> points = getCorners(ball);
    std::vector<cv::Point2f>::const_iterator iter = points.begin();
    for(;iter != points.end();++iter)
        keypoints.push_back(cv::KeyPoint(iter->x,iter->y,1));
    return keypoints;
}

Chessboard::Chessboard(const Parameters &para)
{
    reconfigure(para);
}

void Chessboard::reconfigure(const Parameters &config)
{
    parameters = config;
}

Chessboard::Parameters Chessboard::getPara()const
{
    return parameters;
}

Chessboard::~Chessboard()
{
}

void Chessboard::findKeyPoints(const cv::Mat& image, std::vector<KeyPoint>& keypoints,std::vector<cv::Mat> &feature_maps,
        std::vector<std::vector<float> > &angles ,const cv::Mat& mask)const
{
    keypoints.clear();
    angles.clear();
    vector<KeyPoint> keypoints_temp;
    FastX::Parameters para;

    para.branches = 2;                    // this is always the case for checssboard corners
    para.strength = 10;                   // minimal threshold
    para.resolution = float(CV_PI*0.25);   // this gives the best results taking interpolation into account
    para.filter = 1;
    para.super_resolution = parameters.super_resolution;
    para.min_scale = parameters.min_scale;
    para.max_scale = parameters.max_scale;

    FastX detector(para);
    std::vector<cv::Mat> rotated_images;
    detector.detectImpl(image,rotated_images,feature_maps,mask);

    //calculate seed chessboard corners
    detector.findKeyPoints(feature_maps,keypoints_temp,mask);

    //sort points and limit number
    int max_seeds = std::min((int)keypoints_temp.size(),parameters.max_points);
    if(max_seeds < 9)
        return;

    std::partial_sort(keypoints_temp.begin(),keypoints_temp.begin()+max_seeds-1,
            keypoints_temp.end(),sortKeyPoint);
    keypoints_temp.resize(max_seeds);
    std::vector<std::vector<float> > angles_temp  = detector.calcAngles(rotated_images,keypoints_temp);

    // filter out keypoints which are not symmetric
    std::vector<KeyPoint>::iterator iter1 = keypoints_temp.begin();
    std::vector<std::vector<float> >::const_iterator iter2 = angles_temp.begin();
    for(;iter1 != keypoints_temp.end();++iter1,++iter2)
    {
        cv::KeyPoint &pt = *iter1;
        const std::vector<float> &angles_i3 = *iter2;
        if(angles_i3.size() != 2)// || pt.response < noise)
            continue;
        int result = testPointSymmetry(image,pt.pt,pt.size*0.7F,std::max(10.0F,sqrt(pt.response)+0.5F*pt.size));
        if(result > MAX_SYMMETRY_ERRORS)
            continue;
        else if(result > 3)
            pt.response = - pt.response;
        angles.push_back(angles_i3);
        keypoints.push_back(pt);
    }
}

cv::Mat Chessboard::buildData(const std::vector<KeyPoint>& keypoints)const
{
    cv::Mat data(int(keypoints.size()),4,CV_32FC1);       // x + y + angle + strength
    std::vector<cv::KeyPoint>::const_iterator iter = keypoints.begin();
    float *val = reinterpret_cast<float*>(data.data);
    for(;iter != keypoints.end();++iter)
    {
        (*val++) = iter->pt.x;
        (*val++) = iter->pt.y;
        (*val++) = float(2.0*CV_PI-iter->angle/180.0*CV_PI);
        (*val++) = iter->response;
    }
    return data;
}

std::vector<cv::KeyPoint> Chessboard::getInitialPoints(cv::flann::Index &flann_index,const cv::Mat &data,const cv::KeyPoint &center,float white_angle,float black_angle,float min_response)const
{
    CV_CheckTypeEQ(data.type(), CV_32FC1, "Unsupported source type");
    if(data.cols != 4)
        CV_Error(Error::StsBadArg,"wrong data format");

    std::vector<float> query,dists;
    std::vector<int> indices;
    query.resize(2); query[0] = center.pt.x; query[1] = center.pt.y;
    flann_index.knnSearch(query,indices,dists,21,cv::flann::SearchParams(32));

    // collect all points having a similar angle and response
    std::vector<cv::KeyPoint> points;
    std::vector<int>::const_iterator ids_iter = indices.begin()+1; // first point is center
    points.push_back(center);
    for(;ids_iter != indices.end();++ids_iter)
    {
        // TODO do more angle tests
        // test only one angle against the stored one
        const float &response = data.at<float>(*ids_iter,3);
        if(fabs(response) < min_response)
            continue;
        const float &angle = data.at<float>(*ids_iter,2);
        float angle_temp = fabs(angle-white_angle);
        if(angle_temp > CV_PI*0.5)
            angle_temp = float(fabs(angle_temp-CV_PI));
        if(angle_temp > MAX_ANGLE)
        {
            angle_temp = fabs(angle-black_angle);
            if(angle_temp > CV_PI*0.5)
                angle_temp = float(fabs(angle_temp-CV_PI));
            if(angle_temp >MAX_ANGLE)
                continue;
        }
        points.push_back(cv::KeyPoint(data.at<float>(*ids_iter,0),data.at<float>(*ids_iter,1),center.size,angle,response));
    }
    return points;
}

Chessboard::BState Chessboard::generateBoards(cv::flann::Index &flann_index,const cv::Mat &data,
        const cv::KeyPoint &center,float white_angle,float black_angle,float min_response,const cv::Mat& img,
        std::vector<Chessboard::Board> &boards)const
{
    // collect all points having a similar angle
    std::vector<cv::KeyPoint> kpoints= getInitialPoints(flann_index,data,center,white_angle,black_angle,min_response);
    if(kpoints.size() < 5)
        return MISSING_POINTS;

    if(!img.empty())
    {
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
        cv::Mat out;
        cv::drawKeypoints(img,kpoints,out,cv::Scalar(0,0,255,255),4);
        std::vector<cv::KeyPoint> temp;
        temp.push_back(kpoints.front());
        cv::drawKeypoints(out,temp,out,cv::Scalar(0,255,0,255),4);
        cv::imshow("chessboard",out);
        cv::waitKey(-1);
#endif
    }

    // use angles to filter out points
    std::vector<cv::KeyPoint> points;
    cv::Vec2f n1(cos(white_angle),-sin(white_angle));
    cv::Vec2f n2(cos(black_angle),-sin(black_angle));
    std::vector<cv::KeyPoint>::const_iterator iter1 = kpoints.begin()+1; // first point is center
    for(;iter1 != kpoints.end();++iter1)
    {
        // calc angle
        cv::Vec2f vec(iter1->pt-center.pt);
        vec = vec/cv::norm(vec);
        if(fabs(vec.dot(n1)) < 0.96 && fabs(vec.dot(n2)) < 0.96)   //check that angle is bigger than 15°
            points.push_back(*iter1);
    }

    // genreate pairs those connection goes through the center
    std::vector<std::pair<cv::KeyPoint,cv::KeyPoint> > pairs;
    iter1 = points.begin();
    for(;iter1 != points.end();++iter1)
    {
        std::vector<cv::KeyPoint>::const_iterator iter2 = iter1+1;
        for(;iter2 != points.end();++iter2)
        {
            if(isPointOnLine(iter1->pt,iter2->pt,center.pt,0.97F))
            {
                if(cv::norm(iter1->pt) < cv::norm(iter2->pt))
                    pairs.push_back(std::make_pair(*iter1,*iter2));
                else
                    pairs.push_back(std::make_pair(*iter2,*iter1));
            }
        }
    }

    // generate all possible combinations consisting of two pairs
    if(pairs.size() < 2)
        return MISSING_PAIRS;
    std::vector<std::pair<cv::KeyPoint,cv::KeyPoint> >::iterator iter_pair1 = pairs.begin();

    BState best_state = MISSING_PAIRS;
    for(;iter_pair1 != pairs.end();++iter_pair1)
    {
        cv::Point2f p1 = iter_pair1->second.pt-iter_pair1->first.pt;
        p1 = p1/cv::norm(p1);
        std::vector<std::pair<cv::KeyPoint,cv::KeyPoint> >::iterator iter_pair2 = iter_pair1+1;
        for(;iter_pair2 != pairs.end();++iter_pair2)
        {
            cv::Point2f p2 = iter_pair2->second.pt-iter_pair2->first.pt;
            p2 = p2/cv::norm(p2);
            if(p2.dot(p1) > 0.95)
            {
                if(best_state < WRONG_PAIR_ANGLE)
                    best_state = WRONG_PAIR_ANGLE;
            }
            else
            {
                // check orientations
                if(checkOrientation(iter_pair1->first.pt,iter_pair1->second.pt,iter_pair2->first.pt,iter_pair2->second.pt))
                    std::swap(iter_pair2->first,iter_pair2->second);

                // minimal case
                std::vector<cv::Point2f> board_points;
                board_points.resize(9,cv::Point2f(std::numeric_limits<float>::quiet_NaN(),
                            std::numeric_limits<float>::quiet_NaN()));

                board_points[1] = iter_pair2->first.pt;
                board_points[3] = iter_pair1->first.pt;
                board_points[4] = center.pt;
                board_points[5] = iter_pair1->second.pt;
                board_points[7] = iter_pair2->second.pt;
                boards.push_back(Board(cv::Size(3,3),board_points,white_angle,black_angle));
                Board &board = boards.back();

                if(board.isEmpty())
                {
                    if(best_state < WRONG_CONFIGURATION)
                        best_state = WRONG_CONFIGURATION;
                    boards.pop_back(); // MAKE SURE board is no longer used !!!!
                    continue;
                }
                best_state = FOUND_BOARD;
            }
        }
    }
    return best_state;
}

void Chessboard::detectImpl(const Mat& image, vector<KeyPoint>& keypoints,std::vector<Mat> &feature_maps,const Mat& mask)const
{
    keypoints.clear();
    Board board = detectImpl(image,feature_maps,mask);
    keypoints = board.getKeyPoints();
    return;
}

Chessboard::Board Chessboard::detectImpl(const Mat& gray,std::vector<cv::Mat> &feature_maps,const Mat& mask)const
{
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
    debug_image = gray;
#endif
    CV_CheckTypeEQ(gray.type(),CV_8UC1, "Unsupported image type");

    cv::Size chessboard_size2(parameters.chessboard_size.height,parameters.chessboard_size.width);
    std::vector<KeyPoint> keypoints_seed;
    std::vector<std::vector<float> > angles;
    findKeyPoints(gray,keypoints_seed,feature_maps,angles,mask);
    if(keypoints_seed.empty())
        return Chessboard::Board();

    // check how many points are likely a checkerbord corner
    float response = fabs(keypoints_seed.front().response*MIN_RESPONSE_RATIO);
    std::vector<KeyPoint>::const_iterator seed_iter = keypoints_seed.begin();
    int count = 0;
    int inum = chessboard_size2.width*chessboard_size2.height;
    for(;seed_iter != keypoints_seed.end() && count < inum;++seed_iter,++count)
    {
        // points are sorted based on response
        if(fabs(seed_iter->response) < response)
        {
            seed_iter = keypoints_seed.end();
            return Chessboard::Board();
        }
    }
    // just add dummy points or flann will fail during knnSearch
    if(keypoints_seed.size() < 21)
        keypoints_seed.resize(21, cv::KeyPoint(-99999.0F,-99999.0F,0.0F,0.0F,0.0F));

    //build kd tree
    cv::Mat data = buildData(keypoints_seed);
    cv::Mat flann_data(data.rows,2,CV_32FC1);
    data(cv::Rect(0,0,2,data.rows)).copyTo(flann_data);
    cv::flann::Index flann_index(flann_data,cv::flann::KDTreeIndexParams(1),cvflann::FLANN_DIST_EUCLIDEAN);

    // for each point
    std::vector<std::vector<float> >::const_iterator angles_iter = angles.begin();
    std::vector<cv::KeyPoint>::const_iterator points_iter = keypoints_seed.begin();
    cv::Rect bounding_box(5,5,gray.cols-10,gray.rows-10);
    int max_tests = std::min(parameters.max_tests,int(keypoints_seed.size()));
    for(count=0;count < max_tests;++angles_iter,++points_iter,++count)
    {
        // regard current point as center point
        // which must have two angles!!! (this was already checked)
        float min_response = points_iter->response*MIN_RESPONSE_RATIO;
        if(min_response <= 0)
        {
            if(max_tests+1 < int(keypoints_seed.size()))
                ++max_tests;
            continue;
        }
        const std::vector<float> &angles_i = *angles_iter;
        float white_angle =  fabs(angles_i.front());  // angle is negative if black --> clockwise
        float black_angle =  fabs(angles_i.back());   // angle is negative if black --> clockwise
        if(angles_i.front() < 0)                 // ensure white angle is first
            swap(white_angle,black_angle);

        std::vector<Board> boards;
        generateBoards(flann_index, data,*points_iter,white_angle,black_angle,min_response,gray,boards);
        parallel_for_(Range(0,(int)boards.size()),[&](const Range& range){
            for(int i=range.start;i <range.end;++i)
            {
                auto iter_boards = boards.begin()+i;
                cv::Mat h = iter_boards->estimateHomography();
                int size = iter_boards->validateCorners(data,flann_index,h,min_response);
                if(size != 9 || !iter_boards->validateContour())
                {
                    iter_boards->clear();
                    continue;
                }
                //grow based on kd-tree
                iter_boards->grow(data,flann_index);
                if(!iter_boards->checkUnique())
                {
                    iter_boards->clear();
                    continue;
                }

                // check bounding box
                std::vector<cv::Point2f> contour = iter_boards->getContour();
                std::vector<cv::Point2f>::const_iterator iter = contour.begin();
                for(;iter != contour.end();++iter)
                {
                    if(!bounding_box.contains(*iter))
                        break;
                }
                if(iter != contour.end())
                {
                    iter_boards->clear();
                    continue;
                }

                if(iter_boards->getSize() == parameters.chessboard_size ||
                        iter_boards->getSize() == chessboard_size2)
                {
                    iter_boards->normalizeOrientation(false);
                    if(iter_boards->getSize() != parameters.chessboard_size)
                    {
                        if(iter_boards->isCellBlack(0,0) == iter_boards->isCellBlack(0,int(iter_boards->colCount())-1))
                            iter_boards->rotateLeft();
                        else
                            iter_boards->rotateRight();
                    }
#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
                    cv::Mat img;
                    iter_boards->draw(debug_image,img);
                    cv::imshow("chessboard",img);
                    cv::waitKey(-1);
#endif
                }
                else
                {
                    if(iter_boards->getSize().width*iter_boards->getSize().height < chessboard_size2.width*chessboard_size2.height)
                        iter_boards->clear();
                    else if(!parameters.larger)
                        iter_boards->clear();
                }
            }
        });
        // check if a good board was found
        for(const auto &board : boards)
        {
            if(!board.isEmpty())
                return board;
        }
    }
    return Chessboard::Board();
}

void Chessboard::detectAndCompute(cv::InputArray image,cv::InputArray mask,std::vector<cv::KeyPoint>& keypoints,
        cv::OutputArray descriptors,bool useProvidedKeyPoints)
{
    descriptors.clear();
    useProvidedKeyPoints=false;
    std::vector<cv::Mat> maps;
    detectImpl(image.getMat(),keypoints,maps,mask.getMat());
    if(!useProvidedKeyPoints)        // suppress compiler warning
        return;
    return;
}

void Chessboard::detectImpl(const Mat& image, vector<KeyPoint>& keypoints,const Mat& mask)const
{
    std::vector<cv::Mat> maps;
    detectImpl(image,keypoints,maps,mask);
}

void Chessboard::detectImpl(InputArray image, std::vector<KeyPoint>& keypoints, InputArray mask)const
{
    detectImpl(image.getMat(),keypoints,mask.getMat());
}

} // end namespace details


// public API
bool findChessboardCornersSB(cv::InputArray image_, cv::Size pattern_size,
                             cv::OutputArray corners_, int flags)
{
    CV_INSTRUMENT_REGION();
    int type = image_.type(), depth = CV_MAT_DEPTH(type), cn = CV_MAT_CN(type);
    CV_CheckType(type, depth == CV_8U && (cn == 1 || cn == 3),
            "Only 8-bit grayscale or color images are supported");
    if(pattern_size.width <= 2 || pattern_size.height <= 2)
    {
        CV_Error(Error::StsOutOfRange, "Both width and height of the pattern should have bigger than 2");
    }
    if (!corners_.needed())
        CV_Error(Error::StsNullPtr, "Null pointer to corners");

    Mat img;
    if (image_.channels() != 1)
        cvtColor(image_, img, COLOR_BGR2GRAY);
    else
        img = image_.getMat();

    details::Chessboard::Parameters para;
    para.chessboard_size = pattern_size;
    para.min_scale = 2;
    para.max_scale = 4;
    para.max_tests = 25;
    para.max_points = std::max(100,pattern_size.width*pattern_size.height*2);
    para.super_resolution = false;

    // setup search based on flags
    if(flags & CALIB_CB_NORMALIZE_IMAGE)
    {
        Mat tmp;
        cv::equalizeHist(img, tmp);
        swap(img, tmp);
        flags ^= CALIB_CB_NORMALIZE_IMAGE;
    }
    if(flags & CALIB_CB_EXHAUSTIVE)
    {
        para.max_tests = 100;
        para.max_points = std::max(500,pattern_size.width*pattern_size.height*2);
        flags ^= CALIB_CB_EXHAUSTIVE;
    }
    if(flags & CALIB_CB_ACCURACY)
    {
        para.super_resolution = true;
        flags ^= CALIB_CB_ACCURACY;
    }
    if(flags)
        CV_Error(Error::StsOutOfRange, cv::format("Invalid remaing flags %d", (int)flags));

    std::vector<cv::KeyPoint> corners;
    details::Chessboard board(para);
    board.detect(img,corners);
    if(corners.empty())
    {
        corners_.release();
        return false;
    }
    std::vector<cv::Point2f> points;
    KeyPoint::convert(corners,points);
    Mat(points).copyTo(corners_);
    return true;
}

} // namespace cv