#include "CVUtils.h"
#include "DynamicProgramSearch.h"
#include "StdUtils.h"

// #include "cvmatutils.h"
// #include "cvdrawutils.h"
#include "TransSolver.h"

namespace CyclopsUtils {

Mat gDummyMat;
Scalar gDummyScalar;

bool localMeanVarNorm(const Mat& srcMat, Mat& dstMat, int winRadius, double tarMean/* = 120*/, double tarStd /*= 30*/)
{
	int width = srcMat.cols;
	int height = srcMat.rows;

	if (winRadius * 2 + 1 >= width || winRadius * 2 + 1 >= height)
	{
		return false;
	}

	dstMat = srcMat.clone();

	Mat sumMat, sqSumMat;
	cv::integral(srcMat, sumMat, sqSumMat, CV_64F);
	int area = (2 * winRadius + 1)*(2 * winRadius + 1);
	for (int i = winRadius; i < height - winRadius; i++)
	{
		const uchar *pData = srcMat.ptr(i);
		uchar *tarData = dstMat.ptr(i);
		double *pSumY1 = (double *)sumMat.ptr(i - winRadius);
		double *pSumY2 = (double *)sumMat.ptr(i + winRadius + 1);
		double *pSqSumY1 = (double *)sqSumMat.ptr(i - winRadius);
		double *pSqSumY2 = (double *)sqSumMat.ptr(i + winRadius + 1);
		for (int j = winRadius; j < width - winRadius; j++)
		{
			double sumX1Y1 = pSumY1[j - winRadius];
			double sumX2Y2 = pSumY2[j + winRadius + 1];
			double sumX1Y2 = pSumY2[j - winRadius];
			double sumX2Y1 = pSumY1[j + winRadius + 1];
			double sqSumX1Y1 = pSqSumY1[j - winRadius];
			double sqSumX2Y2 = pSqSumY2[j + winRadius + 1];
			double sqSumX1Y2 = pSqSumY2[j - winRadius];
			double sqSumX2Y1 = pSqSumY1[j + winRadius + 1];
			double val = sumX2Y2 + sumX1Y1 - sumX1Y2 - sumX2Y1;
			double meanVal = val / area;
			double sqSum = sqSumX2Y2 + sqSumX1Y1 - sqSumX1Y2 - sqSumX2Y1;
			double stdVar = sqrt((sqSum - 2 * val*meanVal + area * meanVal * meanVal) / area);
			double s = tarStd / stdVar;
			double t = tarMean - meanVal * s;

			int value = pData[j] * s + t;

			if (value > 255)
			{
				tarData[j] = 255;
			}
			else
			{
				tarData[j] = value;
			}
		}
	}

	return true;
}

void meanvarnorm(Mat& src, Mat &dst, double avg, double var, Mat mask /*= Mat()*/)
{
	Scalar m, v;
	cv::meanStdDev(src, m, v, mask);
	// (I - m)*var/v + avg = I*var/v - m*var/v + avg
	double s = var / v.val[0];
	double t = avg - m.val[0] * var / v.val[0];
	dst = src * s + t;
}

Mat gridThre(const Mat& img, int gridWidth, int gridHeight, float threStdDevScale,
    LogicOper oper /*= LP_LOGIC_SMALLER*/,
    float majority /*= 0.8*/)
{
    Mat ret = Mat::zeros(img.size(), CV_8UC1);
    int gridXNum = img.cols / gridWidth + 1;
    int gridYNum = img.rows / gridHeight + 1;
    for (int gy = 0; gy < gridYNum - 1; ++gy)
    {
        int y = gy*gridHeight;
        for (int gx = 0; gx < gridXNum - 1; ++gx)
        {
            int x = gx*gridWidth;
            Rect roi(x, y, gridWidth, gridHeight);
            Mat roiImg(img, roi);
            double mean, stddev;
            meanStdDev(roiImg, Mat(), &mean, &stddev, majority);
            Mat threImg;
            switch (oper)
            {
            case LP_LOGIC_SMALLER:
                threImg = roiImg < (mean + stddev*threStdDevScale);
                break;
            case LP_LOGIC_LARGER:
                threImg = roiImg > (mean + stddev*threStdDevScale);
                break;
            }
            
            threImg.copyTo(Mat(ret, roi));
        }
    }
    for (int gy = 0; gy < gridYNum; ++gy)
    {
        int y = gy*gridHeight;
        Rect roi(img.cols - gridWidth, y, gridWidth, gridHeight);
        roi &= Rect(0, 0, img.cols, img.rows);
        if (roi.width == 0 || roi.height == 0)
        {
            continue;
        }
        Mat roiImg(img, roi);
        double mean, stddev;
        meanStdDev(roiImg, Mat(), &mean, &stddev, majority);
        Mat threImg;
        switch (oper)
        {
        case LP_LOGIC_SMALLER:
            threImg = roiImg < (mean + stddev*threStdDevScale);
            break;
        case LP_LOGIC_LARGER:
            threImg = roiImg > (mean + stddev*threStdDevScale);
            break;
        }
        threImg.copyTo(Mat(ret, roi));
    }
    return ret;
}
Mat gridWhiteThre(const Mat& img, int gridWidth, int gridHeight, float threStdDevScale, float majority /*= 0.8*/)
{
    return gridThre(img, gridWidth, gridHeight, threStdDevScale, LP_LOGIC_LARGER, majority);
}
Mat gridBlackThre(const Mat& img, int gridWidth, int gridHeight, float threStdDevScale, float majority /*= 0.8*/)
{
    return gridThre(img, gridWidth, gridHeight, threStdDevScale, LP_LOGIC_SMALLER, majority);

    Mat ret = Mat::zeros(img.size(), CV_8UC1);
    int gridXNum = img.cols / gridWidth + 1;
    int gridYNum = img.rows / gridHeight + 1;
    for (int gy = 0; gy < gridYNum - 1; ++gy)
    {
        int y = gy*gridHeight;
        for (int gx = 0; gx < gridXNum - 1; ++gx)
        {
            int x = gx*gridWidth;
            Rect roi(x, y, gridWidth, gridHeight);
            Mat roiImg(img, roi);
            double mean, stddev;
            meanStdDev(roiImg, Mat(), &mean, &stddev, majority);
            Mat threImg = roiImg < (mean + stddev*threStdDevScale);
            threImg.copyTo(Mat(ret, roi));
        }
    }
    for(int gy = 0; gy < gridYNum; ++gy)
    {
        int y = gy*gridHeight;
        Rect roi((gridXNum - 1)*gridWidth, y, gridWidth, gridHeight);
        roi &= Rect(0, 0, img.cols, img.rows);
        if (roi.width == 0 || roi.height == 0)
        {
            continue;
        }
        Mat roiImg(img, roi);
        double mean, stddev;
        meanStdDev(roiImg, Mat(), &mean, &stddev, (float)0.8);
        Mat threImg = roiImg < (mean + stddev*threStdDevScale);
        threImg.copyTo(Mat(ret, roi));
    }
    return ret;
}

void converToType(cv::Mat& mat, int mattype)
{
    cv::Mat _mat;
    mat.convertTo(_mat, mattype);
    mat = _mat;
}

void genScharrImage(Mat& img)
{
    Mat sobelx, sobely;
    Sobel(img, sobelx, CV_32FC1, 1, 0, CV_SCHARR);
    Sobel(img, sobely, CV_32FC1, 0, 1, CV_SCHARR);
    img = sobelx.mul(sobelx) + sobely.mul(sobely);
    Mat tempImg;
    img.convertTo(tempImg, CV_32FC1);
    Mat tempImg0;
    sqrt(tempImg, tempImg0);
    img = tempImg0;
}

void genSobelImage(Mat& img, Mat* pSobelx /*= NULL*/, Mat* pSobely /*= NULL*/)
{
    Mat sobelx, sobely;
    Sobel(img, sobelx, CV_32FC1, 1, 0, BORDER_REPLICATE);
    Sobel(img, sobely, CV_32FC1, 0, 1, BORDER_REPLICATE);
    img = sobelx.mul(sobelx) + sobely.mul(sobely);
    Mat tempImg;
    img.convertTo(tempImg, CV_32FC1);
    Mat tempImg0;
    sqrt(tempImg, tempImg0);
    img = tempImg0;

    if (pSobelx)
    {
        *pSobelx = sobelx;
    }
    if (pSobely)
    {
        *pSobely = sobely;
    }
}

cv::Mat genXDeriLineKernel(int w, int type, double absVal)
{
    assert(w % 2 == 1);

    Mat leftKernel = Mat::ones(1, w / 2, type)*absVal;
    Mat rightKernel = Mat::ones(1, w / 2, type)*absVal*(-1);
    Mat kernel = Mat::zeros(1, w, type);
    leftKernel.copyTo(kernel.colRange(0, w / 2));
    rightKernel.copyTo(kernel.colRange(w / 2 + 1, w));
    return kernel;
}

cv::Mat genXDeriMat(const Mat& img, int w, double absVal)
{
    assert(w % 2 == 1);
    int kernelRadius = w / 2;

    Mat xKernel = genXDeriLineKernel(kernelRadius * 2 + 1, CV_32FC1, 1);
    Mat xDeriMat;
    sepFilter2D(img, xDeriMat, CV_32FC1, xKernel, Mat::ones(1, 1, CV_32FC1));

    return xDeriMat;
}

cv::Mat normEachRow(const Mat& img)
{
    Mat ret(img.rows, img.cols, img.type());
    for (int y = 0; y < img.rows; y++)
    {
        Mat rowMat = img.row(y);
        double minVal, maxVal;
        cv::minMaxIdx(rowMat, &minVal, &maxVal);
        rowMat = rowMat / maxVal;
        rowMat.copyTo(ret.row(y));
    }
    return ret;
}

cv::Mat genGradientDir4EachRow(const Mat& img)
{
    assert(img.cols >= 2);

    Mat xDeriMat = genXDeriMat(img, 3, 1);
    Mat threMat;
    threshold(xDeriMat, threMat, 0, 1, cv::THRESH_BINARY);

    return threMat;
}

void findMatElementsEquals(const Mat& img, vector<Point>& pointVec, float val, int xPadding)
{
    assert(img.type() == CV_32FC1);

    for (int y = 0; y < img.rows; ++y)
    {
        float* pRowData = (float*)img.row(y).data;
        float* pRowDataStart = pRowData;
        float* pRowDataEnd = pRowData + img.cols - xPadding;
        pRowData += xPadding;
        while (pRowData != pRowDataEnd)
        {
            if (abs(*pRowData - val) < 0.0001)
            {
                pointVec.push_back(Point(pRowData - pRowDataStart, y));
            }
            pRowData++;
        }
    }
}

double localMatSum(const Mat& mat, const Rect& roi)
{
    Mat localMat(mat, roi);
    return sum(localMat).val[0];
}

cv::Mat normCanvas(const Mat& img)
{
    Mat canvas;
    double maxVal, minVal;
    minMaxIdx(img, &minVal, &maxVal);
    if (maxVal <= 1.0)
    {
        img.convertTo(canvas, CV_8UC1, 255);
    }
    else
    {
        img.convertTo(canvas, CV_8UC1);
    }
    return canvas;
}

void findEdgePointsEachRow(const Mat& img, vector<Point>& edgePointVec, int xPadding)
{
    // img is a binary image.
    // an edge point in a row satisfies:
    //	1) left neighbor pixels are all black (0) or white (1);
    //	2) right neighbor pixels are all white or black.

    // find candidate turning points
    Mat sumKernelMat = Mat::ones(1, 2, img.type());
    Mat sumMat;
    sepFilter2D(img, sumMat, img.type(), sumKernelMat, Mat::ones(1, 1, img.type()));
    vector<Point> turningPointVec;

    // for pixels ... 0, 0, 1, 1 ..., turning point is
    //                      *
    findMatElementsEquals(sumMat, turningPointVec, 1.0, xPadding);

    // filter turning points
    vector<Point> filteredTurningPointVec;
    int localRadius = 30;
    int localSumTor = 5;
    for (size_t i = 0; i < turningPointVec.size(); ++i)
    {
        Point pt(turningPointVec[i]);
        Rect leftRoi(pt.x - localRadius, pt.y, localRadius, 1);
        double leftSum = localMatSum(img, leftRoi);
        Rect rightRoi(pt.x, pt.y, localRadius, 1);
        double rightSum = localMatSum(img, rightRoi);
        if (leftSum > rightSum && abs(leftSum - localRadius) < localSumTor && rightSum < 0.0001)
        {
            filteredTurningPointVec.push_back(pt);
        }
        else if (leftSum < rightSum && leftSum < 0.0001 && abs(rightSum - localRadius) < localSumTor)
        {
            filteredTurningPointVec.push_back(pt);
        }
    }


	Mat canvas;// = cv::drawPoints(img, filteredTurningPointVec, 125, 10);


    edgePointVec = filteredTurningPointVec;
}

Mat sumEachRow2(const Mat& img)
{
#define _sumEachRow2(t)\
if (img.channels() == 1) return sumEachRow<t>(img);\
else if (img.channels() == 2) return sumEachRowN<t, 2>(img);\
else if (img.channels() == 3) return sumEachRowN<t, 3>(img);\
else if (img.channels() == 4) return sumEachRowN<t, 4>(img);\
else { _ASSERTE(false && "not implemented"); return gDummyMat; }

	switch (img.depth())
	{
	case CV_8U:
		_sumEachRow2(unsigned char);
	case CV_16S:
		_sumEachRow2(short);
	case CV_32S:
		_sumEachRow2(int);
	case CV_32F:
		_sumEachRow2(float);
	case CV_64F:
		_sumEachRow2(double);
	default:
		_ASSERTE(false && "not implemented");
	}
	return gDummyMat;
}

Mat sumEachCol2(const Mat& img)
{
#define _sumEachCol2(t)\
if (img.channels() == 1) return sumEachCol<t>(img);\
else if (img.channels() == 2) return sumEachColN<t, 2>(img);\
else if (img.channels() == 3) return sumEachColN<t, 3>(img);\
else if (img.channels() == 4) return sumEachColN<t, 4>(img);\
else { _ASSERTE(false && "not implemented"); return gDummyMat; }

	switch (img.depth())
	{
	case CV_8U:
		_sumEachCol2(unsigned char);
	case CV_16S:
		_sumEachCol2(short);
	case CV_32S:
		_sumEachCol2(int);
	case CV_32F:
		_sumEachCol2(float);
	case CV_64F:
		_sumEachCol2(double);
	default:
		_ASSERTE(false && "not implemented");
	}
	return gDummyMat;
}

cv::Mat thresholdEachRowLocally(const Mat& img, int localRange /*= 501*/, int C /*= 10*/)
{
    Mat ret = img.clone();
    for (int y = 0; y < img.rows; ++y)
    {
        Mat rowMat = img.row(y);
        uchar* pData = (uchar*)rowMat.data;
        cv::adaptiveThreshold(rowMat, ret.row(y), 255, ADAPTIVE_THRESH_MEAN_C, THRESH_BINARY, localRange, C);
    }
    return ret;
}

cv::Mat thresholdEachRow(const Mat& img, float threScale /*= 0.5*/)
{
    Mat ret = img.clone();
    for (int y = 0; y < img.rows; ++y)
    {
        Mat rowMat = img.row(y);
        double rowSum = sum(rowMat).val[0];
        double rowAvg = rowSum / img.cols * threScale;
        Mat rowThre = ret.row(y);
        threshold(rowMat, rowThre, rowAvg, 255, THRESH_BINARY);
    }
    return ret;
}

cv::Mat thresholdEachRow(const Mat& img, const Mat& rowThre, float threScale)
{
    Mat ret = Mat::zeros(img.rows, img.cols, img.type());
    for (int y = 0; y < img.rows; ++y)
    {
        Mat retRow = ret.row(y);
        uchar* pRetData = retRow.data;
        Mat imgRow = img.row(y);
        uchar* pData = imgRow.data;
        double* pThreData = (double*)rowThre.data;
        for (int x = 0; x < img.cols; ++x)
        {
            if (*pData < (*pThreData)*threScale)
            {
                *pRetData = 0;
            }
            else
            {
                *pRetData = 255;
            }
            pData++;
            pThreData++;
            pRetData++;
        }
    }
    return ret;
}

void convertPointPair2PointfPair(const vector<PointPair>& vec0, vector<PointfPair>& vec1)
{
    vec1.clear();
    for (auto i = vec0.begin(); i != vec0.end(); ++i)
    {
        PointfPair pointfPair(i->first, i->second);
        pointfPair.setAver(i->aver());
        pointfPair.setStr(i->getStr());
        vec1.push_back(pointfPair);
    }
}

double pointPairDis(const PointPair& i, const PointPair& j)
{
    double dis0 = pointDis(i.first, j.first);
    double dis1 = pointDis(i.second, j.second);
    return (dis0 + dis1) / 2.0;
}

void segEachRow(const Mat& img, vector<PointPair>& pointPairVec, int xPadding)
{
    for (int y = 0; y < img.rows; ++y)
    {
        Mat rowMat = img.row(y);
        uchar* pRowData = rowMat.data;
        uchar* pRowStart = pRowData;
        uchar* pRowEnd = rowMat.data + img.cols - xPadding;
        pRowData += xPadding;
        int leftX = -1;
        while (pRowData != pRowEnd)
        {
            uchar val = *pRowData;
            if (val == 0 && leftX == -1)
            {
                leftX = pRowData - pRowStart;
            }
            else if (val != 0 && leftX >= 0)
            {
                Point leftPt(leftX, y);
                Point rightPt(pRowData - pRowStart - 1, y);
                PointPair pointPair(leftPt, rightPt);
                pointPairVec.push_back(pointPair);
                leftX = -1;
            }
            pRowData++;
        }
    }
}

void fillPointPairVal(const Mat& img, std::string filePath, vector<PointPair>& pointPairVec)
{
    for (auto i = pointPairVec.begin(); i != pointPairVec.end(); ++i)
    {
        Mat mat(img, Rect(i->first.x, i->first.y, i->second.x - i->first.x, 1));
        i->setAver(sum(mat).val[0] / (double)mat.cols);
        std::stringstream ss;
        ss << filePath << " x: " << i->first.x << " y: " << i->first.y;
        i->setStr(ss.str());
    }
}

cv::Mat getFirstChannel(const Mat& img)
{
    vector<Mat> channels;
    split(img, channels);
    return channels.front();
}
Mat getChannel(const Mat& img, int i)
{
    if (i < 0 || i >= img.channels())
    {
        return Mat();
    }
    vector<Mat> channels;
    split(img, channels);
    
    return channels[i];
}

void mulEachRow(Mat& img, const Mat& scaleRow)
{
    for (int i = 0; i < img.rows; ++i)
    {
        Mat row = img.row(i);
        uchar* pRowData = (uchar*)row.data;
        double* pScaleData = (double*)scaleRow.data;
        for (int j = 0; j < row.cols; ++j)
        {
            *pRowData *= *pScaleData;
            ++pRowData;
            ++pScaleData;
        }
    }
}

void genRandomPoints(const Mat& img, vector<Point>& points, RNG rng, int sampleCnt /* = 1000 */)
{
    sampleCnt = cv::min(sampleCnt, img.rows * img.cols );
    for (int i = 0; i < sampleCnt; ++i)
    {  
        Vec3b* pData = (Vec3b*)img.row(rng.uniform(0, img.rows)).data;
        Vec3b& d = pData[rng.uniform(0, img.cols)];

        Point p;
        p.x = d[0];
        p.y = d[1];
        points.push_back(p);
    }
}

void plot8uVec(const Mat& vec, float scale /*= 1.0*/)
{

    Mat canvas = Mat::zeros(100 * scale, 100 * scale, CV_8UC1);
    Rect rect(0, 0, canvas.cols * scale, canvas.rows * scale);

    int size = cv::max(vec.rows, vec.cols);
    uchar* pVecData = vec.data;
    for (int i = 0; i < size; ++i)
    {
        Point pt(i * scale, canvas.rows - 1 - pVecData[i] * scale);
        if (rect.contains(pt))
        {
            canvas.at<uchar>(pt.y, pt.x) = 255;
        }
    }
    imshow("plot8uVec", canvas);
}

void plot32fVec(const Mat& vec, float scale /*= 1.0*/)
{
    Mat canvas = Mat::zeros(100 * scale, 100 * scale, CV_8UC1);
    Rect rect(0, 0, canvas.cols * scale, canvas.rows * scale);
    int size = cv::max(vec.rows, vec.cols);
    float* pVecData = (float*)vec.data;
    for (int i = 0; i < size; ++i)
    {
        Point pt(i * scale, canvas.rows - 1 - pVecData[i] * scale);
        if (rect.contains(pt))
        {
            canvas.at<uchar>(pt.y, pt.x) = 255;
        }
    }
    imshow("plot32fVec", canvas);
}

cv::Mat calcHist(const Mat& img, const Mat& mask, int histSize /*= 256*/, int minVal, int maxVal)
{
    Mat histMat;
	float fRange[] = { minVal, maxVal };
	const float* fHistRange = { fRange };

	cv::calcHist(&img, 1, NULL, mask, histMat, 1, &histSize, &fHistRange);
    return histMat;
}

Mat resize(const Mat& img, float s, int interMethod /*= cv::INTER_LINEAR*/)
{
    Mat ret;
    Size newSize(img.cols*s, img.rows*s);
    if (newSize.width == 0 || newSize.height == 0)
    {
        return img;
    }
    resize(img, ret, Size(), s, s, interMethod);
    return ret;
}

void writeFile(const Mat& mat, std::string filePath, std::string matName /*= "mat"*/)
{
    FileStorage fs;
    fs.open(filePath, FileStorage::WRITE);
    fs << matName << mat;
    fs.release();
}

cv::Mat readFile(std::string filePath, std::string matName)
{
    FileStorage fs;
    fs.open(filePath, FileStorage::READ);
    Mat ret;
    fs[matName] >> ret;
    return ret;
}

void gaussianBlurEachRow(const Mat& src, Mat& dst, int ksize /*= 3*/)
{
    dst = Mat::zeros(src.rows, src.cols, src.type());
    for (int y = 0; y < src.rows; y++)
    {
        Mat dstRow = dst.row(y);
        Mat srcRow = src.row(y);
        cv::GaussianBlur(srcRow, dstRow, cv::Size(ksize, 1), 1.0);
    }
}

void medianBlurEachRow(const Mat& src, Mat& dst, int ksize /*= 3*/)
{
    dst = Mat::zeros(src.rows, src.cols, src.type());
    for (int y = 0; y < src.rows; y++)
    {
        Mat dstRow = dst.row(y);
        Mat srcRow = src.row(y);
        cv::medianBlur(srcRow, dstRow, 1);
    }
}

double maxLaplacianX(const Mat& rowMat, float scale /*= 1.0*/)
{
    Mat sRowMat;
    Mat lapMat;

    if (scale != 1.0)
    {
        resize(rowMat, sRowMat, Size(), scale, 1.0, INTER_CUBIC);
        Laplacian(sRowMat, lapMat, CV_32FC1, 3);
    }
    else
    {
        Laplacian(rowMat, lapMat, CV_32FC1);
    }
    double minVal, maxVal;
    int minIdx, maxIdx;
    minMaxIdx(lapMat, &minVal, &maxVal, &minIdx, &maxIdx);
    return maxIdx / scale;
}

double minLaplacianX(const Mat& rowMat, float scale /*= 1.0*/)
{
    Mat sRowMat;
    Mat lapMat;

    if (scale != 1.0)
    {
        resize(rowMat, sRowMat, Size(), scale, 1.0, INTER_CUBIC);
        Laplacian(sRowMat, lapMat, CV_32FC1, 3);
    }
    else
    {
        Laplacian(rowMat, lapMat, CV_32FC1);
    }
    double minVal, maxVal;
    int minIdx, maxIdx;
    minMaxIdx(lapMat, &minVal, &maxVal, &minIdx, &maxIdx);
    return minIdx / scale;
}

void Laplacian1D(const Mat& src, Mat& dst, int ddepth, int ksize /*= 1*/)
{
    assert(src.rows == 1);
    Mat kernel;
    float K1[3] = { 1, -2, 1 };
    float K3[5] = { 1, 2, -6, 2, 1 };
    float K5[7] = { 1, 1, 1, -6, 1, 1, 1 };
    float K7[9] = { 1, 1, 1, 1, -8, 1, 1, 1, 1 };
    float K9[11] = { 1, 1, 1, 1, 1, -10, 1, 1, 1, 1, 1 };
    switch (ksize)
    {
    case 1:
        kernel = Mat(1, 3, CV_32F, K1);
        break;
    case 3:
        kernel = Mat(1, 5, CV_32F, K3);
        break;
    case 5:
        kernel = Mat(1, 7, CV_32F, K5);
        break;
    case 7:
        kernel = Mat(1, 9, CV_32F, K7);
        break;
    case 9:
        kernel = Mat(1, 11, CV_32F, K9);
        break;
    default:
        assert(0);
        break;
    }
    filter2D(src, dst, ddepth, kernel);
}


void _filterKeyPointsByNeighborDistance(vector<KeyPoint>& vec, float ndis)
{
    vector<KeyPoint> ret;
    for (size_t i = 0; i < vec.size(); ++i)
    {
        KeyPoint kp0 = vec[i];
        bool isDiscard = false;
        for (size_t j = i + 1; j < vec.size(); ++j)
        {
            KeyPoint kp1 = vec[j];
            float dis = pointDis(kp0.pt, kp1.pt);
            if (dis < ndis)
            {
                if (kp0.response < kp1.response)
                {
                    isDiscard = true;
                    break;
                }
            }
        }
        if (!isDiscard)
        {
            ret.push_back(kp0);
        }
    }
    vec = ret;
}

void filterKeyPointsByNeighborDistance(vector<KeyPoint>& vec, float ndis)
{
    _filterKeyPointsByNeighborDistance(vec, ndis);
    vec = vector<KeyPoint>(vec.rbegin(), vec.rend());
    _filterKeyPointsByNeighborDistance(vec, ndis);
}

float IC_Angle_u8(const Mat& image, const int half_k, Point2f pt,
    const vector<int> & u_max)
{
    int m_01 = 0, m_10 = 0;

    const uchar* center = &image.at<uchar>(cvRound(pt.y), cvRound(pt.x));

    // Treat the center line differently, v=0
    for (int u = -half_k; u <= half_k; ++u)
        m_10 += u * center[u];

    // Go line by line in the circular patch
    int step = (int)image.step1();
    for (int v = 1; v <= half_k; ++v)
    {
        // Proceed over the two lines
        int v_sum = 0;
        int d = u_max[v];
        for (int u = -d; u <= d; ++u)
        {
            int val_plus = center[u + v*step], val_minus = center[u - v*step];
            v_sum += (val_plus - val_minus);
            m_10 += u * (val_plus + val_minus);
        }
        m_01 += v * v_sum;
    }

    return fastAtan2((float)m_01, (float)m_10);
}

float IC_Angle_f32(const Mat& image, const int half_k, Point2f pt,
    const vector<int> & u_max)
{
    float m_01 = 0, m_10 = 0;

    const float* center = &image.at<float>(cvRound(pt.y), cvRound(pt.x));

    // Treat the center line differently, v=0
    for (int u = -half_k; u <= half_k; ++u)
        m_10 += u * center[u];

    // Go line by line in the circular patch
    int step = (int)image.step1();
    for (int v = 1; v <= half_k; ++v)
    {
        // Proceed over the two lines
        float v_sum = 0;
        int d = u_max[v];
        for (int u = -d; u <= d; ++u)
        {
            float val_plus = center[u + v*step], val_minus = center[u - v*step];
            v_sum += (val_plus - val_minus);
            m_10 += u * (val_plus + val_minus);
        }
        m_01 += v * v_sum;
    }

    return fastAtan2((float)m_01, (float)m_10);
}

float IC_Angle(const Mat& image, const int half_k, Point2f pt,
    const vector<int> & u_max)
{
    if (image.type() == CV_8UC1)
    {
        return IC_Angle_u8(image, half_k, pt, u_max);
    }
    else if (image.type() == CV_32FC1)
    {
        return IC_Angle_f32(image, half_k, pt, u_max);
    }
    else
    {
        std::cout << "image type " << image.type() << " is not supported" << std::endl;
        return FLT_MAX;
    }
}

void computeOrientation(const Mat& image, vector<KeyPoint>& keypoints,
    int halfPatchSize, const vector<int>& umax)
{
    // Process each keypoint
    for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
        keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
    {
        keypoint->angle = IC_Angle(image, halfPatchSize, keypoint->pt, umax);
    }
}

void genUMax(vector<int>& umax, int halfPatchSize)
{
    umax.resize(halfPatchSize + 2);

    int v, v0, vmax = cvFloor(halfPatchSize * sqrt(2.f) / 2 + 1);
    int vmin = cvCeil(halfPatchSize * sqrt(2.f) / 2);
    for (v = 0; v <= vmax; ++v)
        umax[v] = cvRound(sqrt((double)halfPatchSize * halfPatchSize - v * v));

    // Make sure we are symmetric
    for (v = halfPatchSize, v0 = 0; v >= vmin; --v)
    {
        while (umax[v0] == umax[v0 + 1])
            ++v0;
        umax[v] = v0;
        ++v0;
    }
}

void filterKeyPointsByRotationInvariants(vector<KeyPoint>& vec, const Mat& img, 
    FeatureDetector* pFeatDetector, float tor)
{
    Point2f cen(img.cols / 2.0, img.rows / 2.0);

// no longer supported in opencv3.4.1
#if (CV_MAJOR_VERSION < 3)
    int patchSize = pFeatDetector->getInt("patchSize");
#else
	int patchSize = pFeatDetector->descriptorSize();
#endif
    int halfPatchSize = patchSize / 2;
    vector<int> umax;
    genUMax(umax, halfPatchSize);

    for (int i = 0; i < 360; ++i)
    {
        Mat trans = getRotationMatrix2D(cen, i, 1.0);
        Mat transedImg;
        warpAffine(img, transedImg, trans, Size(img.cols, img.rows));
        computeOrientation(img, vec, halfPatchSize, umax);
    }
}

double localIC_Angle(const Mat& img, Point2f center, int patchSize)
{
    int halfPatchSize = patchSize / 2;
    vector<int> umax;
    genUMax(umax, halfPatchSize);

    return IC_Angle(img, halfPatchSize, center, umax);
}

double localAngle_WeightedCen(const Mat& img, Point2f center)
{
#ifdef DEBUG_VIEW_INTERNAL_MAT
    Mat vImg = img/255.0;
#endif
    Point2f weightedCen(0, 0);
    float sum = 0;
    for (int y = 0; y < img.rows; ++y)
    {
        float* pRowData = (float*)img.row(y).data;
        for (int x = 0; x < img.cols; ++x)
        {
            float val = pRowData[x];
            if (val == 0)
            {
                continue;
            }
            weightedCen.x += x*val;
            weightedCen.y += y*val;
            sum += val;
        }
    }
    weightedCen.x /= sum;
    weightedCen.y /= sum;

    weightedCen = weightedCen - center;

    return fastAtan2(weightedCen.y, weightedCen.x);
}

double localAngle_(const Mat& img, Point2f center, Mat mask)
{
#ifdef DEBUG_VIEW_INTERNAL_MAT
    Mat vImg = img / 255.0;
#endif
    Scalar meanScalar, stddevScalar;
    meanStdDev(img, meanScalar, stddevScalar, mask);
    Mat meanNormImg = img - meanScalar.val[0];
    float thre = stddevScalar.val[0] * 4;
    meanNormImg.setTo(0, meanNormImg < thre);
    meanNormImg.setTo(1, meanNormImg > thre);
    return localAngle_WeightedCen(meanNormImg, center);
}

void upperMajorityMask(const Mat& img, Mat& mask, float majority)
{
    Mat hist;
    majorityHistUpper(img, Mat(), hist, majority);
    float* pHistData = (float*)hist.data;
    int histSize = hist.rows > hist.cols ? hist.rows : hist.cols;
    float* pEndHistData = pHistData + histSize;
    int idx = 0;
    while (pHistData != pEndHistData)
    {
        if (*pHistData)
        {
            break;
        }
        idx++;
        pHistData++;
    }
    mask = (img >= idx);
}

void lowerMajorityHist(const Mat& img, const Mat& mask, Mat& hist, float majority)
{
    hist = calcHist(img, mask);

    // suppose there is only one peak
    float* pHistData = (float*)hist.data;
    float curRatio = 1.0;
    int totalNum = sum(hist).val[0];

    float* pFront = pHistData;
    float* pTail = pHistData + 254;
    while (pFront != pTail)
    {
        if (*pTail == 0)
        {
            pTail--;
            continue;
        }
        
        {
            curRatio -= *pTail / totalNum;
            *pTail = 0;
            pTail--;
        }
        if (curRatio <= majority)
        {
            break;
        }
    }

}

void majorityHistLower(const Mat& img, const Mat& mask, Mat& hist, float majority)
{
    hist = calcHist(img, mask);

    // suppose there is only one peak
    float* pHistData = (float*)hist.data;
    float curRatio = 1.0;
    int totalNum = sum(hist).val[0];

    float* pFront = pHistData;
    float* pTail = pHistData + 254;
    while (pFront != pTail)
    {
        if (*pTail == 0)
        {
            pTail--;
            continue;
        }

        {
            curRatio -= *pTail / totalNum;
            *pTail = 0;
            pTail--;
        }
        if (curRatio <= majority)
        {
            break;
        }
    }

}

void majorityHistUpper(const Mat& img, const Mat& mask, Mat& hist, float majority)
{
    hist = calcHist(img, mask);

    // suppose there is only one peak
    float* pHistData = (float*)hist.data;
    float curRatio = 1.0;
    int totalNum = sum(hist).val[0];

    float* pFront = pHistData;
    float* pTail = pHistData + 254;
    while (pFront != pTail)
    {
        if (*pFront == 0)
        {
            pFront++;
            continue;
        }

        {
            curRatio -= *pFront / totalNum;
            *pFront = 0;
            pFront++;
        }
        if (curRatio <= majority)
        {
            break;
        }
    }

}
void majorityHist(const Mat& img, const Mat& mask, Mat& hist, float majority)
{
    hist = calcHist(img, mask);

    // suppose there is only one peak
    float* pHistData = (float*)hist.data;
    float curRatio = 1.0;
    int totalNum = sum(hist).val[0];

    float* pFront = pHistData;
    float* pTail = pHistData + 254;
    while (pFront != pTail)
    {
        if (*pFront == 0)
        {
            pFront++;
            continue;
        }
        if (*pTail == 0)
        {
            pTail--;
            continue;
        }
        if (*pFront < *pTail)
        {
            curRatio -= *pFront / totalNum;
            *pFront = 0;
            pFront++;
        }
        else
        {
            curRatio -= *pTail / totalNum;
            *pTail = 0;
            pTail--;
        }
        if (curRatio <= majority)
        {
            break;
        }
    }

    
}

Mat lowerMajorityMask(const Mat& img, const Mat& mask, float majority)
{
    Mat hist;
    lowerMajorityHist(img, mask, hist, majority);
    float* pHistData = (float*)hist.data;

    Mat ret = mask.clone();

    for (int i = 0; i < img.rows; ++i)
    {
        uchar* pRowData = (uchar*)img.row(i).data;
        uchar* pMaskRowData = (uchar*)ret.row(i).data;
        for (int j = 0; j < img.cols; ++j)
        {
            uchar val = pRowData[j];
            if (!pHistData[val])
            {
                pMaskRowData[j] = 0;
            }
        }
    }

    return ret;
}

void meanStdDev(const Mat& img, const Mat& mask,
    double* pMean, double* pStdDev, float majority, int type)
{
    Mat hist;
    switch (type)
    {
    default:
    case 0:
        majorityHist(img, mask, hist, majority);
        break;
    case 1:
        majorityHistLower(img, mask, hist, majority);
        break;
    case 2:
        majorityHistUpper(img, mask, hist, majority);
        break;
    }

    // suppose there is only one peak
	Mat rowHist;// = toRowVec(hist);
    float* pHistData = (float*)rowHist.data;

    histMeanStddev<float>(rowHist, pMean, pStdDev);
}

float interpolate(float* pY, int n, float stepX, float x)
{
    int lIdx = (int)(x/stepX);
    int rIdx = lIdx + 1;
    if (rIdx > n - 1)
    {
        return pY[n - 1];
    }
    assert(lIdx >= 0 && lIdx < n && rIdx >= 0 && rIdx < n);
    float s = (x - lIdx*stepX)/stepX;
    float ly = pY[lIdx];
    float ry = pY[rIdx];
    return ly + (ry - ly)*s;
}

cv::Mat cocentricNorm(const Mat& img, Point2f center, const Mat& weightMat, float dstMeanVal)
{
    assert(weightMat.empty() || weightMat.type() == CV_32FC1);

    int w = img.cols;
    int h = img.rows;
    vector<Point2f> corners;
    corners.push_back(Point2f(0, 0));
    corners.push_back(Point2f(0, h));
    corners.push_back(Point2f(w, h));
    corners.push_back(Point2f(w, 0));
    vector<double> cornerDisVec;
    for_each(corners.begin(), corners.end(), [&](const Point2f& pt)
    {
        double dis = pointDis(center, pt);
        cornerDisVec.push_back(dis);
    });

    auto farthestCornerDis = max_element(cornerDisVec.begin(), cornerDisVec.end());
    float maxRadius = *farthestCornerDis;
    
    int radiusNum = floorf(maxRadius);
    //radiusNum = 20;
    float radiusStep = (maxRadius / radiusNum);
    Mat cocentricSumMat = Mat::zeros(1, radiusNum, CV_32FC1);
    float* pSumData = (float*)cocentricSumMat.data;
    Mat cocentricWeightSumMat = Mat::zeros(1, radiusNum, CV_32FC1);
    float* pWeightSumData = (float*)cocentricWeightSumMat.data;
    Mat radiusMat(img.rows, img.cols, CV_32FC1);

    for (int y = 0; y < h; y++)
    {        
        const Mat& imgRow = img.row(y);
        float* pImgRowData = (float*)imgRow.data;
        float* pRadiusRowData = (float*)radiusMat.row(y).data;

        float* pWeightRowData = NULL;
        if (!weightMat.empty())
        {
            pWeightRowData = (float*)weightMat.row(y).data;
        }

        for (int x = 0; x < w; x++)
        {
            //std::cout << x << " " << y << std::endl;
            float weight;
            if (pWeightRowData)
            {
                weight = pWeightRowData[x];
            }
            else
            {
                weight = 1.0;
            }
            float val = pImgRowData[x] * weight;
            float radius = pointDis(Point2f(x, y), center);
            pRadiusRowData[x] = radius;
            int radiusIdx0 = (int)(radius / radiusStep);
            assert(radiusIdx0 >= 0);
            int radiusIdx1 = radiusIdx0 + 1;
            if (radiusIdx0 >= radiusNum - 1)
            {
                pSumData[radiusNum - 1] += val;
                pWeightSumData[radiusNum - 1] += weight;
            }
            else
            {
                float s = (radius - radiusStep*radiusIdx0) / radiusStep;
                pSumData[radiusIdx0] += val*s;
                pSumData[radiusIdx1] += val*(1 - s);
                pWeightSumData[radiusIdx0] += s*weight;
                pWeightSumData[radiusIdx1] += (1 - s)*weight;
            }            
        }
    }

    for (int i = 0; i < radiusNum; ++i)
    {
        //float radius = (i*radiusStep + radiusStep) / 2;
        if (pWeightSumData[i] == 0)
        {

        }
        else
        {
            pSumData[i] /= pWeightSumData[i];
        }
    }

    Mat retMat = Mat::zeros(img.rows, img.cols, img.type());
	Mat normMask = Mat::zeros(img.rows, img.cols, img.type());
    for (int y = 0; y < h; y++)
    {
        float* pImgRowData = (float*)img.row(y).data;
        float* pRetRowData = (float*)retMat.row(y).data;
        float* pRadiusData = (float*)radiusMat.row(y).data;
		float *pNormData = (float *)normMask.row(y).data;
        for (int x = 0; x < w; x++)
        {
            float val = pImgRowData[x];
            float radius = pRadiusData[x];
            float mean = interpolate(pSumData, radiusNum, radiusStep, radius);
            if (mean == 0)
            {
                continue;
            }
            float newVal = (float)val * dstMeanVal / mean;
            
		//	if (newVal - dstMeanVal > -30)
			//{
				pRetRowData[x] = newVal;
				//pNormData[x] = 255;
		//	}
			//else
			//{
			//	continue;
			//}

        }
    }

#ifdef DEBUG_VIEW_INTERNAL_MAT
    Mat viewRetMat = retMat/255.0;
#endif 
	/*Mat retNromMask = normMask.mul(retMat);
	Mat normTest = retNromMask / 255.0;*/

    return retMat;
	//return normTest;
}

cv::Mat meanNorm(const Mat& img, const Mat& mask, float dstMeanVal, float majority /*= 1.0*/, PixelSelectionMethod method /*= LP_PIXEL_SELECT_ALL*/)
{
    double meanVal = 0;
    if (method == LP_PIXEL_SELECT_ALL)
    {
        meanVal = mean(img, mask).val[0];
    }
    else if (method == LP_PIXEL_SELECT_MAJORITY_FAST ||
        method == LP_PIXEL_SELECT_MAJORITY)
    {
        meanStdDev(img, mask, &meanVal, NULL, majority);
    }
    return img + (int)(dstMeanVal - meanVal);
}

// cv::Mat getRoiImg(const Mat& img, vector<Point2i> ptVec, int radius)
// {
//     Rect defectRoi(genRectCenRadius(ptVec, radius));
// 
//     defectRoi &= Rect(0, 0, img.cols, img.rows);
//     return Mat(img, defectRoi);
// }

// cv::Mat getRoiImg(const Mat& img, Point2i cen, int radius)
// {
//     Rect defectRoi(genRectCenRadius(cen, radius));
// 
//     defectRoi &= Rect(0, 0, img.cols, img.rows);
//     return Mat(img, defectRoi);
// }

cv::Mat filterY(const Mat& img, float* kernel, int size, int ddepth /*= CV_32FC1*/)
{
    Mat kernelMat(size, 1, CV_32FC1, kernel);

    Mat ret;
    cv::filter2D(img, ret, ddepth, kernelMat);

    return ret;
}

float circleDegree(const vector<Point>& contour)
{
    Rect br = boundingRect(contour);
    float r = sqrt((float)(br.width*br.width / 4 + br.height*br.height / 4));
    float circleArea = CV_PI*r*r;
    float area = contourArea(contour);
    if (circleArea < 0.00001)
    {
        return 0;
    }
    return area / circleArea;
}

float lpContourArea(const vector<Point>& contour)
{
    double area = 0;
    if (contour.size() == 1)
    {
        area = 1;
    }
    else if (contour.size() != 0)
    {
        area = contourArea(contour);
        area += arcLength(contour, true);
    }
    return area;
}

cv::Mat minMaxNorm(const Mat& img, double tarMin, double tarMax, const Mat& mask /*= Mat()*/)
{
    double minVal, maxVal;
    minMaxIdx(img, &minVal, &maxVal, 0, 0, mask);
    double s = ((tarMax - tarMin) / (maxVal - minVal));
    return img * s + (tarMin - minVal * s);
}

double longShortRatio(const Size& s)
{
    if (s.width > s.height)
    {
        if (s.height < 0.000001)
        {
            return 0;
        }
        return (double)s.width / (double)s.height;
    }
    else
    {
        if (s.width < 0.000001)
        {
            return 0;
        }
        return (double)s.height / (double)s.width;
    }
}

void closeShapesToConvex(const Mat& mask, Mat& closedMask)
{
    closedMask.create(mask.rows, mask.cols, mask.type());
    closedMask.setTo(0);

    Mat canvas;
    mask.copyTo(canvas);
    vector< vector<Point> > contours;
    findContours(canvas, contours, RETR_LIST, cv::CHAIN_APPROX_NONE);
    vector< vector<Point> > convextContours;
    for (size_t i = 0; i < contours.size(); ++i)
    {
        const vector<Point>& contour = contours[i];
        vector<Point> convexContour;
        convexHull(contour, convexContour);
        convextContours.push_back(convexContour);
    }
    fillPoly(closedMask, convextContours, Scalar(255, 255, 255));
}

cv::Mat genCircleMask(int rows, int cols, int type, 
    Point cen, double r, const Scalar& color, int thickness, int lineType /*= 8*/, int shift /*= 0*/)
{
    Mat mask = Mat::zeros(rows, cols, type);
    circle(mask, cen, r, color, thickness, lineType, shift);
    return mask;
}

void openOper(Mat& img, int w)
{
    Mat kernel(w, w, CV_8UC1);
    kernel.setTo(1);
    openOper(img, kernel);
}

void openOper(Mat& img, const Mat& kernel)
{
    erode(img, img, kernel);
    dilate(img, img, kernel);
}

void closeOper(Mat& img, int w)
{
    Mat kernel(w, w, CV_8UC1);
    kernel.setTo(1);
    closeOper(img, kernel);
}

void closeOper(Mat& img, const Mat& kernel)
{
    dilate(img, img, kernel);
    erode(img, img, kernel);
}

CV_WRAP GroupMatcher::GroupMatcher(
    int normType /*= NORM_L2*/, bool crossCheck /*= false*/)
    : BFMatcher(normType, crossCheck)
{

}

Ptr<DescriptorMatcher> GroupMatcher::clone(bool emptyTrainData /*= false*/) const
{
    return new GroupMatcher();
}

// no longer supported in opencv3.4.1
#if (CV_MAJOR_VERSION < 3)
AlgorithmInfo* GroupMatcher::info() const
{
    return NULL;
}
#endif

void GroupMatcher::knnMatchImpl(const Mat& queryDescriptors,
    vector<vector<DMatch> >& matches, 
    int k, 
    const vector<Mat>& masks /*= vector<Mat>()*/, 
    bool compactResult /*= false*/)
{
    BFMatcher::knnMatchImpl(queryDescriptors, matches, k, masks, 
        compactResult);


}

void GroupMatcher::radiusMatchImpl(const Mat& queryDescriptors,
    vector<vector<DMatch> >& matches, 
    float maxDistance,
    const vector<Mat>& masks /*= vector<Mat>()*/, 
    bool compactResult /*= false*/)
{
    BFMatcher::radiusMatchImpl(queryDescriptors,
        matches, maxDistance, masks, compactResult);
}


Mat getContourBoundedRoiImg(const Mat& src, const vector<Point>& contour, Rect* pRoiRect /* = NULL*/)
{
	Rect cr = boundingRect(contour);
	Mat cmask(cr.height, cr.width, CV_8UC1);
	cmask.setTo(0);
	cv::fillPoly(cmask, vector< vector<Point> >(1, contour), Scalar(255, 255, 255), 8, 0, cr.tl()*(-1.0));
	Mat localMask;
	Mat(src, cr).copyTo(localMask);
	if (pRoiRect)
	{
		*pRoiRect = cr;
	}
	return localMask & cmask;
}

void filterContours(const vector< vector<Point> >& contours, const Mat& srcImg,
    vector< vector<Point> >& filteredContours,
    int minArea, double minLength, double minLongShortRatio,
    double maxCircleDegree, vector<Point>* pAreaMaxContour)
{
    int areaMaxIdx = -1;
    double maxarea = -1;
    for (int j = 0; j < contours.size(); ++j)
    {
        const vector<Point>& contour = contours[j];
        if (contour.size() < 5)
        {
            continue;
        }

        int area = -1;
        if (!srcImg.empty())
        {
            Mat localMask = getContourBoundedRoiImg(srcImg, contour);
            area = countNonZero(localMask);
        }
        else
        {
            area = contourArea(contour);
        }
        
        RotatedRect rr = minAreaRect(contour);

        double l = max(rr.size.width, rr.size.height);
        if (area < minArea && l < minLength)
        {
            continue;
        }

        double cd = circleDegree(contour);
        if (cd > maxCircleDegree)
        {
            continue;
        }

        double lsr = longShortRatio(rr.size);
        if (lsr < minLongShortRatio)
        {
            continue;
        }

        if (area > maxarea)
        {
            maxarea = area;
            areaMaxIdx = j;
        }

        filteredContours.push_back(contour);
    }

    if (areaMaxIdx >= 0 && pAreaMaxContour)
    {
        *pAreaMaxContour = contours[areaMaxIdx];
    }
}

void filterContours(Mat hsvColorThresImg,
	int minArea, double minLength, double minLongShortRatio,
	double maxCircleDegree, vector<Point>* pAreaMaxContour /*= NULL*/)
{
	vector< vector<Point> > contours;
	Mat canvas = hsvColorThresImg.clone();
	findContours(canvas, contours, RETR_LIST, CHAIN_APPROX_NONE);
	canvas.setTo(0);

	vector< vector<Point> > filteredContours;
    filterContours(contours, hsvColorThresImg, filteredContours,
        minArea, minLength, minLongShortRatio, maxCircleDegree, pAreaMaxContour);

	// redraw
	cv::fillPoly(canvas, filteredContours, Scalar(255, 255, 255));
	hsvColorThresImg &= canvas;
}


int filterSmallAndFindMaxContours(vector< vector<Point> >& contours, double minArea)
{
	vector< vector<Point> > filteredContours;
	int maxindex = -1;
	double maxarea = -1;
	for (int j = 0; j < contours.size(); ++j)
	{
		const vector<Point>& contour = contours[j];
		double area = contourArea(contour);
		if (area > minArea)
		{
			filteredContours.push_back(contour);
		}

		if (area > maxarea)
		{
			maxarea = area;
			maxindex = j;
		}
	}
	contours = filteredContours;
	return maxindex;
}


void filterContours(Mat& img, double minArea)
{
	Mat canvas;
	img.copyTo(canvas);

	vector< vector<Point> > contours;
	findContours(canvas, contours, RETR_LIST, cv::CHAIN_APPROX_NONE);
	filterSmallAndFindMaxContours(contours, minArea);
	canvas.setTo(0);
	cv::fillPoly(canvas, contours, Scalar(255, 255, 255));

	img = canvas;
}


Mat genInRangeMask(const Mat& src, std::map<int, pair<Scalar, Scalar>>& colorRanges)
{
	Mat mask = Mat::zeros(src.rows, src.cols, CV_8UC1);

	for (auto iter = colorRanges.begin(); iter != colorRanges.end(); ++iter)
	{
		int key = iter->first;
		Scalar low = iter->second.first;
		Scalar high = iter->second.second;
		Mat imgThresholded;
		inRange(src, low, high, imgThresholded);
		mask |= imgThresholded;
	}

	return mask;
}

void localCloseOper(Mat& img, vector< vector<Point> >& contours, float longerScale)
{
    Mat tmp = img.clone();
    for (int i = 0; i < contours.size(); ++i)
    {
        const vector<Point>& contour = contours[i];
        RotatedRect rr = minAreaRect(contour);
        int longer = cv::max(rr.size.width, rr.size.height)*longerScale;

        Rect br = boundingRect(contour);
        br.x -= longer*1.5;
        br.y -= longer*1.5;
        br.width += longer * 3;
        br.height += longer * 3;

        br.x = cv::max(0, br.x);
        br.y = cv::max(0, br.y);
        br.width = cv::min(img.cols - br.x - 1, br.width);
        br.height = cv::min(img.rows - br.y - 1, br.height);

        Mat roiMat = Mat(img, br).clone();
        closeOper(roiMat, Mat::ones(longer, 1, CV_8UC1));
        br.x -= 1;
        br.y -= 1;
        br.x = cv::max(0, br.x);
        br.y = cv::max(0, br.y);

        roiMat.copyTo(Mat(tmp, br));
    }
    img |= tmp;
}

Mat genWithCopiedCols(const Mat& m, int multiples = 3)
{
    Mat ret(m.rows, m.cols * multiples, m.type());
    for (int i = 0; i < multiples; ++i)
    {
        m.copyTo(ret.colRange(i*m.cols, i*m.cols + m.cols));
    }
    return ret;
}



double matDisWithReversedRows(const Mat& m0, const Mat& m1)
{
    assert(m0.rows == m1.rows);
    assert(m0.cols == m1.cols);

    int rows = m0.rows;
    int cols = m0.cols;

    Mat rowDisVec(rows, 1, CV_64FC1);
    double* pRowDisVec = (double*)rowDisVec.data;
    for (int i = 0; i < rows; ++i)
    {
        Mat row0 = 255 - m0.row(i);
        Mat row1 = 255 - m1.row(rows - i - 1);
        //Mat weightedRow0 = 255 - row0;
        //*pRowDisVec = ((row0 - row1).dot(weightedRow0));
        Mat row0f;
        row0.convertTo(row0f, CV_32FC1);
        Mat row1f;
        row1.convertTo(row1f, CV_32FC1);
        *pRowDisVec = compareHist(row0f, row1f, CV_COMP_CORREL);
        pRowDisVec++;
    }

    Mat kernel = cv::getGaussianKernel(rows * 2, 1).rowRange(rows, rows * 2);

    return rowDisVec.dot(kernel);
}

double matDis(const Mat& m0, const Mat& m1)
{
    assert(m0.rows == m1.rows);
    assert(m0.cols == m1.cols);

    int rows = m0.rows;
    int cols = m0.cols;

    Mat rowDisVec(rows, 1, CV_64FC1);
    double* pRowDisVec = (double*)rowDisVec.data;
    for (int i = 0; i < rows; ++i)
    {
        Mat row0 = 255 - m0.row(i);
        Mat row1 = 255 - m1.row(i);

        Mat row0f;
        row0.convertTo(row0f, CV_32FC1);
        Mat row1f;
        row1.convertTo(row1f, CV_32FC1);
        *pRowDisVec = compareHist(row0f, row1f, CV_COMP_CORREL);
        pRowDisVec++;
    }

    Mat kernel = cv::getGaussianKernel(rows * 2, 1).rowRange(rows, rows * 2);

    return rowDisVec.dot(kernel);
}

int searchBestMatchDx(int searchRadius, int startCol, int endCol, const Mat& curRow, const Mat& preRowMid)
{
    vector<double> valVec;
    for (int dx = -searchRadius; dx <= searchRadius; dx++)
    {
        int curStartCol = startCol + dx;
        int curEndCol = endCol + dx;
        Mat curRowMid = curRow.colRange(curStartCol, curEndCol);
        double val = -matDisWithReversedRows(curRowMid, preRowMid);
        valVec.push_back(val);
    }
    auto minIter = std::min_element(valVec.begin(), valVec.end());
    int bestDx = minIter - valVec.begin() - searchRadius;

    return bestDx;
}

Mat searchBestMatchSubRowMat(const Mat& img, int startRow, int rowWidth, int searchRadius, const Mat& preRowMid)
{
    Mat curRow = genWithCopiedCols(img.rowRange(startRow, rowWidth + startRow), 3);
    int startCol = img.cols;
    int endCol = img.cols * 2;

    int bestDx = searchBestMatchDx(searchRadius, startCol, endCol, curRow, preRowMid);

    return curRow.colRange(startCol + bestDx, endCol + bestDx);
}

void xEnergeyMat(const Mat& img, Mat& energyMat, 
    int kernelWidth = 12, int backgroundThre = 40, int method = 2)
{
    switch (method)
    {
    case 0:
    {
        // sobel edge
        Mat sobelMat;
        cv::Sobel(img, sobelMat, CV_32FC1, 1, 0);
        sobelMat = cv::abs(sobelMat);
        energyMat = sobelMat;
    }
        break;
    case 1:
    {
        // edge with first derivative
        Mat kernel = cv::getGaussianKernel(kernelWidth, 4).t();
        Mat halfKernel(kernel.colRange(0, kernel.cols / 2));
        halfKernel *= -1.0;
        filter2D(img, energyMat, CV_32FC1, kernel);
        energyMat = abs(energyMat);
    }
        break;
    case 2:
    {
        // edge with second derivative
        Mat kernel = cv::getGaussianKernel(kernelWidth, 4).t();
        Mat halfKernel(kernel.colRange(0, kernel.cols / 2));
        halfKernel *= -1.0;
        filter2D(img, energyMat, CV_32FC1, kernel);
        energyMat = abs(energyMat);

        Mat secondEnergyMat;
        filter2D(energyMat, secondEnergyMat, CV_32FC1, kernel);
        secondEnergyMat = abs(secondEnergyMat);

        energyMat = secondEnergyMat;
    }
        break;
    case 3:
    {
        // darker edge with first derivative
        Mat kernel = cv::getGaussianKernel(kernelWidth, 4).t();
        Mat gaussMat;
        filter2D(img, gaussMat, CV_32FC1, kernel);
        gaussMat = 255.0 - gaussMat;
        Mat halfKernel(kernel.colRange(0, kernel.cols / 2));
        halfKernel *= -1.0;
        filter2D(img, energyMat, CV_32FC1, kernel);
        energyMat = abs(energyMat) + gaussMat*0.2;
    }
    case 4:
    {
        // darker edge with thresholding
        Mat threMat = img < backgroundThre;
        converToType(threMat, CV_32FC1);

        Mat kernel = cv::getGaussianKernel(kernelWidth, 4).t();

        Mat halfKernel(kernel.colRange(0, kernel.cols / 2));
        halfKernel *= -1.0;
        filter2D(img, energyMat, CV_32FC1, kernel);
        energyMat = abs(energyMat) + threMat;
    }
    break;
    }
}

void findBoundaryY(const Mat& img, vector<int>& vec, vector<float>& energyVec,
    int gradientKernelWidth = 12,
    int smoothKernelWidth = 32, 
    float smoothWeight = 3.0, 
    int backgroundThre = 40, 
    int energyType = 4)
{
    // generate energy mat (sobel)
    Mat energyMat;
    xEnergeyMat(img, energyMat, gradientKernelWidth, backgroundThre, energyType);

    int n = 4;
    Mat energyMatBack = energyMat.clone();

    Mat smoothImg, showMat;
    {
        Mat kernel = cv::getGaussianKernel(smoothKernelWidth, 4.0).t();
        filter2D(img, smoothImg, CV_32FC1, kernel);
    }

    float w = pow(10, smoothWeight);
    dynamicProgramYWithSmooth(energyMat, n, w/255.0, smoothImg);
    findMaxYPath(energyMat, vec, energyVec, n);

    recoverPathEnergy(energyVec);
}

void findBoundaryY(const Mat& img0, const Mat& img1, int targetWidth,
    vector<int>& vec, vector<float>& energyVec)
{
    // generate energy mat
    Mat energyMat0, energyMat1;
    xEnergeyMat(img0, energyMat0);
    xEnergeyMat(img1, energyMat1);

    // generate smooth mat (gauss)
    Mat smoothImg0, smoothImg1, showMat;
    {
        Mat kernel = cv::getGaussianKernel(31, 4.0).t();
        filter2D(img0, smoothImg0, CV_32FC1, kernel);
        filter2D(img1, smoothImg1, CV_32FC1, kernel);
    }

    int n = 4;
    float sw = 100.0 / 255.0;
    dynamicProgramYWithSmooth(energyMat0, energyMat1,
        smoothImg0, smoothImg1, targetWidth, n, sw);

    findMaxYPath(energyMat0, vec, energyVec, n);

    recoverPathEnergy(energyVec);
}

// assuming with two boundaries (left and right)
void autoAlignEachRowGloballyWithIndependentBoundaries(Mat& img)
{
    // divide into two part: left and right
    Mat leftImg(img.colRange(0, img.cols / 2));
    Mat rightImg(img.colRange(img.cols / 2, img.cols));

    // search a boundary for each part
    vector<int> leftVec, rightVec;
    vector<float> leftEnergyVec, rightEnergyVec;
    findBoundaryY(leftImg, leftVec, leftEnergyVec);
    findBoundaryY(rightImg, rightVec, rightEnergyVec);

    // draw path
    Mat canvas = img.clone();
    drawPointsY<uchar>(leftVec, canvas, 0, 0, 255);
    drawPointsY<uchar>(rightVec, canvas, canvas.cols / 2, 0, 255);

    // align centers of each row
    int rightBaseX = img.cols / 2;
    int centerX = (leftVec[0] + rightVec[0] + rightBaseX) / 2;
    Mat ret = img.clone();
    ret.setTo(0);

    // move each row to align
    for (int i = 0; i < img.rows; ++i)
    {
        int curCenterX = (leftVec[i] + rightVec[i] + rightBaseX) / 2;
        int dx = centerX - curCenterX;
        Mat curRow = genWithCopiedCols(img.row(i), 3);
        curRow.colRange(img.cols - dx, img.cols * 2 - dx).copyTo(ret.row(i));
    }

    ret.copyTo(img);
}

void autoAlignEachRowWithWeightedXCen(Mat& img, int tarCenX, vector<int>& dxVec)
{
    dxVec.resize(img.rows, 0);

    for (int i = 0; i < img.rows; ++i)
    {
        Mat r = img.row(i);
        uchar* p = r.data;
        uchar* ep = p + r.cols;
        int j = 0;
        float wx = 0;
        float ws = 0;
        while (p != ep)
        {
            float w = (*p) * (*p);
            ws += w;
            wx += j*w;
            j++;
            p++;
        }
        wx /= ws;
        int dx = floorf(tarCenX - wx);
        dxVec[i] = dx;
    }

    alignEachRow(img, dxVec);
}

void alignEachRow(Mat& img, const vector<int>& dxVec)
{
    Mat ret = img.clone();
    ret.setTo(0);

    for (int i = 0; i < img.rows; ++i)
    {
        Mat curRow = genWithCopiedCols(img.row(i), 3);
        int dx = dxVec[i];
        curRow.colRange(img.cols - dx, img.cols * 2 - dx).copyTo(ret.row(i));
    }

    ret.copyTo(img);
}

void autoAlignEachRowGloballyWithWidthConstraint(Mat& img, int tarCenX, vector<int>& dxVec)
{
    // divide into two part: left and right
    Mat leftImg(img.colRange(0, img.cols / 2));
    Mat rightImg(img.colRange(img.cols / 2, img.cols));

    // search a boundary for each part
    vector<int> leftVec, rightVec;
    vector<float> leftEnergyVec, rightEnergyVec;
    findBoundaryY(leftImg, leftVec, leftEnergyVec);
    findBoundaryY(rightImg, rightVec, rightEnergyVec);

    // draw path
    Mat canvas = img.clone();
    drawPointsY<uchar>(leftVec, canvas, 0, 0, 255);
    drawPointsY<uchar>(rightVec, canvas, canvas.cols / 2, 0, 255);

    float avrLeft = sum<int>(leftVec.begin(), leftVec.end()) / leftVec.size();
    float avrRight = sum<int>(rightVec.begin(), rightVec.end()) / rightVec.size();
    int tarWidth = avrRight + leftImg.cols - avrLeft;

    leftVec.clear();
    leftEnergyVec.clear();
    findBoundaryY(leftImg, rightImg, tarWidth, leftVec, leftEnergyVec);

    rightVec = leftVec;
    add<int>(rightVec.begin(), rightVec.end(), tarWidth);

    drawPointsY<uchar>(leftVec, canvas, 0, 0, 180);
    drawPointsY<uchar>(rightVec, canvas, 0, 0, 180);

    // align centers of each row
    int centerX = leftVec[0] + tarWidth / 2;
    if (tarCenX >= 0)
    {
        centerX = tarCenX;
    }
    Mat ret = img.clone();
    ret.setTo(0);

    // move each row to align
    dxVec.resize(img.rows, 0);
    for (int i = 0; i < img.rows; ++i)
    {
        int curCenterX = leftVec[i] + tarWidth / 2;
        int dx = centerX - curCenterX;
        dxVec[i] = dx;
        Mat curRow = genWithCopiedCols(img.row(i), 3);
        curRow.colRange(img.cols - dx, img.cols * 2 - dx).copyTo(ret.row(i));
    }

    ret.copyTo(img);
}

void autoAlignEachRowLocally(Mat& img, int searchRadius /*= 5*/, int rowWidth /*= 1*/, const Mat& preImg /*= Mat()*/)
{
    Mat ret = img.clone();
    ret.setTo(0);

    Mat preRow;
    if (!preImg.empty())
    {
        preRow = genWithCopiedCols(preImg.rowRange(0, rowWidth), 3);
    }
    else
    {
        preRow = genWithCopiedCols(img.rowRange(0, rowWidth), 3);
    }

    Mat preRowMid = preRow.colRange(img.cols, img.cols * 2);
    if (!preImg.empty())
    {
        preRowMid = searchBestMatchSubRowMat(img, 0, rowWidth, searchRadius, preRowMid);
    }
    preRowMid.copyTo(ret.rowRange(0, rowWidth));

    for (int i = rowWidth; i <= img.rows - rowWidth; i += rowWidth)
    {
        preRowMid = searchBestMatchSubRowMat(img, i, rowWidth, searchRadius, preRowMid);

        preRowMid.copyTo(ret.rowRange(i, i + rowWidth));
    }

    ret.copyTo(img);
}

void autoAlignEachRowWithAutoThreshold(Mat& img, int tarCenX, vector<int>& dxVec)
{
    dxVec.resize(img.rows, 0);

    for (int i = 0; i < img.rows; ++i)
    {
        Mat row(img.row(i));
        Mat trow;
        threshold(row, trow, 0, 255, THRESH_OTSU);
        uchar* p = trow.data;
        uchar* ep = p + trow.cols;
        int l, r;
        while (p != ep)
        {
            if (*p)
            {
                l = p - trow.data;
                break;
            }
            p++;
        }
        p = ep - 1;
        do
        {
            if (*p)
            {
                r = p - trow.data;
                break;
            }
            p--;
        } while (p != trow.data);
        int curCenX = floorf((l + r) / 2.0);
        dxVec[i] = tarCenX - curCenX;
    }

    alignEachRow(img, dxVec);
}

void alignEachRowRightBound(Mat& img, int tarCen, vector<int>& dxVec,
    int gradientKernelWidth /*= 12*/,
    int smoothBoundaryKernelWidth /*= 32*/, 
    float smoothBoundaryWeight /*= 3.0*/,
    int backgroundThre /*= 40*/)
{
    // right part
    Mat rightImg(img.colRange(img.cols / 2, img.cols));

    // search a boundary
    vector<int> rightVec;
    vector<float> rightEnergyVec;
    findBoundaryY(rightImg, rightVec, rightEnergyVec, gradientKernelWidth,
        smoothBoundaryKernelWidth, smoothBoundaryWeight, backgroundThre);

    // draw path
    Mat canvas = img.clone();
    drawPointsY<uchar>(rightVec, canvas, canvas.cols / 2, 0, 255);

    // align centers of each row
    int rightBaseX = img.cols / 2;
    int centerX = tarCen;
    Mat ret = img.clone();
    ret.setTo(0);

    // move each row to align
    for (int i = 0; i < img.rows; ++i)
    {
        int dx = img.cols - rightVec[i] - rightBaseX - 1;
        dxVec.push_back(dx);
        Mat curRow = genWithCopiedCols(img.row(i), 3);
        curRow.colRange(img.cols - dx, img.cols * 2 - dx).copyTo(ret.row(i));
    }

    ret.copyTo(img);
}

cv::Mat genDirColorImg(const Mat& img, Mat* pValueMat /*= NULL*/)
{
    Mat hsvImg(img.rows, img.cols, CV_8UC3);
    vector<Mat> channels(3);
    channels[0] = img;
    channels[1] = Mat::zeros(img.rows, img.cols, CV_8UC1);
    channels[2] = channels[1];
    channels[1].setTo(255);
    if (pValueMat)
    {
        channels[2] = *pValueMat;
    }
    else
    {
        channels[2].setTo(255);
    }
    merge(channels, hsvImg);
    Mat rgbImg;
    cvtColor(hsvImg, rgbImg, COLOR_HSV2BGR);
    return rgbImg;
}

void genSobelDir(Mat& img)
{
    Mat srcImg = img.clone();
    Mat sobelX, sobelY;
    genSobelImage(srcImg, &sobelX, &sobelY);
    img = genSobelDir(sobelX, sobelY);
}

cv::Mat genSobelDir(const Mat& sobelX, const Mat& sobelY)
{
    Mat img(sobelX.rows, sobelX.cols, CV_8UC1);
    for (int i = 0; i < img.rows; ++i)
    {
        Mat row = img.row(i);
        float* pDataX = (float*)sobelX.row(i).data;
        float* pDataY = (float*)sobelY.row(i).data;
        uchar* pData = row.data;
        for (int j = 0; j < img.cols; ++j)
        {
            float dx = pDataX[j];
            float dy = pDataY[j];
            float angle = 0;
            if (dx == 0 && dy == 0)
            {
                angle = 0;
            }
            else
            {
                angle = atan2f(dy, dx);
                if (angle < 0)
                {
                    angle += 2.0 * M_PI;
                }
            }
            pData[j] = floorf((angle / M_PI) * 90);
        }
    }
    return img;
}

void genContrastImg(const Mat& img, Mat& contrastImg, const Mat& kernelMat)
{
    Mat maxMat, minMat;
    dilate(img, maxMat, kernelMat);
    erode(img, minMat, kernelMat);

    contrastImg = maxMat - minMat;
}

void genContrastImg(const Mat& img, Mat& contrastImg, int ksize)
{
    Mat kernelMat(ksize, ksize, CV_8UC1);
	genContrastImg(img, contrastImg, kernelMat);
}

void genYContrastImg(const Mat& img, Mat& constrastImg, int ksize)
{
	Mat kernelMat(ksize, 1, CV_8UC1);
    genContrastImg(img, constrastImg, kernelMat);
}

void genHalfContrastImg(const Mat& img, Mat& contrastImg, const Mat& kernelMat)
{
    assert(img.type() == CV_8UC1);

    uchar* pData = img.data;
    int wstep = img.step;
    int kernelXStep = kernelMat.cols;
    int kernelYStep = kernelMat.rows;

    contrastImg.create(img.rows / kernelXStep, img.cols / kernelYStep, CV_8UC1);
    uchar* pContrastData = contrastImg.data;
    int wContrastStep = contrastImg.step;

    for (int y = 0; y < img.rows; y += kernelYStep)
    {
        uchar* pRowData = pData;
        uchar* pContrastRowData = pContrastData;
        for (int x = 0; x < img.cols; x += kernelXStep)
        {
            Mat roiMat(kernelMat.rows, kernelMat.cols, CV_8UC1, pRowData);
            roiMat.step = wstep;
            


            pRowData += kernelXStep;
            pContrastRowData += 1;
        }
        pData += wstep*kernelYStep;
        pContrastData += wContrastStep;
    }
}

void genHalfContrastImg(const Mat& img, Mat& contrastImg, int ksize)
{

}

void cvtColor(Mat& img, int t)
{
    Mat tImg;
    cvtColor(img, tImg, t);
    img = tImg;
}

cv::Mat rotateImage(const Mat& img, Point2f cen, float degree)
{
    Mat t = getRotationMatrix2D(cen, degree, 1.0);
    Mat rImg;
    warpAffine(img, rImg, t, img.size(), INTER_CUBIC, BORDER_CONSTANT);
    return rImg;
}

cv::Mat normAngle(const Mat& img, Mat mask)
{
    Point2f cen(img.cols / 2.0, img.rows / 2.0);
    double angle = localAngle_(img, cen, mask);
    return rotateImage(img, cen, -angle);
}

float normAngle(float angle)
{
    angle = angle - (int)(angle / 360.0) * 360;
    if (angle < 0)
    {
        angle += 360.0;
    }
    return angle;
}

void printMatInfo(const Mat& m, int baseIndentNum /*= 0*/, int indent /*= 4*/)
{
    printIndent(baseIndentNum, indent);
    std::cout << "rows: " << m.rows << ", " << 
        "cols: " << m.cols << ", " << 
        "channels: " << m.channels() << ", " << 
        "elementSize: " << m.elemSize() << "; ";
}

string templateMatchMethod2Str(int method)
{
    switch (method)
    {
    case cv::TM_SQDIFF:
        return "TM_SQDIFF";
    case cv::TM_SQDIFF_NORMED:
        return "TM_SQDIFF_NORMED";
    case cv::TM_CCORR:
        return "TM_CCORR";
    case cv::TM_CCORR_NORMED:
        return "TM_CCORR_NORMED";
    case cv::TM_CCOEFF:
        return "TM_CCOEFF"; 
    case cv::TM_CCOEFF_NORMED:
        return "TM_CCOEFF_NORMED";
    default:
        return "";
    }
}

void printIndent(int indentNum, int indent /*= 4*/)
{
    for (int i = 0; i < indentNum; i++)
    {
        for (int j = 0; j < indent; j++)
        {
            std::cout << " ";
        }
    }
}

Mat genSimpleXGradient(const Mat& m)
{
    Mat kernel(1, 2, CV_32FC1);
    float* p = (float*)kernel.data;
    p[0] = -1;
    p[1] = 1;
    Mat dm;
    filter2D(m, dm, CV_32FC1, kernel,
        Point(-1, -1), 0, BORDER_REFLECT);
    return dm;
}

void uniformLocalMinExtremas(const Mat& vec, Mat& localExtremaValVec,
    vector<int>& idxVec, int n, int refineRange)
{
    idxVec.resize(n);

    float step = (float)(vec.cols - 1) / (float)n;
    float* pVec = (float*)vec.data;
    localExtremaValVec.create(1, n + 1, CV_32FC1);
    float* pExtremaValVec = (float*)localExtremaValVec.data;

    float* pVecData = (float*)vec.data;
    for (int i = 0; i < n; ++i)
    {
        int idx = floorf(step*i);
        int sIdx = idx - refineRange;
        int eIdx = idx + refineRange;
        sIdx = max(0, sIdx);
        eIdx = min(vec.cols - 1, eIdx) + 1;
        float* pExtrema = std::min_element(pVec + sIdx, pVec + eIdx);
        idxVec[i] = pExtrema - pVec;
        pExtremaValVec[i] = *pExtrema;
    }

    pExtremaValVec[n] = pExtremaValVec[0];
}

void equalizeHist(const Mat& src, Mat& dst, const Mat& mask /*= Mat()*/)
{
    if (mask.empty())
    {
        cv::equalizeHist(src, dst);
        return;
    }
    Mat hist = calcHist(src, mask);
    hist *= 255.0 / countNonZero(mask);

    float* pHistData = (float*)hist.data;
    Mat mapHist = Mat::zeros(hist.rows, hist.cols, CV_32FC1);
    float* pMapHistData = (float*)mapHist.data;
    pMapHistData[0] = pHistData[0];
    for (int i = 1; i < 256; ++i)
    {
        pMapHistData[i] = pMapHistData[i - 1] + pHistData[i];
    }

    dst.create(src.size(), src.type());
    for (int y = 0; y < dst.rows; ++y)
    {
        uchar* pDstData = dst.row(y).data;
        uchar* pSrcData = src.row(y).data;
        for (int x = 0; x < dst.cols; ++x)
        {
            pDstData[x] = pMapHistData[pSrcData[x]];
        }
    }
}

void removeHighlights(const Mat& src, Mat& dst, float thre)
{
    dst = Mat::zeros(src.size(), src.type());
    Mat hightlightMask = src < thre;
    src.copyTo(dst, hightlightMask);
}

void genSectorSumVec(const Mat& img, const Mat& weightMat, Mat& hist, 
    Mat& weightHist, const Point2f& cen, float angleStep,
    Mat* pAngMat /*= NULL*/, Mat* pMagMat /*= NULL*/)
{
    int count = floorf(360.0 / angleStep);
    hist = Mat::zeros(1, count, CV_32FC1);
    weightHist = Mat::zeros(1, count, CV_32FC1);
    float* pData = (float*)img.data;
    float* pWeightData = (float*)weightMat.data;
    float* pHistData = (float*)hist.data;
    float* pWeightHistData = (float*)weightHist.data;
    float *x = new float[img.cols];
    for (int i = 0; i < img.cols; ++i) x[i] = i - cen.x;
    Mat xRow(1, img.cols, CV_32FC1, x);
    Mat magMat(img.rows, img.cols, CV_32FC1);
    Mat angMat(img.rows, img.cols, CV_32FC1);
    for (int i = 0; i < img.rows; ++i)
    {
        Mat yRow = Mat::ones(xRow.size(), xRow.type())*(i - cen.y);

        Mat magRow, angRow;
        cartToPolar(xRow, yRow, magRow, angRow, true);
        magRow.copyTo(magMat.row(i));
        angRow.copyTo(angMat.row(i));

        float* pAngleData = (float*)angRow.data;
        float* pRadiusData = (float*)magRow.data;
        for (int j = 0; j < img.cols; ++j)
        {            
			if (i == 100 && j == 90)
			{
				int a = 0;
			}
            float val = pData[j];
            float weight = pWeightData[j];
            float radius = pRadiusData[j];
            if (radius == 0)
            {
                continue;
            }
            float angle = pAngleData[j];

            val *= weight;

            float angleRange = 28.6479 / radius;
            float langle = angle - angleRange;
            float rangle = angle + angleRange;

            int lIntAngle = (int)(langle);
            int rIntAngle = (int)rangle;
            int li = normAngle(lIntAngle);
            int ri = normAngle(rIntAngle);
            float totalRange = angleRange*2.0;
			//pHistData[li] += val*(langle - ceil(langle)) / totalRange;
            pHistData[li] += val*(1 - langle + lIntAngle) / totalRange;
			//pWeightHistData[li] += weight*(langle - ceil(langle)) / totalRange;
			pWeightHistData[li] += weight*(1 - langle + lIntAngle) / totalRange;
            pHistData[ri] += val*(rangle - rIntAngle) / totalRange;
            pWeightHistData[ri] += weight*(rangle - rIntAngle) / totalRange;
            lIntAngle++;
#if defined(LITTLE_CPP11)
			if (_isnan(pHistData[li]) || _isnan(pHistData[ri]))
#else
            if (isnan(pHistData[li]) || isnan(pHistData[ri]))
#endif
            {
                waitKey();
            }
            while (lIntAngle < rIntAngle)
            {
                li = normAngle(lIntAngle);
                pHistData[li] += val / totalRange;
                pWeightHistData[li] += weight / totalRange;
                lIntAngle++;
            }
        }
        pData += img.step1();
        pWeightData += weightMat.step1();
    }

    if (pAngMat)
    {
        *pAngMat = angMat;
    }
    if (pMagMat)
    {
        *pMagMat = magMat;
    }

    free(x);
}

Mat matDivideNonzero(const Mat& m0, const Mat& m1)
{
    // m0/m1
    Mat m1copy = m1.clone();
    m0.copyTo(m1copy, m1copy < 0.0000001);
    return m0 / m1copy;
}


Mat matDivideNonzero(float m0, const Mat& m1)
{
    // m0/m1
    Mat m1copy = m1.clone();
    m1copy.setTo(m0, m1 < 0.0000001);
    return m0 / m1copy;
}


void normSectors(Mat& _img, Mat weightMat, const Point2f& cen,
    float angleStep, float tarVal)
{
    if (weightMat.empty())
    {
        weightMat = Mat::ones(_img.size(), CV_32FC1);
    }
    Mat img = _img;
    if (img.channels() != 1)
    {
        img = getFirstChannel(img);
    }
    if (img.type() != CV_32FC1)
    {
        converToType(img, CV_32FC1);
    }
    int count = floorf(360.0 / angleStep);
    Mat hist = Mat::zeros(1, count, CV_32FC1);
    Mat weightHist = Mat::zeros(1, count, CV_32FC1);
    Mat magMat(img.rows, img.cols, CV_32FC1);
    Mat angMat(img.rows, img.cols, CV_32FC1);

    genSectorSumVec(img, weightMat, hist, weightHist, cen, angleStep,
        &angMat, &magMat);

    hist = matDivideNonzero(hist, weightHist);

#ifdef DEBUG_VIEW_INTERNAL_MAT
    Mat vAngleMat = angMat / 360;
    Mat vMagMat = magMat / sqrt(img.cols*img.cols / 4.0 + img.rows*img.rows / 4.0);
    Mat vHist = hist;
    vHist = minMaxNorm(vHist, 0, 1.0);
    Mat vWeightHist = weightHist;
    vWeightHist = minMaxNorm(vWeightHist, 0, 1.0);
#endif

    Mat scaleHist = matDivideNonzero(tarVal, hist);

    applySectorScales(img, scaleHist, angMat);

    _img = img;
}

void normSectors_tarImg(Mat& img, Mat weightMat, const Point2f& cen, float angleStep, 
    const Mat& tarImg)
{
    Mat hist, weightHist, angMat, magMat;
    genSectorSumVec(img, weightMat, hist, weightHist, cen, angleStep,
        &angMat, &magMat);

#ifdef DEBUG_VIEW_INTERNAL_MAT
    Mat vHist0 = minMaxNorm(hist, 0, 1.0);
    Mat vWeightHist = minMaxNorm(weightHist, 0, 1.0);
#endif

    hist = matDivideNonzero(hist, weightHist);

#ifdef DEBUG_VIEW_INTERNAL_MAT
    Mat vHist1 = hist / 255.0;
#endif

    Mat tarHist;
    genSectorSumVec(tarImg, weightMat, tarHist, weightHist, cen, angleStep);
    tarHist = matDivideNonzero(tarHist, weightHist);

#ifdef DEBUG_VIEW_INTERNAL_MAT
    Mat vTarHist = tarHist/255.0;
#endif

    normSectorsMeans_tarHist(img, hist, cen, angleStep, tarHist, angMat);
}

void normSectorsMeans_tarHist(Mat& img, const Mat& hist, 
    const Point2f& cen, float angleStep, const Mat& tarHist, const Mat& angMat)
{
    Mat scaleHist = matDivideNonzero(tarHist, hist);

#ifdef DEBUG_VIEW_INTERNAL_MAT
    Mat vScaleHist = minMaxNorm(scaleHist, 0, 1.0);
#endif
    applySectorScales(img, scaleHist, angMat);
}

void applySectorScales(Mat& img, const Mat& scaleHist, const Mat& angMat)
{
    float* pData = (float*)img.data;
    float* pHistData = (float*)scaleHist.data;
    for (int i = 0; i < img.rows; ++i)
    {
        Mat angRow = angMat.row(i);

        float* pAngleData = (float*)angRow.data;
        for (int j = 0; j < img.cols; ++j)
        {
            float angle = pAngleData[j];
            int base = (int)angle;
            float s = pHistData[base];
            pData[j] *= s;
        }
        pData += img.step1();
    }
}

cv::Point2f imgCen(const Mat& img)
{
    return Point2f(img.cols / 2.0, img.rows / 2.0);
}

cv::Mat getForeImage(const Mat & src, const Mat &backgroundImg)
{
	Mat resizedBackgroundImg = backgroundImg;
	if (backgroundImg.size() != src.size()) {
		resize(backgroundImg, resizedBackgroundImg, src.size());
	}
	return (src - resizedBackgroundImg);
}
#define ALG_RESIZE_IMAGE_WIDTH 416.0

// cv::Mat findCircleObject(const Mat &src, const Mat& backgroundImg, 
//     int nThres /*= 20*/, luffy_base::luffyCircle *pCircle /*= NULL*/)
// {
// 	if (src.empty() || backgroundImg.empty() || src.rows < 500) {
// 		return Mat();
// 	}
// 
// 	assert(backgroundImg.type() == CV_8UC1);
// 	Mat imgTmp, imgBinary;
// 	const cv::Size cSize = cv::Size(ALG_RESIZE_IMAGE_WIDTH, floorf(ALG_RESIZE_IMAGE_WIDTH / (float)src.cols*(float)src.rows));
// 	cv::resize(src, imgTmp, cSize);
// 	Mat foregroundImg = getForeImage(imgTmp, backgroundImg);
// 
// 	using namespace luffy_base;
// 	luffy_threshold::Threshold(foregroundImg, imgBinary, nThres);
// 
// 	Mat dilatedImgBin;
// 	dilate(imgBinary, dilatedImgBin, Mat::ones(7, 7, CV_32FC1));
// 	erode(dilatedImgBin, imgBinary, Mat::ones(7, 7, CV_32FC1));
// 
//     openOper(imgBinary, Mat::ones(1, 13, CV_32FC1));
// 
// 	vector<vector<Point>> conts;
// 	cv::findContours(imgBinary, conts, RETR_EXTERNAL, CHAIN_APPROX_NONE);
// 	imgBinary.setTo(0);
// 	for (int i = 0; i < conts.size(); i++) {
// 		const vector<Point> &pt = conts.at(i);
// 		if (pt.size() < 20) {
// 			continue;
// 		}
// 		Rect rt = boundingRect(pt);
// 		if (rt.width < 5 || rt.height < 5) {
// 			continue;
// 		}
// 		drawContours(imgBinary, conts, i, Scalar::all(255), -1);
// 	}
// 	Mat hit; vector<Point> pts;
// 	luffy_hit::firstHit4Circle(imgBinary, hit, pts, Point(cSize.width / 2, cSize.height / 2), 0, cSize.width / 2, 360, luffy_hit::emHitOut2In);
// 
// 	//luffy_imageProc::RansacParam rs(0.02, 2.5, 70, 100, 220);
// 	luffy_imageProc::RansacParam rs(0.001, 1.5, 2240, 100, 420);
// 	vector<Point> pts2 = luffy_imageProc::fitModelbyRansac(pts, luffy_imageProc::emModelCircle, &rs);
// 
// #ifdef _DEBUG
// 	Mat imgColor;
// 	cv::cvtColor(imgTmp, imgColor, CV_GRAY2BGR);
// 	for (int i = 0; i < pts.size(); i++) {
// 		imgColor.at<cv::Vec3b>(pts.at(i))[0] = 255;//B  
// 		imgColor.at< cv::Vec3b >(pts.at(i))[1] = 0;//G  
// 		imgColor.at< cv::Vec3b >(pts.at(i))[2] = 0;//R  
// 	}
// 	for (int i = 0; i < pts2.size(); i++) {
// 		imgColor.at<cv::Vec3b>(pts2.at(i))[0] = 0;//B  
// 		imgColor.at< cv::Vec3b >(pts2.at(i))[1] = 0;//G  
// 		imgColor.at< cv::Vec3b >(pts2.at(i))[2] = 255;//R  
// 	}
// #endif
// 
// 	float fRadius;
// 	Point2f ptCenter;
// 	bool bFind = luffy_imageProc::lsCircleFit(pts2, fRadius, ptCenter);
// 
// 	if (!bFind) {
// 		return Mat();
// 	}
// 	Mat dst;
// 	const int nOffset = 1;
// 	fRadius += nOffset;
// 	Rect rt(ptCenter.x - fRadius + nOffset, ptCenter.y - fRadius + nOffset, 2 * fRadius, 2 * fRadius);
// 	rt &= Rect(0, 0, imgTmp.cols, imgTmp.rows);
// 	Mat test;
// 	imgTmp(rt).copyTo(test);
// 	static int nCount = cv::getTickCount();
// 	//QString str = "d://image//" + QString::number(cv::getTickCount());
// 	//QString strTest = str + "_1.jpg";
// 	//cv::imwrite(strTest.toLatin1().data(), test);
// 	//
// 	if (pCircle) {
// 		float fScale = src.cols / ALG_RESIZE_IMAGE_WIDTH;
// 		Mat matBig = src - backgroundImg;
// 		pCircle->fRadius = fRadius * fScale;
// 		pCircle->ptCenter = Point(ptCenter.x * fScale, ptCenter.y * fScale);
// 		Mat matBinary;
// 		luffy_threshold::Threshold(matBig, matBinary, nThres);
// 		Mat hit; vector<Point> pts;
// 		luffy_hit::firstHit4Circle(matBinary, hit, pts, pCircle->ptCenter, 0, pCircle->fRadius + 10, 360, luffy_hit::emHitOut2In);
// 		luffy_imageProc::RansacParam rs(0.01, 2.5, 100, 100, 220);
// 		vector<Point> pts2 = luffy_imageProc::fitModelbyRansac(pts, luffy_imageProc::emModelCircle, &rs);
// 		float fRadius2;
// 		Point2f ptCenter2;
// 		bool bFind = luffy_imageProc::lsCircleFit(pts2, fRadius2, ptCenter2);
// 		if (bFind) {
// 			pCircle->fRadius = fRadius2;
// 			pCircle->ptCenter = ptCenter2;
// 			Rect rt(ptCenter2.x - fRadius2 + nOffset, ptCenter2.y - fRadius2 + nOffset, 2 * fRadius2, 2 * fRadius2);
// 			src(rt).copyTo(dst);
// 			int nWidth = ((int)((double)dst.cols / fScale / 4)) * 4;
// 			cv::resize(dst, dst, cv::Size(nWidth, nWidth));
// 			//QString strTest = str + "_2.jpg";
// 			//cv::imwrite(strTest.toLatin1().data(), dst);
// 		}
// 	}
// 	if (dst.empty()) {
// 		imgTmp(rt).copyTo(dst);
// 	}
// 
// 	return dst;
// }

bool ensureGrayImg(const Mat& img, Mat& gray)
{
	if (img.channels() == 3)
	{
		cvtColor(img, gray, COLOR_BGR2GRAY);
		return true;
	}
	else if (img.channels() == 4)
	{
		cvtColor(img, gray, COLOR_BGRA2GRAY);
		return true;
	}
	else if (img.channels() == 1)
	{
		gray = img;
		return false;
	}
	else
	{
		assert(false && "unsupported image conversion.");
		return false;
	}
}

double hsvDis(const Vec3b& v0, const Vec3b& v1)
{
    float h0 = v0[0] * 2.0 * 0.01745;
    float s0 = v0[1] * (v0[2] / 255.0) * 2.0;
    float h1 = v1[0] * 2.0 * 0.01745;
    float s1 = v1[1] * (v1[2] / 255.0) * 2.0;

    double dVal = norm(Vec3f(cos(h0)*s0, sin(h0)*s0, v0[2]),
        Vec3f(cos(h1)*s1, sin(h1)*s1, v1[2]));

    return dVal;
}

cv::Mat genHSVColorDisMat(const Mat& m0, const Mat& m1)
{
    Scalar mean, stddev;
    meanStdDev(m1, mean, stddev);
    Vec3b baseColor(mean.val[0], mean.val[1], mean.val[2]);
    return genHSVColorDisMat(m0, baseColor);
}

cv::Mat genHSVColorDisMat(const Mat& m, Vec3b baseColor)
{
    Mat ret = Mat::zeros(m.size(), CV_32FC1);
    uchar* pRet = ret.data;
    uchar* pData0 = m.data;
    for (int i = 0; i < m.rows; ++i)
    {
        float* pRet32f = (float*)pRet;
        Vec3b* pData32f0 = (Vec3b*)pData0;
        for (int j = 0; j < m.cols; ++j)
        {
            Vec3b& v0 = pData32f0[j];
            pRet32f[j] = (float)hsvDis(v0, baseColor);
        }

        pRet += ret.step;
        pData0 += m.step;
    }
    return ret;
}

cv::Mat genHSVColorDisMat(const Mat& m)
{
    Scalar mean, stddev;
    meanStdDev(m, mean, stddev);
    return genHSVColorDisMat(m, Vec3b(mean.val[0], mean.val[1], mean.val[2]));
}

cv::Mat genBlueGreenRatioMat(const Mat& m, float alpha /*= 50.0*/, float beta /*= -17.0*/)
{
    vector<Mat> channels;
    split(m, channels);
    Mat blue32f, green32f;
    channels[0].convertTo(blue32f, CV_32FC1);
    channels[1].convertTo(green32f, CV_32FC1, 1.0, -beta);
    Mat ratioMat = green32f / blue32f;
    Mat ratioMat8u;
    ratioMat.convertTo(ratioMat8u, CV_8UC1, alpha);
    return ratioMat8u;
}

Mat genColorFuncDisMat(const Mat& m, float a, float b, float c)
{
    vector<Mat> channels;
    split(m, channels);
    Mat blue32f, green32f;
    channels[0].convertTo(blue32f, CV_32FC1);
    channels[1].convertTo(green32f, CV_32FC1);
    Mat sqrBlue = blue32f.mul(blue32f, a);
    Mat bBlue = blue32f*b;
    //Mat dis = ((sqrBlue + bBlue) - green32f);
    Mat dis = (green32f - sqrBlue + c) / blue32f;
    Mat ret;
    dis.convertTo(ret, CV_8UC1, 255);
    return ret;
}

void setChannel(Mat& img, int i, Mat chnMat)
{
    vector<Mat> channels;
    split(img, channels);
    channels[i] = chnMat;
    merge(channels, img);
}

Rect loadRect(std::string filepath)
{
	std::ifstream fin;
	fin.open(filepath, std::ios_base::in);
	double x, y, w, h;
	fin >> x;
	fin >> y;
	fin >> w;
	fin >> h;
	return Rect(x, y, w, h);
}

Rect rectInImage(const Mat& img, const Point& pt, const Size& rectSize, int xPadding /*= 0*/, int yPadding /*= 0*/)
{
	assert(rectSize.width % 2 == 1);
	assert(rectSize.height % 2 == 1);

	Rect imgRect(xPadding, yPadding, img.cols - xPadding, img.rows - yPadding);
	Rect rect(pt.x - rectSize.width / 2, pt.y - rectSize.height / 2,
		rectSize.width, rectSize.height);

	return imgRect & rect;
}

Rect extRectTopLeftFix(const Rect& r, int w)
{
	return Rect(r.x, r.y, r.width + w * 2, r.height + w * 2);
}

Rect extRectCenFix(const Rect& r, int w)
{
	return Rect(r.x - w, r.y - w, r.width + 2 * w, r.height + 2 * w);
}

inline float isLeft(Point2f p0, Point2f p1, Point2f p)
{
	return ((p1.x - p0.x) * (p.y - p0.y) - (p.x - p0.x) * (p1.y - p0.y));
}

bool isPntInRotatedRect(const RotatedRect& rr, const Point2f& pnt)
{
	Point2f pts[4];
	rr.points(pts);
	return (isLeft(pts[0], pts[1], pnt) > 0 && isLeft(pts[1], pts[2], pnt) > 0 &&
		isLeft(pts[2], pts[3], pnt) > 0 && isLeft(pts[3], pts[0], pnt) > 0);
}

inline bool _isOnLineSegment(const Point2f& pt, const Vec4f& line)
{
	float xRange = line.val[2] - line.val[0];
	float yRange = line.val[3] - line.val[1];
	float dx0 = pt.x - line.val[0];
	float dy0 = pt.y - line.val[1];
	if (abs(dx0) > abs(dy0))
	{
		float dx1 = line.val[2] - pt.x;
		if (dx0 > 0 && dx1 > 0 || dx0 < 0 && dx1 < 0)
		{
			return true;
		}
		else
		{
			return false;
		}
	}
	else
	{
		float dy1 = line.val[3] - pt.y;
		if (dy0 > 0 && dy1 > 0 || dy0 < 0 && dy1 < 0)
		{
			return true;
		}
		else
		{
			return false;
		}
	}
}

bool intersection(Point2f o1, Point2f p1, Point2f o2, Point2f p2, Point2f &r, bool bounded /*= false*/)
{
	Point2f x = o2 - o1;
	Point2f d1 = p1 - o1;
	Point2f d2 = p2 - o2;

	float cross = d1.x*d2.y - d1.y*d2.x;
	if (abs(cross) < /*EPS*/1e-8)
		return false;

	double t1 = (x.x * d2.y - x.y * d2.x) / cross;
	r = o1 + d1 * t1;

	if (bounded)
	{
		Vec4f l0(o1.x, o1.y, p1.x, p1.y);
		Vec4f l1(o2.x, o2.y, p2.x, p2.y);
		return _isOnLineSegment(r, l0) | _isOnLineSegment(r, l1);
	}

	return true;
}

bool intersection(const Vec4f& l1, const Vec4f& l2, Point2f& r, bool bounded /*= false*/)
{
	float d1x = l1.val[2] - l1.val[0];
	float d1y = l1.val[3] - l1.val[1];
	float d2x = l2.val[2] - l2.val[0];
	float d2y = l2.val[3] - l2.val[1];

	float xx = l2.val[0] - l1.val[0];
	float xy = l2.val[1] - l1.val[1];

	float cross = d1x * d2y - d1y * d2x;
	if (abs(cross) < 1e-8)
	{
		return false;
	}

	double t1 = (xx * d2y - xy * d2x) / cross;
	r = Point2f(l1.val[0] + d1x * t1, l1.val[1] + d1y * t1);

	if (bounded)
	{
		return _isOnLineSegment(r, l1) | _isOnLineSegment(r, l2);
	}

	return true;
}

bool intersection(const Vec4f& l, const vector<Point2f>& vertexes, vector<Point2f>& interPtVec, bool bounded /*= false*/)
{
	for (int i = 0; i < vertexes.size(); ++i)
	{
		Point2f prePt;//= vertexes[i - 1];
		if (i == 0)
		{
			prePt = vertexes.back();
		}
		else
		{
			prePt = vertexes[i - 1];
		}
		Point2f curPt = vertexes[i];
		Vec4f ll(prePt.x, prePt.y, curPt.x, curPt.y);
		Point2f interPt;
		if (intersection(l, ll, interPt, bounded))
		{
			interPtVec.push_back(interPt);
		}
	}
	return interPtVec.size() > 0;
}

template<typename T>
int _intersection(const Vec<T, 4>& line, const Vec<T, 3>& circle, Point_<T>& r1, Point_<T>& r2)
{
	double dx = line[2] - line[0];
	double dy = line[3] - line[1];

	double A = dx * dx + dy * dy;
	double B = 2 * (dx * (line[0] - circle[0]) + dy * (line[1] - circle[1]));
	double C = (line[0] - circle[0]) * (line[0] - circle[0]) + (line[1] - circle[1]) * (line[1] - circle[1]) - circle[2] * circle[2];

	double det = B * B - 4 * A * C;
	if ((A <= DBL_EPSILON) || (det < 0))
	{
		return 0;
	}
	else if (det == 0)
	{
		double t = -B / (2 * A);
		r1.x = line[0] + t * dx;
		r1.y = line[1] + t * dy;
		return 1;
	}
	else
	{
		double sqrt_det = sqrt(det);
		double t1 = (-B + sqrt_det) / (2 * A);
		double t2 = (-B - sqrt_det) / (2 * A);
		r1.x = line[0] + t1 * dx;
		r1.y = line[1] + t1 * dy;
		r2.x = line[0] + t2 * dx;
		r2.y = line[1] + t2 * dy;
		return 2;
	}
}

int intersection(const Vec4f& line, const Vec3f& circle, Point2f& r1, Point2f& r2)
{
	return _intersection<float>(line, circle, r1, r2);
}

int intersection(const Vec4d& line, const Vec3d& circle, Point2d& r1, Point2d& r2)
{
	return _intersection<double>(line, circle, r1, r2);
}

Point2f getNearestIntersectionOfMultiLines(const vector<Vec4f>& lines)
{
	int count = lines.size();
	vector<Point2f> linePnts;
	linePnts.reserve(count);
	vector<Vec2f> lineNorms;
	lineNorms.reserve(count);
	for (int i = 0; i < count; ++i)
	{
		const Vec4f& l = lines[i];
		linePnts.push_back(Point2f((l[0] + l[2]) / 2., (l[1] + l[3]) / 2.));
		double dist = distance(l[0], l[1], l[2], l[3]);
		lineNorms.push_back(Vec2f((-(l[1] - l[3])) / dist, (l[0] - l[2]) / dist));
	}

	return getNearestIntersectionOfMultiLines(linePnts, lineNorms);
}

Point2f getNearestIntersectionOfMultiLines(const vector<Point2f>& linePnts, const vector<float>& lineAngles, bool useDegree)
{
	int count = lineAngles.size();
	vector<Vec2f> lineNorms;
	lineNorms.reserve(count);
	for (int i = 0; i < count; ++i)
	{
		float radian = useDegree ? lineAngles[i] / 180 * CV_PI : lineAngles[i];
		lineNorms.push_back(Vec2f(-sin(radian), cos(radian)));
	}

	return getNearestIntersectionOfMultiLines(linePnts, lineNorms);
}

Point2f getNearestIntersectionOfMultiLines(const vector<Point2f>& linePnts, const vector<Vec2f>& lineNorms)
{
	_ASSERTE(linePnts.size() == lineNorms.size());
	if (linePnts.size() != lineNorms.size()) return Point2f();

	int count = linePnts.size();
	Matx22d sum_ninit;
	Matx21d sum_ninitpi;
	for (int i = 0; i < count; ++i)
	{
		const Point2f& pnt = linePnts[i];
		Vec2d pi(pnt.x, pnt.y);
		const Vec2f& ni = lineNorms[i];
		Matx22d ninit = ni * ni.t();
		Matx21d ninitpi = ninit * pi;
		sum_ninit += ninit;
		sum_ninitpi += ninitpi;
	}

	Matx21d x = sum_ninit.inv() * sum_ninitpi;
	return Point2f(x(0, 0), x(1, 0));
}

void transPoints(const vector<Point2f>& vec, vector<Point2f>& oVec, const Matx23f& mat)
{
	oVec.resize(vec.size());
	for (size_t i = 0; i < vec.size(); ++i)
	{
		Point2f p = vec[i];
		Point2f tp = mat * Vec3f(p.x, p.y, 1.0);
		oVec[i] = Point2f(tp.x, tp.y);
	}
}

void transPoints(vector<Point2d>& vec, const Matx33d& mat)
{
	for (size_t i = 0; i < vec.size(); ++i)
	{
		Point2d p = vec[i];
		Point3d tp = mat * p;
		vec[i] = Point2d(tp.x / tp.z, tp.y / tp.z);
	}
}

void transPoints(vector<Point2d>& vec, const Mat& mat)
{
	Matx33d matx = Matx33d::eye();
	Mat matx_(3, 3, CV_64FC1, matx.val);

	if (mat.rows == 2 && mat.cols == 3)
	{
		mat.copyTo(matx_.rowRange(0, 2));
	}
	else if (mat.rows == 3 && mat.cols == 3)
	{
		mat.copyTo(matx_);
	}
	else
	{
		std::cout << "not supported transformation mat with its size as " \
			<< mat.rows << "x" << mat.cols << std::endl;
		return;
	}
	transPoints(vec, matx);
}

Matx23f getRotationMatrix23f(Point2f center, float angle, float scale, float xOffset, float yOffset)
{
	Matx23f rotMat;
	float* pRotVal = rotMat.val;
	float a_pi = -angle * CV_PI / 180;
	float alpha = cos(a_pi)*scale;
	float beta = sin(a_pi)*scale;
	pRotVal[0] = alpha;
	pRotVal[1] = beta;
	pRotVal[2] = -alpha * center.x - beta * center.y + xOffset;
	pRotVal[3] = -beta;
	pRotVal[4] = alpha;
	pRotVal[5] = beta * center.x - alpha * center.y + yOffset;

	return rotMat;
}

void getRigidTransform_(const Point2f& u1, const Point2f& u2, const Point2f& v1, const Point2f& v2,
	double& x0, double& y0, double& angle, double& scale)
{
	vector<Point2d> srcPtVec(2), dstPtVec(2);
	srcPtVec[0] = Point2d(u1.x, u1.y);
	srcPtVec[1] = Point2d(u2.x, u2.y);
	dstPtVec[0] = Point2d(v1.x, v1.y);
	dstPtVec[1] = Point2d(v2.x, v2.y);

	Mat srMat;
	Matx33d t = rigidTrans(srcPtVec, dstPtVec, &srMat);

	double t0 = t.val[0];
	double t1 = t.val[1];
	double l = sqrt(t0 * t0 + t1 * t1);
	if (abs(l) > FLT_EPSILON)
	{
		t0 /= l;
		t1 /= l;
		scale = 1.0 / l;
		angle = acos(t0) / CV_PI * 180;
		angle = normAngle(angle);
	}
	else
	{
		angle = 0;
		scale = 1.0;
	}

	Point2d vCen = v1 + v2;

	x0 = vCen.x / 2.;
	y0 = vCen.y / 2.;
}

void getRigidTransform(const Point2f& u1, const Point2f& u2, const Point2f& v1, const Point2f& v2,
	double& x0, double& y0, double& angle, double& scale)
{
	float theta_u = atan2f(u2.y - u1.y, u2.x - u1.x);
	float theta_v = atan2f(v2.y - v1.y, v2.x - v1.x);
	angle = (theta_v - theta_u) * 180 / CV_PI;
	angle = normAngle(angle);

	float sin_a = sin(angle / 180 * CV_PI);
	float cos_a = cos(angle / 180 * CV_PI);

	double f = (u2.x - u1.x) * cos_a - (u2.y - u1.y) * sin_a;
	if (abs(f) > FLT_EPSILON)
	{
		scale = (v2.x - v1.x) / f;
	}
	else
	{
		scale = sqrt(pow(u2.x - u1.x, 2) + pow(u2.y - u1.y, 2)) / sqrt(pow(v2.x - v1.x, 2) + pow(v2.y - v1.y, 2));
	}

	x0 = v1.x - u1.x * scale * cos_a + u1.y * scale * sin_a;
	y0 = v1.y - u1.x * scale * sin_a - u1.y * scale * cos_a;
}

void getRigidTransform(const Point2f& cen1, const Point2f& u1, const Point2f& u2,
	const Point2f& cen2, const Point2f& v1, const Point2f& v2,
	double& x0, double& y0, double& angle, double& scale)
{
	getRigidTransform(u1 - cen1, u2 - cen1, v1 - cen2, v2 - cen2, x0, y0, angle, scale);
}

void getRigidTransform(const Point2f& cen1, const Point2f& u1, float ua,
	const Point2f& cen2, const Point2f& v1, float va,
	double& x0, double& y0, double& angle, double& scale)
{
	getRigidTransform(u1 - cen1, ua, v1 - cen2, va, x0, y0, angle, scale);
}

void getRigidTransform(const Point2f& u1, float ua,
	const Point2f& v1, float va,
	double& x0, double& y0, double& angle, double& scale)
{
	getRigidTransform(u1, u1 + Point2f(cos(ua / 180 * CV_PI), sin(ua / 180 * CV_PI)),
		v1, v1 + Point2f(cos(va / 180 * CV_PI), sin(va / 180 * CV_PI)),
		x0, y0, angle, scale);
}

void getRigidTransform(const Point2f& cen1, const Point2f& u1, float ua,
	const Point2f& cen2, const Point2f& v1, float va,
	float s,
	double& x0, double& y0, double& angle, double& scale)
{
	getRigidTransform(u1 - cen1, ua, v1 - cen2, va, s, x0, y0, angle, scale);
}

void getRigidTransform(const Point2f& u1, float ua,
	const Point2f& v1, float va,
	float s,
	double& x0, double& y0, double& angle, double& scale)
{
	getRigidTransform(u1, u1 + Point2f(cos(ua / 180 * CV_PI), sin(ua / 180 * CV_PI)),
		v1, v1 + Point2f(cos(va / 180 * CV_PI), sin(va / 180 * CV_PI)) * s,
		x0, y0, angle, scale);
}

Mat applyPerspectiveTransform(const Mat& img, std::vector<Point2f>& transVertexes, int flags)
{
	if (img.empty()) return gDummyMat;

	int w = img.cols, h = img.rows;
#if defined(LITTLE_CPP11)
	std::vector<Point2f> pnts1(4);
	pnts1[0] = Point2f(0, 0);
	pnts1[1] = Point2f(w - 1, 0);
	pnts1[2] = Point2f(w - 1, h - 1);
	pnts1[3] = Point2f(0, h - 1);
#else
	std::vector<Point2f> pnts1 = {
		Point2f(0, 0), Point2f(w - 1, 0), Point2f(w - 1, h - 1), Point2f(0, h - 1)
	};
#endif
	Mat H = findHomography(pnts1, transVertexes);
	Mat img_warp;
	warpPerspective(img, img_warp, H, img.size(), flags);
	return img_warp;
}

float isSameDir(Vec4f& l1, Vec4f& l2, float err_tol)
{
	Vec2f l1_dir = normalize(Vec2f(l1[0] - l1[2], l1[1] - l1[3]));
	Vec2f l2_dir = normalize(Vec2f(l2[0] - l2[2], l2[1] - l2[3]));

	float ret = validate_diff<Vec2f>(l1_dir, l2_dir, err_tol);

	return ret <= err_tol ? 0 : ret;
}

float isCollinear(Vec4f& l1, Vec4f& l2, float err_tol)
{
	float a1 = l1[0] * (l1[3] - l2[1]) + l1[2] * (l2[1] - l1[1]) + l2[0] * (l1[1] - l1[3]);
	float a2 = l2[0] * (l2[3] - l1[3]) + l2[2] * (l1[3] - l2[1]) + l1[2] * (l2[1] - l2[3]);
	float a = (a1 + a2) / 2;

	Vec2f l1v(l1[0] - l1[2], l1[1] - l1[3]);
	float l1_len = sqrt(l1v.ddot(l1v));

	a /= l1_len;

	return a <= err_tol ? 0 : a;
}

//                        *
//                *                *
//           *                           *
//        *                                  *
//      *                                       *
//     *                                         *
//                This is like arch.
// vec must be a row vector.
bool isLikeArch(const Mat& vec, double tor)
{
	int n = vec.cols - 1;

	Mat dvec = genSimpleXGradient(vec);

	// left half extremes must be monotonically increasing
	// right half extremes must be monotonically decreasing
	Mat leftDVec, rightDVec;
	if (n % 2 == 1)
	{
		leftDVec = dvec.colRange(1, n / 2 + 1);
		rightDVec = dvec.colRange(n / 2 + 2, n + 1);
	}
	else
	{
		leftDVec = dvec.colRange(1, n / 2 + 1);
		rightDVec = dvec.colRange(n / 2 + 1, n + 1);
	}
	if (countNonZero(leftDVec < -tor) != 0 ||
		countNonZero(rightDVec > tor) != 0)
	{
		return false;
	}
	return true;
}

bool isLikeHorizontalLine(const Mat& vec, double tor)
{
	Scalar mean, stddev;
	meanStdDev(vec, mean, stddev);
	Mat dvec = vec - mean.val[0];
	dvec = abs(dvec);
	double minVal, maxVal;
	minMaxIdx(dvec, &minVal, &maxVal);
	return maxVal < tor;
}


};