--- /dev/null
- Ellipse::Ellipse(const Ellipse &other)
- {
- center = other.center;
- axes= other.axes;
- angle= other.angle;
- cosf = other.cosf;
- sinf = other.sinf;
- }
-
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+#include "precomp.hpp"
+#include "opencv2/flann.hpp"
+#include "chessboard.hpp"
+#include "math.h"
+
+//#define CV_DETECTORS_CHESSBOARD_DEBUG
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+#include <opencv2/highgui.hpp>
+static cv::Mat debug_image;
+#endif
+
+using namespace std;
+namespace cv {
+namespace details {
+
+/////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////
+// magic numbers used for chessboard corner detection
+/////////////////////////////////////////////////////////////////////////////
+static const float CORNERS_SEARCH = 0.5F; // percentage of the edge length to the next corner used to find new corners
+static const float MAX_ANGLE = float(48.0/180.0*CV_PI); // max angle between line segments supposed to be straight
+static const float MIN_COS_ANGLE = float(cos(35.0/180*CV_PI)); // min cos angle between board edges
+static const float MIN_RESPONSE_RATIO = 0.1F;
+static const float ELLIPSE_WIDTH = 0.35F; // width of the search ellipse in percentage of its length
+static const float RAD2DEG = float(180.0/CV_PI);
+static const int MAX_SYMMETRY_ERRORS = 5; // maximal number of failures during point symmetry test (filtering out lines)
+/////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////
+
+// some helper methods
+static bool isPointOnLine(cv::Point2f l1,cv::Point2f l2,cv::Point2f pt,float min_angle);
+static int testPointSymmetry(const cv::Mat& mat,cv::Point2f pt,float dist,float max_error);
+static float calcSubpixel(const float &x_l,const float &x,const float &x_r);
+static float calcSubPos(const float &x_l,const float &x,const float &x_r);
+static void polyfit(const Mat& src_x, const Mat& src_y, Mat& dst, int order);
+static float calcSignedDistance(const cv::Vec2f &n,const cv::Point2f &a,const cv::Point2f &pt);
+static void normalizePoints1D(cv::InputArray _points,cv::OutputArray _T,cv::OutputArray _new_points);
+static cv::Mat findHomography1D(cv::InputArray _src,cv::InputArray _dst);
+
+void normalizePoints1D(cv::InputArray _points,cv::OutputArray _T,cv::OutputArray _new_points)
+{
+ cv::Mat points = _points.getMat();
+ if(points.cols > 1 && points.rows == 1)
+ points = points.reshape(1,points.cols);
+ CV_CheckChannelsEQ(points.channels(), 1, "points must have only one channel");
+
+ // calc centroid
+ double centroid= cv::mean(points)[0];
+
+ // shift origin to centroid
+ cv::Mat new_points = points-centroid;
+
+ // calc mean distance
+ double mean_dist = cv::mean(cv::abs(new_points))[0];
+ if(mean_dist<= DBL_EPSILON)
+ CV_Error(Error::StsBadArg, "all given points are identical");
+ double scale = 1.0/mean_dist;
+
+
+ // generate transformation
+ cv::Matx22d Tx(
+ scale, -scale*centroid,
+ 0, 1
+ );
+ Mat(Tx, false).copyTo(_T);
+
+ // calc normalized points;
+ _new_points.create(points.rows,1,points.type());
+ new_points = _new_points.getMat();
+ switch(points.type())
+ {
+ case CV_32FC1:
+ for(int i=0;i < points.rows;++i)
+ {
+ cv::Vec2d p(points.at<float>(i), 1.0);
+ p = Tx*p;
+ new_points.at<float>(i) = float(p(0)/p(1));
+ }
+ break;
+ case CV_64FC1:
+ for(int i=0;i < points.rows;++i)
+ {
+ cv::Vec2d p(points.at<double>(i), 1.0);
+ p = Tx*p;
+ new_points.at<double>(i) = p(0)/p(1);
+ }
+ break;
+ default:
+ CV_Error(Error::StsUnsupportedFormat, "unsupported point type");
+ }
+}
+
+cv::Mat findHomography1D(cv::InputArray _src,cv::InputArray _dst)
+{
+ // check inputs
+ cv::Mat src = _src.getMat();
+ cv::Mat dst = _dst.getMat();
+ if(src.cols > 1 && src.rows == 1)
+ src = src.reshape(1,src.cols);
+ if(dst.cols > 1 && dst.rows == 1)
+ dst = dst.reshape(1,dst.cols);
+ CV_CheckEQ(src.rows, dst.rows, "size mismatch");
+ CV_CheckChannelsEQ(src.channels(), 1, "data with only one channel are supported");
+ CV_CheckChannelsEQ(dst.channels(), 1, "data with only one channel are supported");
+ CV_CheckTypeEQ(src.type(), dst.type(), "src and dst must have the same type");
+ CV_Check(src.rows, src.rows >= 3,"at least three point pairs are needed");
+
+ // normalize points
+ cv::Mat src_T,dst_T, src_n,dst_n;
+ normalizePoints1D(src,src_T,src_n);
+ normalizePoints1D(dst,dst_T,dst_n);
+
+ int count = src_n.rows;
+ cv::Mat A = cv::Mat::zeros(count,3,CV_64FC1);
+ cv::Mat b = cv::Mat::zeros(count,1,CV_64FC1);
+
+ // fill A;b and perform singular value decomposition
+ // it is assumed that w is one for both coordinates
+ // h22 is kept to 1
+ switch(src_n.type())
+ {
+ case CV_32FC1:
+ for(int i=0;i<count;++i)
+ {
+ double s = src_n.at<float>(i);
+ double d = dst_n.at<float>(i);
+ A.at<double>(i,0) = s;
+ A.at<double>(i,1) = 1.0;
+ A.at<double>(i,2) = -s*d;
+ b.at<double>(i) = d;
+ }
+ break;
+ case CV_64FC1:
+ for(int i=0;i<count;++i)
+ {
+ double s = src_n.at<double>(i);
+ double d = dst_n.at<double>(i);
+ A.at<double>(i,0) = s;
+ A.at<double>(i,1) = 1.0;
+ A.at<double>(i,2) = -s*d;
+ b.at<double>(i) = d;
+ }
+ break;
+ default:
+ CV_Error(Error::StsUnsupportedFormat,"unsupported type");
+ }
+
+ cv::Mat u,d,vt;
+ cv::SVD::compute(A,d,u,vt);
+ cv::Mat b_ = u.t()*b;
+
+ cv::Mat y(b_.rows,1,CV_64FC1);
+ for(int i=0;i<b_.rows;++i)
+ y.at<double>(i) = b_.at<double>(i)/d.at<double>(i);
+
+ cv::Mat x = vt.t()*y;
+ cv::Matx22d H_(x.at<double>(0), x.at<double>(1), x.at<double>(2), 1.0);
+
+ // denormalize
+ Mat H = dst_T.inv()*Mat(H_, false)*src_T;
+
+ // enforce frobeniusnorm of one
+ double scale = cv::norm(H);
+ CV_Assert(fabs(scale) > DBL_EPSILON);
+ scale = 1.0 / scale;
+ return H*scale;
+}
+void polyfit(const Mat& src_x, const Mat& src_y, Mat& dst, int order)
+{
+ int npoints = src_x.checkVector(1);
+ int nypoints = src_y.checkVector(1);
+ CV_Assert(npoints == nypoints && npoints >= order+1);
+ Mat_<double> srcX(src_x), srcY(src_y);
+ Mat_<double> A = Mat_<double>::ones(npoints,order + 1);
+ // build A matrix
+ for (int y = 0; y < npoints; ++y)
+ {
+ for (int x = 1; x < A.cols; ++x)
+ A.at<double>(y,x) = srcX.at<double>(y)*A.at<double>(y,x-1);
+ }
+ cv::Mat w;
+ solve(A,srcY,w,DECOMP_SVD);
+ w.convertTo(dst, ((src_x.depth() == CV_64F || src_y.depth() == CV_64F) ? CV_64F : CV_32F));
+}
+
+float calcSignedDistance(const cv::Vec2f &n,const cv::Point2f &a,const cv::Point2f &pt)
+{
+ cv::Vec3f v1(n[0],n[1],0);
+ cv::Vec3f v2(pt.x-a.x,pt.y-a.y,0);
+ return v1.cross(v2)[2];
+}
+
+bool isPointOnLine(cv::Point2f l1,cv::Point2f l2,cv::Point2f pt,float min_angle)
+{
+ cv::Vec2f vec1(l1-pt);
+ cv::Vec2f vec2(pt-l2);
+ if(vec1.dot(vec2) < min_angle*cv::norm(vec1)*cv::norm(vec2))
+ return false;
+ return true;
+}
+
+// returns how many tests fails out of 10
+int testPointSymmetry(const cv::Mat& mat,cv::Point2f pt,float dist,float max_error)
+{
+ cv::Rect image_rect(int(0.5*dist),int(0.5*dist),int(mat.cols-0.5*dist),int(mat.rows-0.5*dist));
+ cv::Size size(int(0.5*dist),int(0.5*dist));
+ int count = 0;
+ cv::Mat patch1,patch2;
+ cv::Point2f center1,center2;
+ for (int angle_i = 0; angle_i < 10; angle_i++)
+ {
+ double angle = angle_i * (CV_PI * 0.1);
+ cv::Point2f n(float(cos(angle)),float(-sin(angle)));
+ center1 = pt+dist*n;
+ if(!image_rect.contains(center1))
+ return false;
+ center2 = pt-dist*n;
+ if(!image_rect.contains(center2))
+ return false;
+ cv::getRectSubPix(mat,size,center1,patch1);
+ cv::getRectSubPix(mat,size,center2,patch2);
+ if(fabs(cv::mean(patch1)[0]-cv::mean(patch2)[0]) > max_error)
+ ++count;
+ }
+ return count;
+}
+
+float calcSubpixel(const float &x_l,const float &x,const float &x_r)
+{
+ // prevent zero values
+ if(x_l <= 0)
+ return 0;
+ if(x <= 0)
+ return 0;
+ if(x_r <= 0)
+ return 0;
+ const float l0 = float(std::log(x_l+1e-6));
+ const float l1 = float(std::log(x+1e-6));
+ const float l2 = float(std::log(x_r+1e-6));
+ float delta = l2-l1-l1+l0;
+ if(!delta) // this happens if all values are identical
+ return 0;
+ delta = (l0-l2)/(delta+delta);
+ return delta;
+}
+
+float calcSubPos(const float &x_l,const float &x,const float &x_r)
+{
+ float val = 2.0F *(x_l-2.0F*x+x_r);
+ if(val == 0.0F)
+ return 0.0F;
+ val = (x_l-x_r)/val;
+ if(val > 1.0F)
+ return 1.0F;
+ if(val < -1.0F)
+ return -1.0F;
+ return val;
+}
+
+FastX::FastX(const Parameters ¶)
+{
+ 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::Matx23d m = cv::getRotationMatrix2D(cv::Point2f(float(img.cols*0.5),float(img.rows*0.5)),float(angle/CV_PI*180),1);
+ m(0,2) += 0.5*(size.width-img.cols);
+ m(1,2) += 0.5*(size.height-img.rows);
+ cv::warpAffine(img,out,m,size);
+ }
+}
+
+void FastX::calcFeatureMap(const Mat &images,Mat& out)const
+{
+ if(images.empty())
+ CV_Error(Error::StsBadArg,"no rotation images");
+ int type = images.type(), depth = CV_MAT_DEPTH(type);
+ CV_CheckType(type,depth == CV_8U,
+ "Only 8-bit grayscale or color images are supported");
+ if(!images.isContinuous())
+ CV_Error(Error::StsBadArg,"image must be continuous");
+
+ float signal,noise,rating;
+ int count1;
+ unsigned char val1,val2,val3;
+ const unsigned char* wrap_around;
+ const unsigned char* pend;
+ const unsigned char* pimages = images.data;
+ const int channels = images.channels();
+ if(channels < 4)
+ CV_Error(Error::StsBadArg,"images must have at least four channels");
+
+ // for each pixel
+ out = cv::Mat::zeros(images.rows,images.cols,CV_32FC1);
+ const float *pout_end = reinterpret_cast<const float*>(out.dataend);
+ for(float *pout=out.ptr<float>(0,0);pout != pout_end;++pout)
+ {
+ //reset values
+ rating = 0.0; count1 = 0;
+ noise = 255; signal = 0;
+
+ //calc rating
+ pend = pimages+channels;
+ val1 = *(pend-1); // wrap around (last value)
+ wrap_around = pimages++; // store for wrap around (first value)
+ val2 = *wrap_around; // first value
+ for(;pimages != pend;++pimages)
+ {
+ val3 = *pimages;
+ if(val1 <= val2)
+ {
+ if(val3 < val2) // maxima
+ {
+ if(signal < val2)
+ signal = val2;
+ ++count1;
+ }
+ }
+ else if(val1 > val2 && val3 >= val2) // minima
+ {
+ if(noise > val2)
+ noise = val2;
+ ++count1;
+ }
+ val1 = val2;
+ val2 = val3;
+ }
+ // wrap around
+ if(val1 <= val2) // maxima
+ {
+ if(*wrap_around < val2)
+ {
+ if(signal < val2)
+ signal = val2;
+ ++count1;
+ }
+ }
+ else if(val1 > val2 && *wrap_around >= val2) // minima
+ {
+ if(noise > val2)
+ noise = val2;
+ ++count1;
+ }
+
+ // store rating
+ if(count1 == parameters.branches)
+ {
+ rating = signal-noise;
+ *pout = rating*rating; //store rating in the feature map
+ }
+ }
+}
+
+std::vector<std::vector<float> > FastX::calcAngles(const std::vector<cv::Mat> &rotated_images,std::vector<cv::KeyPoint> &keypoints)const
+{
+ // validate rotated_images
+ if(rotated_images.empty())
+ CV_Error(Error::StsBadArg,"no rotated images");
+ std::vector<cv::Mat>::const_iterator iter = rotated_images.begin();
+ for(;iter != rotated_images.end();++iter)
+ {
+ if(iter->empty())
+ CV_Error(Error::StsBadArg,"empty rotated images");
+ if(iter->channels() < 4)
+ CV_Error(Error::StsBadArg,"rotated images must have at least four channels");
+ }
+
+ // assuming all elements of the same channel
+ const int channels = rotated_images.front().channels();
+ int channels_1 = channels-1;
+ float resolution = float(CV_PI/channels);
+
+ float angle;
+ float val1,val2,val3,wrap_around;
+ const unsigned char *pimages1,*pimages2,*pimages3,*pimages4;
+ std::vector<std::vector<float> > angles;
+ angles.resize(keypoints.size());
+ float scale = float(parameters.super_resolution)+1.0F;
+
+ // for each keypoint
+ std::vector<cv::KeyPoint>::iterator pt_iter = keypoints.begin();
+ for(int id=0;pt_iter != keypoints.end();++pt_iter,++id)
+ {
+ int scale_id = pt_iter->octave - parameters.min_scale;
+ if(scale_id>= int(rotated_images.size()) ||scale_id < 0)
+ CV_Error(Error::StsBadArg,"no rotated image for requested keypoint octave");
+ const cv::Mat &s_rotated_images = rotated_images[scale_id];
+
+ float x2 = pt_iter->pt.x*scale;
+ float y2 = pt_iter->pt.y*scale;
+ int row = int(y2);
+ int col = int(x2);
+ x2 -= col;
+ y2 -= row;
+ float x1 = 1.0F-x2; float y1 = 1.0F-y2;
+ float a = x1*y1; float b = x2*y1; float c = x1*y2; float d = x2*y2;
+ pimages1 = s_rotated_images.ptr<unsigned char>(row,col);
+ pimages2 = s_rotated_images.ptr<unsigned char>(row,col+1);
+ pimages3 = s_rotated_images.ptr<unsigned char>(row+1,col);
+ pimages4 = s_rotated_images.ptr<unsigned char>(row+1,col+1);
+ std::vector<float> &angles_i = angles[id];
+
+ //calc rating
+ val1 = a**(pimages1+channels_1)+b**(pimages2+channels_1)+
+ c**(pimages3+channels_1)+d**(pimages4+channels_1); // wrap around (last value)
+ wrap_around = a**(pimages1++)+b**(pimages2++)+c**(pimages3++)+d**(pimages4++); // first value
+ val2 = wrap_around; // first value
+ for(int i=0;i<channels-1;++pimages1,++pimages2,++pimages3,++pimages4,++i)
+ {
+ val3 = a**(pimages1)+b**(pimages2)+c**(pimages3)+d**(pimages4);
+ if(val1 <= val2)
+ {
+ if(val3 < val2)
+ {
+ angle = float((calcSubPos(val1,val2,val3)+i)*resolution);
+ if(angle < 0)
+ angle += float(CV_PI);
+ else if(angle > CV_PI)
+ angle -= float(CV_PI);
+ angles_i.push_back(angle);
+ pt_iter->angle = 360.0F-angle*RAD2DEG;
+ }
+ }
+ else if(val1 > val2 && val3 >= val2)
+ {
+ angle = float((calcSubPos(val1,val2,val3)+i)*resolution);
+ if(angle < 0)
+ angle += float(CV_PI);
+ else if(angle > CV_PI)
+ angle -= float(CV_PI);
+ angles_i.push_back(-angle);
+ pt_iter->angle = 360.0F-angle*RAD2DEG;
+ }
+ val1 = val2;
+ val2 = val3;
+ }
+ // wrap around
+ if(val1 <= val2)
+ {
+ if(wrap_around< val2)
+ {
+ angle = float((calcSubPos(val1,val2,wrap_around)+channels-1)*resolution);
+ if(angle < 0)
+ angle += float(CV_PI);
+ else if(angle > CV_PI)
+ angle -= float(CV_PI);
+ angles_i.push_back(angle);
+ pt_iter->angle = 360.0F-angle*RAD2DEG;
+ }
+ }
+ else if(val1 > val2 && wrap_around >= val2)
+ {
+ angle = float((calcSubPos(val1,val2,wrap_around)+channels-1)*resolution);
+ if(angle < 0)
+ angle += float(CV_PI);
+ else if(angle > CV_PI)
+ angle -= float(CV_PI);
+ angles_i.push_back(-angle);
+ pt_iter->angle = 360.0F-angle*RAD2DEG;
+ }
+ }
+ return angles;
+}
+
+void FastX::findKeyPoints(const std::vector<cv::Mat> &feature_maps, std::vector<KeyPoint>& keypoints,const Mat& _mask) const
+{
+ //TODO check that all feature_maps have the same size
+ int num_scales = parameters.max_scale-parameters.min_scale;
+ CV_CheckGE(int(feature_maps.size()), num_scales, "missing feature maps");
+ if (!_mask.empty())
+ {
+ CV_CheckTypeEQ(_mask.type(), CV_8UC1, "wrong mask type");
+ CV_CheckEQ(_mask.size(), feature_maps.front().size(),"wrong mask type or size");
+ }
+ keypoints.clear();
+
+ cv::Mat mask;
+ if(!_mask.empty())
+ mask = _mask;
+ else
+ mask = cv::Mat::ones(feature_maps.front().size(),CV_8UC1);
+
+ int super_res = int(parameters.super_resolution);
+ int super_scale = super_res+1;
+ float super_comp = 0.25F*super_res;
+
+ // for each scale
+ float strength = parameters.strength;
+ std::vector<int> windows;
+ cv::Point pt,pt2;
+ double min,max;
+ cv::Mat src;
+ for(int scale=parameters.max_scale;scale>=parameters.min_scale;--scale)
+ {
+ int window_size = (1 << (scale + super_res)) + 1;
+ float window_size2 = 0.5F*window_size;
+ float window_size4 = 0.25F*window_size;
+ int window_size2i = cvRound(window_size2);
+
+ const cv::Mat &feature_map = feature_maps[scale-parameters.min_scale];
+ int y = ((feature_map.rows)/window_size)-6;
+ int x = ((feature_map.cols)/window_size)-6;
+ for(int row=5;row<y;++row)
+ {
+ for(int col=5;col<x;++col)
+ {
+ Rect rect(col*window_size,row*window_size,window_size,window_size);
+ src = feature_map(rect);
+ cv::minMaxLoc(src,&min,&max,NULL,&pt);
+ if(min == max || max < strength)
+ continue;
+
+ cv::Point pos(pt.x+rect.x,pt.y+rect.y);
+ if(mask.at<unsigned char>(pos.y,pos.x) == 0)
+ continue;
+
+ Rect rect2(int(pos.x-window_size2),int(pos.y-window_size2),window_size,window_size);
+ src = feature_map(rect2);
+ cv::minMaxLoc(src,NULL,NULL,NULL,&pt2);
+ if(pos.x == pt2.x+rect2.x && pos.y == pt2.y+rect2.y)
+ {
+ // the point is the best one on the current scale
+ // check all larger scales if there is a stronger one
+ double max2;
+ int scale2= scale-1;
+ //parameters.min_scale;
+ for(;scale2>=parameters.min_scale;--scale2)
+ {
+ cv::minMaxLoc(feature_maps[scale2-parameters.min_scale](rect),NULL,&max2,NULL,NULL);
+ if(max2 > max)
+ break;
+ }
+ if(scale2<parameters.min_scale && pos.x+1 < feature_map.cols && pos.y+1 < feature_map.rows)
+ {
+ float sub_x = float(calcSubpixel(feature_map.at<float>(pos.y,pos.x-1),
+ feature_map.at<float>(pos.y,pos.x),
+ feature_map.at<float>(pos.y,pos.x+1)));
+ float sub_y = float(calcSubpixel(feature_map.at<float>(pos.y-1,pos.x),
+ feature_map.at<float>(pos.y,pos.x),
+ feature_map.at<float>(pos.y+1,pos.x)));
+ cv::KeyPoint kpt(sub_x+pos.x,sub_y+pos.y,float(window_size),0.F,float(max),scale);
+ int x2 = std::max(0,int(kpt.pt.x-window_size4));
+ int y2 = std::max(0,int(kpt.pt.y-window_size4));
+ int w = std::min(int(mask.cols-x2),window_size2i);
+ int h = std::min(int(mask.rows-y2),window_size2i);
+ mask(cv::Rect(x2,y2,w,h)) = 0.0;
+ if(super_scale != 1)
+ {
+ kpt.pt.x /= super_scale;
+ kpt.pt.y /= super_scale;
+ kpt.pt.x -= super_comp;
+ kpt.pt.y -= super_comp;
+ kpt.size /= super_scale;
+ }
+ keypoints.push_back(kpt);
+ }
+ }
+ }
+ }
+ }
+}
+
+void FastX::detectAndCompute(cv::InputArray image,cv::InputArray mask,std::vector<cv::KeyPoint>& keypoints,
+ cv::OutputArray _descriptors,bool useProvidedKeyPoints)
+{
+ useProvidedKeyPoints = false;
+ detectImpl(image.getMat(),keypoints,mask.getMat());
+ if(!_descriptors.needed())
+ return;
+
+ // generate descriptors based on their position
+ _descriptors.create(int(keypoints.size()),2,CV_32FC1);
+ cv::Mat descriptors = _descriptors.getMat();
+ std::vector<cv::KeyPoint>::const_iterator iter = keypoints.begin();
+ for(int row=0;iter != keypoints.end();++iter,++row)
+ {
+ descriptors.at<float>(row,0) = iter->pt.x;
+ descriptors.at<float>(row,1) = iter->pt.y;
+ }
+ if(!useProvidedKeyPoints) // suppress compiler warning
+ return;
+ return;
+}
+
+void FastX::detectImpl(const cv::Mat& _gray_image,
+ std::vector<cv::Mat> &rotated_images,
+ std::vector<cv::Mat> &feature_maps,
+ const cv::Mat &_mask)const
+{
+ if(!_mask.empty())
+ CV_Error(Error::StsBadSize, "Mask is not supported");
+ CV_CheckTypeEQ(_gray_image.type(), CV_8UC1, "Unsupported image type");
+
+ // up-sample if needed
+ cv::Mat gray_image;
+ int super_res = int(parameters.super_resolution);
+ if(super_res)
+ cv::resize(_gray_image,gray_image,cv::Size(),2,2);
+ else
+ gray_image = _gray_image;
+
+ //for each scale
+ int num_scales = parameters.max_scale-parameters.min_scale+1;
+ rotated_images.resize(num_scales);
+ feature_maps.resize(num_scales);
+ parallel_for_(Range(parameters.min_scale,parameters.max_scale+1),[&](const Range& range){
+ for(int scale=range.start;scale < range.end;++scale)
+ {
+ // calc images
+ // for each angle step
+ int scale_id = scale-parameters.min_scale;
+ cv::Mat rotated,filtered_h,filtered_v;
+ int diag = int(sqrt(gray_image.rows*gray_image.rows+gray_image.cols*gray_image.cols));
+ cv::Size size(diag,diag);
+ int num = int(0.5001*CV_PI/parameters.resolution);
+ std::vector<cv::Mat> images;
+ images.resize(2*num);
+ int scale_size = int(1+pow(2.0,scale+1+super_res));
+ int scale_size2 = int((scale_size/10)*2+1);
+ for(int i=0;i<num;++i)
+ {
+ float angle = parameters.resolution*i;
+ rotate(-angle,gray_image,size,rotated);
+ cv::blur(rotated,filtered_h,cv::Size(scale_size,scale_size2));
+ cv::blur(rotated,filtered_v,cv::Size(scale_size2,scale_size));
+
+ // rotate filtered images back
+ rotate(angle,filtered_h,gray_image.size(),images[i]);
+ rotate(angle,filtered_v,gray_image.size(),images[i+num]);
+ }
+ cv::merge(images,rotated_images[scale_id]);
+
+ // calc feature map
+ calcFeatureMap(rotated_images[scale_id],feature_maps[scale_id]);
+ // filter feature map to improve impulse responses
+ if(parameters.filter)
+ {
+ cv::Mat high,low;
+ cv::blur(feature_maps[scale_id],low,cv::Size(scale_size,scale_size));
+ int scale2 = int((scale_size/6))*2+1;
+ cv::blur(feature_maps[scale_id],high,cv::Size(scale2,scale2));
+ feature_maps[scale_id] = high-0.8*low;
+ }
+ }
+ });
+}
+
+void FastX::detectImpl(const cv::Mat& image,std::vector<cv::KeyPoint>& keypoints,std::vector<cv::Mat> &feature_maps,const cv::Mat &mask)const
+{
+ std::vector<cv::Mat> rotated_images;
+ detectImpl(image,rotated_images,feature_maps,mask);
+ findKeyPoints(feature_maps,keypoints,mask);
+}
+
+void FastX::detectImpl(InputArray image, std::vector<KeyPoint>& keypoints, InputArray mask)const
+{
+ std::vector<cv::Mat> feature_maps;
+ detectImpl(image.getMat(),keypoints,feature_maps,mask.getMat());
+}
+
+void FastX::detectImpl(const Mat& src, std::vector<KeyPoint>& keypoints, const Mat& mask)const
+{
+ std::vector<cv::Mat> feature_maps;
+ detectImpl(src,keypoints,feature_maps,mask);
+}
+
+
+Ellipse::Ellipse():
+ angle(0),
+ cosf(0),
+ sinf(0)
+{
+}
+
+Ellipse::Ellipse(const cv::Point2f &_center, const cv::Size2f &_axes, float _angle):
+ center(_center),
+ axes(_axes),
+ angle(_angle),
+ cosf(cos(-_angle)),
+ sinf(sin(-_angle))
+{
+}
+
+const cv::Size2f &Ellipse::getAxes()const
+{
+ return axes;
+}
+
+cv::Point2f Ellipse::getCenter()const
+{
+ return center;
+}
+
+void Ellipse::draw(cv::InputOutputArray img,const cv::Scalar &color)const
+{
+ cv::ellipse(img,center,axes,360-angle/CV_PI*180,0,360,color);
+}
+
+bool Ellipse::contains(const cv::Point2f &pt)const
+{
+ cv::Point2f ptc = pt-center;
+ float x = cosf*ptc.x+sinf*ptc.y;
+ float y = -sinf*ptc.x+cosf*ptc.y;
+ if(x*x/(axes.width*axes.width)+y*y/(axes.height*axes.height) <= 1.0)
+ return true;
+ return false;
+}
+
+
+// returns false if the angle from the line pt1-pt2 to the line pt3-pt4 is negative
+static bool checkOrientation(const cv::Point2f &pt1,const cv::Point2f &pt2,
+ const cv::Point2f &pt3,const cv::Point2f &pt4)
+{
+ cv::Point3f p1(pt2.x-pt1.x,pt2.y-pt1.y,0);
+ cv::Point3f p2(pt4.x-pt3.x,pt4.y-pt3.y,0);
+ return p1.cross(p2).z > 0;
+}
+
+static bool sortKeyPoint(const cv::KeyPoint &pt1,const cv::KeyPoint &pt2)
+{
+ // used as comparison function for partial sort
+ // the keypoints with the best score should be first
+ return pt1.response > pt2.response;
+}
+
+cv::Mat Chessboard::getObjectPoints(const cv::Size &pattern_size,float cell_size)
+{
+ cv::Mat result(pattern_size.width*pattern_size.height,1,CV_32FC3);
+ for(int row=0;row < pattern_size.height;++row)
+ {
+ for(int col=0;col< pattern_size.width;++col)
+ {
+ cv::Point3f &pt = *result.ptr<cv::Point3f>(row*pattern_size.width+col);
+ pt.x = cell_size*col;
+ pt.y = cell_size*row;
+ pt.z = 0;
+ }
+ }
+ return result;
+}
+
+bool Chessboard::Board::Cell::empty()const
+{
+ // check if one of its corners has NaN
+ if(top_left->x != top_left->x || top_left->y != top_left->y)
+ return true;
+ if(top_right->x != top_right->x || top_right->y != top_right->y)
+ return true;
+ if(bottom_right->x != bottom_right->x || bottom_right->y != bottom_right->y)
+ return true;
+ if(bottom_left->x != bottom_left->x || bottom_left->y != bottom_left->y)
+ return true;
+ return false;
+}
+
+int Chessboard::Board::Cell::getRow()const
+{
+ int row = 0;
+ Cell const* temp = this;
+ for(;temp->top;temp=temp->top,++row);
+ return row;
+}
+
+int Chessboard::Board::Cell::getCol()const
+{
+ int col = 0;
+ Cell const* temp = this;
+ for(;temp->left;temp=temp->left,++col);
+ return col;
+}
+
+Chessboard::Board::Cell::Cell() :
+ top_left(NULL), top_right(NULL), bottom_right(NULL), bottom_left(NULL),
+ left(NULL), top(NULL), right(NULL), bottom(NULL),black(false)
+{}
+
+Chessboard::Board::PointIter::PointIter(Cell *_cell,CornerIndex _corner_index):
+ corner_index(_corner_index),
+ cell(_cell)
+{
+}
+
+Chessboard::Board::PointIter::PointIter(const PointIter &other)
+{
+ this->operator=(other);
+}
+
+void Chessboard::Board::PointIter::operator=(const PointIter &other)
+{
+ corner_index = other.corner_index;
+ cell = other.cell;
+}
+
+Chessboard::Board::Cell* Chessboard::Board::PointIter::getCell()
+{
+ return cell;
+}
+
+bool Chessboard::Board::PointIter::valid()const
+{
+ return cell != NULL;
+}
+
+bool Chessboard::Board::PointIter::isNaN()const
+{
+ const cv::Point2f *pt = operator*();
+ if(pt->x != pt->x || pt->y != pt->y) // NaN check
+ return true;
+ return false;
+}
+
+bool Chessboard::Board::PointIter::checkCorner()const
+{
+ if(!cell->empty())
+ return true;
+ // test all other cells
+ switch(corner_index)
+ {
+ case BOTTOM_LEFT:
+ if(cell->left)
+ {
+ if(!cell->left->empty())
+ return true;
+ if(cell->left->bottom && !cell->left->bottom->empty())
+ return true;
+ }
+ if(cell->bottom)
+ {
+ if(!cell->bottom->empty())
+ return true;
+ if(cell->bottom->left && !cell->bottom->left->empty())
+ return true;
+ }
+ break;
+ case TOP_LEFT:
+ if(cell->left)
+ {
+ if(!cell->left->empty())
+ return true;
+ if(cell->left->top && !cell->left->top->empty())
+ return true;
+ }
+ if(cell->top)
+ {
+ if(!cell->top->empty())
+ return true;
+ if(cell->top->left && !cell->top->left->empty())
+ return true;
+ }
+ break;
+ case TOP_RIGHT:
+ if(cell->right)
+ {
+ if(!cell->right->empty())
+ return true;
+ if(cell->right->top && !cell->right->top->empty())
+ return true;
+ }
+ if(cell->top)
+ {
+ if(!cell->top->empty())
+ return true;
+ if(cell->top->right && !cell->top->right->empty())
+ return true;
+ }
+ break;
+ case BOTTOM_RIGHT:
+ if(cell->right)
+ {
+ if(!cell->right->empty())
+ return true;
+ if(cell->right->bottom && !cell->right->bottom->empty())
+ return true;
+ }
+ if(cell->bottom)
+ {
+ if(!cell->bottom->empty())
+ return true;
+ if(cell->bottom->right && !cell->bottom->right->empty())
+ return true;
+ }
+ break;
+ default:
+ CV_Assert(false);
+ }
+ return false;
+}
+
+
+bool Chessboard::Board::PointIter::left(bool check_empty)
+{
+ switch(corner_index)
+ {
+ case BOTTOM_LEFT:
+ if(cell->left && (!check_empty || !cell->left->empty()))
+ cell = cell->left;
+ else if(check_empty && cell->bottom && cell->bottom->left && !cell->bottom->left->empty())
+ {
+ cell = cell->bottom->left;
+ corner_index = TOP_LEFT;
+ }
+ else
+ return false;
+ break;
+ case TOP_LEFT:
+ if(cell->left && (!check_empty || !cell->left->empty()))
+ cell = cell->left;
+ else if(check_empty && cell->top && cell->top->left && !cell->top->left->empty())
+ {
+ cell = cell->top->left;
+ corner_index = BOTTOM_LEFT;
+ }
+ else
+ return false;
+ break;
+ case TOP_RIGHT:
+ corner_index = TOP_LEFT;
+ break;
+ case BOTTOM_RIGHT:
+ corner_index = BOTTOM_LEFT;
+ break;
+ default:
+ CV_Assert(false);
+ }
+ return true;
+}
+
+bool Chessboard::Board::PointIter::top(bool check_empty)
+
+{
+ switch(corner_index)
+ {
+ case TOP_RIGHT:
+ if(cell->top && (!check_empty || !cell->top->empty()))
+ cell = cell->top;
+ else if(check_empty && cell->right && cell->right->top&& !cell->right->top->empty())
+ {
+ cell = cell->right->top;
+ corner_index = TOP_LEFT;
+ }
+ else
+ return false;
+ break;
+ case TOP_LEFT:
+ if(cell->top && (!check_empty || !cell->top->empty()))
+ cell = cell->top;
+ else if(check_empty && cell->left && cell->left->top&& !cell->left->top->empty())
+ {
+ cell = cell->left->top;
+ corner_index = TOP_RIGHT;
+ }
+ else
+ return false;
+ break;
+ case BOTTOM_LEFT:
+ corner_index = TOP_LEFT;
+ break;
+ case BOTTOM_RIGHT:
+ corner_index = TOP_RIGHT;
+ break;
+ default:
+ CV_Assert(false);
+ }
+ return true;
+}
+
+bool Chessboard::Board::PointIter::right(bool check_empty)
+{
+ switch(corner_index)
+ {
+ case TOP_RIGHT:
+ if(cell->right && (!check_empty || !cell->right->empty()))
+ cell = cell->right;
+ else if(check_empty && cell->top && cell->top->right && !cell->top->right->empty())
+ {
+ cell = cell->top->right;
+ corner_index = BOTTOM_RIGHT;
+ }
+ else
+ return false;
+ break;
+ case BOTTOM_RIGHT:
+ if(cell->right && (!check_empty || !cell->right->empty()))
+ cell = cell->right;
+ else if(check_empty && cell->bottom && cell->bottom->right && !cell->bottom->right->empty())
+ {
+ cell = cell->bottom->right;
+ corner_index = TOP_RIGHT;
+ }
+ else
+ return false;
+ break;
+ case TOP_LEFT:
+ corner_index = TOP_RIGHT;
+ break;
+ case BOTTOM_LEFT:
+ corner_index = BOTTOM_RIGHT;
+ break;
+ default:
+ CV_Assert(false);
+ }
+ return true;
+}
+
+bool Chessboard::Board::PointIter::bottom(bool check_empty)
+{
+ switch(corner_index)
+ {
+ case BOTTOM_LEFT:
+ if(cell->bottom && (!check_empty || !cell->bottom->empty()))
+ cell = cell->bottom;
+ else if(check_empty && cell->left && cell->left->bottom && !cell->left->bottom->empty())
+ {
+ cell = cell->left->bottom;
+ corner_index = BOTTOM_RIGHT;
+ }
+ else
+ return false;
+ break;
+ case BOTTOM_RIGHT:
+ if(cell->bottom && (!check_empty || !cell->bottom->empty()))
+ cell = cell->bottom;
+ else if(check_empty && cell->right && cell->right->bottom && !cell->right->bottom->empty())
+ {
+ cell = cell->right->bottom;
+ corner_index = BOTTOM_LEFT;
+ }
+ else
+ return false;
+ break;
+ case TOP_LEFT:
+ corner_index = BOTTOM_LEFT;
+ break;
+ case TOP_RIGHT:
+ corner_index = BOTTOM_RIGHT;
+ break;
+ default:
+ CV_Assert(false);
+ }
+ return true;
+}
+
+
+const cv::Point2f* Chessboard::Board::PointIter::operator*()const
+{
+ switch(corner_index)
+ {
+ case TOP_LEFT:
+ return cell->top_left;
+ case TOP_RIGHT:
+ return cell->top_right;
+ case BOTTOM_RIGHT:
+ return cell->bottom_right;
+ case BOTTOM_LEFT:
+ return cell->bottom_left;
+ }
+ CV_Assert(false);
+}
+
+const cv::Point2f* Chessboard::Board::PointIter::operator->()const
+{
+ return operator*();
+}
+
+cv::Point2f* Chessboard::Board::PointIter::operator*()
+{
+ const cv::Point2f *pt = const_cast<const PointIter*>(this)->operator*();
+ return const_cast<cv::Point2f*>(pt);
+}
+
+cv::Point2f* Chessboard::Board::PointIter::operator->()
+{
+ return operator*();
+}
+
+Chessboard::Board::Board(float _white_angle,float _black_angle):
+ top_left(NULL),
+ rows(0),
+ cols(0),
+ white_angle(_white_angle),
+ black_angle(_black_angle)
+{
+}
+
+
+Chessboard::Board::Board(const Chessboard::Board &other):
+ top_left(NULL),
+ rows(0),
+ cols(0)
+{
+ *this = other;
+}
+
+Chessboard::Board::Board(const cv::Size &size, const std::vector<cv::Point2f> &points,float _white_angle,float _black_angle):
+ top_left(NULL),
+ rows(0),
+ cols(0),
+ white_angle(_white_angle),
+ black_angle(_black_angle)
+{
+ if(size.width*size.height != int(points.size()))
+ CV_Error(Error::StsBadArg,"size mismatch");
+ if(size.width < 3 || size.height < 3)
+ CV_Error(Error::StsBadArg,"at least 3 rows and cols are needed to initialize the board");
+
+ // init board with 3x3
+ // TODO write function speeding up the copying
+ cv::Mat data = cv::Mat(points).reshape(2,size.height);
+ cv::Mat temp;
+ data(cv::Rect(0,0,3,3)).copyTo(temp);
+ std::vector<cv::Point2f> ipoints = temp.reshape(2,1);
+ if(!init(ipoints))
+ return;
+
+ // add all cols
+ for(int col=3 ; col< data.cols;++col)
+ {
+ data(cv::Rect(col,0,1,3)).copyTo(temp);
+ ipoints = temp.reshape(2,1);
+ addColumnRight(ipoints);
+ }
+
+ // add all rows
+ for(int row=3; row < data.rows;++row)
+ {
+ data(cv::Rect(0,row,cols,1)).copyTo(temp);
+ ipoints = temp.reshape(2,1);
+ addRowBottom(ipoints);
+ }
+}
+
+Chessboard::Board::~Board()
+{
+ clear();
+}
+
+std::vector<cv::Point2f> Chessboard::Board::getCellCenters()const
+{
+ int icols = int(colCount());
+ int irows = int(rowCount());
+ if(icols < 3 || irows < 3)
+ throw std::runtime_error("getCellCenters: Chessboard must be at least consist of 3 rows and cols to calculate the cell centers");
+
+ std::vector<cv::Point2f> points;
+ cv::Matx33d H(estimateHomography(DUMMY_FIELD_SIZE));
+ cv::Vec3d pt1,pt2;
+ pt1[2] = 1;
+ for(int row = 0;row < irows;++row)
+ {
+ pt1[1] = (0.5+row)*DUMMY_FIELD_SIZE;
+ for(int col= 0;col< icols;++col)
+ {
+ pt1[0] = (0.5+col)*DUMMY_FIELD_SIZE;
+ pt2 = H*pt1;
+ points.push_back(cv::Point2f(float(pt2[0]/pt2[2]),float(pt2[1]/pt2[2])));
+ }
+ }
+ return points;
+}
+
+void Chessboard::Board::draw(cv::InputArray m,cv::OutputArray out,cv::InputArray _H)const
+{
+ cv::Mat H = _H.getMat();
+ if(H.empty())
+ H = estimateHomography();
+ cv::Mat image = m.getMat().clone();
+ if(image.type() == CV_32FC1)
+ {
+ double maxVal,minVal;
+ cv::minMaxLoc(image, &minVal, &maxVal);
+ double scale = 255.0/(maxVal-minVal);
+ image.convertTo(image,CV_8UC1,scale,-scale*minVal);
+ cv::applyColorMap(image,image,cv::COLORMAP_JET);
+ }
+
+ // draw all points and search areas
+ std::vector<cv::Point2f> points = getCorners();
+ std::vector<cv::Point2f>::const_iterator iter1 = points.begin();
+ int icols = int(colCount());
+ int irows = int(rowCount());
+ int count=0;
+ for(int row=0;row<irows;++row)
+ {
+ for(int col=0;col<icols;++col,++iter1)
+ {
+ if(iter1->x != iter1->x) // NaN check
+ {
+ // draw search ellipse
+ Ellipse ellipse = estimateSearchArea(H,row,col,0.4F);
+ ellipse.draw(image,cv::Scalar::all(200));
+ }
+ else
+ {
+ cv::circle(image,*iter1,4,cv::Scalar(count*20,count*20,count*20,255),-1);
+ ++count;
+ }
+ }
+ }
+
+ // draw field colors
+ for(int row=0;row<irows-1;++row)
+ {
+ for(int col=0;col<icols-1;++col)
+ {
+ const Cell *cell = getCell(row,col);
+ cv::Point2f center = *cell->top_left+*cell->top_right+*cell->bottom_left+*cell->bottom_right;
+ center.x /=4;
+ center.y /=4;
+ int size = 4;
+ if(row==0&&col==0)
+ size=8;
+ if(row==0&&col==1)
+ size=7;
+ if(cell->black)
+ cv::circle(image,center,size,cv::Scalar::all(255),-1);
+ else
+ cv::circle(image,center,size,cv::Scalar(0,0,10,255),-1);
+ }
+ }
+
+ out.create(image.rows,image.cols,image.type());
+ image.copyTo(out.getMat());
+}
+
+bool Chessboard::Board::estimatePose(const cv::Size2f &real_size,cv::InputArray _K,cv::OutputArray rvec,cv::OutputArray tvec)const
+{
+ cv::Mat K = _K.getMat();
+ CV_CheckTypeEQ(K.type(), CV_64FC1, "wrong K type");
+ CV_CheckEQ(K.size(), Size(3, 3), "wrong K size");
+ if(isEmpty())
+ return false;
+
+ int icols = int(colCount());
+ int irows = int(rowCount());
+ float field_width = real_size.width/(icols+1);
+ float field_height= real_size.height/(irows+1);
+ // the center of the board is placed at (0,0,1)
+ int offset_x = int(-(icols-1)*field_width*0.5F);
+ int offset_y = int(-(irows-1)*field_width*0.5F);
+
+ std::vector<cv::Point2f> image_points;
+ std::vector<cv::Point3f> object_points;
+ std::vector<cv::Point2f> corners_temp = getCorners(true);
+ std::vector<cv::Point2f>::const_iterator iter = corners_temp.begin();
+ for(int row = 0;row < irows;++row)
+ {
+ for(int col= 0;col<icols;++col,++iter)
+ {
+ if(iter == corners_temp.end())
+ CV_Error(Error::StsInternal,"internal error");
+ if(iter->x != iter->x) // NaN check
+ continue;
+ image_points.push_back(*iter);
+ object_points.push_back(cv::Point3f(field_width*col-offset_x,field_height*row-offset_y,1.0));
+ }
+ }
+ return cv::solvePnP(object_points,image_points,K,cv::Mat(),rvec,tvec);//,cv::SOLVEPNP_P3P);
+}
+
+float Chessboard::Board::getBlackAngle()const
+{
+ return black_angle;
+}
+
+float Chessboard::Board::getWhiteAngle()const
+{
+ return white_angle;
+}
+
+void Chessboard::Board::swap(Chessboard::Board &other)
+{
+ corners.swap(other.corners);
+ cells.swap(other.cells);
+ std::swap(rows,other.rows);
+ std::swap(cols,other.cols);
+ std::swap(top_left,other.top_left);
+ std::swap(white_angle,other.white_angle);
+ std::swap(black_angle,other.black_angle);
+}
+
+Chessboard::Board& Chessboard::Board::operator=(const Chessboard::Board &other)
+{
+ if(this == &other)
+ return *this;
+ clear();
+ rows = other.rows;
+ cols = other.cols;
+ white_angle = other.white_angle;
+ black_angle = other.black_angle;
+ cells.reserve(other.cells.size());
+ corners.reserve(other.corners.size());
+
+ //copy all points and generate mapping
+ std::map<cv::Point2f*,cv::Point2f*> point_point_mapping;
+ point_point_mapping[NULL] = NULL;
+ std::vector<cv::Point2f*>::const_iterator iter = other.corners.begin();
+ for(;iter != other.corners.end();++iter)
+ {
+ cv::Point2f *pt = new cv::Point2f(**iter);
+ point_point_mapping[*iter] = pt;
+ corners.push_back(pt);
+ }
+
+ //copy all cells using mapping
+ std::map<Cell*,Cell*> cell_cell_mapping;
+ std::vector<Cell*>::const_iterator iter2 = other.cells.begin();
+ for(;iter2 != other.cells.end();++iter2)
+ {
+ Cell *cell = new Cell;
+ cell->top_left = point_point_mapping[(*iter2)->top_left];
+ cell->top_right= point_point_mapping[(*iter2)->top_right];
+ cell->bottom_right= point_point_mapping[(*iter2)->bottom_right];
+ cell->bottom_left = point_point_mapping[(*iter2)->bottom_left];
+ cell->black = (*iter2)->black;
+ cell_cell_mapping[*iter2] = cell;
+ cells.push_back(cell);
+ }
+
+ //set cell connections using mapping
+ cell_cell_mapping[NULL] = NULL;
+ iter2 = other.cells.begin();
+ std::vector<Cell*>::iterator iter3 = cells.begin();
+ for(;iter2 != other.cells.end();++iter2,++iter3)
+ {
+ (*iter3)->left = cell_cell_mapping[(*iter2)->left];
+ (*iter3)->top = cell_cell_mapping[(*iter2)->top];
+ (*iter3)->right = cell_cell_mapping[(*iter2)->right];
+ (*iter3)->bottom= cell_cell_mapping[(*iter2)->bottom];
+ }
+ top_left = cell_cell_mapping[other.top_left];
+ return *this;
+}
+
+void Chessboard::Board::normalizeOrientation(bool bblack)
+{
+ // fix ordering
+ cv::Point2f y = getCorner(0,1)-getCorner(2,1);
+ cv::Point2f x = getCorner(1,2)-getCorner(1,0);
+ cv::Point3f y3d(y.x,y.y,0);
+ cv::Point3f x3d(x.x,x.y,0);
+ if(x3d.cross(y3d).z > 0)
+ flipHorizontal();
+
+ //normalize orientation so that first element is black or white
+ const Cell* cell = getCell(0,0);
+ if(cell->black != bblack && colCount()%2 != 0)
+ rotateLeft();
+ else if(cell->black != bblack && rowCount()%2 != 0)
+ {
+ rotateLeft();
+ rotateLeft();
+ }
+
+ //find closest point to top left image corner
+ //in case of symmetric checkerboard
+ if(colCount() == rowCount())
+ {
+ PointIter iter_top_right(top_left,TOP_RIGHT);
+ while(iter_top_right.right());
+ PointIter iter_bottom_right(iter_top_right);
+ while(iter_bottom_right.bottom());
+ PointIter iter_bottom_left(top_left,BOTTOM_LEFT);
+ while(iter_bottom_left.bottom());
+ // check if one of the cell is empty and do not normalize if so
+ if(top_left->empty() || iter_top_right.getCell()->empty() ||
+ iter_bottom_left.getCell()->empty() || iter_bottom_right.getCell()->empty())
+ return;
+
+ float d1 = pow(top_left->top_left->x,2)+pow(top_left->top_left->y,2);
+ float d2 = pow((*iter_top_right)->x,2)+pow((*iter_top_right)->y,2);
+ float d3 = pow((*iter_bottom_left)->x,2)+pow((*iter_bottom_left)->y,2);
+ float d4 = pow((*iter_bottom_right)->x,2)+pow((*iter_bottom_right)->y,2);
+ if(d2 <= d1 && d2 <= d3 && d2 <= d4) // top left is top right
+ rotateLeft();
+ else if(d3 <= d1 && d3 <= d2 && d3 <= d4) // top left is bottom left
+ rotateRight();
+ else if(d4 <= d1 && d4 <= d2 && d4 <= d3) // top left is bottom right
+ {
+ rotateLeft();
+ rotateLeft();
+ }
+ }
+}
+
+void Chessboard::Board::rotateRight()
+{
+ PointIter p_iter(top_left,BOTTOM_LEFT);
+ while(p_iter.bottom());
+
+ std::vector<Cell*>::iterator iter = cells.begin();
+ for(;iter != cells.end();++iter)
+ {
+ Cell *temp = (*iter)->bottom;
+ (*iter)->bottom = (*iter)->right;
+ (*iter)->right= (*iter)->top;
+ (*iter)->top= (*iter)->left;
+ (*iter)->left = temp;
+
+ cv::Point2f *ptemp = (*iter)->bottom_left;
+ (*iter)->bottom_left= (*iter)->bottom_right;
+ (*iter)->bottom_right= (*iter)->top_right;
+ (*iter)->top_right= (*iter)->top_left;
+ (*iter)->top_left= ptemp;
+ }
+ int temp = rows;
+ rows = cols;
+ cols = temp;
+ top_left = p_iter.getCell();
+}
+
+
+void Chessboard::Board::rotateLeft()
+{
+ PointIter p_iter(top_left,TOP_RIGHT);
+ while(p_iter.right());
+
+ std::vector<Cell*>::iterator iter = cells.begin();
+ for(;iter != cells.end();++iter)
+ {
+ Cell *temp = (*iter)->top;
+ (*iter)->top = (*iter)->right;
+ (*iter)->right= (*iter)->bottom;
+ (*iter)->bottom= (*iter)->left;
+ (*iter)->left = temp;
+
+ cv::Point2f *ptemp = (*iter)->top_left;
+ (*iter)->top_left = (*iter)->top_right;
+ (*iter)->top_right= (*iter)->bottom_right;
+ (*iter)->bottom_right = (*iter)->bottom_left;
+ (*iter)->bottom_left = ptemp;
+ }
+ int temp = rows;
+ rows = cols;
+ cols = temp;
+ top_left = p_iter.getCell();
+}
+
+void Chessboard::Board::flipHorizontal()
+{
+ PointIter p_iter(top_left,TOP_RIGHT);
+ while(p_iter.right());
+
+ std::vector<Cell*>::iterator iter = cells.begin();
+ for(;iter != cells.end();++iter)
+ {
+ Cell *temp = (*iter)->right;
+ (*iter)->right= (*iter)->left;
+ (*iter)->left = temp;
+
+ cv::Point2f *ptemp = (*iter)->top_left;
+ (*iter)->top_left = (*iter)->top_right;
+ (*iter)->top_right = ptemp;
+
+ ptemp = (*iter)->bottom_left;
+ (*iter)->bottom_left = (*iter)->bottom_right;
+ (*iter)->bottom_right = ptemp;
+ }
+ top_left = p_iter.getCell();
+}
+
+void Chessboard::Board::flipVertical()
+{
+ PointIter p_iter(top_left,BOTTOM_LEFT);
+ while(p_iter.bottom());
+
+ std::vector<Cell*>::iterator iter = cells.begin();
+ for(;iter != cells.end();++iter)
+ {
+ Cell *temp = (*iter)->top;
+ (*iter)->top= (*iter)->bottom;
+ (*iter)->bottom = temp;
+
+ cv::Point2f *ptemp = (*iter)->top_left;
+ (*iter)->top_left = (*iter)->bottom_left;
+ (*iter)->bottom_left = ptemp;
+
+ ptemp = (*iter)->top_right;
+ (*iter)->top_right = (*iter)->bottom_right;
+ (*iter)->bottom_right = ptemp;
+ }
+ top_left = p_iter.getCell();
+}
+
+// returns the best found score
+// if NaN is returned for a point no point at all was found
+// if 0 is returned the point lies outside of the ellipse
+float Chessboard::Board::findMaxPoint(cv::flann::Index &index,const cv::Mat &data,const Ellipse &ellipse,float white_angle,float black_angle,cv::Point2f &point)
+{
+ // flann data type enriched with angles (third column)
+ CV_CheckType(data.type(), CV_32FC1, "type of flann data is not supported");
+ CV_CheckEQ(data.cols, 4, "4-cols flann data is expected");
+
+ std::vector<float> query,dists;
+ std::vector<int> indices;
+ query.resize(2);
+ point = ellipse.getCenter();
+ query[0] = point.x;
+ query[1] = point.y;
+ index.knnSearch(query,indices,dists,4,cv::flann::SearchParams(64));
+ std::vector<int>::const_iterator iter = indices.begin();
+ float best_score = -std::numeric_limits<float>::max();
+ point.x = std::numeric_limits<float>::quiet_NaN();
+ point.y = std::numeric_limits<float>::quiet_NaN();
+ for(;iter != indices.end();++iter)
+ {
+ const float *val = data.ptr<float>(*iter);
+ const float &response = *(val+3);
+ if(response < best_score)
+ continue;
+ const float &a0 = *(val+2);
+ float a1 = std::fabs(a0-white_angle);
+ float a2 = std::fabs(a0-black_angle);
+ if(a1 > CV_PI*0.5)
+ a1 = std::fabs(float(a1-CV_PI));
+ if(a2> CV_PI*0.5)
+ a2 = std::fabs(float(a2-CV_PI));
+ if(a1 < MAX_ANGLE || a2 < MAX_ANGLE )
+ {
+ cv::Point2f pt(val[0], val[1]);
+ if(point.x != point.x) // NaN check
+ point = pt;
+ if(best_score < response && ellipse.contains(pt))
+ {
+ best_score = response;
+ point = pt;
+ }
+ }
+ }
+ if(best_score == -std::numeric_limits<float>::max())
+ return 0;
+ else
+ return best_score;
+}
+
+void Chessboard::Board::clear()
+{
+ top_left = NULL; rows = 0; cols = 0;
+ std::vector<Cell*>::iterator iter = cells.begin();
+ for(;iter != cells.end();++iter)
+ delete *iter;
+ cells.clear();
+ std::vector<cv::Point2f*>::iterator iter2 = corners.begin();
+ for(;iter2 != corners.end();++iter2)
+ delete *iter2;
+ corners.clear();
+}
+
+// p0 p1 p2
+// p3 p4 p5
+// p6 p7 p8
+bool Chessboard::Board::init(const std::vector<cv::Point2f> points)
+{
+ clear();
+ if(points.size() != 9)
+ CV_Error(Error::StsBadArg,"exact nine points are expected to initialize the board");
+
+ // generate cells
+ corners.resize(9);
+ for(int i=0;i < 9;++i)
+ corners[i] = new cv::Point2f(points[i]);
+ cells.resize(4);
+ for(int i=0;i<4;++i)
+ cells[i] = new Cell();
+
+ //cell 0
+ cells[0]->top_left = corners[0];
+ cells[0]->top_right = corners[1];
+ cells[0]->bottom_right = corners[4];
+ cells[0]->bottom_left = corners[3];
+ cells[0]->right = cells[1];
+ cells[0]->bottom = cells[2];
+
+ //cell 1
+ cells[1]->top_left = corners[1];
+ cells[1]->top_right = corners[2];
+ cells[1]->bottom_right = corners[5];
+ cells[1]->bottom_left = corners[4];
+ cells[1]->left = cells[0];
+ cells[1]->bottom = cells[3];
+
+ //cell 2
+ cells[2]->top_left = corners[3];
+ cells[2]->top_right = corners[4];
+ cells[2]->bottom_right = corners[7];
+ cells[2]->bottom_left = corners[6];
+ cells[2]->top = cells[0];
+ cells[2]->right = cells[3];
+
+ //cell 3
+ cells[3]->top_left = corners[4];
+ cells[3]->top_right = corners[5];
+ cells[3]->bottom_right = corners[8];
+ cells[3]->bottom_left = corners[7];
+ cells[3]->top = cells[1];
+ cells[3]->left= cells[2];
+
+ top_left = cells.front();
+ rows = 3;
+ cols = 3;
+
+ // set initial cell colors
+ Point2f pt1 = *(cells[0]->top_right)-*(cells[0]->bottom_left);
+ pt1 /= cv::norm(pt1);
+ cv::Point2f pt2(cos(white_angle),-sin(white_angle));
+ cv::Point2f pt3(cos(black_angle),-sin(black_angle));
+ if(fabs(pt1.dot(pt2)) < fabs(pt1.dot(pt3)))
+ {
+ cells[0]->black = false;
+ cells[1]->black = true;
+ cells[2]->black = true;
+ cells[3]->black = false;
+ }
+ else
+ {
+ cells[0]->black = true;
+ cells[1]->black = false;
+ cells[2]->black = false;
+ cells[3]->black = true;
+ }
+ return true;
+}
+
+//TODO magic number
+bool Chessboard::Board::estimatePoint(const cv::Point2f &p0,const cv::Point2f &p1,const cv::Point2f &p2, cv::Point2f &p3)
+{
+ // use cross ration to find new point
+ if(p0 == p1 || p0 == p2 || p1 == p2)
+ return false;
+ cv::Point2f p01 = p1-p0;
+ cv::Point2f p12 = p2-p1;
+ float a = float(cv::norm(p01));
+ float b = float(cv::norm(p12));
+ float t = (0.75F*a-0.25F*b);
+ if(t <= 0)
+ return false;
+ float c = 0.25F*b*(a+b)/t;
+ if(c < 0.1F)
+ return false;
+ p01 = p01/a;
+ p12 = p12/b;
+ // check angle between p01 and p12 < 25°
+ if(p01.dot(p12) < 0.9)
+ return false;
+ // calc mean
+ // p12 = (p01+p12)*0.5;
+ // p3 = p2+p12*c;
+ p3 = p2+p12*c;
+
+ // compensate radial distortion by fitting polynom
+ std::vector<double> x,y;
+ x.resize(3,0); y.resize(3,0);
+ x[1] = b;
+ x[2] = b+a;
+ y[2] = calcSignedDistance(-p12,p2,p0);
+ cv::Mat dst;
+ polyfit(cv::Mat(x),cv::Mat(y),dst,2);
+ double d = dst.at<double>(0)-dst.at<double>(1)*c+dst.at<double>(2)*c*c;
+ cv::Vec3f v1(p12.x,p12.y,0);
+ cv::Vec3f v2(0,0,1);
+ cv::Vec3f v3 = v1.cross(v2);
+ cv::Point2f n2(v3[0],v3[1]);
+ p3 += d*n2;
+ return true;
+}
+
+bool Chessboard::Board::estimatePoint(const cv::Point2f &p0,const cv::Point2f &p1,const cv::Point2f &p2, const cv::Point2f &p3, cv::Point2f &p4)
+{
+ // use 1D homography to find fith point minimizing square error
+ if(p0 == p1 || p0 == p2 || p0 == p3 || p1 == p2 || p1 == p3 || p2 == p3 )
+ return false;
+ static const cv::Mat src = (cv::Mat_<double>(1,4) << 0,10,20,30);
+ cv::Point2f p01 = p1-p0;
+ cv::Point2f p02 = p2-p0;
+ cv::Point2f p03 = p3-p0;
+ float a = float(cv::norm(p01));
+ float b = float(cv::norm(p02));
+ float c = float(cv::norm(p03));
+ cv::Mat dst = (cv::Mat_<double>(1,4) << 0,a,b,c);
+ cv::Mat h = findHomography1D(src,dst);
+ float d = float((h.at<double>(0,0)*40+h.at<double>(0,1))/(h.at<double>(1,0)*40+h.at<double>(1,1)));
+ cv::Point2f p12 = p2-p1;
+ cv::Point2f p23 = p3-p2;
+ p01 = p01/a;
+ p12 = p12/cv::norm(p12);
+ p23 = p23/cv::norm(p23);
+ p4 = p3+(d-c)*p23;
+
+ // compensate radial distortion by fitting polynom
+ std::vector<double> x,y;
+ x.resize(4,0); y.resize(4,0);
+ x[1] = c-b;
+ x[2] = c-a;
+ x[3] = c;
+ y[2] = calcSignedDistance(-p23,p3,p1);
+ y[3] = calcSignedDistance(-p23,p3,p0);
+ polyfit(cv::Mat(x),cv::Mat(y),dst,2);
+ d = d-c;
+ double e = dst.at<double>(0)-dst.at<double>(1)*fabs(d)+dst.at<double>(2)*d*d;
+ cv::Vec3f v1(p23.x,p23.y,0);
+ cv::Vec3f v2(0,0,1);
+ cv::Vec3f v3 = v1.cross(v2);
+ cv::Point2f n2(v3[0],v3[1]);
+ p4 += e*n2;
+ return true;
+}
+
+// H is describing the transformation from dummy to reality
+Ellipse Chessboard::Board::estimateSearchArea(cv::Mat _H,int row, int col,float p,int field_size)
+{
+ cv::Matx31d point1,point2,center;
+ center(0) = (1+col)*field_size;
+ center(1) = (1+row)*field_size;
+ center(2) = 1.0;
+ point1(0) = center(0)-p*field_size;
+ point1(1) = center(1);
+ point1(2) = center(2);
+ point2(0) = center(0);
+ point2(1) = center(1)-p*field_size;
+ point2(2) = center(2);
+
+ cv::Matx33d H(_H);
+ point1 = H*point1;
+ point2 = H*point2;
+ center = H*center;
+ cv::Point2f pt(float(center(0)/center(2)),float(center(1)/center(2)));
+ cv::Point2f pt1(float(point1(0)/point1(2)),float(point1(1)/point1(2)));
+ cv::Point2f pt2(float(point2(0)/point2(2)),float(point2(1)/point2(2)));
+
+ cv::Point2f p01(pt1-pt);
+ cv::Point2f p02(pt2-pt);
+ float norm1 = float(cv::norm(p01));
+ float norm2 = float(cv::norm(p02));
+ float angle = float(acos(p01.dot(p02)/norm1/norm2));
+ cv::Size2f axes(norm1,norm2);
+ return Ellipse(pt,axes,angle);
+}
+
+bool Chessboard::Board::estimateSearchArea(const cv::Point2f &p1,const cv::Point2f &p2,const cv::Point2f &p3,float p,Ellipse &ellipse,const cv::Point2f *p0)
+{
+ cv::Point2f p4,n;
+ if(p0)
+ {
+ // use 1D homography
+ if(!estimatePoint(*p0,p1,p2,p3,p4))
+ return false;
+ n = p4-*p0;
+ }
+ else
+ {
+ // use cross ratio
+ if(!estimatePoint(p1,p2,p3,p4))
+ return false;
+ n = p4-p1;
+ }
+ float norm = float(cv::norm(n));
+ n = n/norm;
+ float angle = acos(n.x);
+ if(n.y > 0)
+ angle = float(2.0F*CV_PI-angle);
+ n = p4-p3;
+ norm = float(cv::norm(n));
+ double delta = std::max(3.0F,p*norm);
+ ellipse = Ellipse(p4,cv::Size(int(delta),int(std::max(2.0,delta*ELLIPSE_WIDTH))),angle);
+ return true;
+}
+
+bool Chessboard::Board::checkRowColumn(const std::vector<cv::Point2f> &points)
+{
+ if(points.size() < 4)
+ {
+ if(points.size() == 3)
+ return true;
+ else
+ return false;
+ }
+ std::vector<cv::Point2f>::const_iterator iter = points.begin();
+ std::vector<cv::Point2f>::const_iterator iter2 = iter+1;
+ std::vector<cv::Point2f>::const_iterator iter3 = iter2+1;
+ std::vector<cv::Point2f>::const_iterator iter4 = iter3+1;
+ Ellipse ellipse;
+ if(!estimateSearchArea(*iter4,*iter3,*iter2,CORNERS_SEARCH*3,ellipse))
+ return false;
+ if(!ellipse.contains(*iter))
+ return false;
+
+ std::vector<cv::Point2f>::const_iterator iter5 = iter4+1;
+ for(;iter5 != points.end();++iter5)
+ {
+ if(!estimateSearchArea(*iter2,*iter3,*iter4,CORNERS_SEARCH,ellipse,&(*iter)))
+ return false;
+ if(!ellipse.contains(*iter5))
+ return false;
+ iter = iter2;
+ iter2 = iter3;
+ iter3 = iter4;
+ iter4 = iter5;
+ }
+ return true;
+}
+
+cv::Point2f &Chessboard::Board::getCorner(int _row,int _col)
+{
+ int _rows = int(rowCount());
+ int _cols = int(colCount());
+ if(_row >= _rows || _col >= _cols)
+ CV_Error(Error::StsBadArg,"out of bound");
+ if(_row == 0)
+ {
+ PointIter iter(top_left,TOP_LEFT);
+ int count = 0;
+ do
+ {
+ if(count == _col)
+ return *(*iter);
+ ++count;
+ }while(iter.right());
+ }
+ else
+ {
+ Cell *row_start = top_left;
+ int count = 1;
+ do
+ {
+ if(count == _row)
+ {
+ PointIter iter(row_start,BOTTOM_LEFT);
+ int count2 = 0;
+ do
+ {
+ if(count2 == _col)
+ return *(*iter);
+ ++count2;
+ }while(iter.right());
+ }
+ ++count;
+ row_start = row_start->bottom;
+ }while(_row);
+ }
+ CV_Error(Error::StsInternal,"cannot find corner");
+ // return *top_left->top_left; // never reached
+}
+
+bool Chessboard::Board::isCellBlack(int row,int col)const
+{
+ return getCell(row,col)->black;
+}
+
+bool Chessboard::Board::isCellEmpty(int row,int col)
+{
+ return getCell(row,col)->empty();
+}
+
+Chessboard::Board::Cell* Chessboard::Board::getCell(int row,int col)
+{
+ const Cell *cell = const_cast<const Board*>(this)->getCell(row,col);
+ return const_cast<Cell*>(cell);
+}
+
+const Chessboard::Board::Cell* Chessboard::Board::getCell(int row,int col)const
+{
+ if(row > rows-1 || row < 0 || col > cols-1 || col < 0)
+ CV_Error(Error::StsBadArg,"out of bound");
+ PointIter p_iter(top_left,BOTTOM_RIGHT);
+ for(int i=0; i< row; p_iter.bottom(),++i);
+ for(int i=0; i< col; p_iter.right(),++i);
+ return p_iter.getCell();
+}
+
+
+bool Chessboard::Board::isEmpty()const
+{
+ return cells.empty();
+}
+
+size_t Chessboard::Board::colCount()const
+{
+ return cols;
+}
+
+size_t Chessboard::Board::rowCount()const
+{
+ return rows;
+}
+
+cv::Size Chessboard::Board::getSize()const
+{
+ return cv::Size(int(colCount()),int(rowCount()));
+}
+
+void Chessboard::Board::drawEllipses(const std::vector<Ellipse> &ellipses)
+{
+ // currently there is no global image find way to store global image
+ // without polluting namespace
+ if(ellipses.empty())
+ return; //avoid compiler warning
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+ cv::Mat img;
+ draw(debug_image,img);
+ std::vector<Ellipse>::iterator iter;
+ for(;iter != ellipses.end();++iter)
+ iter->draw(img);
+ cv::imshow("chessboard",img);
+ cv::waitKey(-1);
+#endif
+}
+
+
+void Chessboard::Board::growLeft()
+{
+ if(isEmpty())
+ CV_Error(Error::StsInternal,"Board is empty");
+ PointIter iter(top_left,TOP_LEFT);
+ std::vector<cv::Point2f> points;
+ cv::Point2f pt;
+ do
+ {
+ PointIter iter2(iter);
+ cv::Point2f *p0 = *iter2;
+ iter2.right();
+ cv::Point2f *p1 = *iter2;
+ iter2.right();
+ cv::Point2f *p2 = *iter2;
+ if(iter2.right())
+ estimatePoint(**iter2,*p2,*p1,*p0,pt);
+ else
+ estimatePoint(*p2,*p1,*p0,pt);
+ points.push_back(pt);
+ }
+ while(iter.bottom());
+ addColumnLeft(points);
+}
+
+bool Chessboard::Board::growLeft(const cv::Mat &map,cv::flann::Index &flann_index)
+{
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+ std::vector<Ellipse> ellipses;
+#endif
+ if(isEmpty())
+ CV_Error(Error::StsInternal,"growLeft: Board is empty");
+ PointIter iter(top_left,TOP_LEFT);
+ std::vector<cv::Point2f> points;
+ int count = 0;
+ Ellipse ellipse;
+ cv::Point2f pt;
+ do
+ {
+ PointIter iter2(iter);
+ cv::Point2f *p0 = *iter2;
+ iter2.right();
+ cv::Point2f *p1 = *iter2;
+ iter2.right();
+ cv::Point2f *p2 = *iter2;
+ cv::Point2f *p3 = NULL;
+ if(iter2.right())
+ p3 = *iter2;
+ if(!estimateSearchArea(*p2,*p1,*p0,CORNERS_SEARCH,ellipse,p3))
+ return false;
+ float result = findMaxPoint(flann_index,map,ellipse,white_angle,black_angle,pt);
+ if(pt == *p0)
+ {
+ ++count;
+ points.push_back(ellipse.getCenter());
+ }
+ else if(result != 0)
+ {
+ points.push_back(pt);
+ if(result < 0)
+ ++count;
+ }
+ else
+ {
+ ++count;
+ if(pt.x != pt.x) // NaN check
+ points.push_back(ellipse.getCenter());
+ else
+ points.push_back(pt);
+ }
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+ ellipses.push_back(ellipse);
+#endif
+ }
+ while(iter.bottom());
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+ drawEllipses(ellipses);
+#endif
+ if(points.size()-count <= 2)
+ return false;
+ if(count > points.size()*0.5 || !checkRowColumn(points))
+ return false;
+ addColumnLeft(points);
+ return true;
+}
+
+void Chessboard::Board::growTop()
+{
+ if(isEmpty())
+ CV_Error(Error::StsInternal,"Board is empty");
+ PointIter iter(top_left,TOP_LEFT);
+ std::vector<cv::Point2f> points;
+ cv::Point2f pt;
+ do
+ {
+ PointIter iter2(iter);
+ cv::Point2f *p0 = *iter2;
+ iter2.bottom();
+ cv::Point2f *p1 = *iter2;
+ iter2.bottom();
+ cv::Point2f *p2 = *iter2;
+ if(iter2.bottom())
+ estimatePoint(**iter2,*p2,*p1,*p0,pt);
+ else
+ estimatePoint(*p2,*p1,*p0,pt);
+ points.push_back(pt);
+ }
+ while(iter.right());
+ addRowTop(points);
+}
+
+bool Chessboard::Board::growTop(const cv::Mat &map,cv::flann::Index &flann_index)
+{
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+ std::vector<Ellipse> ellipses;
+#endif
+ if(isEmpty())
+ CV_Error(Error::StsInternal,"Board is empty");
+
+ PointIter iter(top_left,TOP_LEFT);
+ std::vector<cv::Point2f> points;
+ int count = 0;
+ Ellipse ellipse;
+ cv::Point2f pt;
+ do
+ {
+ PointIter iter2(iter);
+ cv::Point2f *p0 = *iter2;
+ iter2.bottom();
+ cv::Point2f *p1 = *iter2;
+ iter2.bottom();
+ cv::Point2f *p2 = *iter2;
+ cv::Point2f *p3 = NULL;
+ if(iter2.bottom())
+ p3 = *iter2;
+ if(!estimateSearchArea(*p2,*p1,*p0,CORNERS_SEARCH,ellipse,p3))
+ return false;
+ float result = findMaxPoint(flann_index,map,ellipse,white_angle,black_angle,pt);
+ if(pt == *p0)
+ {
+ ++count;
+ points.push_back(ellipse.getCenter());
+ }
+ else if(result != 0)
+ {
+ points.push_back(pt);
+ if(result < 0)
+ ++count;
+ }
+ else
+ {
+ ++count;
+ if(pt.x != pt.x) // NaN check
+ points.push_back(ellipse.getCenter());
+ else
+ points.push_back(pt);
+ }
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+ ellipses.push_back(ellipse);
+#endif
+ }
+ while(iter.right());
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+ drawEllipses(ellipses);
+#endif
+ if(count > points.size()*0.5 || !checkRowColumn(points))
+ return false;
+ addRowTop(points);
+ return true;
+}
+
+void Chessboard::Board::growRight()
+{
+ if(isEmpty())
+ CV_Error(Error::StsInternal,"Board is empty");
+ PointIter iter(top_left,TOP_RIGHT);
+ while(iter.right());
+ std::vector<cv::Point2f> points;
+ cv::Point2f pt;
+ do
+ {
+ PointIter iter2(iter);
+ cv::Point2f *p0 = *iter2;
+ iter2.left();
+ cv::Point2f *p1 = *iter2;
+ iter2.left();
+ cv::Point2f *p2 = *iter2;
+ if(iter2.left())
+ estimatePoint(**iter2,*p2,*p1,*p0,pt);
+ else
+ estimatePoint(*p2,*p1,*p0,pt);
+ points.push_back(pt);
+ }
+ while(iter.bottom());
+ addColumnRight(points);
+}
+
+bool Chessboard::Board::growRight(const cv::Mat &map,cv::flann::Index &flann_index)
+{
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+ std::vector<Ellipse> ellipses;
+#endif
+ if(isEmpty())
+ CV_Error(Error::StsInternal,"Board is empty");
+
+ PointIter iter(top_left,TOP_RIGHT);
+ while(iter.right());
+ std::vector<cv::Point2f> points;
+ cv::Point2f pt;
+ Ellipse ellipse;
+ int count = 0;
+ do
+ {
+ PointIter iter2(iter);
+ cv::Point2f *p0 = *iter2;
+ iter2.left();
+ cv::Point2f *p1 = *iter2;
+ iter2.left();
+ cv::Point2f *p2 = *iter2;
+ cv::Point2f *p3 = NULL;
+ if(iter2.left())
+ p3 = *iter2;
+ if(!estimateSearchArea(*p2,*p1,*p0,CORNERS_SEARCH,ellipse,p3))
+ return false;
+ float result = findMaxPoint(flann_index,map,ellipse,white_angle,black_angle,pt);
+ if(pt == *p0)
+ {
+ ++count;
+ points.push_back(ellipse.getCenter());
+ }
+ else if(result != 0)
+ {
+ points.push_back(pt);
+ if(result < 0)
+ ++count;
+ }
+ else
+ {
+ ++count;
+ if(pt.x != pt.x) // NaN check
+ points.push_back(ellipse.getCenter());
+ else
+ points.push_back(pt);
+ }
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+ ellipses.push_back(ellipse);
+#endif
+ }
+ while(iter.bottom());
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+ drawEllipses(ellipses);
+#endif
+ if(count > points.size()*0.5 || !checkRowColumn(points))
+ return false;
+ addColumnRight(points);
+ return true;
+}
+
+void Chessboard::Board::growBottom()
+{
+ if(isEmpty())
+ CV_Error(Error::StsInternal,"Board is empty");
+
+ PointIter iter(top_left,BOTTOM_LEFT);
+ while(iter.bottom());
+ std::vector<cv::Point2f> points;
+ cv::Point2f pt;
+ do
+ {
+ PointIter iter2(iter);
+ cv::Point2f *p0 = *iter2;
+ iter2.top();
+ cv::Point2f *p1 = *iter2;
+ iter2.top();
+ cv::Point2f *p2 = *iter2;
+ if(iter2.top())
+ estimatePoint(**iter2,*p2,*p1,*p0,pt);
+ else
+ estimatePoint(*p2,*p1,*p0,pt);
+ points.push_back(pt);
+ }
+ while(iter.right());
+ addRowBottom(points);
+}
+
+bool Chessboard::Board::growBottom(const cv::Mat &map,cv::flann::Index &flann_index)
+{
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+ std::vector<Ellipse> ellipses;
+#endif
+ if(isEmpty())
+ CV_Error(Error::StsInternal,"Board is empty");
+
+ PointIter iter(top_left,BOTTOM_LEFT);
+ while(iter.bottom());
+ std::vector<cv::Point2f> points;
+ cv::Point2f pt;
+ Ellipse ellipse;
+ int count = 0;
+ do
+ {
+ PointIter iter2(iter);
+ cv::Point2f *p0 = *iter2;
+ iter2.top();
+ cv::Point2f *p1 = *iter2;
+ iter2.top();
+ cv::Point2f *p2 = *iter2;
+ cv::Point2f *p3 = NULL;
+ if(iter2.top())
+ p3 = *iter2;
+ if(!estimateSearchArea(*p2,*p1,*p0,CORNERS_SEARCH,ellipse,p3))
+ return false;
+ float result = findMaxPoint(flann_index,map,ellipse,white_angle,black_angle,pt);
+ if(pt == *p0)
+ {
+ ++count;
+ points.push_back(ellipse.getCenter());
+ }
+ else if(result != 0)
+ {
+ points.push_back(pt);
+ if(result < 0)
+ ++count;
+ }
+ else
+ {
+ ++count;
+ if(pt.x != pt.x) // NaN check
+ points.push_back(ellipse.getCenter());
+ else
+ points.push_back(pt);
+ }
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+ ellipses.push_back(ellipse);
+#endif
+ }
+ while(iter.right());
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+ drawEllipses(ellipses);
+#endif
+ if(count > points.size()*0.5 || !checkRowColumn(points))
+ return false;
+ addRowBottom(points);
+ return true;
+}
+
+void Chessboard::Board::addColumnLeft(const std::vector<cv::Point2f> &points)
+{
+ if(points.empty() || points.size() != rowCount())
+ CV_Error(Error::StsBadArg,"wrong number of points");
+
+ int offset = int(cells.size());
+ cells.resize(offset+points.size()-1);
+ for(int i = offset;i < (int) cells.size();++i)
+ cells[i] = new Cell();
+ corners.push_back(new cv::Point2f(points.front()));
+
+ Cell *cell = top_left;
+ std::vector<cv::Point2f>::const_iterator iter = points.begin()+1;
+ for(int pos=offset;iter != points.end();++iter,cell = cell->bottom,++pos)
+ {
+ cell->left = cells[pos];
+ cells[pos]->black = !cell->black;
+ if(pos != offset)
+ cells[pos]->top = cells[pos-1];
+ cells[pos]->right = cell;
+ if(pos +1 < (int)cells.size())
+ cells[pos]->bottom= cells[pos+1];
+ cells[pos]->top_left = corners.back();
+ corners.push_back(new cv::Point2f(*iter));
+ cells[pos]->bottom_left = corners.back();
+ cells[pos]->top_right=cell->top_left;
+ cells[pos]->bottom_right=cell->bottom_left;
+ }
+ top_left = cells[offset];
+ ++cols;
+}
+
+void Chessboard::Board::addRowTop(const std::vector<cv::Point2f> &points)
+{
+ if(points.empty() || points.size() != colCount())
+ CV_Error(Error::StsBadArg,"wrong number of points");
+
+ int offset = int(cells.size());
+ cells.resize(offset+points.size()-1);
+ for(int i = offset;i < (int) cells.size();++i)
+ cells[i] = new Cell();
+ corners.push_back(new cv::Point2f(points.front()));
+
+ Cell *cell = top_left;
+ std::vector<cv::Point2f>::const_iterator iter = points.begin()+1;
+ for(int pos=offset;iter != points.end();++iter,cell = cell->right,++pos)
+ {
+ cell->top = cells[pos];
+ cells[pos]->black = !cell->black;
+ if(pos != offset)
+ cells[pos]->left= cells[pos-1];
+ cells[pos]->bottom= cell;
+ if(pos +1 <(int) cells.size())
+ cells[pos]->right= cells[pos+1];
+
+ cells[pos]->top_left = corners.back();
+ corners.push_back(new cv::Point2f(*iter));
+ cells[pos]->top_right = corners.back();
+ cells[pos]->bottom_left = cell->top_left;
+ cells[pos]->bottom_right = cell->top_right;
+ }
+ top_left = cells[offset];
+ ++rows;
+}
+
+void Chessboard::Board::addColumnRight(const std::vector<cv::Point2f> &points)
+{
+ if(points.empty() || points.size() != rowCount())
+ CV_Error(Error::StsBadArg,"wrong number of points");
+
+ int offset = int(cells.size());
+ cells.resize(offset+points.size()-1);
+ for(int i = offset;i < (int) cells.size();++i)
+ cells[i] = new Cell();
+ corners.push_back(new cv::Point2f(points.front()));
+
+ Cell *cell = top_left;
+ for(;cell->right;cell = cell->right);
+ std::vector<cv::Point2f>::const_iterator iter = points.begin()+1;
+ for(int pos=offset;iter != points.end();++iter,cell = cell->bottom,++pos)
+ {
+ cell->right = cells[pos];
+ cells[pos]->black = !cell->black;
+ if(pos != offset)
+ cells[pos]->top= cells[pos-1];
+ cells[pos]->left = cell;
+ if(pos +1 <(int) cells.size())
+ cells[pos]->bottom= cells[pos+1];
+
+ cells[pos]->top_right = corners.back();
+ corners.push_back(new cv::Point2f(*iter));
+ cells[pos]->bottom_right = corners.back();
+ cells[pos]->top_left =cell->top_right;
+ cells[pos]->bottom_left =cell->bottom_right;
+ }
+ ++cols;
+}
+
+void Chessboard::Board::addRowBottom(const std::vector<cv::Point2f> &points)
+{
+ if(points.empty() || points.size() != colCount())
+ CV_Error(Error::StsBadArg,"wrong number of points");
+
+ int offset = int(cells.size());
+ cells.resize(offset+points.size()-1);
+ for(int i = offset;i < (int) cells.size();++i)
+ cells[i] = new Cell();
+ corners.push_back(new cv::Point2f(points.front()));
+
+ Cell *cell = top_left;
+ for(;cell->bottom;cell = cell->bottom);
+ std::vector<cv::Point2f>::const_iterator iter = points.begin()+1;
+ for(int pos=offset;iter != points.end();++iter,cell = cell->right,++pos)
+ {
+ cell->bottom = cells[pos];
+ cells[pos]->black = !cell->black;
+ if(pos != offset)
+ cells[pos]->left = cells[pos-1];
+ cells[pos]->top = cell;
+ if(pos +1 < (int)cells.size())
+ cells[pos]->right= cells[pos+1];
+
+ cells[pos]->bottom_left = corners.back();
+ corners.push_back(new cv::Point2f(*iter));
+ cells[pos]->bottom_right = corners.back();
+ cells[pos]->top_left = cell->bottom_left;
+ cells[pos]->top_right = cell->bottom_right;
+ }
+ ++rows;
+}
+
+bool Chessboard::Board::checkUnique()const
+{
+ std::vector<cv::Point2f> points = getCorners(false);
+ std::vector<cv::Point2f>::const_iterator iter = points.begin();
+ for(;iter != points.end();++iter)
+ {
+ std::vector<cv::Point2f>::const_iterator iter2 = iter+1;
+ for(;iter2 != points.end();++iter2)
+ {
+ if(*iter == *iter2)
+ return false;
+ }
+ }
+ return true;
+}
+
+int Chessboard::Board::validateCorners(const cv::Mat &data,cv::flann::Index &flann_index,const cv::Mat &h,float min_response)
+{
+ // TODO check input
+ if(isEmpty() || h.empty())
+ return 0;
+ int count = 0; int icol = 0;
+ // first row
+ PointIter iter(top_left,TOP_LEFT);
+ cv::Point2f point;
+ do
+ {
+ if((*iter)->x == (*iter)->x)
+ ++count;
+ else
+ {
+ Ellipse ellipse = estimateSearchArea(h,0,icol,0.4F);
+ float result = findMaxPoint(flann_index,data,ellipse,white_angle,black_angle,point);
+ if(fabs(result) >= min_response)
+ {
+ ++count;
+ **iter = point;
+ }
+ }
+ ++icol;
+ }while(iter.right());
+
+ // all other rows
+ int irow = 1;
+ Cell *row = top_left;
+ do
+ {
+ PointIter iter2(row,BOTTOM_LEFT);
+ icol = 0;
+ do
+ {
+ if((*iter2)->x == (*iter2)->x)
+ ++count;
+ else
+ {
+ Ellipse ellipse = estimateSearchArea(h,irow,icol,0.4F);
+ if(min_response <= findMaxPoint(flann_index,data,ellipse,white_angle,black_angle,point))
+ {
+ ++count;
+ **iter2 = point;
+ }
+ }
+ ++icol;
+ }while(iter2.right());
+ row = row->bottom;
+ ++irow;
+ }while(row);
+
+ // check that there are no points with the same coordinate
+ std::vector<cv::Point2f> points = getCorners(false);
+ std::vector<cv::Point2f>::const_iterator iter1 = points.begin();
+ for(;iter1 != points.end();++iter1)
+ {
+ // we do not have to check for NaN because of getCorners(flase)
+ std::vector<cv::Point2f>::const_iterator iter2 = iter1+1;
+ for(;iter2 != points.end();++iter2)
+ if(*iter1 == *iter2)
+ return -1; // one corner is there twice -> not valid configuration
+ }
+ return count;
+}
+
+bool Chessboard::Board::validateContour()const
+{
+ std::vector<cv::Point2f> contour = getContour();
+ if(contour.size() != 4)
+ {
+ return false;
+ }
+ cv::Point2f n1 = contour[1]-contour[0];
+ cv::Point2f n2 = contour[2]-contour[1];
+ cv::Point2f n3 = contour[3]-contour[2];
+ cv::Point2f n4 = contour[0]-contour[3];
+ n1 = n1/cv::norm(n1);
+ n2 = n2/cv::norm(n2);
+ n3 = n3/cv::norm(n3);
+ n4 = n4/cv::norm(n4);
+ // a > b => cos(a) < cos(b)
+ if(fabs(n1.dot(n2)) > MIN_COS_ANGLE||
+ fabs(n2.dot(n3)) > MIN_COS_ANGLE||
+ fabs(n3.dot(n4)) > MIN_COS_ANGLE||
+ fabs(n4.dot(n1)) > MIN_COS_ANGLE)
+ return false;
+ return true;
+}
+
+std::vector<cv::Point2f> Chessboard::Board::getContour()const
+{
+ std::vector<cv::Point2f> points;
+ if(isEmpty())
+ return points;
+
+ //find start cell part of the contour
+ Cell* start_cell = NULL;
+ PointIter iter(top_left,TOP_LEFT);
+ do
+ {
+ PointIter iter2(iter);
+ do
+ {
+ if(!iter2.getCell()->empty())
+ {
+ start_cell = iter2.getCell();
+ iter = iter2;
+ break;
+ }
+ }while(iter2.right());
+ }while(!start_cell && iter.bottom());
+ if(start_cell == NULL)
+ return points;
+
+ // trace contour
+ const cv::Point2f *start_pt = *iter;
+ int mode = 2; int last = -1;
+ do
+ {
+ PointIter current_iter(iter);
+ switch(mode)
+ {
+ case 1: // top
+ if(iter.top(true))
+ {
+ if(last != 1)
+ points.push_back(**current_iter);
+ mode = 4;
+ last = 1;
+ break;
+ }
+ /* fallthrough */
+ case 2: // right
+ if(iter.right(true))
+ {
+ if(last != 2)
+ points.push_back(**current_iter);
+ mode = 1;
+ last = 2;
+ break;
+ }
+ /* fallthrough */
+ case 3: // bottom
+ if(iter.bottom(true))
+ {
+ if(last != 3)
+ points.push_back(**current_iter);
+ mode = 2;
+ last = 3;
+ break;
+ }
+ /* fallthrough */
+ case 4: // left
+ if(iter.left(true))
+ {
+ if(last != 4)
+ points.push_back(**current_iter);
+ mode = 3;
+ last = 4;
+ break;
+ }
+ mode = 1;
+ break;
+ default:
+ CV_Error(Error::StsInternal,"cannot retrieve contour");
+ }
+ }while(*iter != start_pt);
+ return points;
+}
+
+
+cv::Mat Chessboard::Board::estimateHomography(cv::Rect rect,int field_size)const
+{
+ int _rows = int(rowCount());
+ int _cols = int(colCount());
+ if(_rows < 3 || _cols < 3)
+ return cv::Mat();
+ if(rect.width <= 0)
+ rect.width= _cols;
+ if(rect.height <= 0)
+ rect.height= _rows;
+
+ int col_end = std::min(rect.x+rect.width,_cols);
+ int row_end = std::min(rect.y+rect.height,_rows);
+ std::vector<cv::Point2f> points = getCorners(true);
+
+ // build src and dst
+ std::vector<cv::Point2f> src,dst;
+ for(int row =rect.y;row < row_end;++row)
+ {
+ for(int col=rect.x;col <col_end;++col)
+ {
+ const cv::Point2f &pt = points[row*_rows+col];
+ if(pt.x != pt.x) // NaN check
+ continue;
+ src.push_back(cv::Point2f(float(field_size)*(col+1),float(field_size)*(row+1)));
+ dst.push_back(pt);
+ }
+ }
+ if(dst.size() < 4)
+ return cv::Mat();
+ return cv::findHomography(src, dst,cv::LMEDS);
+}
+
+cv::Mat Chessboard::Board::estimateHomography(int field_size)const
+{
+ int _rows = int(rowCount());
+ int _cols = int(colCount());
+ if(_rows < 3 || _cols < 3)
+ return cv::Mat();
+ std::vector<cv::Point2f> src,dst;
+ std::vector<cv::Point2f> points = getCorners(true);
+ std::vector<cv::Point2f>::const_iterator iter = points.begin();
+ for(int row =0;row < _rows;++row)
+ {
+ for(int col=0;col <_cols;++col,++iter)
+ {
+ const cv::Point2f &pt = *iter;
+ if(pt.x == pt.x)
+ {
+ src.push_back(cv::Point2f(float(field_size)*(col+1),float(field_size)*(row+1)));
+ dst.push_back(pt);
+ }
+ }
+ }
+ if(dst.size() < 4)
+ return cv::Mat();
+ return cv::findHomography(src, dst);
+}
+
+bool Chessboard::Board::findNextPoint(cv::flann::Index &index,const cv::Mat &data,
+ const cv::Point2f &pt1,const cv::Point2f &pt2, const cv::Point2f &pt3,
+ float white_angle,float black_angle,float min_response,cv::Point2f &point)
+{
+ Ellipse ellipse;
+ if(!estimateSearchArea(pt1,pt2,pt3,0.4F,ellipse))
+ return false;
+ if(min_response > fabs(findMaxPoint(index,data,ellipse,white_angle,black_angle,point)))
+ return false;
+ return true;
+}
+
+int Chessboard::Board::grow(const cv::Mat &map,cv::flann::Index &flann_index)
+{
+ if(isEmpty())
+ CV_Error(Error::StsInternal,"Board is empty");
+ bool bleft = true;
+ bool btop = true;
+ bool bright = true;
+ bool bbottom= true;
+ int count = 0;
+ do
+ {
+ // grow to the left
+ if(bleft)
+ {
+ bleft = growLeft(map,flann_index);
+ if(bleft)
+ ++count;
+ }
+ if(btop)
+ {
+ btop= growTop(map,flann_index);
+ if(btop)
+ ++count;
+ }
+ if(bright)
+ {
+ bright= growRight(map,flann_index);
+ if(bright)
+ ++count;
+ }
+ if(bbottom)
+ {
+ bbottom= growBottom(map,flann_index);
+ if(bbottom)
+ ++count;
+ }
+ }while(bleft || btop || bright || bbottom );
+ return count;
+}
+
+std::map<int,int> Chessboard::Board::getMapping()const
+{
+ std::map<int,int> map;
+ std::vector<cv::Point2f> points = getCorners();
+ std::vector<cv::Point2f>::iterator iter = points.begin();
+ for(int idx1=0,idx2=0;iter != points.end();++iter,++idx1)
+ {
+ if(iter->x != iter->x) // NaN check
+ continue;
+ map[idx1] = idx2++;
+ }
+ return map;
+}
+
+std::vector<cv::Point2f> Chessboard::Board::getCorners(bool ball)const
+{
+ std::vector<cv::Point2f> points;
+ if(isEmpty())
+ return points;
+
+ // first row
+ PointIter iter(top_left,TOP_LEFT);
+ do
+ {
+ if(ball || !iter.isNaN())
+ points.push_back(*(*iter));
+ }while(iter.right());
+
+ // all other rows
+ Cell *row = top_left;
+ do
+ {
+ PointIter iter2(row,BOTTOM_LEFT);
+ do
+ {
+ if(ball || !iter2.isNaN())
+ points.push_back(*(*iter2));
+ }while(iter2.right());
+ row = row->bottom;
+ }while(row);
+ return points;
+}
+
+std::vector<cv::KeyPoint> Chessboard::Board::getKeyPoints(bool ball)const
+{
+ std::vector<cv::KeyPoint> keypoints;
+ std::vector<cv::Point2f> points = getCorners(ball);
+ std::vector<cv::Point2f>::const_iterator iter = points.begin();
+ for(;iter != points.end();++iter)
+ keypoints.push_back(cv::KeyPoint(iter->x,iter->y,1));
+ return keypoints;
+}
+
+Chessboard::Chessboard(const Parameters ¶)
+{
+ reconfigure(para);
+}
+
+void Chessboard::reconfigure(const Parameters &config)
+{
+ parameters = config;
+}
+
+Chessboard::Parameters Chessboard::getPara()const
+{
+ return parameters;
+}
+
+Chessboard::~Chessboard()
+{
+}
+
+void Chessboard::findKeyPoints(const cv::Mat& image, std::vector<KeyPoint>& keypoints,std::vector<cv::Mat> &feature_maps,
+ std::vector<std::vector<float> > &angles ,const cv::Mat& mask)const
+{
+ keypoints.clear();
+ angles.clear();
+ vector<KeyPoint> keypoints_temp;
+ FastX::Parameters para;
+
+ para.branches = 2; // this is always the case for checssboard corners
+ para.strength = 10; // minimal threshold
+ para.resolution = float(CV_PI*0.25); // this gives the best results taking interpolation into account
+ para.filter = 1;
+ para.super_resolution = parameters.super_resolution;
+ para.min_scale = parameters.min_scale;
+ para.max_scale = parameters.max_scale;
+
+ FastX detector(para);
+ std::vector<cv::Mat> rotated_images;
+ detector.detectImpl(image,rotated_images,feature_maps,mask);
+
+ //calculate seed chessboard corners
+ detector.findKeyPoints(feature_maps,keypoints_temp,mask);
+
+ //sort points and limit number
+ int max_seeds = std::min((int)keypoints_temp.size(),parameters.max_points);
+ if(max_seeds < 9)
+ return;
+
+ std::partial_sort(keypoints_temp.begin(),keypoints_temp.begin()+max_seeds-1,
+ keypoints_temp.end(),sortKeyPoint);
+ keypoints_temp.resize(max_seeds);
+ std::vector<std::vector<float> > angles_temp = detector.calcAngles(rotated_images,keypoints_temp);
+
+ // filter out keypoints which are not symmetric
+ std::vector<KeyPoint>::iterator iter1 = keypoints_temp.begin();
+ std::vector<std::vector<float> >::const_iterator iter2 = angles_temp.begin();
+ for(;iter1 != keypoints_temp.end();++iter1,++iter2)
+ {
+ cv::KeyPoint &pt = *iter1;
+ const std::vector<float> &angles_i3 = *iter2;
+ if(angles_i3.size() != 2)// || pt.response < noise)
+ continue;
+ int result = testPointSymmetry(image,pt.pt,pt.size*0.7F,std::max(10.0F,sqrt(pt.response)+0.5F*pt.size));
+ if(result > MAX_SYMMETRY_ERRORS)
+ continue;
+ else if(result > 3)
+ pt.response = - pt.response;
+ angles.push_back(angles_i3);
+ keypoints.push_back(pt);
+ }
+}
+
+cv::Mat Chessboard::buildData(const std::vector<KeyPoint>& keypoints)const
+{
+ cv::Mat data(int(keypoints.size()),4,CV_32FC1); // x + y + angle + strength
+ std::vector<cv::KeyPoint>::const_iterator iter = keypoints.begin();
+ float *val = reinterpret_cast<float*>(data.data);
+ for(;iter != keypoints.end();++iter)
+ {
+ (*val++) = iter->pt.x;
+ (*val++) = iter->pt.y;
+ (*val++) = float(2.0*CV_PI-iter->angle/180.0*CV_PI);
+ (*val++) = iter->response;
+ }
+ return data;
+}
+
+std::vector<cv::KeyPoint> Chessboard::getInitialPoints(cv::flann::Index &flann_index,const cv::Mat &data,const cv::KeyPoint ¢er,float white_angle,float black_angle,float min_response)const
+{
+ CV_CheckTypeEQ(data.type(), CV_32FC1, "Unsupported source type");
+ if(data.cols != 4)
+ CV_Error(Error::StsBadArg,"wrong data format");
+
+ std::vector<float> query,dists;
+ std::vector<int> indices;
+ query.resize(2); query[0] = center.pt.x; query[1] = center.pt.y;
+ flann_index.knnSearch(query,indices,dists,21,cv::flann::SearchParams(32));
+
+ // collect all points having a similar angle and response
+ std::vector<cv::KeyPoint> points;
+ std::vector<int>::const_iterator ids_iter = indices.begin()+1; // first point is center
+ points.push_back(center);
+ for(;ids_iter != indices.end();++ids_iter)
+ {
+ // TODO do more angle tests
+ // test only one angle against the stored one
+ const float &response = data.at<float>(*ids_iter,3);
+ if(fabs(response) < min_response)
+ continue;
+ const float &angle = data.at<float>(*ids_iter,2);
+ float angle_temp = fabs(angle-white_angle);
+ if(angle_temp > CV_PI*0.5)
+ angle_temp = float(fabs(angle_temp-CV_PI));
+ if(angle_temp > MAX_ANGLE)
+ {
+ angle_temp = fabs(angle-black_angle);
+ if(angle_temp > CV_PI*0.5)
+ angle_temp = float(fabs(angle_temp-CV_PI));
+ if(angle_temp >MAX_ANGLE)
+ continue;
+ }
+ points.push_back(cv::KeyPoint(data.at<float>(*ids_iter,0),data.at<float>(*ids_iter,1),center.size,angle,response));
+ }
+ return points;
+}
+
+Chessboard::BState Chessboard::generateBoards(cv::flann::Index &flann_index,const cv::Mat &data,
+ const cv::KeyPoint ¢er,float white_angle,float black_angle,float min_response,const cv::Mat& img,
+ std::vector<Chessboard::Board> &boards)const
+{
+ // collect all points having a similar angle
+ std::vector<cv::KeyPoint> kpoints= getInitialPoints(flann_index,data,center,white_angle,black_angle,min_response);
+ if(kpoints.size() < 5)
+ return MISSING_POINTS;
+
+ if(!img.empty())
+ {
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+ cv::Mat out;
+ cv::drawKeypoints(img,kpoints,out,cv::Scalar(0,0,255,255),4);
+ std::vector<cv::KeyPoint> temp;
+ temp.push_back(kpoints.front());
+ cv::drawKeypoints(out,temp,out,cv::Scalar(0,255,0,255),4);
+ cv::imshow("chessboard",out);
+ cv::waitKey(-1);
+#endif
+ }
+
+ // use angles to filter out points
+ std::vector<cv::KeyPoint> points;
+ cv::Vec2f n1(cos(white_angle),-sin(white_angle));
+ cv::Vec2f n2(cos(black_angle),-sin(black_angle));
+ std::vector<cv::KeyPoint>::const_iterator iter1 = kpoints.begin()+1; // first point is center
+ for(;iter1 != kpoints.end();++iter1)
+ {
+ // calc angle
+ cv::Vec2f vec(iter1->pt-center.pt);
+ vec = vec/cv::norm(vec);
+ if(fabs(vec.dot(n1)) < 0.96 && fabs(vec.dot(n2)) < 0.96) //check that angle is bigger than 15°
+ points.push_back(*iter1);
+ }
+
+ // generate pairs those connection goes through the center
+ std::vector<std::pair<cv::KeyPoint,cv::KeyPoint> > pairs;
+ iter1 = points.begin();
+ for(;iter1 != points.end();++iter1)
+ {
+ std::vector<cv::KeyPoint>::const_iterator iter2 = iter1+1;
+ for(;iter2 != points.end();++iter2)
+ {
+ if(isPointOnLine(iter1->pt,iter2->pt,center.pt,0.97F))
+ {
+ if(cv::norm(iter1->pt) < cv::norm(iter2->pt))
+ pairs.push_back(std::make_pair(*iter1,*iter2));
+ else
+ pairs.push_back(std::make_pair(*iter2,*iter1));
+ }
+ }
+ }
+
+ // generate all possible combinations consisting of two pairs
+ if(pairs.size() < 2)
+ return MISSING_PAIRS;
+ std::vector<std::pair<cv::KeyPoint,cv::KeyPoint> >::iterator iter_pair1 = pairs.begin();
+
+ BState best_state = MISSING_PAIRS;
+ for(;iter_pair1 != pairs.end();++iter_pair1)
+ {
+ cv::Point2f p1 = iter_pair1->second.pt-iter_pair1->first.pt;
+ p1 = p1/cv::norm(p1);
+ std::vector<std::pair<cv::KeyPoint,cv::KeyPoint> >::iterator iter_pair2 = iter_pair1+1;
+ for(;iter_pair2 != pairs.end();++iter_pair2)
+ {
+ cv::Point2f p2 = iter_pair2->second.pt-iter_pair2->first.pt;
+ p2 = p2/cv::norm(p2);
+ if(p2.dot(p1) > 0.95)
+ {
+ if(best_state < WRONG_PAIR_ANGLE)
+ best_state = WRONG_PAIR_ANGLE;
+ }
+ else
+ {
+ // check orientations
+ if(checkOrientation(iter_pair1->first.pt,iter_pair1->second.pt,iter_pair2->first.pt,iter_pair2->second.pt))
+ std::swap(iter_pair2->first,iter_pair2->second);
+
+ // minimal case
+ std::vector<cv::Point2f> board_points;
+ board_points.resize(9,cv::Point2f(std::numeric_limits<float>::quiet_NaN(),
+ std::numeric_limits<float>::quiet_NaN()));
+
+ board_points[1] = iter_pair2->first.pt;
+ board_points[3] = iter_pair1->first.pt;
+ board_points[4] = center.pt;
+ board_points[5] = iter_pair1->second.pt;
+ board_points[7] = iter_pair2->second.pt;
+ boards.push_back(Board(cv::Size(3,3),board_points,white_angle,black_angle));
+ Board &board = boards.back();
+
+ if(board.isEmpty())
+ {
+ if(best_state < WRONG_CONFIGURATION)
+ best_state = WRONG_CONFIGURATION;
+ boards.pop_back(); // MAKE SURE board is no longer used !!!!
+ continue;
+ }
+ best_state = FOUND_BOARD;
+ }
+ }
+ }
+ return best_state;
+}
+
+void Chessboard::detectImpl(const Mat& image, vector<KeyPoint>& keypoints,std::vector<Mat> &feature_maps,const Mat& mask)const
+{
+ keypoints.clear();
+ Board board = detectImpl(image,feature_maps,mask);
+ keypoints = board.getKeyPoints();
+ return;
+}
+
+Chessboard::Board Chessboard::detectImpl(const Mat& gray,std::vector<cv::Mat> &feature_maps,const Mat& mask)const
+{
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+ debug_image = gray;
+#endif
+ CV_CheckTypeEQ(gray.type(),CV_8UC1, "Unsupported image type");
+
+ cv::Size chessboard_size2(parameters.chessboard_size.height,parameters.chessboard_size.width);
+ std::vector<KeyPoint> keypoints_seed;
+ std::vector<std::vector<float> > angles;
+ findKeyPoints(gray,keypoints_seed,feature_maps,angles,mask);
+ if(keypoints_seed.empty())
+ return Chessboard::Board();
+
+ // check how many points are likely a checkerbord corner
+ float response = fabs(keypoints_seed.front().response*MIN_RESPONSE_RATIO);
+ std::vector<KeyPoint>::const_iterator seed_iter = keypoints_seed.begin();
+ int count = 0;
+ int inum = chessboard_size2.width*chessboard_size2.height;
+ for(;seed_iter != keypoints_seed.end() && count < inum;++seed_iter,++count)
+ {
+ // points are sorted based on response
+ if(fabs(seed_iter->response) < response)
+ {
+ seed_iter = keypoints_seed.end();
+ return Chessboard::Board();
+ }
+ }
+ // just add dummy points or flann will fail during knnSearch
+ if(keypoints_seed.size() < 21)
+ keypoints_seed.resize(21, cv::KeyPoint(-99999.0F,-99999.0F,0.0F,0.0F,0.0F));
+
+ //build kd tree
+ cv::Mat data = buildData(keypoints_seed);
+ cv::Mat flann_data(data.rows,2,CV_32FC1);
+ data(cv::Rect(0,0,2,data.rows)).copyTo(flann_data);
+ cv::flann::Index flann_index(flann_data,cv::flann::KDTreeIndexParams(1),cvflann::FLANN_DIST_EUCLIDEAN);
+
+ // for each point
+ std::vector<std::vector<float> >::const_iterator angles_iter = angles.begin();
+ std::vector<cv::KeyPoint>::const_iterator points_iter = keypoints_seed.begin();
+ cv::Rect bounding_box(5,5,gray.cols-10,gray.rows-10);
+ int max_tests = std::min(parameters.max_tests,int(keypoints_seed.size()));
+ for(count=0;count < max_tests;++angles_iter,++points_iter,++count)
+ {
+ // regard current point as center point
+ // which must have two angles!!! (this was already checked)
+ float min_response = points_iter->response*MIN_RESPONSE_RATIO;
+ if(min_response <= 0)
+ {
+ if(max_tests+1 < int(keypoints_seed.size()))
+ ++max_tests;
+ continue;
+ }
+ const std::vector<float> &angles_i = *angles_iter;
+ float white_angle = fabs(angles_i.front()); // angle is negative if black --> clockwise
+ float black_angle = fabs(angles_i.back()); // angle is negative if black --> clockwise
+ if(angles_i.front() < 0) // ensure white angle is first
+ swap(white_angle,black_angle);
+
+ std::vector<Board> boards;
+ generateBoards(flann_index, data,*points_iter,white_angle,black_angle,min_response,gray,boards);
+ parallel_for_(Range(0,(int)boards.size()),[&](const Range& range){
+ for(int i=range.start;i <range.end;++i)
+ {
+ auto iter_boards = boards.begin()+i;
+ cv::Mat h = iter_boards->estimateHomography();
+ int size = iter_boards->validateCorners(data,flann_index,h,min_response);
+ if(size != 9 || !iter_boards->validateContour())
+ {
+ iter_boards->clear();
+ continue;
+ }
+ //grow based on kd-tree
+ iter_boards->grow(data,flann_index);
+ if(!iter_boards->checkUnique())
+ {
+ iter_boards->clear();
+ continue;
+ }
+
+ // check bounding box
+ std::vector<cv::Point2f> contour = iter_boards->getContour();
+ std::vector<cv::Point2f>::const_iterator iter = contour.begin();
+ for(;iter != contour.end();++iter)
+ {
+ if(!bounding_box.contains(*iter))
+ break;
+ }
+ if(iter != contour.end())
+ {
+ iter_boards->clear();
+ continue;
+ }
+
+ if(iter_boards->getSize() == parameters.chessboard_size ||
+ iter_boards->getSize() == chessboard_size2)
+ {
+ iter_boards->normalizeOrientation(false);
+ if(iter_boards->getSize() != parameters.chessboard_size)
+ {
+ if(iter_boards->isCellBlack(0,0) == iter_boards->isCellBlack(0,int(iter_boards->colCount())-1))
+ iter_boards->rotateLeft();
+ else
+ iter_boards->rotateRight();
+ }
+#ifdef CV_DETECTORS_CHESSBOARD_DEBUG
+ cv::Mat img;
+ iter_boards->draw(debug_image,img);
+ cv::imshow("chessboard",img);
+ cv::waitKey(-1);
+#endif
+ }
+ else
+ {
+ if(iter_boards->getSize().width*iter_boards->getSize().height < chessboard_size2.width*chessboard_size2.height)
+ iter_boards->clear();
+ else if(!parameters.larger)
+ iter_boards->clear();
+ }
+ }
+ });
+ // check if a good board was found
+ for(const auto &board : boards)
+ {
+ if(!board.isEmpty())
+ return board;
+ }
+ }
+ return Chessboard::Board();
+}
+
+void Chessboard::detectAndCompute(cv::InputArray image,cv::InputArray mask,std::vector<cv::KeyPoint>& keypoints,
+ cv::OutputArray descriptors,bool useProvidedKeyPoints)
+{
+ descriptors.clear();
+ useProvidedKeyPoints=false;
+ std::vector<cv::Mat> maps;
+ detectImpl(image.getMat(),keypoints,maps,mask.getMat());
+ if(!useProvidedKeyPoints) // suppress compiler warning
+ return;
+ return;
+}
+
+void Chessboard::detectImpl(const Mat& image, vector<KeyPoint>& keypoints,const Mat& mask)const
+{
+ std::vector<cv::Mat> maps;
+ detectImpl(image,keypoints,maps,mask);
+}
+
+void Chessboard::detectImpl(InputArray image, std::vector<KeyPoint>& keypoints, InputArray mask)const
+{
+ detectImpl(image.getMat(),keypoints,mask.getMat());
+}
+
+} // end namespace details
+
+
+// public API
+bool findChessboardCornersSB(cv::InputArray image_, cv::Size pattern_size,
+ cv::OutputArray corners_, int flags)
+{
+ CV_INSTRUMENT_REGION();
+ int type = image_.type(), depth = CV_MAT_DEPTH(type), cn = CV_MAT_CN(type);
+ CV_CheckType(type, depth == CV_8U && (cn == 1 || cn == 3),
+ "Only 8-bit grayscale or color images are supported");
+ if(pattern_size.width <= 2 || pattern_size.height <= 2)
+ {
+ CV_Error(Error::StsOutOfRange, "Both width and height of the pattern should have bigger than 2");
+ }
+ if (!corners_.needed())
+ CV_Error(Error::StsNullPtr, "Null pointer to corners");
+
+ Mat img;
+ if (image_.channels() != 1)
+ cvtColor(image_, img, COLOR_BGR2GRAY);
+ else
+ img = image_.getMat();
+
+ details::Chessboard::Parameters para;
+ para.chessboard_size = pattern_size;
+ para.min_scale = 2;
+ para.max_scale = 4;
+ para.max_tests = 25;
+ para.max_points = std::max(100,pattern_size.width*pattern_size.height*2);
+ para.super_resolution = false;
+
+ // setup search based on flags
+ if(flags & CALIB_CB_NORMALIZE_IMAGE)
+ {
+ Mat tmp;
+ cv::equalizeHist(img, tmp);
+ swap(img, tmp);
+ flags ^= CALIB_CB_NORMALIZE_IMAGE;
+ }
+ if(flags & CALIB_CB_EXHAUSTIVE)
+ {
+ para.max_tests = 100;
+ para.max_points = std::max(500,pattern_size.width*pattern_size.height*2);
+ flags ^= CALIB_CB_EXHAUSTIVE;
+ }
+ if(flags & CALIB_CB_ACCURACY)
+ {
+ para.super_resolution = true;
+ flags ^= CALIB_CB_ACCURACY;
+ }
+ if(flags)
+ CV_Error(Error::StsOutOfRange, cv::format("Invalid remaining flags %d", (int)flags));
+
+ std::vector<cv::KeyPoint> corners;
+ details::Chessboard board(para);
+ board.detect(img,corners);
+ if(corners.empty())
+ {
+ corners_.release();
+ return false;
+ }
+ std::vector<cv::Point2f> points;
+ KeyPoint::convert(corners,points);
+ Mat(points).copyTo(corners_);
+ return true;
+}
+
+} // namespace cv
--- /dev/null
- Ellipse(const Ellipse &other);
-
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+#ifndef CHESSBOARD_HPP_
+#define CHESSBOARD_HPP_
+
+#include "opencv2/core.hpp"
+#include "opencv2/features2d.hpp"
+#include <vector>
+#include <set>
+#include <map>
+
+namespace cv {
+namespace details{
+/**
+ * \brief Fast point sysmetric cross detector based on a localized radon transformation
+ */
+class FastX : public cv::Feature2D
+{
+ public:
+ struct Parameters
+ {
+ float strength; //!< minimal strength of a valid junction in dB
+ float resolution; //!< angle resolution in radians
+ int branches; //!< the number of branches
+ int min_scale; //!< scale level [0..8]
+ int max_scale; //!< scale level [0..8]
+ bool filter; //!< post filter feature map to improve impulse response
+ bool super_resolution; //!< up-sample
+
+ Parameters()
+ {
+ strength = 40;
+ resolution = float(CV_PI*0.25);
+ branches = 2;
+ min_scale = 2;
+ max_scale = 5;
+ super_resolution = 1;
+ filter = true;
+ }
+ };
+
+ public:
+ FastX(const Parameters &config = Parameters());
+ virtual ~FastX(){};
+
+ void reconfigure(const Parameters ¶);
+
+ //declaration to be wrapped by rbind
+ void detect(cv::InputArray image,std::vector<cv::KeyPoint>& keypoints, cv::InputArray mask=cv::Mat())override
+ {cv::Feature2D::detect(image.getMat(),keypoints,mask.getMat());}
+
+ virtual void detectAndCompute(cv::InputArray image,
+ cv::InputArray mask,
+ std::vector<cv::KeyPoint>& keypoints,
+ cv::OutputArray descriptors,
+ bool useProvidedKeyPoints = false)override;
+
+ void detectImpl(const cv::Mat& image,
+ std::vector<cv::KeyPoint>& keypoints,
+ std::vector<cv::Mat> &feature_maps,
+ const cv::Mat& mask=cv::Mat())const;
+
+ void detectImpl(const cv::Mat& image,
+ std::vector<cv::Mat> &rotated_images,
+ std::vector<cv::Mat> &feature_maps,
+ const cv::Mat& mask=cv::Mat())const;
+
+ void findKeyPoints(const std::vector<cv::Mat> &feature_map,
+ std::vector<cv::KeyPoint>& keypoints,
+ const cv::Mat& mask = cv::Mat())const;
+
+ std::vector<std::vector<float> > calcAngles(const std::vector<cv::Mat> &rotated_images,
+ std::vector<cv::KeyPoint> &keypoints)const;
+ // define pure virtual methods
+ virtual int descriptorSize()const override{return 0;};
+ virtual int descriptorType()const override{return 0;};
+ virtual void operator()( cv::InputArray image, cv::InputArray mask, std::vector<cv::KeyPoint>& keypoints, cv::OutputArray descriptors, bool useProvidedKeypoints=false )const
+ {
+ descriptors.clear();
+ detectImpl(image.getMat(),keypoints,mask);
+ if(!useProvidedKeypoints) // suppress compiler warning
+ return;
+ return;
+ }
+
+ protected:
+ virtual void computeImpl( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors)const
+ {
+ descriptors = cv::Mat();
+ detectImpl(image,keypoints);
+ }
+
+ private:
+ void detectImpl(const cv::Mat& _src, std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask)const;
+ virtual void detectImpl(cv::InputArray image, std::vector<cv::KeyPoint>& keypoints, cv::InputArray mask=cv::noArray())const;
+
+ void rotate(float angle,const cv::Mat &img,cv::Size size,cv::Mat &out)const;
+ void calcFeatureMap(const cv::Mat &images,cv::Mat& out)const;
+
+ private:
+ Parameters parameters;
+};
+
+/**
+ * \brief Ellipse class
+ */
+class Ellipse
+{
+ public:
+ Ellipse();
+ Ellipse(const cv::Point2f ¢er, const cv::Size2f &axes, float angle);
+
+ void draw(cv::InputOutputArray img,const cv::Scalar &color = cv::Scalar::all(120))const;
+ bool contains(const cv::Point2f &pt)const;
+ cv::Point2f getCenter()const;
+ const cv::Size2f &getAxes()const;
+
+ private:
+ cv::Point2f center;
+ cv::Size2f axes;
+ float angle,cosf,sinf;
+};
+
+/**
+ * \brief Chessboard corner detector
+ *
+ * The detectors tries to find all chessboard corners of an imaged
+ * chessboard and returns them as an ordered vector of KeyPoints.
+ * Thereby, the left top corner has index 0 and the bottom right
+ * corner n*m-1.
+ */
+class Chessboard: public cv::Feature2D
+{
+ public:
+ static const int DUMMY_FIELD_SIZE = 100; // in pixel
+
+ /**
+ * \brief Configuration of a chessboard corner detector
+ *
+ */
+ struct Parameters
+ {
+ cv::Size chessboard_size; //!< size of the chessboard
+ int min_scale; //!< scale level [0..8]
+ int max_scale; //!< scale level [0..8]
+ int max_points; //!< maximal number of points regarded
+ int max_tests; //!< maximal number of tested hypothesis
+ bool super_resolution; //!< use super-repsolution for chessboard detection
+ bool larger; //!< indicates if larger boards should be returned
+
+ Parameters()
+ {
+ chessboard_size = cv::Size(9,6);
+ min_scale = 2;
+ max_scale = 4;
+ super_resolution = true;
+ max_points = 400;
+ max_tests = 100;
+ larger = false;
+ }
+
+ Parameters(int scale,int _max_points):
+ min_scale(scale),
+ max_scale(scale),
+ max_points(_max_points)
+ {
+ chessboard_size = cv::Size(9,6);
+ }
+ };
+
+
+ /**
+ * \brief Gets the 3D objects points for the chessboard assuming the
+ * left top corner is located at the origin.
+ *
+ * \param[in] pattern_size Number of rows and cols of the pattern
+ * \param[in] cell_size Size of one cell
+ *
+ * \returns Returns the object points as CV_32FC3
+ */
+ static cv::Mat getObjectPoints(const cv::Size &pattern_size,float cell_size);
+
+ /**
+ * \brief Class for searching and storing chessboard corners.
+ *
+ * The search is based on a feature map having strong pixel
+ * values at positions where a chessboard corner is located.
+ *
+ * The board must be rectangular but supports empty cells
+ *
+ */
+ class Board
+ {
+ public:
+ /**
+ * \brief Estimates the position of the next point on a line using cross ratio constrain
+ *
+ * cross ratio:
+ * d12/d34 = d13/d24
+ *
+ * point order on the line:
+ * pt1 --> pt2 --> pt3 --> pt4
+ *
+ * \param[in] pt1 First point coordinate
+ * \param[in] pt2 Second point coordinate
+ * \param[in] pt3 Third point coordinate
+ * \param[out] pt4 Forth point coordinate
+ *
+ */
+ static bool estimatePoint(const cv::Point2f &p0,const cv::Point2f &p1,const cv::Point2f &p2,cv::Point2f &p3);
+
+ // using 1D homography
+ static bool estimatePoint(const cv::Point2f &p0,const cv::Point2f &p1,const cv::Point2f &p2,const cv::Point2f &p3, cv::Point2f &p4);
+
+ /**
+ * \brief Checks if all points of a row or column have a valid cross ratio constraint
+ *
+ * cross ratio:
+ * d12/d34 = d13/d24
+ *
+ * point order on the row/column:
+ * pt1 --> pt2 --> pt3 --> pt4
+ *
+ * \param[in] points THe points of the row/column
+ *
+ */
+ static bool checkRowColumn(const std::vector<cv::Point2f> &points);
+
+ /**
+ * \brief Estimates the search area for the next point on the line using cross ratio
+ *
+ * point order on the line:
+ * (p0) --> p1 --> p2 --> p3 --> search area
+ *
+ * \param[in] p1 First point coordinate
+ * \param[in] p2 Second point coordinate
+ * \param[in] p3 Third point coordinate
+ * \param[in] p Percentage of d34 used for the search area width and height [0..1]
+ * \param[out] ellipse The search area
+ * \param[in] p0 optional point to improve accuracy
+ *
+ * \return Returns false if no search area can be calculated
+ *
+ */
+ static bool estimateSearchArea(const cv::Point2f &p1,const cv::Point2f &p2,const cv::Point2f &p3,float p,
+ Ellipse &ellipse,const cv::Point2f *p0 =NULL);
+
+ /**
+ * \brief Estimates the search area for a specific point based on the given homography
+ *
+ * \param[in] H homography describing the transformation from ideal board to real one
+ * \param[in] row Row of the point
+ * \param[in] col Col of the point
+ * \param[in] p Percentage [0..1]
+ *
+ * \return Returns false if no search area can be calculated
+ *
+ */
+ static Ellipse estimateSearchArea(cv::Mat H,int row, int col,float p,int field_size = DUMMY_FIELD_SIZE);
+
+ /**
+ * \brief Searches for the maximum in a given search area
+ *
+ * \param[in] map feature map
+ * \param[in] ellipse search area
+ * \param[in] min_val Minimum value of the maximum to be accepted as maximum
+ *
+ * \return Returns a negative value if all points are outside the ellipse
+ *
+ */
+ static float findMaxPoint(cv::flann::Index &index,const cv::Mat &data,const Ellipse &ellipse,float white_angle,float black_angle,cv::Point2f &pt);
+
+ /**
+ * \brief Searches for the next point using cross ratio constrain
+ *
+ * \param[in] index flann index
+ * \param[in] data extended flann data
+ * \param[in] pt1
+ * \param[in] pt2
+ * \param[in] pt3
+ * \param[in] white_angle
+ * \param[in] black_angle
+ * \param[in] min_response
+ * \param[out] point The resulting point
+ *
+ * \return Returns false if no point could be found
+ *
+ */
+ static bool findNextPoint(cv::flann::Index &index,const cv::Mat &data,
+ const cv::Point2f &pt1,const cv::Point2f &pt2, const cv::Point2f &pt3,
+ float white_angle,float black_angle,float min_response,cv::Point2f &point);
+
+ /**
+ * \brief Creates a new Board object
+ *
+ */
+ Board(float white_angle=0,float black_angle=0);
+ Board(const cv::Size &size, const std::vector<cv::Point2f> &points,float white_angle=0,float black_angle=0);
+ Board(const Chessboard::Board &other);
+ virtual ~Board();
+
+ Board& operator=(const Chessboard::Board &other);
+
+ /**
+ * \brief Draws the corners into the given image
+ *
+ * \param[in] m The image
+ * \param[out] m The resulting image
+ * \param[in] H optional homography to calculate search area
+ *
+ */
+ void draw(cv::InputArray m,cv::OutputArray out,cv::InputArray H=cv::Mat())const;
+
+ /**
+ * \brief Estimates the pose of the chessboard
+ *
+ */
+ bool estimatePose(const cv::Size2f &real_size,cv::InputArray _K,cv::OutputArray rvec,cv::OutputArray tvec)const;
+
+ /**
+ * \brief Clears all internal data of the object
+ *
+ */
+ void clear();
+
+ /**
+ * \brief Returns the angle of the black diagnonale
+ *
+ */
+ float getBlackAngle()const;
+
+ /**
+ * \brief Returns the angle of the black diagnonale
+ *
+ */
+ float getWhiteAngle()const;
+
+ /**
+ * \brief Initializes a 3x3 grid from 9 corner coordinates
+ *
+ * All points must be ordered:
+ * p0 p1 p2
+ * p3 p4 p5
+ * p6 p7 p8
+ *
+ * \param[in] points vector of points
+ *
+ * \return Returns false if the grid could not be initialized
+ */
+ bool init(const std::vector<cv::Point2f> points);
+
+ /**
+ * \brief Returns true if the board is empty
+ *
+ */
+ bool isEmpty() const;
+
+ /**
+ * \brief Returns all board corners as ordered vector
+ *
+ * The left top corner has index 0 and the bottom right
+ * corner rows*cols-1. All corners which only belong to
+ * empty cells are returned as NaN.
+ */
+ std::vector<cv::Point2f> getCorners(bool ball=true) const;
+
+ /**
+ * \brief Returns all board corners as ordered vector of KeyPoints
+ *
+ * The left top corner has index 0 and the bottom right
+ * corner rows*cols-1.
+ *
+ * \param[in] ball if set to false only non empty points are returned
+ *
+ */
+ std::vector<cv::KeyPoint> getKeyPoints(bool ball=true) const;
+
+ /**
+ * \brief Returns the centers of the chessboard cells
+ *
+ * The left top corner has index 0 and the bottom right
+ * corner (rows-1)*(cols-1)-1.
+ *
+ */
+ std::vector<cv::Point2f> getCellCenters() const;
+
+ /**
+ * \brief Estimates the homography between an ideal board
+ * and reality based on the already recovered points
+ *
+ * \param[in] rect selecting a subset of the already recovered points
+ * \param[in] field_size The field size of the ideal board
+ *
+ */
+ cv::Mat estimateHomography(cv::Rect rect,int field_size = DUMMY_FIELD_SIZE)const;
+
+ /**
+ * \brief Estimates the homography between an ideal board
+ * and reality based on the already recovered points
+ *
+ * \param[in] field_size The field size of the ideal board
+ *
+ */
+ cv::Mat estimateHomography(int field_size = DUMMY_FIELD_SIZE)const;
+
+ /**
+ * \brief Returns the size of the board
+ *
+ */
+ cv::Size getSize() const;
+
+ /**
+ * \brief Returns the number of cols
+ *
+ */
+ size_t colCount() const;
+
+ /**
+ * \brief Returns the number of rows
+ *
+ */
+ size_t rowCount() const;
+
+ /**
+ * \brief Returns the inner contour of the board including only valid corners
+ *
+ * \info the contour might be non squared if not all points of the board are defined
+ *
+ */
+ std::vector<cv::Point2f> getContour()const;
+
+
+ /**
+ * \brief Grows the board in all direction until no more corners are found in the feature map
+ *
+ * \param[in] data CV_32FC1 data of the flann index
+ * \param[in] flann_index flann index
+ *
+ * \returns the number of grows
+ */
+ int grow(const cv::Mat &data,cv::flann::Index &flann_index);
+
+ /**
+ * \brief Validates all corners using guided search based on the given homography
+ *
+ * \param[in] data CV_32FC1 data of the flann index
+ * \param[in] flann_index flann index
+ * \param[in] h Homography describing the transformation from ideal board to the real one
+ * \param[in] min_response Min response
+ *
+ * \returns the number of valid corners
+ */
+ int validateCorners(const cv::Mat &data,cv::flann::Index &flann_index,const cv::Mat &h,float min_response=0);
+
+ /**
+ * \brief check that no corner is used more than once
+ *
+ * \returns Returns false if a corner is used more than once
+ */
+ bool checkUnique()const;
+
+ /**
+ * \brief Returns false if the angles of the contour are smaller than 35°
+ *
+ */
+ bool validateContour()const;
+
+ /**
+ * \brief Grows the board to the left by adding one column.
+ *
+ * \param[in] map CV_32FC1 feature map
+ *
+ * \returns Returns false if the feature map has no maxima at the requested positions
+ */
+ bool growLeft(const cv::Mat &map,cv::flann::Index &flann_index);
+ void growLeft();
+
+ /**
+ * \brief Grows the board to the top by adding one row.
+ *
+ * \param[in] map CV_32FC1 feature map
+ *
+ * \returns Returns false if the feature map has no maxima at the requested positions
+ */
+ bool growTop(const cv::Mat &map,cv::flann::Index &flann_index);
+ void growTop();
+
+ /**
+ * \brief Grows the board to the right by adding one column.
+ *
+ * \param[in] map CV_32FC1 feature map
+ *
+ * \returns Returns false if the feature map has no maxima at the requested positions
+ */
+ bool growRight(const cv::Mat &map,cv::flann::Index &flann_index);
+ void growRight();
+
+ /**
+ * \brief Grows the board to the bottom by adding one row.
+ *
+ * \param[in] map CV_32FC1 feature map
+ *
+ * \returns Returns false if the feature map has no maxima at the requested positions
+ */
+ bool growBottom(const cv::Mat &map,cv::flann::Index &flann_index);
+ void growBottom();
+
+ /**
+ * \brief Adds one column on the left side
+ *
+ * \param[in] points The corner coordinates
+ *
+ */
+ void addColumnLeft(const std::vector<cv::Point2f> &points);
+
+ /**
+ * \brief Adds one column at the top
+ *
+ * \param[in] points The corner coordinates
+ *
+ */
+ void addRowTop(const std::vector<cv::Point2f> &points);
+
+ /**
+ * \brief Adds one column on the right side
+ *
+ * \param[in] points The corner coordinates
+ *
+ */
+ void addColumnRight(const std::vector<cv::Point2f> &points);
+
+ /**
+ * \brief Adds one row at the bottom
+ *
+ * \param[in] points The corner coordinates
+ *
+ */
+ void addRowBottom(const std::vector<cv::Point2f> &points);
+
+ /**
+ * \brief Rotates the board 90° degrees to the left
+ */
+ void rotateLeft();
+
+ /**
+ * \brief Rotates the board 90° degrees to the right
+ */
+ void rotateRight();
+
+ /**
+ * \brief Flips the board along its local x(width) coordinate direction
+ */
+ void flipVertical();
+
+ /**
+ * \brief Flips the board along its local y(height) coordinate direction
+ */
+ void flipHorizontal();
+
+ /**
+ * \brief Flips and rotates the board so that the angle of
+ * either the black or white diagonal is bigger than the x
+ * and y axis of the board and from a right handed
+ * coordinate system
+ */
+ void normalizeOrientation(bool bblack=true);
+
+ /**
+ * \brief Exchanges the stored board with the board stored in other
+ */
+ void swap(Chessboard::Board &other);
+
+ bool operator==(const Chessboard::Board& other) const {return rows*cols == other.rows*other.cols;};
+ bool operator< (const Chessboard::Board& other) const {return rows*cols < other.rows*other.cols;};
+ bool operator> (const Chessboard::Board& other) const {return rows*cols > other.rows*other.cols;};
+ bool operator>= (const cv::Size& size)const { return rows*cols >= size.width*size.height; };
+
+ /**
+ * \brief Returns a specific corner
+ *
+ * \info raises runtime_error if row col does not exists
+ */
+ cv::Point2f& getCorner(int row,int col);
+
+ /**
+ * \brief Returns true if the cell is empty meaning at least one corner is NaN
+ */
+ bool isCellEmpty(int row,int col);
+
+ /**
+ * \brief Returns the mapping from all corners idx to only valid corners idx
+ */
+ std::map<int,int> getMapping()const;
+
+ /**
+ * \brief Estimates rotation of the board around the camera axis
+ */
+ double estimateRotZ()const;
+
+ /**
+ * \brief Returns true if the cell is black
+ *
+ */
+ bool isCellBlack(int row,int cola)const;
+
+ private:
+ // stores one cell
+ // in general a cell is initialized by the Board so that:
+ // * all corners are always pointing to a valid cv::Point2f
+ // * depending on the position left,top,right and bottom might be set to NaN
+ // * A cell is empty if at least one corner is NaN
+ struct Cell
+ {
+ cv::Point2f *top_left,*top_right,*bottom_right,*bottom_left; // corners
+ Cell *left,*top,*right,*bottom; // neighbouring cells
+ bool black; // set to true if cell is black
+ Cell();
+ bool empty()const; // indicates if the cell is empty (one of its corners has NaN)
+ int getRow()const;
+ int getCol()const;
+ };
+
+ // corners
+ enum CornerIndex
+ {
+ TOP_LEFT,
+ TOP_RIGHT,
+ BOTTOM_RIGHT,
+ BOTTOM_LEFT
+ };
+
+ Cell* getCell(int row,int column); // returns a specific cell
+ const Cell* getCell(int row,int column)const; // returns a specific cell
+ void drawEllipses(const std::vector<Ellipse> &ellipses);
+
+ // Iterator for iterating over board corners
+ class PointIter
+ {
+ public:
+ PointIter(Cell *cell,CornerIndex corner_index);
+ PointIter(const PointIter &other);
+ void operator=(const PointIter &other);
+ bool valid() const; // returns if the pointer is pointing to a cell
+
+ bool left(bool check_empty=false); // moves one corner to the left or returns false
+ bool right(bool check_empty=false); // moves one corner to the right or returns false
+ bool bottom(bool check_empty=false); // moves one corner to the bottom or returns false
+ bool top(bool check_empty=false); // moves one corner to the top or returns false
+ bool checkCorner()const; // returns true if the current corner belongs to at least one
+ // none empty cell
+ bool isNaN()const; // returns true if the currnet corner is NaN
+
+ const cv::Point2f* operator*() const; // current corner coordinate
+ cv::Point2f* operator*(); // current corner coordinate
+ const cv::Point2f* operator->() const; // current corner coordinate
+ cv::Point2f* operator->(); // current corner coordinate
+
+ Cell *getCell(); // current cell
+ private:
+ CornerIndex corner_index;
+ Cell *cell;
+ };
+
+ std::vector<Cell*> cells; // storage for all board cells
+ std::vector<cv::Point2f*> corners; // storage for all corners
+ Cell *top_left; // pointer to the top left corner of the board in its local coordinate system
+ int rows; // number of row cells
+ int cols; // number of col cells
+ float white_angle,black_angle;
+ };
+ public:
+
+ /**
+ * \brief Creates a chessboard corner detectors
+ *
+ * \param[in] config Configuration used to detect chessboard corners
+ *
+ */
+ Chessboard(const Parameters &config = Parameters());
+ virtual ~Chessboard();
+ void reconfigure(const Parameters &config = Parameters());
+ Parameters getPara()const;
+
+ /*
+ * \brief Detects chessboard corners in the given image.
+ *
+ * The detectors tries to find all chessboard corners of an imaged
+ * chessboard and returns them as an ordered vector of KeyPoints.
+ * Thereby, the left top corner has index 0 and the bottom right
+ * corner n*m-1.
+ *
+ * \param[in] image The image
+ * \param[out] keypoints The detected corners as a vector of ordered KeyPoints
+ * \param[in] mask Currently not supported
+ *
+ */
+ void detect(cv::InputArray image,std::vector<cv::KeyPoint>& keypoints, cv::InputArray mask=cv::Mat())override
+ {cv::Feature2D::detect(image.getMat(),keypoints,mask.getMat());}
+
+ virtual void detectAndCompute(cv::InputArray image,cv::InputArray mask, std::vector<cv::KeyPoint>& keypoints,cv::OutputArray descriptors,
+ bool useProvidedKeyPoints = false)override;
+
+ /*
+ * \brief Detects chessboard corners in the given image.
+ *
+ * The detectors tries to find all chessboard corners of an imaged
+ * chessboard and returns them as an ordered vector of KeyPoints.
+ * Thereby, the left top corner has index 0 and the bottom right
+ * corner n*m-1.
+ *
+ * \param[in] image The image
+ * \param[out] keypoints The detected corners as a vector of ordered KeyPoints
+ * \param[out] feature_maps The feature map generated by LRJT and used to find the corners
+ * \param[in] mask Currently not supported
+ *
+ */
+ void detectImpl(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints,std::vector<cv::Mat> &feature_maps,const cv::Mat& mask)const;
+ Chessboard::Board detectImpl(const cv::Mat& image,std::vector<cv::Mat> &feature_maps,const cv::Mat& mask)const;
+
+ // define pure virtual methods
+ virtual int descriptorSize()const override{return 0;};
+ virtual int descriptorType()const override{return 0;};
+ virtual void operator()( cv::InputArray image, cv::InputArray mask, std::vector<cv::KeyPoint>& keypoints, cv::OutputArray descriptors, bool useProvidedKeypoints=false )const
+ {
+ descriptors.clear();
+ detectImpl(image.getMat(),keypoints,mask);
+ if(!useProvidedKeypoints) // suppress compiler warning
+ return;
+ return;
+ }
+
+ protected:
+ virtual void computeImpl( const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, cv::Mat& descriptors)const
+ {
+ descriptors = cv::Mat();
+ detectImpl(image,keypoints);
+ }
+
+ // indicates why a board could not be initialized for a certain keypoint
+ enum BState
+ {
+ MISSING_POINTS = 0, // at least 5 points are needed
+ MISSING_PAIRS = 1, // at least two pairs are needed
+ WRONG_PAIR_ANGLE = 2, // angle between pairs is too small
+ WRONG_CONFIGURATION = 3, // point configuration is wrong and does not belong to a board
+ FOUND_BOARD = 4 // board was found
+ };
+
+ void findKeyPoints(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints,std::vector<cv::Mat> &feature_maps,
+ std::vector<std::vector<float> > &angles ,const cv::Mat& mask)const;
+ cv::Mat buildData(const std::vector<cv::KeyPoint>& keypoints)const;
+ std::vector<cv::KeyPoint> getInitialPoints(cv::flann::Index &flann_index,const cv::Mat &data,const cv::KeyPoint ¢er,float white_angle,float black_angle, float min_response = 0)const;
+ BState generateBoards(cv::flann::Index &flann_index,const cv::Mat &data, const cv::KeyPoint ¢er,
+ float white_angle,float black_angle,float min_response,const cv::Mat &img,
+ std::vector<Chessboard::Board> &boards)const;
+
+ private:
+ void detectImpl(const cv::Mat&,std::vector<cv::KeyPoint>&, const cv::Mat& mast =cv::Mat())const;
+ virtual void detectImpl(cv::InputArray image, std::vector<cv::KeyPoint>& keypoints, cv::InputArray mask=cv::noArray())const;
+
+ private:
+ Parameters parameters; // storing the configuration of the detector
+};
+}} // end namespace details and cv
+
+#endif
return elem_type;
}
-void icvWriteCollection( CvFileStorage* fs, const CvFileNode* node )
+}
+
+#if defined __i386__ || defined(_M_IX86) || defined __x86_64__ || defined(_M_X64)
+#define CV_UNALIGNED_LITTLE_ENDIAN_MEM_ACCESS 1
+#else
+#define CV_UNALIGNED_LITTLE_ENDIAN_MEM_ACCESS 0
+#endif
+
+static inline int readInt(const uchar* p)
+{
+#if CV_UNALIGNED_LITTLE_ENDIAN_MEM_ACCESS
+ return *(const int*)p;
+#else
+ int val = (int)(p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24));
+ return val;
+#endif
+}
+
+static inline double readReal(const uchar* p)
+{
+#if CV_UNALIGNED_LITTLE_ENDIAN_MEM_ACCESS
+ return *(const double*)p;
+#else
+ unsigned val0 = (unsigned)(p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24));
+ unsigned val1 = (unsigned)(p[4] | (p[5] << 8) | (p[6] << 16) | (p[7] << 24));
+ Cv64suf val;
+ val.u = val0 | ((uint64)val1 << 32);
+ return val.f;
+#endif
+}
+
+static inline void writeInt(uchar* p, int ival)
+{
+#if CV_UNALIGNED_LITTLE_ENDIAN_MEM_ACCESS
+ int* ip = (int*)p;
+ *ip = ival;
+#else
+ p[0] = (uchar)ival;
+ p[1] = (uchar)(ival >> 8);
+ p[2] = (uchar)(ival >> 16);
+ p[3] = (uchar)(ival >> 24);
+#endif
+}
+
+static inline void writeReal(uchar* p, double fval)
{
- int i, total = node->data.seq->total;
- int elem_size = node->data.seq->elem_size;
- int is_map = CV_NODE_IS_MAP(node->tag);
- CvSeqReader reader;
+#if CV_UNALIGNED_LITTLE_ENDIAN_MEM_ACCESS
+ double* fp = (double*)p;
+ *fp = fval;
+#else
+ Cv64suf v;
+ v.f = fval;
+ p[0] = (uchar)v.u;
+ p[1] = (uchar)(v.u >> 8);
+ p[2] = (uchar)(v.u >> 16);
+ p[3] = (uchar)(v.u >> 24);
+ p[4] = (uchar)(v.u >> 32);
+ p[5] = (uchar)(v.u >> 40);
+ p[6] = (uchar)(v.u >> 48);
+ p[7] = (uchar)(v.u >> 56);
+#endif
+}
+
+class FileStorage::Impl : public FileStorage_API
+{
+public:
+ void init()
+ {
+ flags = 0;
+ buffer.clear();
+ bufofs = 0;
+ state = UNDEFINED;
+ is_opened = false;
+ dummy_eof = false;
+ write_mode = false;
+ mem_mode = false;
+ space = 0;
+ wrap_margin = 71;
+ fmt = 0;
+ file = 0;
+ gzfile = 0;
+ empty_stream = true;
+
+ strbufv.clear();
+ strbuf = 0;
+ strbufsize = strbufpos = 0;
+ roots.clear();
+
+ fs_data.clear();
+ fs_data_ptrs.clear();
+ fs_data_blksz.clear();
+ freeSpaceOfs = 0;
+
+ str_hash.clear();
+ str_hash_data.clear();
+ str_hash_data.resize(1);
+ str_hash_data[0] = '\0';
+
+ filename.clear();
+ lineno = 0;
+ }
+
+ Impl(FileStorage* _fs)
+ {
+ fs_ext = _fs;
+ init();
+ }
- cvStartReadSeq( node->data.seq, &reader, 0 );
+ virtual ~Impl()
+ {
+ release();
+ }
- for( i = 0; i < total; i++ )
+ void release(String* out=0)
{
- CvFileMapNode* elem = (CvFileMapNode*)reader.ptr;
- if( !is_map || CV_IS_SET_ELEM(elem) )
+ if( is_opened )
{
- const char* name = is_map ? elem->key->str.ptr : 0;
- icvWriteFileNode( fs, name, &elem->value );
+ if(out)
+ out->clear();
+ if( write_mode )
+ {
+ while( write_stack.size() > 1 )
+ {
+ endWriteStruct();
+ }
+ flush();
+ if( fmt == FileStorage::FORMAT_XML )
+ puts( "</opencv_storage>\n" );
+ else if ( fmt == FileStorage::FORMAT_JSON )
+ puts( "}\n" );
+ }
+
+ closeFile();
+ if( mem_mode && out )
+ {
+ *out = cv::String(outbuf.begin(), outbuf.end());
+ }
+ init();
}
- CV_NEXT_SEQ_ELEM( elem_size, reader );
}
-}
-void icvWriteFileNode( CvFileStorage* fs, const char* name, const CvFileNode* node )
-{
- switch( CV_NODE_TYPE(node->tag) )
+ void analyze_file_name( const std::string& file_name, std::vector<std::string>& params )
{
- case CV_NODE_INT:
- fs->write_int( fs, name, node->data.i );
- break;
- case CV_NODE_REAL:
- fs->write_real( fs, name, node->data.f );
- break;
- case CV_NODE_STR:
- fs->write_string( fs, name, node->data.str.ptr, 0 );
- break;
- case CV_NODE_SEQ:
- case CV_NODE_MAP:
- cvStartWriteStruct( fs, name, CV_NODE_TYPE(node->tag) +
- (CV_NODE_SEQ_IS_SIMPLE(node->data.seq) ? CV_NODE_FLOW : 0),
- node->info ? node->info->type_name : 0 );
- icvWriteCollection( fs, node );
- cvEndWriteStruct( fs );
- break;
- case CV_NODE_NONE:
- cvStartWriteStruct( fs, name, CV_NODE_SEQ, 0 );
- cvEndWriteStruct( fs );
- break;
- default:
- CV_Error( CV_StsBadFlag, "Unknown type of file node" );
+ params.clear();
+ static const char not_file_name = '\n';
+ static const char parameter_begin = '?';
+ static const char parameter_separator = '&';
+
+ if( file_name.find(not_file_name, (size_t)0) != std::string::npos )
+ return;
+
+ size_t beg = file_name.find_last_of(parameter_begin);
+ params.push_back(file_name.substr((size_t)0, beg));
+
+ if( beg != std::string::npos )
+ {
+ size_t end = file_name.size();
+ beg++;
+ for( size_t param_beg = beg, param_end = beg;
+ param_end < end;
+ param_beg = param_end + 1 )
+ {
+ param_end = file_name.find_first_of( parameter_separator, param_beg );
+ if( (param_end == std::string::npos || param_end != param_beg) && param_beg + 1 < end )
+ {
+ params.push_back( file_name.substr( param_beg, param_end - param_beg ) );
+ }
+ }
+ }
}
+
+ bool open( const char* filename_or_buf, int _flags, const char* encoding )
+ {
+ _flags &= ~FileStorage::BASE64;
+
+ bool ok = true;
+ release();
+
+ bool append = (_flags & 3) == FileStorage::APPEND;
+ mem_mode = (_flags & FileStorage::MEMORY) != 0;
+
+ write_mode = (_flags & 3) != 0;
+
+ bool isGZ = false;
+ size_t fnamelen = 0;
+
+ std::vector<std::string> params;
+ //if ( !mem_mode )
+ {
+ analyze_file_name( filename_or_buf, params );
+ if( !params.empty() )
+ filename = params[0];
+
+ /*if( !write_base64 && params.size() >= 2 &&
+ std::find(params.begin()+1, params.end(), std::string("base64")) != params.end())
+ write_base64 = (write_mode || append);*/
+ }
+
+ if( filename.size() == 0 && !mem_mode && !write_mode )
+ CV_Error( CV_StsNullPtr, "NULL or empty filename" );
+
+ if( mem_mode && append )
+ CV_Error( CV_StsBadFlag, "FileStorage::APPEND and FileStorage::MEMORY are not currently compatible" );
+
+ flags = _flags;
+
+ if( !mem_mode )
+ {
+ char* dot_pos = strrchr((char*)filename.c_str(), '.');
+ char compression = '\0';
+
+ if( dot_pos && dot_pos[1] == 'g' && dot_pos[2] == 'z' &&
+ (dot_pos[3] == '\0' || (cv_isdigit(dot_pos[3]) && dot_pos[4] == '\0')) )
+ {
+ if( append )
+ {
+ CV_Error(CV_StsNotImplemented, "Appending data to compressed file is not implemented" );
+ }
+ isGZ = true;
+ compression = dot_pos[3];
+ if( compression )
+ dot_pos[3] = '\0', fnamelen--;
+ }
+
+ if( !isGZ )
+ {
+ file = fopen(filename.c_str(), !write_mode ? "rt" : !append ? "wt" : "a+t" );
+ if( !file )
+ return false;
+ }
+ else
+ {
+#if USE_ZLIB
+ char mode[] = { write_mode ? 'w' : 'r', 'b', compression ? compression : '3', '\0' };
+ gzfile = gzopen(filename.c_str(), mode);
+ if( !gzfile )
+ return false;
+#else
+ CV_Error(CV_StsNotImplemented, "There is no compressed file storage support in this configuration");
+#endif
+ }
+ }
+
+ roots.clear();
+ fs_data.clear();
+ wrap_margin = 71;
+ fmt = FileStorage::FORMAT_AUTO;
+
+ if( write_mode )
+ {
+ fmt = flags & FileStorage::FORMAT_MASK;
+
+ if( mem_mode )
+ outbuf.clear();
+
+ if( fmt == FileStorage::FORMAT_AUTO && !filename.empty() )
+ {
+ const char* dot_pos = NULL;
+ const char* dot_pos2 = NULL;
+ // like strrchr() implementation, but save two last positions simultaneously
+ for (const char* pos = &filename[0]; pos[0] != 0; pos++)
+ {
+ if( pos[0] == '.' )
+ {
+ dot_pos2 = dot_pos;
+ dot_pos = pos;
+ }
+ }
+ if (fs::strcasecmp(dot_pos, ".gz") == 0 && dot_pos2 != NULL)
+ {
+ dot_pos = dot_pos2;
+ }
+ fmt = (fs::strcasecmp(dot_pos, ".xml") == 0 || fs::strcasecmp(dot_pos, ".xml.gz") == 0 )
+ ? FileStorage::FORMAT_XML
+ : (fs::strcasecmp(dot_pos, ".json") == 0 || fs::strcasecmp(dot_pos, ".json.gz") == 0)
+ ? FileStorage::FORMAT_JSON
+ : FileStorage::FORMAT_YAML;
+ }
+ else if( fmt == FileStorage::FORMAT_AUTO )
+ {
+ fmt = FileStorage::FORMAT_XML;
+ }
+
+ // we use factor=6 for XML (the longest characters (' and ") are encoded with 6 bytes (' and ")
+ // and factor=4 for YAML ( as we use 4 bytes for non ASCII characters (e.g. \xAB))
+ int buf_size = CV_FS_MAX_LEN*(fmt == FileStorage::FORMAT_XML ? 6 : 4) + 1024;
+
+ if (append)
+ {
+ fseek( file, 0, SEEK_END );
+ if (ftell(file) == 0)
+ append = false;
+ }
+
+ write_stack.clear();
+ empty_stream = true;
+ write_stack.push_back(FStructData("", FileNode::MAP | FileNode::EMPTY, 0));
+ buffer.reserve(buf_size + 1024);
+ buffer.resize(buf_size);
+ bufofs = 0;
+
+ if( fmt == FileStorage::FORMAT_XML )
+ {
+ size_t file_size = file ? (size_t)ftell(file) : (size_t)0;
+ if( !append || file_size == 0 )
+ {
+ if( encoding && *encoding != '\0' )
+ {
+ if( fs::strcasecmp(encoding, "UTF-16" ) == 0 )
+ {
+ release();
+ CV_Error( CV_StsBadArg, "UTF-16 XML encoding is not supported! Use 8-bit encoding\n");
+ }
+
+ CV_Assert( strlen(encoding) < 1000 );
+ char buf[1100];
+ sprintf(buf, "<?xml version=\"1.0\" encoding=\"%s\"?>\n", encoding);
+ puts( buf );
+ }
+ else
+ puts( "<?xml version=\"1.0\"?>\n" );
+ puts( "<opencv_storage>\n" );
+ }
+ else
+ {
+ int xml_buf_size = 1 << 10;
+ char substr[] = "</opencv_storage>";
+ int last_occurrence = -1;
+ xml_buf_size = MIN(xml_buf_size, int(file_size));
+ fseek( file, -xml_buf_size, SEEK_END );
+ std::vector<char> xml_buf_(xml_buf_size+2);
+ // find the last occurrence of </opencv_storage>
+ for(;;)
+ {
+ int line_offset = (int)ftell( file );
+ const char* ptr0 = this->gets(&xml_buf_[0], xml_buf_size );
+ const char* ptr = NULL;
+ if( !ptr0 )
+ break;
+ ptr = ptr0;
+ for(;;)
+ {
+ ptr = strstr( ptr, substr );
+ if( !ptr )
+ break;
+ last_occurrence = line_offset + (int)(ptr - ptr0);
+ ptr += strlen(substr);
+ }
+ }
+ if( last_occurrence < 0 )
+ {
+ release();
+ CV_Error( CV_StsError, "Could not find </opencv_storage> in the end of file.\n" );
+ }
+ closeFile();
+ file = fopen( filename.c_str(), "r+t" );
+ CV_Assert(file != 0);
+ fseek( file, last_occurrence, SEEK_SET );
+ // replace the last "</opencv_storage>" with " <!-- resumed -->", which has the same length
+ puts( " <!-- resumed -->" );
+ fseek( file, 0, SEEK_END );
+ puts( "\n" );
+ }
+
+ emitter = createXMLEmitter(this);
+ }
+ else if( fmt == FileStorage::FORMAT_YAML )
+ {
+ if( !append)
+ puts( "%YAML:1.0\n---\n" );
+ else
+ puts( "...\n---\n" );
+
+ emitter = createYAMLEmitter(this);
+ }
+ else
+ {
+ CV_Assert( fmt == FileStorage::FORMAT_JSON );
+ if( !append )
+ puts( "{\n" );
+ else
+ {
+ bool valid = false;
+ long roffset = 0;
+ for ( ;
+ fseek( file, roffset, SEEK_END ) == 0;
+ roffset -= 1 )
+ {
+ const char end_mark = '}';
+ if ( fgetc( file ) == end_mark )
+ {
+ fseek( file, roffset, SEEK_END );
+ valid = true;
+ break;
+ }
+ }
+
+ if ( valid )
+ {
+ closeFile();
+ file = fopen( filename.c_str(), "r+t" );
+ CV_Assert(file != 0);
+ fseek( file, roffset, SEEK_END );
+ fputs( ",", file );
+ }
+ else
+ {
+ CV_Error( CV_StsError, "Could not find '}' in the end of file.\n" );
+ }
+ }
+ write_stack.back().indent = 4;
+ emitter = createJSONEmitter(this);
+ }
+ is_opened = true;
+ }
+ else
+ {
+ const size_t buf_size0 = 1 << 20;
+ size_t buf_size = buf_size0;
+ if( mem_mode )
+ {
+ strbuf = (char*)filename_or_buf;
+ strbufsize = strlen(strbuf);
+ }
+
+ const char* yaml_signature = "%YAML";
+ const char* json_signature = "{";
+ const char* xml_signature = "<?xml";
+ char buf[16];
+ this->gets( buf, sizeof(buf)-2 );
+ char* bufPtr = cv_skip_BOM(buf);
+ size_t bufOffset = bufPtr - buf;
+
+ if(strncmp( bufPtr, yaml_signature, strlen(yaml_signature) ) == 0)
+ fmt = FileStorage::FORMAT_YAML;
+ else if(strncmp( bufPtr, json_signature, strlen(json_signature) ) == 0)
+ fmt = FileStorage::FORMAT_JSON;
+ else if(strncmp( bufPtr, xml_signature, strlen(xml_signature) ) == 0)
+ fmt = FileStorage::FORMAT_XML;
+ else if(strbufsize == bufOffset)
+ CV_Error(CV_BADARG_ERR, "Input file is invalid");
+ else
+ CV_Error(CV_BADARG_ERR, "Unsupported file storage format");
+
+ if( !isGZ )
+ {
+ if( !mem_mode )
+ {
+ fseek( file, 0, SEEK_END );
+ buf_size = ftell( file );
+ }
+ else
+ buf_size = strbufsize;
+ buf_size = MIN( buf_size, buf_size0 );
+ buf_size = MAX( buf_size, (size_t)(CV_FS_MAX_LEN*6 + 1024) );
+ }
+ rewind();
+ strbufpos = bufOffset;
+
+ buffer.reserve(buf_size + 256);
+ buffer.resize(buf_size);
+ bufofs = 0;
+
+ try
+ {
+ char* ptr = bufferStart();
+ ptr[0] = ptr[1] = ptr[2] = '\0';
+ FileNode root_nodes(fs_ext, 0, 0);
+
+ uchar* rptr = reserveNodeSpace(root_nodes, 9);
+ *rptr = FileNode::SEQ;
+ writeInt(rptr + 1, 4);
+ writeInt(rptr + 5, 0);
+
+ roots.clear();
+
+ switch (fmt)
+ {
+ case FileStorage::FORMAT_XML: parser = createXMLParser(this); break;
+ case FileStorage::FORMAT_YAML: parser = createYAMLParser(this); break;
+ case FileStorage::FORMAT_JSON: parser = createJSONParser(this); break;
+ default: parser = Ptr<FileStorageParser>();
+ }
+
+ if( !parser.empty() )
+ {
+ ok = parser->parse(ptr);
+ if( ok )
+ {
+ finalizeCollection(root_nodes);
+
+ CV_Assert( !fs_data_ptrs.empty() );
+ FileNode roots_node(fs_ext, 0, 0);
+ size_t i, nroots = roots_node.size();
+ FileNodeIterator it = roots_node.begin();
+
+ for( i = 0; i < nroots; i++, ++it )
+ roots.push_back(*it);
+ }
+ }
+ }
+ catch(...)
+ {
+ is_opened = true;
+ release();
+ throw;
+ }
+
+ // release resources that we do not need anymore
+ closeFile();
+ is_opened = true;
+ std::vector<char> tmpbuf;
+ std::swap(buffer, tmpbuf);
+ bufofs = 0;
+ }
+ return ok;
+ }
+
+ void puts( const char* str )
+ {
+ CV_Assert( write_mode );
+ if( mem_mode )
+ std::copy(str, str + strlen(str), std::back_inserter(outbuf));
+ else if( file )
+ fputs( str, file );
+#if USE_ZLIB
+ else if( gzfile )
+ gzputs( gzfile, str );
+#endif
+ else
+ CV_Error( CV_StsError, "The storage is not opened" );
+ }
+
+ char* gets( char* str, int maxCount )
+ {
+ if( strbuf )
+ {
+ size_t i = strbufpos, len = strbufsize;
+ int j = 0;
+ const char* instr = strbuf;
+ while( i < len && j < maxCount-1 )
+ {
+ char c = instr[i++];
+ if( c == '\0' )
+ break;
+ str[j++] = c;
+ if( c == '\n' )
+ break;
+ }
+ str[j++] = '\0';
+ strbufpos = i;
+ if (maxCount > 256 && !(flags & cv::FileStorage::BASE64))
+ CV_Assert(j < maxCount - 1 && "OpenCV persistence doesn't support very long lines");
+ return j > 1 ? str : 0;
+ }
+ if( file )
+ {
+ char* ptr = fgets( str, maxCount, file );
+ if (ptr && maxCount > 256 && !(flags & cv::FileStorage::BASE64))
+ {
+ size_t sz = strnlen(ptr, maxCount);
+ CV_Assert(sz < (size_t)(maxCount - 1) && "OpenCV persistence doesn't support very long lines");
+ }
+ return ptr;
+ }
+#if USE_ZLIB
+ if( gzfile )
+ {
+ char* ptr = gzgets( gzfile, str, maxCount );
+ if (ptr && maxCount > 256 && !(flags & cv::FileStorage::BASE64))
+ {
+ size_t sz = strnlen(ptr, maxCount);
+ CV_Assert(sz < (size_t)(maxCount - 1) && "OpenCV persistence doesn't support very long lines");
+ }
+ return ptr;
+ }
+#endif
+ CV_Error(CV_StsError, "The storage is not opened");
+ }
+
+ char* gets()
+ {
+ char* ptr = this->gets(bufferStart(), (int)(bufferEnd() - bufferStart()));
+ if( !ptr )
+ {
+ ptr = bufferStart(); // FIXIT Why do we need this hack? What is about other parsers JSON/YAML?
+ *ptr = '\0';
+ setEof();
+ return 0;
+ }
+ else
+ {
+ FileStorage_API* fs = this;
+ int l = (int)strlen(ptr);
+ if( l > 0 && ptr[l-1] != '\n' && ptr[l-1] != '\r' && !eof() )
+ CV_PARSE_ERROR_CPP("Too long string or a last string w/o newline");
+ }
+ lineno++;
+ return ptr;
+ }
+
+ bool eof()
+ {
+ if( dummy_eof )
+ return true;
+ if( strbuf )
+ return strbufpos >= strbufsize;
+ if( file )
+ return feof(file) != 0;
+#if USE_ZLIB
+ if( gzfile )
+ return gzeof(gzfile) != 0;
+#endif
+ return false;
+ }
+
+ void setEof()
+ {
+ dummy_eof = true;
+ }
+
+ void closeFile()
+ {
+ if( file )
+ fclose( file );
+#if USE_ZLIB
+ else if( gzfile )
+ gzclose( gzfile );
+#endif
+ file = 0;
+ gzfile = 0;
+ strbuf = 0;
+ strbufpos = 0;
+ is_opened = false;
+ }
+
+ void rewind()
+ {
+ if( file )
+ ::rewind(file);
+#if USE_ZLIB
+ else if( gzfile )
+ gzrewind(gzfile);
+#endif
+ strbufpos = 0;
+ }
+
+ char* resizeWriteBuffer( char* ptr, int len )
+ {
+ const char* buffer_end = &buffer[0] + buffer.size();
+ if( ptr + len < buffer_end )
+ return ptr;
+
+ const char* buffer_start = &buffer[0];
+ int written_len = (int)(ptr - buffer_start);
+
+ CV_Assert(written_len <= (int)buffer.size());
+ int new_size = (int)((buffer_end - buffer_start)*3/2);
+ new_size = MAX( written_len + len, new_size );
+ buffer.reserve( new_size + 256 );
+ buffer.resize( new_size );
+ bufofs = written_len;
+ return &buffer[0] + bufofs;
+ }
+
+ char* flush()
+ {
+ char* buffer_start = &buffer[0];
+ char* ptr = buffer_start + bufofs;
+
+ if( ptr > buffer_start + space )
+ {
+ ptr[0] = '\n';
+ ptr[1] = '\0';
+ puts( buffer_start );
+ bufofs = 0;
+ }
+
+ int indent = write_stack.back().indent;
+
+ if( space != indent )
+ {
+ memset( buffer_start, ' ', indent );
+ space = indent;
+ }
+ bufofs = space;
+ ptr = buffer_start + bufofs;
+
+ return ptr;
+ }
+
+ void endWriteStruct()
+ {
+ CV_Assert( write_mode );
+ CV_Assert( !write_stack.empty() );
+
+ FStructData& current_struct = write_stack.back();
+ if( fmt == FileStorage::FORMAT_JSON && !FileNode::isFlow(current_struct.flags) && write_stack.size() > 1 )
+ current_struct.indent = write_stack[write_stack.size() - 2].indent;
+
+ emitter->endWriteStruct(current_struct);
+
+ write_stack.pop_back();
+ if( !write_stack.empty() )
+ write_stack.back().flags &= ~FileNode::EMPTY;
+ }
+
+ void startWriteStruct( const char* key, int struct_flags,
+ const char* type_name )
+ {
+ CV_Assert( write_mode );
+
+ struct_flags = (struct_flags & (FileNode::TYPE_MASK|FileNode::FLOW)) | FileNode::EMPTY;
+ if( !FileNode::isCollection(struct_flags))
+ CV_Error( CV_StsBadArg,
+ "Some collection type: FileNode::SEQ or FileNode::MAP must be specified" );
+
+ if( type_name && type_name[0] == '\0' )
+ type_name = 0;
+
+ FStructData s = emitter->startWriteStruct( write_stack.back(), key, struct_flags, type_name );
+ write_stack.push_back(s);
+ size_t write_stack_size = write_stack.size();
+ if( write_stack_size > 1 )
+ write_stack[write_stack_size-2].flags &= ~FileNode::EMPTY;
+
+ if( !FileNode::isFlow(s.flags) )
+ flush();
+
+ if( fmt == FileStorage::FORMAT_JSON && type_name && type_name[0] && FileNode::isMap(struct_flags))
+ {
+ emitter->write("type_id", type_name, false);
+ }
+ }
+
+ void writeComment( const char* comment, bool eol_comment )
+ {
+ CV_Assert(write_mode);
+ emitter->writeComment( comment, eol_comment );
+ }
+
+ void startNextStream()
+ {
+ CV_Assert(write_mode);
+ if( !empty_stream )
+ {
+ while( !write_stack.empty() )
+ endWriteStruct();
+ flush();
+ emitter->startNextStream();
+ empty_stream = true;
+ write_stack.push_back(FStructData("", FileNode::EMPTY, 0));
+ bufofs = 0;
+ }
+ }
+
+ void write( const String& key, int value )
+ {
+ CV_Assert(write_mode);
+ emitter->write(key.c_str(), value);
+ }
+
+ void write( const String& key, double value )
+ {
+ CV_Assert(write_mode);
+ emitter->write(key.c_str(), value);
+ }
+
+ void write( const String& key, const String& value )
+ {
+ CV_Assert(write_mode);
+ emitter->write(key.c_str(), value.c_str(), false);
+ }
+
+ void writeRawData( const std::string& dt, const void* _data, size_t len )
+ {
+ CV_Assert(write_mode);
+
+ size_t elemSize = fs::calcStructSize(dt.c_str(), 0);
+ CV_Assert( len % elemSize == 0 );
+ len /= elemSize;
+
+ bool explicitZero = fmt == FileStorage::FORMAT_JSON;
+ const uchar* data0 = (const uchar*)_data;
+ int fmt_pairs[CV_FS_MAX_FMT_PAIRS*2], k, fmt_pair_count;
+ char buf[256] = "";
+
+ fmt_pair_count = fs::decodeFormat( dt.c_str(), fmt_pairs, CV_FS_MAX_FMT_PAIRS );
+
+ if( !len )
+ return;
+
+ if( !data0 )
+ CV_Error( CV_StsNullPtr, "Null data pointer" );
+
+ if( fmt_pair_count == 1 )
+ {
+ fmt_pairs[0] *= (int)len;
+ len = 1;
+ }
+
+ for(;len--; data0 += elemSize)
+ {
+ int offset = 0;
+ for( k = 0; k < fmt_pair_count; k++ )
+ {
+ int i, count = fmt_pairs[k*2];
+ int elem_type = fmt_pairs[k*2+1];
+ int elem_size = CV_ELEM_SIZE(elem_type);
+ const char *ptr;
+
+ offset = cvAlign( offset, elem_size );
+ const uchar* data = data0 + offset;
+
+ for( i = 0; i < count; i++ )
+ {
+ switch( elem_type )
+ {
+ case CV_8U:
+ ptr = fs::itoa( *(uchar*)data, buf, 10 );
+ data++;
+ break;
+ case CV_8S:
+ ptr = fs::itoa( *(char*)data, buf, 10 );
+ data++;
+ break;
+ case CV_16U:
+ ptr = fs::itoa( *(ushort*)data, buf, 10 );
+ data += sizeof(ushort);
+ break;
+ case CV_16S:
+ ptr = fs::itoa( *(short*)data, buf, 10 );
+ data += sizeof(short);
+ break;
+ case CV_32S:
+ ptr = fs::itoa( *(int*)data, buf, 10 );
+ data += sizeof(int);
+ break;
+ case CV_32F:
+ ptr = fs::floatToString( buf, *(float*)data, false, explicitZero );
+ data += sizeof(float);
+ break;
+ case CV_64F:
+ ptr = fs::doubleToString( buf, *(double*)data, explicitZero );
+ data += sizeof(double);
+ break;
+ case CV_16F: /* reference */
+ ptr = fs::floatToString( buf, (float)*(float16_t*)data, true, explicitZero );
+ data += sizeof(float16_t);
+ break;
+ default:
+ CV_Error( CV_StsUnsupportedFormat, "Unsupported type" );
+ return;
+ }
+
+ emitter->writeScalar(0, ptr);
+ }
+
+ offset = (int)(data - data0);
+ }
+ }
+ }
+
+ void writeRawDataBase64(const void* /*data*/, int /*len*/, const char* /*dt*/ )
+ {
+
+ }
+
+ String releaseAndGetString();
+
+ FileNode getFirstTopLevelNode() const
+ {
+ return roots.empty() ? FileNode() : roots[0];
+ }
+
+ FileNode root(int streamIdx=0) const
+ {
+ return streamIdx >= 0 && streamIdx < (int)roots.size() ? roots[streamIdx] : FileNode();
+ }
+
+ FileNode operator[](const String& nodename) const
+ {
+ return this->operator[](nodename.c_str());
+ }
+
+ FileNode operator[](const char* /*nodename*/) const
+ {
+ return FileNode();
+ }
+
+ int getFormat() const { return fmt; }
+
+ char* bufferPtr() const { return (char*)(&buffer[0] + bufofs); }
+ char* bufferStart() const { return (char*)&buffer[0]; }
+ char* bufferEnd() const { return (char*)(&buffer[0] + buffer.size()); }
+ void setBufferPtr(char* ptr)
+ {
+ char* bufferstart = bufferStart();
+ CV_Assert( ptr >= bufferstart && ptr <= bufferEnd() );
+ bufofs = ptr - bufferstart;
+ }
+ int wrapMargin() const { return wrap_margin; }
+
+ FStructData& getCurrentStruct()
+ {
+ CV_Assert(!write_stack.empty());
+ return write_stack.back();
+ }
+
+ void setNonEmpty()
+ {
+ empty_stream = false;
+ }
+
+ void processSpecialDouble( char* buf, double* value, char** endptr )
+ {
+ FileStorage_API* fs = this;
+ char c = buf[0];
+ int inf_hi = 0x7ff00000;
+
+ if( c == '-' || c == '+' )
+ {
+ inf_hi = c == '-' ? 0xfff00000 : 0x7ff00000;
+ c = *++buf;
+ }
+
+ if( c != '.' )
+ CV_PARSE_ERROR_CPP( "Bad format of floating-point constant" );
+
+ Cv64suf v;
+ v.f = 0.;
+ if( toupper(buf[1]) == 'I' && toupper(buf[2]) == 'N' && toupper(buf[3]) == 'F' )
+ v.u = (uint64)inf_hi << 32;
+ else if( toupper(buf[1]) == 'N' && toupper(buf[2]) == 'A' && toupper(buf[3]) == 'N' )
+ v.u = (uint64)-1;
+ else
+ CV_PARSE_ERROR_CPP( "Bad format of floating-point constant" );
+ *value = v.f;
+ *endptr = buf + 4;
+ }
+
+ double strtod( char* ptr, char** endptr )
+ {
+ double fval = ::strtod( ptr, endptr );
+ if( **endptr == '.' )
+ {
+ char* dot_pos = *endptr;
+ *dot_pos = ',';
+ double fval2 = ::strtod( ptr, endptr );
+ *dot_pos = '.';
+ if( *endptr > dot_pos )
+ fval = fval2;
+ else
+ *endptr = dot_pos;
+ }
+
+ if( *endptr == ptr || cv_isalpha(**endptr) )
+ processSpecialDouble( ptr, &fval, endptr );
+
+ return fval;
+ }
+
+ void convertToCollection(int type, FileNode& node)
+ {
+ CV_Assert( type == FileNode::SEQ || type == FileNode::MAP );
+
+ int node_type = node.type();
+ if( node_type == type )
+ return;
+
+ bool named = node.isNamed();
+ uchar* ptr = node.ptr() + 1 + (named ? 4 : 0);
+
+ int ival = 0;
+ double fval = 0;
+ std::string sval;
+ bool add_first_scalar = false;
+
+ if( node_type != FileNode::NONE )
+ {
+ // scalar nodes can only be converted to sequences, e.g. in XML:
+ // <a>5[parser_position]... => create 5 with name "a"
+ // <a>5 6[parser_position]... => 5 is converted to [5] and then 6 is added to it
+ //
+ // otherwise we don't know where to get the element names from
+ CV_Assert( type == FileNode::SEQ );
+ if( node_type == FileNode::INT )
+ {
+ ival = readInt(ptr);
+ add_first_scalar = true;
+ }
+ else if( node_type == FileNode::REAL )
+ {
+ fval = readReal(ptr);
+ add_first_scalar = true;
+ }
+ else if( node_type == FileNode::STRING )
+ {
+ sval = std::string(node);
+ add_first_scalar = true;
+ }
+ else
+ CV_Error_(Error::StsError, ("The node of type %d cannot be converted to collection", node_type));
+ }
+
+ ptr = reserveNodeSpace(node, 1 + (named ? 4 : 0) + 4 + 4);
+ *ptr++ = (uchar)(type | (named ? FileNode::NAMED : 0));
+ // name has been copied automatically
+ if( named )
+ ptr += 4;
+ // set raw_size(collection)==4, nelems(collection)==1
+ writeInt(ptr, 4);
+ writeInt(ptr + 4, 0);
+
+ if( add_first_scalar )
+ addNode(node, std::string(), node_type,
+ node_type == FileNode::INT ? (const void*)&ival :
+ node_type == FileNode::REAL ? (const void*)&fval :
+ node_type == FileNode::STRING ? (const void*)sval.c_str() : 0,
+ -1);
+ }
+
+ // a) allocates new FileNode (for that just set blockIdx to the last block and ofs to freeSpaceOfs) or
+ // b) reallocates just created new node (blockIdx and ofs must be taken from FileNode).
+ // If there is no enough space in the current block (it should be the last block added so far),
+ // the last block is shrunk so that it ends immediately before the reallocated node. Then,
+ // a new block of sufficient size is allocated and the FileNode is placed in the beginning of it.
+ // The case (a) can be used to allocate the very first node by setting blockIdx == ofs == 0.
+ // In the case (b) the existing tag and the name are copied automatically.
+ uchar* reserveNodeSpace(FileNode& node, size_t sz)
+ {
+ bool shrinkBlock = false;
+ size_t shrinkBlockIdx = 0, shrinkSize = 0;
+
+ uchar *ptr = 0, *blockEnd = 0;
+
+ if( !fs_data_ptrs.empty() )
+ {
+ size_t blockIdx = node.blockIdx;
+ size_t ofs = node.ofs;
+ CV_Assert( blockIdx == fs_data_ptrs.size()-1 );
+ CV_Assert( ofs <= fs_data_blksz[blockIdx] );
+ CV_Assert( freeSpaceOfs <= fs_data_blksz[blockIdx] );
+ //CV_Assert( freeSpaceOfs <= ofs + sz );
+
+ ptr = fs_data_ptrs[blockIdx] + ofs;
+ blockEnd = fs_data_ptrs[blockIdx] + fs_data_blksz[blockIdx];
+
+ CV_Assert(ptr >= fs_data_ptrs[blockIdx] && ptr <= blockEnd);
+ if( ptr + sz <= blockEnd )
+ {
+ freeSpaceOfs = ofs + sz;
+ return ptr;
+ }
+
+ if (ofs == 0) // FileNode is a first component of this block. Resize current block instead of allocation of new one.
+ {
+ fs_data[blockIdx]->resize(sz);
+ ptr = &fs_data[blockIdx]->at(0);
+ fs_data_ptrs[blockIdx] = ptr;
+ fs_data_blksz[blockIdx] = sz;
+ freeSpaceOfs = sz;
+ return ptr;
+ }
+
+ shrinkBlock = true;
+ shrinkBlockIdx = blockIdx;
+ shrinkSize = ofs;
+ }
+
+ size_t blockSize = std::max((size_t)CV_FS_MAX_LEN*4 - 256, sz) + 256;
+ Ptr<std::vector<uchar> > pv = makePtr<std::vector<uchar> >(blockSize);
+ fs_data.push_back(pv);
+ uchar* new_ptr = &pv->at(0);
+ fs_data_ptrs.push_back(new_ptr);
+ fs_data_blksz.push_back(blockSize);
+ node.blockIdx = fs_data_ptrs.size()-1;
+ node.ofs = 0;
+ freeSpaceOfs = sz;
+
+ if( ptr && ptr + 5 <= blockEnd )
+ {
+ new_ptr[0] = ptr[0];
+ if( ptr[0] & FileNode::NAMED )
+ {
+ new_ptr[1] = ptr[1];
+ new_ptr[2] = ptr[2];
+ new_ptr[3] = ptr[3];
+ new_ptr[4] = ptr[4];
+ }
+ }
+
+ if (shrinkBlock)
+ {
+ fs_data[shrinkBlockIdx]->resize(shrinkSize);
+ fs_data_blksz[shrinkBlockIdx] = shrinkSize;
+ }
+
+ return new_ptr;
+ }
+
+ unsigned getStringOfs( const std::string& key )
+ {
+ str_hash_t::iterator it = str_hash.find(key);
+ return it != str_hash.end() ? it->second : 0;
+ }
+
+ FileNode addNode( FileNode& collection, const std::string& key,
+ int elem_type, const void* value, int len )
+ {
+ FileStorage_API* fs = this;
+ bool noname = key.empty() || (fmt == FileStorage::FORMAT_XML && strcmp(key.c_str(), "_") == 0);
+ convertToCollection( noname ? FileNode::SEQ : FileNode::MAP, collection );
+
+ bool isseq = collection.empty() ? false : collection.isSeq();
+ if( noname != isseq )
+ CV_PARSE_ERROR_CPP( noname ? "Map element should have a name" :
+ "Sequence element should not have name (use <_></_>)" );
+ unsigned strofs = 0;
+ if( !noname )
+ {
+ strofs = getStringOfs(key);
+ if( !strofs )
+ {
+ strofs = (unsigned)str_hash_data.size();
+ size_t keysize = key.size() + 1;
+ str_hash_data.resize(strofs + keysize);
+ memcpy(&str_hash_data[0] + strofs, &key[0], keysize);
+ str_hash.insert(std::make_pair(key, strofs));
+ }
+ }
+
+ uchar* cp = collection.ptr();
+
+ size_t blockIdx = fs_data_ptrs.size() - 1;
+ size_t ofs = freeSpaceOfs;
+ FileNode node(fs_ext, blockIdx, ofs);
+
+ size_t sz0 = 1 + (noname ? 0 : 4) + 8;
+ uchar* ptr = reserveNodeSpace(node, sz0);
+
+ *ptr++ = (uchar)(elem_type | (noname ? 0 : FileNode::NAMED));
+ if( elem_type == FileNode::NONE )
+ freeSpaceOfs -= 8;
+
+ if( !noname )
+ {
+ writeInt(ptr, (int)strofs);
+ ptr += 4;
+ }
+
+ if( elem_type == FileNode::SEQ || elem_type == FileNode::MAP )
+ {
+ writeInt(ptr, 4);
+ writeInt(ptr, 0);
+ }
+
+ if( value )
+ node.setValue(elem_type, value, len);
+
+ if( collection.isNamed() )
+ cp += 4;
+ int nelems = readInt(cp + 5);
+ writeInt(cp + 5, nelems + 1);
+
+ return node;
+ }
+
+ void finalizeCollection( FileNode& collection )
+ {
+ if( !collection.isSeq() && !collection.isMap() )
+ return;
+ uchar* ptr0 = collection.ptr(), *ptr = ptr0 + 1;
+ if( *ptr0 & FileNode::NAMED )
+ ptr += 4;
+ size_t blockIdx = collection.blockIdx;
+ size_t ofs = collection.ofs + (size_t)(ptr + 8 - ptr0);
+ size_t rawSize = 4;
+ unsigned sz = (unsigned)readInt(ptr + 4);
+ if( sz > 0 )
+ {
+ size_t lastBlockIdx = fs_data_ptrs.size() - 1;
+
+ for( ; blockIdx < lastBlockIdx; blockIdx++ )
+ {
+ rawSize += fs_data_blksz[blockIdx] - ofs;
+ ofs = 0;
+ }
+ }
+ rawSize += freeSpaceOfs - ofs;
+ writeInt(ptr, (int)rawSize);
+ }
+
+ void normalizeNodeOfs(size_t& blockIdx, size_t& ofs)
+ {
+ while( ofs >= fs_data_blksz[blockIdx] )
+ {
+ if( blockIdx == fs_data_blksz.size() - 1 )
+ {
+ CV_Assert( ofs == fs_data_blksz[blockIdx] );
+ break;
+ }
+ ofs -= fs_data_blksz[blockIdx];
+ blockIdx++;
+ }
+ }
+
+ class Base64Decoder
+ {
+ public:
+ Base64Decoder() { ofs = 0; ptr = 0; indent = 0; totalchars = 0; eos = true; }
+ void init(Ptr<FileStorageParser>& _parser, char* _ptr, int _indent)
+ {
+ parser = _parser;
+ ptr = _ptr;
+ indent = _indent;
+ encoded.clear();
+ decoded.clear();
+ ofs = 0;
+ totalchars = 0;
+ eos = false;
+ }
+
+ bool readMore(int needed)
+ {
+ static const uchar base64tab[] =
+ {
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 62, 0, 0, 0, 63,
+ 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 0, 0, 0, 0, 0, 0,
+ 0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
+ 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 0, 0, 0, 0, 0,
+ 0, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40,
+ 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+ };
+
+ if( eos )
+ return false;
+
+ size_t sz = decoded.size();
+ CV_Assert( ofs <= sz );
+ sz -= ofs;
+ for( size_t i = 0; i < sz; i++ )
+ decoded[i] = decoded[ofs + i];
+
+ decoded.resize(sz);
+ ofs = 0;
+
+ CV_Assert( !parser.empty() && ptr );
+ char *beg = 0, *end = 0;
+ bool ok = parser->getBase64Row(ptr, indent, beg, end);
+ ptr = end;
+ std::copy(beg, end, std::back_inserter(encoded));
+ totalchars += end - beg;
+
+ if( !ok || beg == end )
+ {
+ // in the end of base64 sequence pad it with '=' characters so that
+ // its total length is multiple of
+ eos = true;
+ size_t tc = totalchars;
+ for( ; tc % 4 != 0; tc++ )
+ encoded.push_back('=');
+ }
+
+ int i = 0, j, n = (int)encoded.size();
+ if( n > 0 )
+ {
+ const uchar* tab = base64tab;
+ char* src = &encoded[0];
+
+ for( ; i <= n - 4; i += 4 )
+ {
+ // dddddd cccccc bbbbbb aaaaaa => ddddddcc ccccbbbb bbaaaaaa
+ uchar d = tab[(int)(uchar)src[i]], c = tab[(int)(uchar)src[i+1]];
+ uchar b = tab[(int)(uchar)src[i+2]], a = tab[(int)(uchar)src[i+3]];
+
+ decoded.push_back((uchar)((d << 2) | (c >> 4)));
+ decoded.push_back((uchar)((c << 4) | (b >> 2)));
+ decoded.push_back((uchar)((b << 6) | a));
+ }
+ }
+
+ if( i > 0 && encoded[i-1] == '=' )
+ {
+ if( i > 1 && encoded[i-2] == '=' && !decoded.empty() )
+ decoded.pop_back();
+ if( !decoded.empty() )
+ decoded.pop_back();
+ }
+
+ n -= i;
+ for( j = 0; j < n; j++ )
+ encoded[j] = encoded[i + j];
+ encoded.resize(n);
+
+ return (int)decoded.size() >= needed;
+ }
+
+ uchar getUInt8()
+ {
+ size_t sz = decoded.size();
+ if( ofs >= sz && !readMore(1) )
+ return (uchar)0;
+ return decoded[ofs++];
+ }
+
+ ushort getUInt16()
+ {
+ size_t sz = decoded.size();
+ if( ofs + 2 > sz && !readMore(2) )
+ return (ushort)0;
+ ushort val = (decoded[ofs] + (decoded[ofs + 1] << 8));
+ ofs += 2;
+ return val;
+ }
+
+ int getInt32()
+ {
+ size_t sz = decoded.size();
+ if( ofs + 4 > sz && !readMore(4) )
+ return 0;
+ int ival = readInt(&decoded[ofs]);
+ ofs += 4;
+ return ival;
+ }
+
+ double getFloat64()
+ {
+ size_t sz = decoded.size();
+ if( ofs + 8 > sz && !readMore(8) )
+ return 0;
+ double fval = readReal(&decoded[ofs]);
+ ofs += 8;
+ return fval;
+ }
+
+ bool endOfStream() const { return eos; }
+ char* getPtr() const { return ptr; }
+ protected:
+
+ Ptr<FileStorageParser> parser;
+ char* ptr;
+ int indent;
+ std::vector<char> encoded;
+ std::vector<uchar> decoded;
+ size_t ofs;
+ size_t totalchars;
+ bool eos;
+ };
+
+ char* parseBase64(char* ptr, int indent, FileNode& collection)
+ {
+ const int BASE64_HDR_SIZE = 24;
+ char dt[BASE64_HDR_SIZE+1] = {0};
+ base64decoder.init(parser, ptr, indent);
+
+ int i, k;
+
+ for( i = 0; i < BASE64_HDR_SIZE; i++ )
+ dt[i] = (char)base64decoder.getUInt8();
+ for( i = 0; i < BASE64_HDR_SIZE; i++ )
+ if( isspace(dt[i]))
+ break;
+ dt[i] = '\0';
+
+ CV_Assert( !base64decoder.endOfStream() );
+
+ int fmt_pairs[CV_FS_MAX_FMT_PAIRS*2];
+ int fmt_pair_count = fs::decodeFormat( dt, fmt_pairs, CV_FS_MAX_FMT_PAIRS );
+ int ival = 0;
+ double fval = 0;
+
+ for(;;)
+ {
+ for( k = 0; k < fmt_pair_count; k++ )
+ {
+ int elem_type = fmt_pairs[k*2+1];
+ int count = fmt_pairs[k*2];
+
+ for( i = 0; i < count; i++ )
+ {
+ int node_type = FileNode::INT;
+ switch( elem_type )
+ {
+ case CV_8U:
+ ival = base64decoder.getUInt8();
+ break;
+ case CV_8S:
+ ival = (char)base64decoder.getUInt8();
+ break;
+ case CV_16U:
+ ival = base64decoder.getUInt16();
+ break;
+ case CV_16S:
+ ival = (short)base64decoder.getUInt16();
+ break;
+ case CV_32S:
+ ival = base64decoder.getInt32();
+ break;
+ case CV_32F:
+ {
+ Cv32suf v;
+ v.i = base64decoder.getInt32();
+ fval = v.f;
+ node_type = FileNode::REAL;
+ }
+ break;
+ case CV_64F:
+ fval = base64decoder.getFloat64();
+ node_type = FileNode::REAL;
+ break;
+ case CV_16F:
+ fval = (float)float16_t::fromBits(base64decoder.getUInt16());
+ node_type = FileNode::REAL;
+ break;
+ default:
+ CV_Error( Error::StsUnsupportedFormat, "Unsupported type" );
+ }
+
+ if( base64decoder.endOfStream() )
+ break;
+ addNode(collection, std::string(), node_type,
+ node_type == FileNode::INT ? (void*)&ival : (void*)&fval, -1);
+ }
+ }
+ if( base64decoder.endOfStream() )
+ break;
+ }
+
+ finalizeCollection(collection);
+ return base64decoder.getPtr();
+ }
+
+ void parseError( const char* func_name, const std::string& err_msg, const char* source_file, int source_line )
+ {
+ std::string msg = format("%s(%d): %s", filename.c_str(), lineno, err_msg.c_str());
+ error(Error::StsParseError, func_name, msg.c_str(), source_file, source_line );
+ }
+
+ const uchar* getNodePtr(size_t blockIdx, size_t ofs) const
+ {
+ CV_Assert( blockIdx < fs_data_ptrs.size());
+ CV_Assert( ofs < fs_data_blksz[blockIdx]);
+
+ return fs_data_ptrs[blockIdx] + ofs;
+ }
+
+ std::string getName( size_t nameofs ) const
+ {
+ CV_Assert( nameofs < str_hash_data.size() );
+ return std::string(&str_hash_data[nameofs]);
+ }
+
+ FileStorage* getFS() { return fs_ext; }
+
+ FileStorage* fs_ext;
+
+ std::string filename;
+ int flags;
+ bool empty_stream;
+
+ FILE* file;
+ gzFile gzfile;
+
+ bool is_opened;
+ bool dummy_eof;
+ bool write_mode;
+ bool mem_mode;
+ int fmt;
+
+ State state; //!< current state of the FileStorage (used only for writing)
+ int space, wrap_margin;
+ std::deque<FStructData> write_stack;
+ std::vector<char> buffer;
+ size_t bufofs;
+
+ std::deque<char> outbuf;
+
+ Ptr<FileStorageEmitter> emitter;
+ Ptr<FileStorageParser> parser;
+ Base64Decoder base64decoder;
+
+ std::vector<FileNode> roots;
+ std::vector<Ptr<std::vector<uchar> > > fs_data;
+ std::vector<uchar*> fs_data_ptrs;
+ std::vector<size_t> fs_data_blksz;
+ size_t freeSpaceOfs;
+ typedef std::unordered_map<std::string, unsigned> str_hash_t;
+ str_hash_t str_hash;
+ std::vector<char> str_hash_data;
+
+ std::vector<char> strbufv;
+ char* strbuf;
+ size_t strbufsize;
+ size_t strbufpos;
+ int lineno;
+};
+
+FileStorage::FileStorage()
+ : state(0)
+{
+ p = makePtr<FileStorage::Impl>(this);
+}
+
+FileStorage::FileStorage(const String& filename, int flags, const String& encoding)
+ : state(0)
+{
+ p = makePtr<FileStorage::Impl>(this);
+ bool ok = p->open(filename.c_str(), flags, encoding.c_str());
+ if(ok)
+ state = FileStorage::NAME_EXPECTED + FileStorage::INSIDE_MAP;
+}
+
+void FileStorage::startWriteStruct(const String& name, int struct_flags, const String& typeName)
+{
+ p->startWriteStruct(name.c_str(), struct_flags, typeName.c_str());
+ elname = String();
+ if ((struct_flags & FileNode::TYPE_MASK) == FileNode::SEQ)
+ state = FileStorage::VALUE_EXPECTED;
+ else
+ state = FileStorage::NAME_EXPECTED + FileStorage::INSIDE_MAP;
+}
+
+void FileStorage::endWriteStruct()
+{
+ p->endWriteStruct();
+ state = p->write_stack.empty() || FileNode::isMap(p->write_stack.back().flags) ?
+ FileStorage::NAME_EXPECTED + FileStorage::INSIDE_MAP :
+ FileStorage::VALUE_EXPECTED;
+ elname = String();
+}
+
+FileStorage::~FileStorage()
+{
+ p.release();
+}
+
+bool FileStorage::open(const String& filename, int flags, const String& encoding)
+{
+ bool ok = p->open(filename.c_str(), flags, encoding.c_str());
+ if(ok)
+ state = FileStorage::NAME_EXPECTED + FileStorage::INSIDE_MAP;
+ return ok;
+}
+
+bool FileStorage::isOpened() const { return p->is_opened; }
+
+void FileStorage::release()
+{
+ p->release();
+}
+
+FileNode FileStorage::root(int i) const
+{
+ if( p.empty() || p->roots.empty() || i < 0 || i >= (int)p->roots.size() )
+ return FileNode();
+
+ return p->roots[i];
+}
+
+FileNode FileStorage::getFirstTopLevelNode() const
+{
+ FileNode r = root();
+ FileNodeIterator it = r.begin();
+ return it != r.end() ? *it : FileNode();
+}
+
+std::string FileStorage::getDefaultObjectName(const std::string& _filename)
+{
+ static const char* stubname = "unnamed";
+ const char* filename = _filename.c_str();
+ const char* ptr2 = filename + _filename.size();
+ const char* ptr = ptr2 - 1;
+ cv::AutoBuffer<char> name_buf(_filename.size()+1);
+
+ while( ptr >= filename && *ptr != '\\' && *ptr != '/' && *ptr != ':' )
+ {
+ if( *ptr == '.' && (!*ptr2 || strncmp(ptr2, ".gz", 3) == 0) )
+ ptr2 = ptr;
+ ptr--;
+ }
+ ptr++;
+ if( ptr == ptr2 )
+ CV_Error( CV_StsBadArg, "Invalid filename" );
+
+ char* name = name_buf.data();
+
+ // name must start with letter or '_'
+ if( !cv_isalpha(*ptr) && *ptr!= '_' ){
+ *name++ = '_';
+ }
+
+ while( ptr < ptr2 )
+ {
+ char c = *ptr++;
+ if( !cv_isalnum(c) && c != '-' && c != '_' )
+ c = '_';
+ *name++ = c;
+ }
+ *name = '\0';
+ name = name_buf.data();
+ if( strcmp( name, "_" ) == 0 )
+ strcpy( name, stubname );
+ return name;
+}
+
+
+int FileStorage::getFormat() const
+{
+ return p->fmt;
+}
+
+FileNode FileStorage::operator [](const char* key) const
+{
+ return this->operator[](std::string(key));
+}
+
+FileNode FileStorage::operator [](const std::string& key) const
+{
+ FileNode res;
+ for (size_t i = 0; i < p->roots.size(); i++)
+ {
+ res = p->roots[i][key];
+ if (!res.empty())
+ break;
+ }
+ return res;
+}
+
+String FileStorage::releaseAndGetString()
+{
+ String buf;
+ p->release(&buf);
+ return buf;
+}
+
+void FileStorage::writeRaw( const String& fmt, const void* vec, size_t len )
+{
+ p->writeRawData(fmt, (const uchar*)vec, len);
+}
+
+void FileStorage::writeComment( const String& comment, bool eol_comment )
+{
+ p->writeComment(comment.c_str(), eol_comment);
+}
+
+void writeScalar( FileStorage& fs, int value )
+{
+ fs.p->write(String(), value);
+}
+
+void writeScalar( FileStorage& fs, float value )
+{
+ fs.p->write(String(), (double)value);
+}
+
+void writeScalar( FileStorage& fs, double value )
+{
+ fs.p->write(String(), value);
+}
+
+void writeScalar( FileStorage& fs, const String& value )
+{
+ fs.p->write(String(), value);
+}
+
+void write( FileStorage& fs, const String& name, int value )
+{
+ fs.p->write(name, value);
+}
+
+void write( FileStorage& fs, const String& name, float value )
+{
+ fs.p->write(name, (double)value);
+}
+
+void write( FileStorage& fs, const String& name, double value )
+{
+ fs.p->write(name, value);
+}
+
+void write( FileStorage& fs, const String& name, const String& value )
+{
+ fs.p->write(name, value);
+}
+
+void FileStorage::write(const String& name, int val) { p->write(name, val); }
+void FileStorage::write(const String& name, double val) { p->write(name, val); }
+void FileStorage::write(const String& name, const String& val) { p->write(name, val); }
+void FileStorage::write(const String& name, const Mat& val) { cv::write(*this, name, val); }
+void FileStorage::write(const String& name, const std::vector<String>& val) { cv::write(*this, name, val); }
+
+FileStorage& operator << (FileStorage& fs, const String& str)
+{
+ enum { NAME_EXPECTED = FileStorage::NAME_EXPECTED,
+ VALUE_EXPECTED = FileStorage::VALUE_EXPECTED,
+ INSIDE_MAP = FileStorage::INSIDE_MAP };
+ const char* _str = str.c_str();
+ if( !fs.isOpened() || !_str )
+ return fs;
+ Ptr<FileStorage::Impl>& fs_impl = fs.p;
+ char c = *_str;
+
+ if( c == '}' || c == ']' )
+ {
+ if( fs_impl->write_stack.empty() )
+ CV_Error_( CV_StsError, ("Extra closing '%c'", *_str) );
+
+ int struct_flags = fs_impl->write_stack.back().flags;
+ char expected_bracket = FileNode::isMap(struct_flags) ? '}' : ']';
+ if( c != expected_bracket )
+ CV_Error_( CV_StsError, ("The closing '%c' does not match the opening '%c'", c, expected_bracket));
+ fs_impl->endWriteStruct();
+ CV_Assert(!fs_impl->write_stack.empty());
+ struct_flags = fs_impl->write_stack.back().flags;
+ fs.state = FileNode::isMap(struct_flags) ? INSIDE_MAP + NAME_EXPECTED : VALUE_EXPECTED;
+ fs.elname = String();
+ }
+ else if( fs.state == NAME_EXPECTED + INSIDE_MAP )
+ {
+ if (!cv_isalpha(c) && c != '_')
+ CV_Error_( CV_StsError, ("Incorrect element name %s; should start with a letter or '_'", _str) );
+ fs.elname = str;
+ fs.state = VALUE_EXPECTED + INSIDE_MAP;
+ }
+ else if( (fs.state & 3) == VALUE_EXPECTED )
+ {
+ if( c == '{' || c == '[' )
+ {
+ int struct_flags = c == '{' ? FileNode::MAP : FileNode::SEQ;
+ fs.state = struct_flags == FileNode::MAP ? INSIDE_MAP + NAME_EXPECTED : VALUE_EXPECTED;
+ _str++;
+ if( *_str == ':' )
+ {
+ _str++;
+ if( !*_str )
+ struct_flags |= FileNode::FLOW;
+ }
+ fs_impl->startWriteStruct(!fs.elname.empty() ? fs.elname.c_str() : 0, struct_flags, *_str ? _str : 0 );
+ fs.elname = String();
+ }
+ else
+ {
+ write( fs, fs.elname, (c == '\\' && (_str[1] == '{' || _str[1] == '}' ||
+ _str[1] == '[' || _str[1] == ']')) ? String(_str+1) : str );
+ if( fs.state == INSIDE_MAP + VALUE_EXPECTED )
+ fs.state = INSIDE_MAP + NAME_EXPECTED;
+ }
+ }
+ else
+ CV_Error( CV_StsError, "Invalid fs.state" );
+ return fs;
+}
+
+
+FileNode::FileNode()
+{
+ fs = 0;
+ blockIdx = ofs = 0;
+}
+
+FileNode::FileNode(const FileStorage* _fs, size_t _blockIdx, size_t _ofs)
+{
+ fs = _fs;
+ blockIdx = _blockIdx;
+ ofs = _ofs;
+}
+
+FileNode::FileNode(const FileNode& node)
+{
+ fs = node.fs;
+ blockIdx = node.blockIdx;
+ ofs = node.ofs;
+}
+
++FileNode& FileNode::operator=(const FileNode& node)
++{
++ fs = node.fs;
++ blockIdx = node.blockIdx;
++ ofs = node.ofs;
++ return *this;
++}
++
+FileNode FileNode::operator[](const std::string& nodename) const
+{
+ if(!fs)
+ return FileNode();
+
+ CV_Assert( isMap() );
+
+ unsigned key = fs->p->getStringOfs(nodename);
+ size_t i, sz = size();
+ FileNodeIterator it = begin();
+
+ for( i = 0; i < sz; i++, ++it )
+ {
+ FileNode n = *it;
+ const uchar* p = n.ptr();
+ unsigned key2 = (unsigned)readInt(p + 1);
+ CV_Assert( key2 < fs->p->str_hash_data.size() );
+ if( key == key2 )
+ return n;
+ }
+ return FileNode();
+}
+
+FileNode FileNode::operator[](const char* nodename) const
+{
+ return this->operator[](std::string(nodename));
+}
+
+FileNode FileNode::operator[](int i) const
+{
+ if(!fs)
+ return FileNode();
+
+ CV_Assert( isSeq() );
+
+ int sz = (int)size();
+ CV_Assert( 0 <= i && i < sz );
+
+ FileNodeIterator it = begin();
+ it += i;
+
+ return *it;
+}
+
+std::vector<String> FileNode::keys() const
+{
+ CV_Assert(isMap());
+
+ std::vector<String> res;
+ res.reserve(size());
+ for (FileNodeIterator it = begin(); it != end(); ++it)
+ {
+ res.push_back((*it).name());
+ }
+ return res;
+}
+
+int FileNode::type() const
+{
+ const uchar* p = ptr();
+ if(!p)
+ return NONE;
+ return (*p & TYPE_MASK);
+}
+
+bool FileNode::isMap(int flags) { return (flags & TYPE_MASK) == MAP; }
+bool FileNode::isSeq(int flags) { return (flags & TYPE_MASK) == SEQ; }
+bool FileNode::isCollection(int flags) { return isMap(flags) || isSeq(flags); }
+bool FileNode::isFlow(int flags) { return (flags & FLOW) != 0; }
+bool FileNode::isEmptyCollection(int flags) { return (flags & EMPTY) != 0; }
+
+bool FileNode::empty() const { return fs == 0; }
+bool FileNode::isNone() const { return type() == NONE; }
+bool FileNode::isSeq() const { return type() == SEQ; }
+bool FileNode::isMap() const { return type() == MAP; }
+bool FileNode::isInt() const { return type() == INT; }
+bool FileNode::isReal() const { return type() == REAL; }
+bool FileNode::isString() const { return type() == STRING; }
+bool FileNode::isNamed() const
+{
+ const uchar* p = ptr();
+ if(!p)
+ return false;
+ return (*p & NAMED) != 0;
+}
+
+std::string FileNode::name() const
+{
+ const uchar* p = ptr();
+ if(!p)
+ return std::string();
+ size_t nameofs = p[1] | (p[2]<<8) | (p[3]<<16) | (p[4]<<24);
+ return fs->p->getName(nameofs);
+}
+
+FileNode::operator int() const
+{
+ const uchar* p = ptr();
+ if(!p)
+ return 0;
+ int tag = *p;
+ int type = (tag & TYPE_MASK);
+ p += (tag & NAMED) ? 5 : 1;
+
+ if( type == INT )
+ {
+ return readInt(p);
+ }
+ else if( type == REAL )
+ {
+ return cvRound(readReal(p));
+ }
+ else
+ return 0x7fffffff;
+}
+
+FileNode::operator float() const
+{
+ const uchar* p = ptr();
+ if(!p)
+ return 0.f;
+ int tag = *p;
+ int type = (tag & TYPE_MASK);
+ p += (tag & NAMED) ? 5 : 1;
+
+ if( type == INT )
+ {
+ return (float)readInt(p);
+ }
+ else if( type == REAL )
+ {
+ return (float)readReal(p);
+ }
+ else
+ return FLT_MAX;
+}
+
+FileNode::operator double() const
+{
+ const uchar* p = ptr();
+ if(!p)
+ return 0.f;
+ int tag = *p;
+ int type = (tag & TYPE_MASK);
+ p += (tag & NAMED) ? 5 : 1;
+
+ if( type == INT )
+ {
+ return (double)readInt(p);
+ }
+ else if( type == REAL )
+ {
+ return readReal(p);
+ }
+ else
+ return DBL_MAX;
+}
+
+double FileNode::real() const { return double(*this); }
+std::string FileNode::string() const
+{
+ const uchar* p = ptr();
+ if( !p || (*p & TYPE_MASK) != STRING )
+ return std::string();
+ p += (*p & NAMED) ? 5 : 1;
+ size_t sz = (size_t)(unsigned)readInt(p);
+ return std::string((const char*)(p + 4), sz - 1);
+}
+Mat FileNode::mat() const { Mat value; read(*this, value, Mat()); return value; }
+
+FileNodeIterator FileNode::begin() const { return FileNodeIterator(*this, false); }
+FileNodeIterator FileNode::end() const { return FileNodeIterator(*this, true); }
+
+void FileNode::readRaw( const std::string& fmt, void* vec, size_t len ) const
+{
+ FileNodeIterator it = begin();
+ it.readRaw( fmt, vec, len );
+}
+
+size_t FileNode::size() const
+{
+ const uchar* p = ptr();
+ if( !p )
+ return 0;
+ int tag = *p;
+ int tp = tag & TYPE_MASK;
+ if( tp == MAP || tp == SEQ )
+ {
+ if( tag & NAMED )
+ p += 4;
+ return (size_t)(unsigned)readInt(p + 5);
+ }
+ return tp != NONE;
+}
+
+size_t FileNode::rawSize() const
+{
+ const uchar* p0 = ptr(), *p = p0;
+ if( !p )
+ return 0;
+ int tag = *p++;
+ int tp = tag & TYPE_MASK;
+ if( tag & NAMED )
+ p += 4;
+ size_t sz0 = (size_t)(p - p0);
+ if( tp == INT )
+ return sz0 + 4;
+ if( tp == REAL )
+ return sz0 + 8;
+ if( tp == NONE )
+ return sz0;
+ CV_Assert( tp == STRING || tp == SEQ || tp == MAP );
+ return sz0 + 4 + readInt(p);
+}
+
+uchar* FileNode::ptr()
+{
+ return !fs ? 0 : (uchar*)fs->p->getNodePtr(blockIdx, ofs);
+}
+
+const uchar* FileNode::ptr() const
+{
+ return !fs ? 0 : fs->p->getNodePtr(blockIdx, ofs);
+}
+
+void FileNode::setValue( int type, const void* value, int len )
+{
+ uchar *p = ptr();
+ CV_Assert(p != 0);
+
+ int tag = *p;
+ int current_type = tag & TYPE_MASK;
+ CV_Assert( current_type == NONE || current_type == type );
+
+ int sz = 1;
+
+ if( tag & NAMED )
+ sz += 4;
+
+ if( type == INT )
+ sz += 4;
+ else if( type == REAL )
+ sz += 8;
+ else if( type == STRING )
+ {
+ if( len < 0 )
+ len = (int)strlen((const char*)value);
+ sz += 4 + len + 1; // besides the string content,
+ // take the size (4 bytes) and the final '\0' into account
+ }
+ else
+ CV_Error(Error::StsNotImplemented, "Only scalar types can be dynamically assigned to a file node");
+
+ p = fs->p->reserveNodeSpace(*this, sz);
+ *p++ = (uchar)(type | (tag & NAMED));
+ if( tag & NAMED )
+ p += 4;
+
+ if( type == INT )
+ {
+ int ival = *(const int*)value;
+ writeInt(p, ival);
+ }
+ else if( type == REAL )
+ {
+ double dbval = *(const double*)value;
+ writeReal(p, dbval);
+ }
+ else if( type == STRING )
+ {
+ const char* str = (const char*)value;
+ writeInt(p, len + 1);
+ memcpy(p + 4, str, len);
+ p[4 + len] = (uchar)'\0';
+ }
+}
+
+FileNodeIterator::FileNodeIterator()
+{
+ fs = 0;
+ blockIdx = 0;
+ ofs = 0;
+ blockSize = 0;
+ nodeNElems = 0;
+ idx = 0;
+}
+
+FileNodeIterator::FileNodeIterator( const FileNode& node, bool seekEnd )
+{
+ fs = node.fs;
+ idx = 0;
+ if( !fs )
+ blockIdx = ofs = blockSize = nodeNElems = 0;
+ else
+ {
+ blockIdx = node.blockIdx;
+ ofs = node.ofs;
+
+ bool collection = node.isSeq() || node.isMap();
+ if( node.isNone() )
+ {
+ nodeNElems = 0;
+ }
+ else if( !collection )
+ {
+ nodeNElems = 1;
+ if( seekEnd )
+ {
+ idx = 1;
+ ofs += node.rawSize();
+ }
+ }
+ else
+ {
+ nodeNElems = node.size();
+ const uchar* p0 = node.ptr(), *p = p0 + 1;
+ if(*p0 & FileNode::NAMED )
+ p += 4;
+ if( !seekEnd )
+ ofs += (p - p0) + 8;
+ else
+ {
+ size_t rawsz = (size_t)(unsigned)readInt(p);
+ ofs += (p - p0) + 4 + rawsz;
+ idx = nodeNElems;
+ }
+ }
+ fs->p->normalizeNodeOfs(blockIdx, ofs);
+ blockSize = fs->p->fs_data_blksz[blockIdx];
+ }
+}
+
+FileNodeIterator::FileNodeIterator(const FileNodeIterator& it)
+{
+ fs = it.fs;
+ blockIdx = it.blockIdx;
+ ofs = it.ofs;
+ blockSize = it.blockSize;
+ nodeNElems = it.nodeNElems;
+ idx = it.idx;
+}
++
++FileNodeIterator& FileNodeIterator::operator=(const FileNodeIterator& it)
++{
++ fs = it.fs;
++ blockIdx = it.blockIdx;
++ ofs = it.ofs;
++ blockSize = it.blockSize;
++ nodeNElems = it.nodeNElems;
++ idx = it.idx;
++ return *this;
++}
+
+FileNode FileNodeIterator::operator *() const
+{
+ return FileNode(idx < nodeNElems ? fs : 0, blockIdx, ofs);
+}
+
+FileNodeIterator& FileNodeIterator::operator ++ ()
+{
+ if( idx == nodeNElems || !fs )
+ return *this;
+ idx++;
+ FileNode n(fs, blockIdx, ofs);
+ ofs += n.rawSize();
+ if( ofs >= blockSize )
+ {
+ fs->p->normalizeNodeOfs(blockIdx, ofs);
+ blockSize = fs->p->fs_data_blksz[blockIdx];
+ }
+ return *this;
+}
+
+FileNodeIterator FileNodeIterator::operator ++ (int)
+{
+ FileNodeIterator it = *this;
+ ++(*this);
+ return it;
+}
+
+FileNodeIterator& FileNodeIterator::operator += (int _ofs)
+{
+ CV_Assert( _ofs >= 0 );
+ for( ; _ofs > 0; _ofs-- )
+ this->operator ++();
+ return *this;
+}
+
+FileNodeIterator& FileNodeIterator::readRaw( const String& fmt, void* _data0, size_t maxsz)
+{
+ if( fs && idx < nodeNElems )
+ {
+ uchar* data0 = (uchar*)_data0;
+ int fmt_pairs[CV_FS_MAX_FMT_PAIRS*2];
+ int fmt_pair_count = fs::decodeFormat( fmt.c_str(), fmt_pairs, CV_FS_MAX_FMT_PAIRS );
+ size_t esz = fs::calcStructSize( fmt.c_str(), 0 );
+
+ CV_Assert( maxsz % esz == 0 );
+ maxsz /= esz;
+
+ for( ; maxsz > 0; maxsz--, data0 += esz )
+ {
+ size_t offset = 0;
+ for( int k = 0; k < fmt_pair_count; k++ )
+ {
+ int elem_type = fmt_pairs[k*2+1];
+ int elem_size = CV_ELEM_SIZE(elem_type);
+
+ int count = fmt_pairs[k*2];
+ offset = alignSize( offset, elem_size );
+ uchar* data = data0 + offset;
+
+ for( int i = 0; i < count; i++, ++(*this) )
+ {
+ FileNode node = *(*this);
+ if( node.isInt() )
+ {
+ int ival = (int)node;
+ switch( elem_type )
+ {
+ case CV_8U:
+ *(uchar*)data = saturate_cast<uchar>(ival);
+ data++;
+ break;
+ case CV_8S:
+ *(char*)data = saturate_cast<schar>(ival);
+ data++;
+ break;
+ case CV_16U:
+ *(ushort*)data = saturate_cast<ushort>(ival);
+ data += sizeof(ushort);
+ break;
+ case CV_16S:
+ *(short*)data = saturate_cast<short>(ival);
+ data += sizeof(short);
+ break;
+ case CV_32S:
+ *(int*)data = ival;
+ data += sizeof(int);
+ break;
+ case CV_32F:
+ *(float*)data = (float)ival;
+ data += sizeof(float);
+ break;
+ case CV_64F:
+ *(double*)data = (double)ival;
+ data += sizeof(double);
+ break;
+ case CV_16F:
+ *(float16_t*)data = float16_t((float)ival);
+ data += sizeof(float16_t);
+ break;
+ default:
+ CV_Error( Error::StsUnsupportedFormat, "Unsupported type" );
+ }
+ }
+ else if( node.isReal() )
+ {
+ double fval = (double)node;
+
+ switch( elem_type )
+ {
+ case CV_8U:
+ *(uchar*)data = saturate_cast<uchar>(fval);
+ data++;
+ break;
+ case CV_8S:
+ *(char*)data = saturate_cast<schar>(fval);
+ data++;
+ break;
+ case CV_16U:
+ *(ushort*)data = saturate_cast<ushort>(fval);
+ data += sizeof(ushort);
+ break;
+ case CV_16S:
+ *(short*)data = saturate_cast<short>(fval);
+ data += sizeof(short);
+ break;
+ case CV_32S:
+ *(int*)data = saturate_cast<int>(fval);
+ data += sizeof(int);
+ break;
+ case CV_32F:
+ *(float*)data = (float)fval;
+ data += sizeof(float);
+ break;
+ case CV_64F:
+ *(double*)data = fval;
+ data += sizeof(double);
+ break;
+ case CV_16F:
+ *(float16_t*)data = float16_t((float)fval);
+ data += sizeof(float16_t);
+ break;
+ default:
+ CV_Error( Error::StsUnsupportedFormat, "Unsupported type" );
+ }
+ }
+ else
+ CV_Error( Error::StsError, "readRawData can only be used to read plain sequences of numbers" );
+ }
+ offset = (int)(data - data0);
+ }
+ }
+ }
+
+ return *this;
+}
+
+bool FileNodeIterator::equalTo(const FileNodeIterator& it) const
+{
+ return fs == it.fs && blockIdx == it.blockIdx && ofs == it.ofs &&
+ idx == it.idx && nodeNElems == it.nodeNElems;
+}
+
+size_t FileNodeIterator::remaining() const
+{
+ return nodeNElems - idx;
+}
+
+bool operator == ( const FileNodeIterator& it1, const FileNodeIterator& it2 )
+{
+ return it1.equalTo(it2);
+}
+
+bool operator != ( const FileNodeIterator& it1, const FileNodeIterator& it2 )
+{
+ return !it1.equalTo(it2);
+}
+
+void read(const FileNode& node, int& val, int default_val)
+{
+ val = default_val;
+ if( !node.empty() )
+ {
+ val = (int)node;
+ }
+}
+
+void read(const FileNode& node, double& val, double default_val)
+{
+ val = default_val;
+ if( !node.empty() )
+ {
+ val = (double)node;
+ }
+}
+
+void read(const FileNode& node, float& val, float default_val)
+{
+ val = default_val;
+ if( !node.empty() )
+ {
+ val = (float)node;
+ }
+}
+
+void read(const FileNode& node, std::string& val, const std::string& default_val)
+{
+ val = default_val;
+ if( !node.empty() )
+ {
+ val = (std::string)node;
+ }
+}
+
+FileStorage_API::~FileStorage_API() {}
+
+namespace internal
+{
+
+WriteStructContext::WriteStructContext(FileStorage& _fs, const std::string& name,
+ int flags, const std::string& typeName)
+{
+ fs = &_fs;
+ fs->startWriteStruct(name, flags, typeName);
+}
+
+WriteStructContext::~WriteStructContext()
+{
+ fs->endWriteStruct();
+}
+
+}
+
}
projects / platform / upstream / opencv.git / commitdiff