diff --git a/doc/opencv.bib b/doc/opencv.bib index 7c8303f7f4..e059096dc4 100644 --- a/doc/opencv.bib +++ b/doc/opencv.bib @@ -1016,17 +1016,22 @@ year = {2017}, organization = {IEEE} } - @ARTICLE{gonzalez, title={Digital Image Fundamentals, Digital Imaging Processing}, author={Gonzalez, Rafael C and others}, year={1987}, publisher={Addison Wesley Publishing Company} } - @ARTICLE{gruzman, title={Цифровая обработка изображений в информационных системах}, author={Грузман, И.С. and Киричук, В.С. and Косых, В.П. and Перетягин, Г.И. and Спектор, А.А.}, year={2000}, publisher={Изд-во НГТУ Новосибирск} } +@INPROCEEDINGS{duda2018, + title = {Accurate Detection and Localization of Checkerboard Corners for Calibration}, + year = {2018}, + booktitle = {29th British Machine Vision Conference. British Machine Vision Conference (BMVC-29), September 3-6, Newcastle, United Kingdom}, + publisher = {BMVA Press}, + author = {Alexander Duda and Udo Frese}, +} diff --git a/modules/calib3d/CMakeLists.txt b/modules/calib3d/CMakeLists.txt index b79944da59..67ea078246 100644 --- a/modules/calib3d/CMakeLists.txt +++ b/modules/calib3d/CMakeLists.txt @@ -1,2 +1,2 @@ set(the_description "Camera Calibration and 3D Reconstruction") -ocv_define_module(calib3d opencv_imgproc opencv_features2d WRAP java python) +ocv_define_module(calib3d opencv_imgproc opencv_features2d opencv_flann WRAP java python) diff --git a/modules/calib3d/doc/pics/checkerboard_radon.png b/modules/calib3d/doc/pics/checkerboard_radon.png new file mode 100644 index 0000000000..48d6041990 Binary files /dev/null and b/modules/calib3d/doc/pics/checkerboard_radon.png differ diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index be5fc86f19..28d6862e59 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -793,7 +793,7 @@ CV_EXPORTS_W Mat initCameraMatrix2D( InputArrayOfArrays objectPoints, @param image Source chessboard view. It must be an 8-bit grayscale or color image. @param patternSize Number of inner corners per a chessboard row and column -( patternSize = cvSize(points_per_row,points_per_colum) = cvSize(columns,rows) ). +( patternSize = cv::Size(points_per_row,points_per_colum) = cv::Size(columns,rows) ). @param corners Output array of detected corners. @param flags Various operation flags that can be zero or a combination of the following values: - **CALIB_CB_ADAPTIVE_THRESH** Use adaptive thresholding to convert the image to black @@ -841,6 +841,34 @@ square grouping and ordering algorithm fails. CV_EXPORTS_W bool findChessboardCorners( InputArray image, Size patternSize, OutputArray corners, int flags = CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE ); +/** @brief Finds the positions of internal corners of the chessboard using a sector based approach. + +@param image Source chessboard view. It must be an 8-bit grayscale or color image. +@param patternSize Number of inner corners per a chessboard row and column +( patternSize = cv::Size(points_per_row,points_per_colum) = cv::Size(columns,rows) ). +@param corners Output array of detected corners. +@param flags operation flags for future improvements + +The function is analog to findchessboardCorners but uses a localized radon +transformation approximated by box filters being more robust to all sort of +noise, faster on larger images and is able to directly return the sub-pixel +position of the internal chessboard corners. The Method is based on the paper +@cite duda2018 "Accurate Detection and Localization of Checkerboard Corners for +Calibration" demonstrating that the returned sub-pixel positions are more +accurate than the one returned by cornerSubPix allowing a precise camera +calibration for demanding applications. + +@note The function requires a white boarder with roughly the same width as one +of the checkerboard fields around the whole board to improve the detection in +various environments. In addition, because of the localized radon +transformation it is beneficial to use round corners for the field corners +which are located on the outside of the board. The following figure illustrates +a sample checkerboard optimized for the detection. However, any other checkerboard +can be used as well. +![Checkerboard](pics/checkerboard_radon.png) + */ +CV_EXPORTS_W bool findChessboardCornersSB(InputArray image,Size patternSize, OutputArray corners,int flags=0); + //! finds subpixel-accurate positions of the chessboard corners CV_EXPORTS bool find4QuadCornerSubpix( InputArray img, InputOutputArray corners, Size region_size ); diff --git a/modules/calib3d/src/chessboard.cpp b/modules/calib3d/src/chessboard.cpp new file mode 100644 index 0000000000..d754283832 --- /dev/null +++ b/modules/calib3d/src/chessboard.cpp @@ -0,0 +1,3205 @@ +// 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 +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(0,0) = scale; + T.at(0,1) = -scale*centroid; + T.at(1,0) = 0; + T.at(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(i); + p(1) = 1.0; + p = Tx*p; + new_points.at(i) = float(p(0)/p(1)); + } + break; + case CV_64FC1: + for(int i=0;i < points.rows;++i) + { + p(0) = points.at(i); + p(1) = 1.0; + p = Tx*p; + new_points.at(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(i); + double d = dst_n.at(i); + A.at(i,0) = s; + A.at(i,1) = 1.0; + A.at(i,2) = -s*d; + b.at(i) = d; + } + break; + case CV_64FC1: + for(int i=0;i(i); + double d = dst_n.at(i); + A.at(i,0) = s; + A.at(i,1) = 1.0; + A.at(i,2) = -s*d; + b.at(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(i) = b_.at(i)/d.at(i); + + cv::Mat x = vt.t()*y; + cv::Mat H = (cv::Mat_(2,2) << x.at(0), x.at(1), x.at(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_(src_x), srcY = Mat_(src_y); + Mat A = Mat_::ones(npoints,order + 1); + // build A matrix + for (int y = 0; y < npoints; ++y) + { + for (int x = 1; x < A.cols; ++x) + A.at(y,x) = srcX.at(y)*A.at(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(0,2) += 0.5*(size.width-img.cols); + m.at(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(out.dataend); + for(float *pout=out.ptr(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 > FastX::calcAngles(const std::vector &rotated_images,std::vector &keypoints)const +{ + // validate rotated_images + if(rotated_images.empty()) + CV_Error(Error::StsBadArg,"no rotated images"); + std::vector::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 > angles; + angles.resize(keypoints.size()); + float scale = float(parameters.super_resolution)+1.0F; + + // for each keypoint + std::vector::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(row,col); + pimages2 = s_rotated_images.ptr(row,col+1); + pimages3 = s_rotated_images.ptr(row+1,col); + pimages4 = s_rotated_images.ptr(row+1,col+1); + std::vector &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 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 &feature_maps, std::vector& 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 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(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(pos.y,pos.x-1), + feature_map.at(pos.y,pos.x), + feature_map.at(pos.y,pos.x+1))); + float sub_y = float(calcSubpixel(feature_map.at(pos.y-1,pos.x), + feature_map.at(pos.y,pos.x), + feature_map.at(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& 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::const_iterator iter = keypoints.begin(); + for(int row=0;iter != keypoints.end();++iter,++row) + { + descriptors.at(row,0) = iter->pt.x; + descriptors.at(row,1) = iter->pt.y; + } + if(!useProvidedKeyPoints) // suppress compiler warning + return; + return; +} + +void FastX::detectImpl(const cv::Mat& gray_image, + std::vector &rotated_images, + std::vector &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 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& keypoints,std::vector &feature_maps,const cv::Mat &mask)const +{ + std::vector rotated_images; + detectImpl(image,rotated_images,feature_maps,mask); + findKeyPoints(feature_maps,keypoints,mask); +} + +void FastX::detectImpl(InputArray image, std::vector& keypoints, InputArray mask)const +{ + std::vector feature_maps; + detectImpl(image.getMat(),keypoints,feature_maps,mask.getMat()); +} + +void FastX::detectImpl(const Mat& src, std::vector& keypoints, const Mat& mask)const +{ + std::vector 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(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(this)->operator*(); + return const_cast(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 &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 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 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 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 points = getCorners(); + std::vector::const_iterator iter1 = points.begin(); + int icols = int(colCount()); + int irows = int(rowCount()); + int count=0; + for(int row=0;rowx != 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;rowtop_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 image_points; + std::vector object_points; + std::vector corners_temp = getCorners(true); + std::vector::const_iterator iter = corners_temp.begin(); + for(int row = 0;row < irows;++row) + { + for(int col= 0;colx != 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 point_point_mapping; + point_point_mapping[NULL] = NULL; + std::vector::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_mapping; + std::vector::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::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::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::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::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::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 query,dists; + std::vector 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::const_iterator iter = indices.begin(); + float best_score = -std::numeric_limits::max(); + point.x = std::numeric_limits::quiet_NaN(); + point.y = std::numeric_limits::quiet_NaN(); + for(;iter != indices.end();++iter) + { + const float *val = data.ptr(*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::max()) + return 0; + else + return best_score; +} + +void Chessboard::Board::clear() +{ + top_left = NULL; rows = 0; cols = 0; + std::vector::iterator iter = cells.begin(); + for(;iter != cells.end();++iter) + delete *iter; + cells.clear(); + std::vector::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 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 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(0)-dst.at(1)*c+dst.at(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_(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_(1,4) << 0,a,b,c); + cv::Mat h = findHomography1D(src,dst); + float d = float((h.at(0,0)*40+h.at(0,1))/(h.at(1,0)*40+h.at(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 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(0)-dst.at(1)*fabs(d)+dst.at(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 &points) +{ + if(points.size() < 4) + { + if(points.size() == 3) + return true; + else + return false; + } + std::vector::const_iterator iter = points.begin(); + std::vector::const_iterator iter2 = iter+1; + std::vector::const_iterator iter3 = iter2+1; + std::vector::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::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(this)->getCell(row,col); + return const_cast(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 &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::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 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 ellipses; +#endif + if(isEmpty()) + CV_Error(Error::StsInternal,"growLeft: Board is empty"); + PointIter iter(top_left,TOP_LEFT); + std::vector 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 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 ellipses; +#endif + if(isEmpty()) + CV_Error(Error::StsInternal,"Board is empty"); + + PointIter iter(top_left,TOP_LEFT); + std::vector 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 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 ellipses; +#endif + if(isEmpty()) + CV_Error(Error::StsInternal,"Board is empty"); + + PointIter iter(top_left,TOP_RIGHT); + while(iter.right()); + std::vector 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 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 ellipses; +#endif + if(isEmpty()) + CV_Error(Error::StsInternal,"Board is empty"); + + PointIter iter(top_left,BOTTOM_LEFT); + while(iter.bottom()); + std::vector 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 &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::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 &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::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 &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::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 &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::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 points = getCorners(false); + std::vector::const_iterator iter = points.begin(); + for(;iter != points.end();++iter) + { + std::vector::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 points = getCorners(false); + std::vector::const_iterator iter1 = points.begin(); + for(;iter1 != points.end();++iter1) + { + // we do not have to check for NaN because of getCorners(flase) + std::vector::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 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 Chessboard::Board::getContour()const +{ + std::vector 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 points = getCorners(true); + + // build src and dst + std::vector src,dst; + for(int row =rect.y;row < row_end;++row) + { + for(int col=rect.x;col src,dst; + std::vector points = getCorners(true); + std::vector::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 Chessboard::Board::getMapping()const +{ + std::map map; + std::vector points = getCorners(); + std::vector::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 Chessboard::Board::getCorners(bool ball)const +{ + std::vector 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 Chessboard::Board::getKeyPoints(bool ball)const +{ + std::vector keypoints; + std::vector points = getCorners(ball); + std::vector::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& keypoints,std::vector &feature_maps, + std::vector > &angles ,const cv::Mat& mask)const +{ + keypoints.clear(); + angles.clear(); + vector 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 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 > angles_temp = detector.calcAngles(rotated_images,keypoints_temp); + + // filter out keypoints which are not symmetric + std::vector::iterator iter1 = keypoints_temp.begin(); + std::vector >::const_iterator iter2 = angles_temp.begin(); + for(;iter1 != keypoints_temp.end();++iter1,++iter2) + { + cv::KeyPoint &pt = *iter1; + const std::vector &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& keypoints)const +{ + cv::Mat data(int(keypoints.size()),4,CV_32FC1); // x + y + angle + strength + std::vector::const_iterator iter = keypoints.begin(); + float *val = reinterpret_cast(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 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 query,dists; + std::vector 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 points; + std::vector::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(*ids_iter,3); + if(fabs(response) < min_response) + continue; + const float &angle = data.at(*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(*ids_iter,0),data.at(*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 &boards)const +{ + // collect all points having a similar angle + std::vector 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 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 points; + cv::Vec2f n1(cos(white_angle),-sin(white_angle)); + cv::Vec2f n2(cos(black_angle),-sin(black_angle)); + std::vector::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 > pairs; + iter1 = points.begin(); + for(;iter1 != points.end();++iter1) + { + std::vector::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 >::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 >::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 board_points; + board_points.resize(9,cv::Point2f(std::numeric_limits::quiet_NaN(), + std::numeric_limits::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& keypoints,std::vector &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 &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 keypoints_seed; + std::vector > 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::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 >::const_iterator angles_iter = angles.begin(); + std::vector::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 &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 boards; + generateBoards(flann_index, data,*points_iter,white_angle,black_angle,min_response,gray,boards); + std::vector::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 contour = iter_boards->getContour(); + std::vector::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& keypoints, + cv::OutputArray descriptors,bool useProvidedKeyPoints) +{ + descriptors.clear(); + useProvidedKeyPoints=false; + std::vector maps; + detectImpl(image.getMat(),keypoints,maps,mask.getMat()); + if(!useProvidedKeyPoints) // suppress compiler warning + return; + return; +} + +void Chessboard::detectImpl(const Mat& image, vector& keypoints,const Mat& mask)const +{ + std::vector maps; + detectImpl(image,keypoints,maps,mask); +} + +void Chessboard::detectImpl(InputArray image, std::vector& 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 corners; + details::Chessboard board(para); + board.detect(img,corners); + if(corners.empty()) + { + corners_.release(); + return false; + } + std::vector points; + KeyPoint::convert(corners,points); + Mat(points).copyTo(corners_); + return true; +} diff --git a/modules/calib3d/src/chessboard.hpp b/modules/calib3d/src/chessboard.hpp new file mode 100644 index 0000000000..7453355ee5 --- /dev/null +++ b/modules/calib3d/src/chessboard.hpp @@ -0,0 +1,770 @@ +// 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 +#include +#include + +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& 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& keypoints, + cv::OutputArray descriptors, + bool useProvidedKeyPoints = false)override; + + void detectImpl(const cv::Mat& image, + std::vector& keypoints, + std::vector &feature_maps, + const cv::Mat& mask=cv::Mat())const; + + void detectImpl(const cv::Mat& image, + std::vector &rotated_images, + std::vector &feature_maps, + const cv::Mat& mask=cv::Mat())const; + + void findKeyPoints(const std::vector &feature_map, + std::vector& keypoints, + const cv::Mat& mask = cv::Mat())const; + + std::vector > calcAngles(const std::vector &rotated_images, + std::vector &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& 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& keypoints, cv::Mat& descriptors)const + { + descriptors = cv::Mat(); + detectImpl(image,keypoints); + } + + private: + void detectImpl(const cv::Mat& _src, std::vector& keypoints, const cv::Mat& mask)const; + virtual void detectImpl(cv::InputArray image, std::vector& 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 &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 &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 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 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 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 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 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 &points); + + /** + * \brief Adds one column at the top + * + * \param[in] points The corner coordinates + * + */ + void addRowTop(const std::vector &points); + + /** + * \brief Adds one column on the right side + * + * \param[in] points The corner coordinates + * + */ + void addColumnRight(const std::vector &points); + + /** + * \brief Adds one row at the bottom + * + * \param[in] points The corner coordinates + * + */ + void addRowBottom(const std::vector &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 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 &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 cells; // storage for all board cells + std::vector 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& 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& 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& keypoints,std::vector &feature_maps,const cv::Mat& mask)const; + Chessboard::Board detectImpl(const cv::Mat& image,std::vector &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& 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& 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& keypoints,std::vector &feature_maps, + std::vector > &angles ,const cv::Mat& mask)const; + cv::Mat buildData(const std::vector& keypoints)const; + std::vector 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 &boards)const; + + private: + void detectImpl(const cv::Mat&,std::vector&, const cv::Mat& mast =cv::Mat())const; + virtual void detectImpl(cv::InputArray image, std::vector& keypoints, cv::InputArray mask=cv::noArray())const; + + private: + Parameters parameters; // storing the configuration of the detector +}; +}} // end namespace details and cv + +#endif diff --git a/modules/calib3d/test/test_chesscorners.cpp b/modules/calib3d/test/test_chesscorners.cpp index e55d069de0..f0e3378bb6 100644 --- a/modules/calib3d/test/test_chesscorners.cpp +++ b/modules/calib3d/test/test_chesscorners.cpp @@ -73,7 +73,7 @@ void show_points( const Mat& gray, const Mat& expected, const vector& a #define show_points(...) #endif -enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID }; +enum Pattern { CHESSBOARD,CHESSBOARD_SB,CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID}; class CV_ChessboardDetectorTest : public cvtest::BaseTest { @@ -83,6 +83,10 @@ protected: void run(int); void run_batch(const string& filename); bool checkByGenerator(); + bool checkByGeneratorHighAccuracy(); + + // wraps calls based on the given pattern + bool findChessboardCornersWrapper(InputArray image, Size patternSize, OutputArray corners,int flags); Pattern pattern; int algorithmFlags; @@ -142,6 +146,8 @@ void CV_ChessboardDetectorTest::run( int /*start_from */) return;*/ switch( pattern ) { + case CHESSBOARD_SB: + checkByGeneratorHighAccuracy(); // not supported by CHESSBOARD case CHESSBOARD: checkByGenerator(); if (ts->get_err_code() != cvtest::TS::OK) @@ -183,6 +189,7 @@ void CV_ChessboardDetectorTest::run_batch( const string& filename ) switch( pattern ) { case CHESSBOARD: + case CHESSBOARD_SB: folder = string(ts->get_data_path()) + "cv/cameracalibration/"; break; case CIRCLES_GRID: @@ -238,24 +245,24 @@ void CV_ChessboardDetectorTest::run_batch( const string& filename ) Size pattern_size = expected.size(); vector v; - bool result = false; + int flags = 0; switch( pattern ) { case CHESSBOARD: - result = findChessboardCorners(gray, pattern_size, v, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE); + flags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE; break; case CIRCLES_GRID: - result = findCirclesGrid(gray, pattern_size, v); - break; + case CHESSBOARD_SB: case ASYMMETRIC_CIRCLES_GRID: - result = findCirclesGrid(gray, pattern_size, v, CALIB_CB_ASYMMETRIC_GRID | algorithmFlags); - break; + default: + flags = 0; } - - if( result ^ doesContatinChessboard || v.size() != count_exp ) + bool result = findChessboardCornersWrapper(gray, pattern_size,v,flags); + if(result ^ doesContatinChessboard || (doesContatinChessboard && v.size() != count_exp)) { ts->printf( cvtest::TS::LOG, "chessboard is detected incorrectly in %s\n", img_file.c_str() ); ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_OUTPUT ); + show_points( gray, expected, v, result ); return; } @@ -355,6 +362,28 @@ bool validateData(const ChessBoardGenerator& cbg, const Size& imgSz, return imgsize * threshold < cbsize; } +bool CV_ChessboardDetectorTest::findChessboardCornersWrapper(InputArray image, Size patternSize, OutputArray corners,int flags) +{ + switch(pattern) + { + case CHESSBOARD: + return findChessboardCorners(image,patternSize,corners,flags); + case CHESSBOARD_SB: + // check default settings until flags have been specified + return findChessboardCornersSB(image,patternSize,corners,0); + case ASYMMETRIC_CIRCLES_GRID: + flags |= CALIB_CB_ASYMMETRIC_GRID | algorithmFlags; + return findCirclesGrid(image, patternSize,corners,flags); + case CIRCLES_GRID: + flags |= CALIB_CB_SYMMETRIC_GRID; + return findCirclesGrid(image, patternSize,corners,flags); + default: + ts->printf( cvtest::TS::LOG, "Internal Error: unsupported chessboard pattern" ); + ts->set_failed_test_info( cvtest::TS::FAIL_GENERIC); + } + return false; +} + bool CV_ChessboardDetectorTest::checkByGenerator() { bool res = true; @@ -399,7 +428,7 @@ bool CV_ChessboardDetectorTest::checkByGenerator() vector corners_found; int flags = i % 8; // need to check branches for all flags - bool found = findChessboardCorners(cb, cbg.cornersSize(), corners_found, flags); + bool found = findChessboardCornersWrapper(cb, cbg.cornersSize(), corners_found, flags); if (!found) { ts->printf( cvtest::TS::LOG, "Chess board corners not found\n" ); @@ -421,7 +450,7 @@ bool CV_ChessboardDetectorTest::checkByGenerator() /* ***** negative ***** */ { vector corners_found; - bool found = findChessboardCorners(bg, Size(8, 7), corners_found); + bool found = findChessboardCornersWrapper(bg, Size(8, 7), corners_found,0); if (found) res = false; @@ -430,7 +459,7 @@ bool CV_ChessboardDetectorTest::checkByGenerator() vector cg; Mat cb = cbg(bg, camMat, distCoeffs, cg); - found = findChessboardCorners(cb, Size(3, 4), corners_found); + found = findChessboardCornersWrapper(cb, Size(3, 4), corners_found,0); if (found) res = false; @@ -441,7 +470,7 @@ bool CV_ChessboardDetectorTest::checkByGenerator() Mat sh; warpAffine(cb, sh, aff, cb.size()); - found = findChessboardCorners(sh, cbg.cornersSize(), corners_found); + found = findChessboardCornersWrapper(sh, cbg.cornersSize(), corners_found,0); if (found) res = false; @@ -451,7 +480,7 @@ bool CV_ChessboardDetectorTest::checkByGenerator() cnt.push_back(cg[7+0]); cnt.push_back(cg[7+2]); cv::drawContours(cb, cnts, -1, Scalar::all(128), FILLED); - found = findChessboardCorners(cb, cbg.cornersSize(), corners_found); + found = findChessboardCornersWrapper(cb, cbg.cornersSize(), corners_found,0); if (found) res = false; @@ -461,7 +490,127 @@ bool CV_ChessboardDetectorTest::checkByGenerator() return res; } +// generates artificial checkerboards using warpPerspective which supports +// subpixel rendering. The transformation is found by transferring corners to +// the camera image using a virtual plane. +bool CV_ChessboardDetectorTest::checkByGeneratorHighAccuracy() +{ + // draw 2D pattern + cv::Size pattern_size(6,5); + int cell_size = 80; + bool bwhite = true; + cv::Mat image = cv::Mat::ones((pattern_size.height+3)*cell_size,(pattern_size.width+3)*cell_size,CV_8UC1)*255; + cv::Mat pimage = image(Rect(cell_size,cell_size,(pattern_size.width+1)*cell_size,(pattern_size.height+1)*cell_size)); + pimage = 0; + for(int row=0;row<=pattern_size.height;++row) + { + int y = int(cell_size*row+0.5F); + bool bwhite2 = bwhite; + for(int col=0;col<=pattern_size.width;++col) + { + if(bwhite2) + { + int x = int(cell_size*col+0.5F); + pimage(cv::Rect(x,y,cell_size,cell_size)) = 255; + } + bwhite2 = !bwhite2; + + } + bwhite = !bwhite; + } + + // generate 2d points + std::vector pts1,pts2,pts1_all,pts2_all; + std::vector pts3d; + for(int row=0;rowprintf( cvtest::TS::LOG, "Internal Error: ray and plane are parallel" ); + ts->set_failed_test_info( cvtest::TS::FAIL_GENERIC); + return false; + } + pts3d.push_back(Point3f(ray/val1*cv::Vec3f((p0-l0)).dot(n))+l0); + } + + // generate multiple rotations + for(int i=15;i<90;i=i+15) + { + // project 3d points to new camera + Vec3f rvec(0.0F,0.05F,float(float(i)/180.0*M_PI)); + Vec3f tvec(0,0,0); + cv::Mat k = (cv::Mat_(3,3) << fx/2,0,center.x*2, 0,fy/2,center.y, 0,0,1); + cv::projectPoints(pts3d,rvec,tvec,k,cv::Mat(),pts2_all); + + // get perspective transform using four correspondences and wrap original image + pts1.clear(); + pts2.clear(); + pts1.push_back(pts1_all[0]); + pts1.push_back(pts1_all[pattern_size.width-1]); + pts1.push_back(pts1_all[pattern_size.width*pattern_size.height-1]); + pts1.push_back(pts1_all[pattern_size.width*(pattern_size.height-1)]); + pts2.push_back(pts2_all[0]); + pts2.push_back(pts2_all[pattern_size.width-1]); + pts2.push_back(pts2_all[pattern_size.width*pattern_size.height-1]); + pts2.push_back(pts2_all[pattern_size.width*(pattern_size.height-1)]); + Mat m2 = getPerspectiveTransform(pts1,pts2); + Mat out(image.size(),image.type()); + warpPerspective(image,out,m2,out.size()); + + // find checkerboard + vector corners_found; + bool found = findChessboardCornersWrapper(out,pattern_size,corners_found,0); + if (!found) + { + ts->printf( cvtest::TS::LOG, "Chess board corners not found\n" ); + ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY ); + return false; + } + double err = calcErrorMinError(pattern_size,corners_found,pts2_all); + if(err > 0.08) + { + ts->printf( cvtest::TS::LOG, "bad accuracy of corner guesses" ); + ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY ); + return false; + } + //cv::cvtColor(out,out,cv::COLOR_GRAY2BGR); + //cv::drawChessboardCorners(out,pattern_size,corners_found,true); + //cv::imshow("img",out); + //cv::waitKey(-1); + } + return true; +} + TEST(Calib3d_ChessboardDetector, accuracy) { CV_ChessboardDetectorTest test( CHESSBOARD ); test.safe_run(); } +TEST(Calib3d_ChessboardDetector2, accuracy) { CV_ChessboardDetectorTest test( CHESSBOARD_SB ); test.safe_run(); } TEST(Calib3d_CirclesPatternDetector, accuracy) { CV_ChessboardDetectorTest test( CIRCLES_GRID ); test.safe_run(); } TEST(Calib3d_AsymmetricCirclesPatternDetector, accuracy) { CV_ChessboardDetectorTest test( ASYMMETRIC_CIRCLES_GRID ); test.safe_run(); } #ifdef HAVE_OPENCV_FLANN