--- /dev/null
+// 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>
+cv::Mat debug_image;
+#endif
+
+using namespace std;
+namespace cv {
+namespace details {
+
+/////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////
+// magic numbers used for chessboard corner detection
+/////////////////////////////////////////////////////////////////////////////
+const float CORNERS_SEARCH = 0.5F; // percentage of the edge length to the next corner used to find new corners
+const float MAX_ANGLE = float(48.0/180.0*M_PI); // max angle between line segments supposed to be straight
+const float MIN_COS_ANGLE = float(cos(35.0/180*M_PI)); // min cos angle between board edges
+const float MIN_RESPONSE_RATIO = 0.3F;
+const float ELLIPSE_WIDTH = 0.35F; // width of the search ellipse in percentage of its length
+const float RAD2DEG = float(180.0/M_PI);
+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(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
+ _T.create(2,2,CV_64FC1);
+ cv::Mat T = _T.getMat();
+ T.at<double>(0,0) = scale;
+ T.at<double>(0,1) = -scale*centroid;
+ T.at<double>(1,0) = 0;
+ T.at<double>(1,1) = 1;
+
+ // calc normalized points;
+ cv::Matx22d Tx(T);
+ _new_points.create(points.rows,1,points.type());
+ new_points = _new_points.getMat();
+ cv::Vec2d p;
+ switch(points.type())
+ {
+ case CV_32FC1:
+ for(int i=0;i < points.rows;++i)
+ {
+ p(0) = points.at<float>(i);
+ p(1) = 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)
+ {
+ p(0) = points.at<double>(i);
+ p(1) = 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);
+ if(src.rows != dst.rows)
+ CV_Error(Error::StsBadArg, "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::Mat H = (cv::Mat_<double>(2,2) << x.at<double>(0), x.at<double>(1), x.at<double>(2), 1.0);
+
+ // denormalize
+ H = dst_T.inv()*H*src_T;
+
+ // enforce frobeniusnorm of one
+ double scale = 1.0/cv::norm(H);
+ 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 srcX = Mat_<double>(src_x), srcY = Mat_<double>(src_y);
+ Mat 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,std::max(std::max(src_x.depth(), src_y.depth()), 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(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(double angle=0;angle <= M_PI;angle+=M_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 ¶)
+{
+ reconfigure(para);
+}
+
+void FastX::reconfigure(const Parameters ¶)
+{
+ 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 m = cv::getRotationMatrix2D(cv::Point2f(float(img.cols*0.5),float(img.rows*0.5)),float(angle/M_PI*180),1);
+ CV_Assert(m.type() == CV_64FC1);
+ 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(M_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(M_PI);
+ else if(angle > M_PI)
+ angle -= float(M_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(M_PI);
+ else if(angle > M_PI)
+ angle -= float(M_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(M_PI);
+ else if(angle > M_PI)
+ angle -= float(M_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(M_PI);
+ else if(angle > M_PI)
+ angle -= float(M_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;
+ if(int(feature_maps.size()) < num_scales)
+ CV_Error(Error::StsBadArg,"missing feature maps");
+ if(_mask.data && (_mask.type() != CV_8UC1 || _mask.size() != feature_maps.front().size()))
+ CV_Error(Error::StsBadMask,"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 = int(pow(2.0,scale+super_res)+1);
+ float window_size2 = 0.5F*window_size;
+ float window_size4 = 0.25F*window_size;
+ int window_size2i = int(round(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
+ int super_res = int(parameters.super_resolution);
+ if(super_res)
+ cv::resize(gray_image,gray_image,cv::Size(),2,2);
+
+ //for each scale
+ int num_scales = parameters.max_scale-parameters.min_scale+1;
+ rotated_images.resize(num_scales);
+ feature_maps.resize(num_scales);
+ for(int scale=parameters.min_scale;scale <= parameters.max_scale;++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*M_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/M_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;
+ default:
+ CV_Assert(false);
+ }
+ return NULL;
+}
+
+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();
+ if(K.type() != CV_64FC1)
+ throw std::runtime_error("wrong K type");
+ if(K.rows != 3|| K.cols != 3)
+ throw std::runtime_error("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)
+ if(data.type() != CV_32FC1 || data.cols != 4)
+ CV_Error(Error::StsBadArg,"type of flann data is not supported. Expect CV_32FC1");
+ 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 = fabs(a0-white_angle);
+ float a2 = fabs(a0-black_angle);
+ if(a1 > M_PI*0.5)
+ a1= float(fabs(a1-M_PI));
+ if(a2> M_PI*0.5)
+ a2= float(fabs(a2-M_PI));
+ if(a1 < MAX_ANGLE || a2 < MAX_ANGLE )
+ {
+ cv::Point2f pt(*val,*(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*M_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");
+}
+
+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;
+ }
+ case 2: // right
+ if(iter.right(true))
+ {
+ if(last != 2)
+ points.push_back(**current_iter);
+ mode = 1;
+ last = 2;
+ break;
+ }
+ case 3: // bottom
+ if(iter.bottom(true))
+ {
+ if(last != 3)
+ points.push_back(**current_iter);
+ mode = 2;
+ last = 3;
+ break;
+ }
+ 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 ¶)
+{
+ 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(M_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*M_PI-iter->angle/180.0*M_PI);
+ (*val++) = iter->response;
+ }
+ return data;
+}
+
+std::vector<cv::KeyPoint> Chessboard::getInitialPoints(cv::flann::Index &flann_index,const cv::Mat &data,const cv::KeyPoint ¢er,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 > M_PI*0.5)
+ angle_temp = float(fabs(angle_temp-M_PI));
+ if(angle_temp > MAX_ANGLE)
+ {
+ angle_temp = fabs(angle-black_angle);
+ if(angle_temp > M_PI*0.5)
+ angle_temp = float(fabs(angle_temp-M_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 ¢er,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");
+
+ //TODO is this needed?
+ // double min,max;
+ // cv::minMaxLoc(gray,&min,&max);
+ // gray = (gray-min)*(255.0/(max-min));
+
+ 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();++seed_iter)
+ {
+ if(fabs(seed_iter->response) > response)
+ {
+ ++count;
+ if(count >= inum)
+ break;
+ }
+ }
+ if(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);
+ std::vector<Chessboard::Board>::iterator iter_boards = boards.begin();
+ for(;iter_boards != boards.end();++iter_boards)
+ {
+ cv::Mat h = iter_boards->estimateHomography();
+ int size = iter_boards->validateCorners(data,flann_index,h,min_response);
+ if(size != 9)
+ continue;
+ if(!iter_boards->validateContour())
+ continue;
+ //grow based on kd-tree
+ iter_boards->grow(data,flann_index);
+ if(!iter_boards->checkUnique())
+ 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())
+ 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
+ return *iter_boards;
+ }
+ else
+ {
+ if(iter_boards->getSize().width*iter_boards->getSize().height > chessboard_size2.width*chessboard_size2.height)
+ {
+ if(parameters.larger)
+ return *iter_boards;
+ else
+ return Chessboard::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 and cv
+
+
+// public API
+bool cv::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);
+ Mat img = image_.getMat();
+ 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");
+ if (img.channels() != 1)
+ cvtColor(img, img, COLOR_BGR2GRAY);
+
+ details::Chessboard::Parameters para;
+ para.chessboard_size = pattern_size;
+
+ switch(flags)
+ {
+ case 1: // high accuracy profile
+ para.min_scale = 2;
+ para.max_scale = 4;
+ para.max_tests = 100;
+ para.super_resolution = true;
+ para.max_points = std::max(500,pattern_size.width*pattern_size.height*2);
+ break;
+ default: // default profile
+ para.min_scale = 2;
+ para.max_scale = 3;
+ para.max_tests = 20;
+ para.max_points = pattern_size.width*pattern_size.height*2;
+ para.super_resolution = false;
+ break;
+ }
+ 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;
+}
--- /dev/null
+// 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.
+
+#ifndef CHESSBOARD_HPP_
+#define CHESSBOARD_HPP_
+
+#include "opencv2/core.hpp"
+#include "opencv2/features2d.hpp"
+#include <vector>
+#include <set>
+#include <map>
+
+namespace cv {
+namespace details{
+/**
+ * \brief Fast point sysmetric cross detector based on a localized radon transformation
+ */
+class FastX : public cv::Feature2D
+{
+ public:
+ struct Parameters
+ {
+ float strength; //!< minimal strength of a valid junction in dB
+ float resolution; //!< angle resolution in radians
+ int branches; //!< the number of branches
+ int min_scale; //!< scale level [0..8]
+ int max_scale; //!< scale level [0..8]
+ bool filter; //!< post filter feature map to improve impulse response
+ bool super_resolution; //!< up-sample
+
+ Parameters()
+ {
+ strength = 40;
+ resolution = float(M_PI*0.25);
+ branches = 2;
+ min_scale = 2;
+ max_scale = 5;
+ super_resolution = 1;
+ filter = true;
+ }
+ };
+
+ public:
+ FastX(const Parameters &config = Parameters());
+ virtual ~FastX(){};
+
+ void reconfigure(const Parameters ¶);
+
+ //declaration to be wrapped by rbind
+ void detect(cv::InputArray image,std::vector<cv::KeyPoint>& keypoints, cv::InputArray mask=cv::Mat())override
+ {cv::Feature2D::detect(image.getMat(),keypoints,mask.getMat());}
+
+ virtual void detectAndCompute(cv::InputArray image,
+ cv::InputArray mask,
+ std::vector<cv::KeyPoint>& keypoints,
+ cv::OutputArray descriptors,
+ bool useProvidedKeyPoints = false)override;
+
+ void detectImpl(const cv::Mat& image,
+ std::vector<cv::KeyPoint>& keypoints,
+ std::vector<cv::Mat> &feature_maps,
+ const cv::Mat& mask=cv::Mat())const;
+
+ void detectImpl(const cv::Mat& image,
+ std::vector<cv::Mat> &rotated_images,
+ std::vector<cv::Mat> &feature_maps,
+ const cv::Mat& mask=cv::Mat())const;
+
+ void findKeyPoints(const std::vector<cv::Mat> &feature_map,
+ std::vector<cv::KeyPoint>& keypoints,
+ const cv::Mat& mask = cv::Mat())const;
+
+ std::vector<std::vector<float> > calcAngles(const std::vector<cv::Mat> &rotated_images,
+ std::vector<cv::KeyPoint> &keypoints)const;
+ // define pure virtual methods
+ virtual int descriptorSize()const override{return 0;};
+ virtual int descriptorType()const override{return 0;};
+ virtual void operator()( cv::InputArray image, cv::InputArray mask, std::vector<cv::KeyPoint>& keypoints, cv::OutputArray descriptors, bool useProvidedKeypoints=false )const
+ {
+ descriptors.clear();
+ detectImpl(image.getMat(),keypoints,mask);
+ if(!useProvidedKeypoints) // suppress compiler warning
+ return;
+ return;
+ }
+
+ protected:
+ virtual void computeImpl( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors)const
+ {
+ descriptors = cv::Mat();
+ detectImpl(image,keypoints);
+ }
+
+ private:
+ void detectImpl(const cv::Mat& _src, std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask)const;
+ virtual void detectImpl(cv::InputArray image, std::vector<cv::KeyPoint>& keypoints, cv::InputArray mask=cv::noArray())const;
+
+ void rotate(float angle,const cv::Mat &img,cv::Size size,cv::Mat &out)const;
+ void calcFeatureMap(const cv::Mat &images,cv::Mat& out)const;
+
+ private:
+ Parameters parameters;
+};
+
+/**
+ * \brief Ellipse class
+ */
+class Ellipse
+{
+ public:
+ Ellipse();
+ Ellipse(const cv::Point2f ¢er, const cv::Size2f &axes, float angle);
+ Ellipse(const Ellipse &other);
+
+
+ void draw(cv::InputOutputArray img,const cv::Scalar &color = cv::Scalar::all(120))const;
+ bool contains(const cv::Point2f &pt)const;
+ cv::Point2f getCenter()const;
+ const cv::Size2f &getAxes()const;
+
+ private:
+ cv::Point2f center;
+ cv::Size2f axes;
+ float angle,cosf,sinf;
+};
+
+/**
+ * \brief Chessboard corner detector
+ *
+ * The detectors tries to find all chessboard corners of an imaged
+ * chessboard and returns them as an ordered vector of KeyPoints.
+ * Thereby, the left top corner has index 0 and the bottom right
+ * corner n*m-1.
+ */
+class Chessboard: public cv::Feature2D
+{
+ public:
+ static const int DUMMY_FIELD_SIZE = 100; // in pixel
+
+ /**
+ * \brief Configuration of a chessboard corner detector
+ *
+ */
+ struct Parameters
+ {
+ cv::Size chessboard_size; //!< size of the chessboard
+ int min_scale; //!< scale level [0..8]
+ int max_scale; //!< scale level [0..8]
+ int max_points; //!< maximal number of points regarded
+ int max_tests; //!< maximal number of tested hypothesis
+ bool super_resolution; //!< use super-repsolution for chessboard detection
+ bool larger; //!< indicates if larger boards should be returned
+
+ Parameters()
+ {
+ chessboard_size = cv::Size(9,6);
+ min_scale = 2;
+ max_scale = 4;
+ super_resolution = true;
+ max_points = 400;
+ max_tests = 100;
+ larger = false;
+ }
+
+ Parameters(int scale,int _max_points):
+ min_scale(scale),
+ max_scale(scale),
+ max_points(_max_points)
+ {
+ chessboard_size = cv::Size(9,6);
+ }
+ };
+
+
+ /**
+ * \brief Gets the 3D objects points for the chessboard assuming the
+ * left top corner is located at the origin.
+ *
+ * \param[in] pattern_size Number of rows and cols of the pattern
+ * \param[in] cell_size Size of one cell
+ *
+ * \returns Returns the object points as CV_32FC3
+ */
+ static cv::Mat getObjectPoints(const cv::Size &pattern_size,float cell_size);
+
+ /**
+ * \brief Class for searching and storing chessboard corners.
+ *
+ * The search is based on a feature map having strong pixel
+ * values at positions where a chessboard corner is located.
+ *
+ * The board must be rectangular but supports empty cells
+ *
+ */
+ class Board
+ {
+ public:
+ /**
+ * \brief Estimates the position of the next point on a line using cross ratio constrain
+ *
+ * cross ratio:
+ * d12/d34 = d13/d24
+ *
+ * point order on the line:
+ * pt1 --> pt2 --> pt3 --> pt4
+ *
+ * \param[in] pt1 First point coordinate
+ * \param[in] pt2 Second point coordinate
+ * \param[in] pt3 Third point coordinate
+ * \param[out] pt4 Forth point coordinate
+ *
+ */
+ static bool estimatePoint(const cv::Point2f &p0,const cv::Point2f &p1,const cv::Point2f &p2,cv::Point2f &p3);
+
+ // using 1D homography
+ static bool estimatePoint(const cv::Point2f &p0,const cv::Point2f &p1,const cv::Point2f &p2,const cv::Point2f &p3, cv::Point2f &p4);
+
+ /**
+ * \brief Checks if all points of a row or column have a valid cross ratio constraint
+ *
+ * cross ratio:
+ * d12/d34 = d13/d24
+ *
+ * point order on the row/column:
+ * pt1 --> pt2 --> pt3 --> pt4
+ *
+ * \param[in] points THe points of the row/column
+ *
+ */
+ static bool checkRowColumn(const std::vector<cv::Point2f> &points);
+
+ /**
+ * \brief Estimates the search area for the next point on the line using cross ratio
+ *
+ * point order on the line:
+ * (p0) --> p1 --> p2 --> p3 --> search area
+ *
+ * \param[in] p1 First point coordinate
+ * \param[in] p2 Second point coordinate
+ * \param[in] p3 Third point coordinate
+ * \param[in] p Percentage of d34 used for the search area width and height [0..1]
+ * \param[out] ellipse The search area
+ * \param[in] p0 optional point to improve accuracy
+ *
+ * \return Returns false if no search area can be calculated
+ *
+ */
+ static bool estimateSearchArea(const cv::Point2f &p1,const cv::Point2f &p2,const cv::Point2f &p3,float p,
+ Ellipse &ellipse,const cv::Point2f *p0 =NULL);
+
+ /**
+ * \brief Estimates the search area for a specific point based on the given homography
+ *
+ * \param[in] H homography descriping the transformation from ideal board to real one
+ * \param[in] row Row of the point
+ * \param[in] col Col of the point
+ * \param[in] p Percentage [0..1]
+ *
+ * \return Returns false if no search area can be calculated
+ *
+ */
+ static Ellipse estimateSearchArea(cv::Mat H,int row, int col,float p,int field_size = DUMMY_FIELD_SIZE);
+
+ /**
+ * \brief Searches for the maximum in a given search area
+ *
+ * \param[in] map feature map
+ * \param[in] ellipse search area
+ * \param[in] min_val Minimum value of the maximum to be accepted as maximum
+ *
+ * \return Returns a negative value if all points are outside the ellipse
+ *
+ */
+ static float findMaxPoint(cv::flann::Index &index,const cv::Mat &data,const Ellipse &ellipse,float white_angle,float black_angle,cv::Point2f &pt);
+
+ /**
+ * \brief Searches for the next point using cross ratio constrain
+ *
+ * \param[in] index flann index
+ * \param[in] data extended flann data
+ * \param[in] pt1
+ * \param[in] pt2
+ * \param[in] pt3
+ * \param[in] white_angle
+ * \param[in] black_angle
+ * \param[in] min_response
+ * \param[out] point The resulting point
+ *
+ * \return Returns false if no point could be found
+ *
+ */
+ static bool 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);
+
+ /**
+ * \brief Creates a new Board object
+ *
+ */
+ Board(float white_angle=0,float black_angle=0);
+ Board(const cv::Size &size, const std::vector<cv::Point2f> &points,float white_angle=0,float black_angle=0);
+ Board(const Chessboard::Board &other);
+ virtual ~Board();
+
+ Board& operator=(const Chessboard::Board &other);
+
+ /**
+ * \brief Draws the corners into the given image
+ *
+ * \param[in] m The image
+ * \param[out] m The resulting image
+ * \param[in] H optional homography to calculate search area
+ *
+ */
+ void draw(cv::InputArray m,cv::OutputArray out,cv::InputArray H=cv::Mat())const;
+
+ /**
+ * \brief Estimates the pose of the chessboard
+ *
+ */
+ bool estimatePose(const cv::Size2f &real_size,cv::InputArray _K,cv::OutputArray rvec,cv::OutputArray tvec)const;
+
+ /**
+ * \brief Clears all internal data of the object
+ *
+ */
+ void clear();
+
+ /**
+ * \brief Returns the angle of the black diagnonale
+ *
+ */
+ float getBlackAngle()const;
+
+ /**
+ * \brief Returns the angle of the black diagnonale
+ *
+ */
+ float getWhiteAngle()const;
+
+ /**
+ * \brief Initializes a 3x3 grid from 9 corner coordinates
+ *
+ * All points must be ordered:
+ * p0 p1 p2
+ * p3 p4 p5
+ * p6 p7 p8
+ *
+ * \param[in] points vector of points
+ *
+ * \return Returns false if the grid could not be initialized
+ */
+ bool init(const std::vector<cv::Point2f> points);
+
+ /**
+ * \brief Returns true if the board is empty
+ *
+ */
+ bool isEmpty() const;
+
+ /**
+ * \brief Returns all board corners as ordered vector
+ *
+ * The left top corner has index 0 and the bottom right
+ * corner rows*cols-1. All corners which only belong to
+ * empty cells are returned as NaN.
+ */
+ std::vector<cv::Point2f> getCorners(bool ball=true) const;
+
+ /**
+ * \brief Returns all board corners as ordered vector of KeyPoints
+ *
+ * The left top corner has index 0 and the bottom right
+ * corner rows*cols-1.
+ *
+ * \param[in] ball if set to false only non empty points are returned
+ *
+ */
+ std::vector<cv::KeyPoint> getKeyPoints(bool ball=true) const;
+
+ /**
+ * \brief Returns the centers of the chessboard cells
+ *
+ * The left top corner has index 0 and the bottom right
+ * corner (rows-1)*(cols-1)-1.
+ *
+ */
+ std::vector<cv::Point2f> getCellCenters() const;
+
+ /**
+ * \brief Estimates the homography between an ideal board
+ * and reality based on the already recovered points
+ *
+ * \param[in] rect selecting a subset of the already recovered points
+ * \param[in] field_size The field size of the ideal board
+ *
+ */
+ cv::Mat estimateHomography(cv::Rect rect,int field_size = DUMMY_FIELD_SIZE)const;
+
+ /**
+ * \brief Estimates the homography between an ideal board
+ * and reality based on the already recovered points
+ *
+ * \param[in] field_size The field size of the ideal board
+ *
+ */
+ cv::Mat estimateHomography(int field_size = DUMMY_FIELD_SIZE)const;
+
+ /**
+ * \brief Returns the size of the board
+ *
+ */
+ cv::Size getSize() const;
+
+ /**
+ * \brief Returns the number of cols
+ *
+ */
+ size_t colCount() const;
+
+ /**
+ * \brief Returns the number of rows
+ *
+ */
+ size_t rowCount() const;
+
+ /**
+ * \brief Returns the inner contour of the board inlcuding only valid corners
+ *
+ * \info the contour might be non squared if not all points of the board are defined
+ *
+ */
+ std::vector<cv::Point2f> getContour()const;
+
+
+ /**
+ * \brief Grows the board in all direction until no more corners are found in the feature map
+ *
+ * \param[in] data CV_32FC1 data of the flann index
+ * \param[in] flann_index flann index
+ *
+ * \returns the number of grows
+ */
+ int grow(const cv::Mat &data,cv::flann::Index &flann_index);
+
+ /**
+ * \brief Validates all corners using guided search based on the given homography
+ *
+ * \param[in] data CV_32FC1 data of the flann index
+ * \param[in] flann_index flann index
+ * \param[in] h Homography describing the transformation from ideal board to the real one
+ * \param[in] min_response Min response
+ *
+ * \returns the number of valid corners
+ */
+ int validateCorners(const cv::Mat &data,cv::flann::Index &flann_index,const cv::Mat &h,float min_response=0);
+
+ /**
+ * \brief check that no corner is used more than once
+ *
+ * \returns Returns false if a corner is used more than once
+ */
+ bool checkUnique()const;
+
+ /**
+ * \brief Returns false if the angles of the contour are smaller than 35°
+ *
+ */
+ bool validateContour()const;
+
+ /**
+ * \brief Grows the board to the left by adding one column.
+ *
+ * \param[in] map CV_32FC1 feature map
+ *
+ * \returns Returns false if the feature map has no maxima at the requested positions
+ */
+ bool growLeft(const cv::Mat &map,cv::flann::Index &flann_index);
+ void growLeft();
+
+ /**
+ * \brief Grows the board to the top by adding one row.
+ *
+ * \param[in] map CV_32FC1 feature map
+ *
+ * \returns Returns false if the feature map has no maxima at the requested positions
+ */
+ bool growTop(const cv::Mat &map,cv::flann::Index &flann_index);
+ void growTop();
+
+ /**
+ * \brief Grows the board to the right by adding one column.
+ *
+ * \param[in] map CV_32FC1 feature map
+ *
+ * \returns Returns false if the feature map has no maxima at the requested positions
+ */
+ bool growRight(const cv::Mat &map,cv::flann::Index &flann_index);
+ void growRight();
+
+ /**
+ * \brief Grows the board to the bottom by adding one row.
+ *
+ * \param[in] map CV_32FC1 feature map
+ *
+ * \returns Returns false if the feature map has no maxima at the requested positions
+ */
+ bool growBottom(const cv::Mat &map,cv::flann::Index &flann_index);
+ void growBottom();
+
+ /**
+ * \brief Adds one column on the left side
+ *
+ * \param[in] points The corner coordinates
+ *
+ */
+ void addColumnLeft(const std::vector<cv::Point2f> &points);
+
+ /**
+ * \brief Adds one column at the top
+ *
+ * \param[in] points The corner coordinates
+ *
+ */
+ void addRowTop(const std::vector<cv::Point2f> &points);
+
+ /**
+ * \brief Adds one column on the right side
+ *
+ * \param[in] points The corner coordinates
+ *
+ */
+ void addColumnRight(const std::vector<cv::Point2f> &points);
+
+ /**
+ * \brief Adds one row at the bottom
+ *
+ * \param[in] points The corner coordinates
+ *
+ */
+ void addRowBottom(const std::vector<cv::Point2f> &points);
+
+ /**
+ * \brief Rotates the board 90° degrees to the left
+ */
+ void rotateLeft();
+
+ /**
+ * \brief Rotates the board 90° degrees to the right
+ */
+ void rotateRight();
+
+ /**
+ * \brief Flips the board along its local x(width) coordinate direction
+ */
+ void flipVertical();
+
+ /**
+ * \brief Flips the board along its local y(height) coordinate direction
+ */
+ void flipHorizontal();
+
+ /**
+ * \brief Flips and rotates the board so that the anlge of
+ * either the black or white diagonale is bigger than the x
+ * and y axis of the board and from a right handed
+ * coordinate system
+ */
+ void normalizeOrientation(bool bblack=true);
+
+ /**
+ * \brief Exchanges the stored board with the board stored in other
+ */
+ void swap(Chessboard::Board &other);
+
+ bool operator==(const Chessboard::Board& other) const {return rows*cols == other.rows*other.cols;};
+ bool operator< (const Chessboard::Board& other) const {return rows*cols < other.rows*other.cols;};
+ bool operator> (const Chessboard::Board& other) const {return rows*cols > other.rows*other.cols;};
+ bool operator>= (const cv::Size& size)const { return rows*cols >= size.width*size.height; };
+
+ /**
+ * \brief Returns a specific corner
+ *
+ * \info raises runtime_error if row col does not exists
+ */
+ cv::Point2f& getCorner(int row,int col);
+
+ /**
+ * \brief Returns true if the cell is empty meaning at least one corner is NaN
+ */
+ bool isCellEmpty(int row,int col);
+
+ /**
+ * \brief Returns the mapping from all corners idx to only valid corners idx
+ */
+ std::map<int,int> getMapping()const;
+
+ /**
+ * \brief Estimates rotation of the board around the camera axis
+ */
+ double estimateRotZ()const;
+
+ /**
+ * \brief Returns true if the cell is black
+ *
+ */
+ bool isCellBlack(int row,int cola)const;
+
+ private:
+ // stores one cell
+ // in general a cell is initialized by the Board so that:
+ // * all corners are always pointing to a valid cv::Point2f
+ // * depending on the position left,top,right and bottom might be set to NaN
+ // * A cell is empty if at least one corner is NaN
+ struct Cell
+ {
+ cv::Point2f *top_left,*top_right,*bottom_right,*bottom_left; // corners
+ Cell *left,*top,*right,*bottom; // neighbouring cells
+ bool black; // set to true if cell is black
+ Cell();
+ bool empty()const; // indicates if the cell is empty (one of its corners has NaN)
+ int getRow()const;
+ int getCol()const;
+ };
+
+ // corners
+ enum CornerIndex
+ {
+ TOP_LEFT,
+ TOP_RIGHT,
+ BOTTOM_RIGHT,
+ BOTTOM_LEFT
+ };
+
+ Cell* getCell(int row,int column); // returns a specific cell
+ const Cell* getCell(int row,int column)const; // returns a specific cell
+ void drawEllipses(const std::vector<Ellipse> &ellipses);
+
+ // Iterator for iterating over board corners
+ class PointIter
+ {
+ public:
+ PointIter(Cell *cell,CornerIndex corner_index);
+ PointIter(const PointIter &other);
+ void operator=(const PointIter &other);
+ bool valid() const; // returns if the pointer is pointing to a cell
+
+ bool left(bool check_empty=false); // moves one corner to the left or returns false
+ bool right(bool check_empty=false); // moves one corner to the right or returns false
+ bool bottom(bool check_empty=false); // moves one corner to the bottom or returns false
+ bool top(bool check_empty=false); // moves one corner to the top or returns false
+ bool checkCorner()const; // returns ture if the current corner belongs to at least one
+ // none empty cell
+ bool isNaN()const; // returns true if the currnet corner is NaN
+
+ const cv::Point2f* operator*() const; // current corner coordinate
+ cv::Point2f* operator*(); // current corner coordinate
+ const cv::Point2f* operator->() const; // current corner coordinate
+ cv::Point2f* operator->(); // current corner coordinate
+
+ Cell *getCell(); // current cell
+ private:
+ CornerIndex corner_index;
+ Cell *cell;
+ };
+
+ std::vector<Cell*> cells; // storage for all board cells
+ std::vector<cv::Point2f*> corners; // storage for all corners
+ Cell *top_left; // pointer to the top left corner of the board in its local coordinate system
+ int rows; // number of row cells
+ int cols; // number of col cells
+ float white_angle,black_angle;
+ };
+ public:
+
+ /**
+ * \brief Creates a chessboard corner detectors
+ *
+ * \param[in] config Configuration used to detect chessboard corners
+ *
+ */
+ Chessboard(const Parameters &config = Parameters());
+ virtual ~Chessboard();
+ void reconfigure(const Parameters &config = Parameters());
+ Parameters getPara()const;
+
+ /*
+ * \brief Detects chessboard corners in the given image.
+ *
+ * The detectors tries to find all chessboard corners of an imaged
+ * chessboard and returns them as an ordered vector of KeyPoints.
+ * Thereby, the left top corner has index 0 and the bottom right
+ * corner n*m-1.
+ *
+ * \param[in] image The image
+ * \param[out] keypoints The detected corners as a vector of ordered KeyPoints
+ * \param[in] mask Currently not supported
+ *
+ */
+ void detect(cv::InputArray image,std::vector<cv::KeyPoint>& keypoints, cv::InputArray mask=cv::Mat())override
+ {cv::Feature2D::detect(image.getMat(),keypoints,mask.getMat());}
+
+ virtual void detectAndCompute(cv::InputArray image,cv::InputArray mask, std::vector<cv::KeyPoint>& keypoints,cv::OutputArray descriptors,
+ bool useProvidedKeyPoints = false)override;
+
+ /*
+ * \brief Detects chessboard corners in the given image.
+ *
+ * The detectors tries to find all chessboard corners of an imaged
+ * chessboard and returns them as an ordered vector of KeyPoints.
+ * Thereby, the left top corner has index 0 and the bottom right
+ * corner n*m-1.
+ *
+ * \param[in] image The image
+ * \param[out] keypoints The detected corners as a vector of ordered KeyPoints
+ * \param[out] feature_maps The feature map generated by LRJT and used to find the corners
+ * \param[in] mask Currently not supported
+ *
+ */
+ void detectImpl(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints,std::vector<cv::Mat> &feature_maps,const cv::Mat& mask)const;
+ Chessboard::Board detectImpl(const cv::Mat& image,std::vector<cv::Mat> &feature_maps,const cv::Mat& mask)const;
+
+ // define pure virtual methods
+ virtual int descriptorSize()const override{return 0;};
+ virtual int descriptorType()const override{return 0;};
+ virtual void operator()( cv::InputArray image, cv::InputArray mask, std::vector<cv::KeyPoint>& keypoints, cv::OutputArray descriptors, bool useProvidedKeypoints=false )const
+ {
+ descriptors.clear();
+ detectImpl(image.getMat(),keypoints,mask);
+ if(!useProvidedKeypoints) // suppress compiler warning
+ return;
+ return;
+ }
+
+ protected:
+ virtual void computeImpl( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors)const
+ {
+ descriptors = cv::Mat();
+ detectImpl(image,keypoints);
+ }
+
+ // indicates why a board could not be initialized for a certain keypoint
+ enum BState
+ {
+ MISSING_POINTS = 0, // at least 5 points are needed
+ MISSING_PAIRS = 1, // at least two pairs are needed
+ WRONG_PAIR_ANGLE = 2, // angle between pairs is too small
+ WRONG_CONFIGURATION = 3, // point configuration is wrong and does not belong to a board
+ FOUND_BOARD = 4 // board was found
+ };
+
+ void findKeyPoints(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints,std::vector<cv::Mat> &feature_maps,
+ std::vector<std::vector<float> > &angles ,const cv::Mat& mask)const;
+ cv::Mat buildData(const std::vector<cv::KeyPoint>& keypoints)const;
+ std::vector<cv::KeyPoint> getInitialPoints(cv::flann::Index &flann_index,const cv::Mat &data,const cv::KeyPoint ¢er,float white_angle,float black_angle, float min_response = 0)const;
+ BState generateBoards(cv::flann::Index &flann_index,const cv::Mat &data, const cv::KeyPoint ¢er,
+ float white_angle,float black_angle,float min_response,const cv::Mat &img,
+ std::vector<Chessboard::Board> &boards)const;
+
+ private:
+ void detectImpl(const cv::Mat&,std::vector<cv::KeyPoint>&, const cv::Mat& mast =cv::Mat())const;
+ virtual void detectImpl(cv::InputArray image, std::vector<cv::KeyPoint>& keypoints, cv::InputArray mask=cv::noArray())const;
+
+ private:
+ Parameters parameters; // storing the configuration of the detector
+};
+}} // end namespace details and cv
+
+#endif
projects / platform / upstream / opencv.git / commitdiff
