OpenCV SqDiff

// Copyright (C) 2000, Intel Corporation, all rights reserved.
static void common_matchTemplate( Mat& img, Mat& templ, Mat& result, int method, int cn )
{
    int numType = 2;
    bool isNormed = method == CV_TM_SQDIFF_NORMED;

    double invArea = 1./((double)templ.rows * templ.cols);

    Mat sum, sqsum;
    Scalar templMean, templSdv;
    double *q0 = 0, *q1 = 0, *q2 = 0, *q3 = 0;
    double templNorm = 0, templSum2 = 0;

    integral(img, sum, sqsum, CV_64F);
    meanStdDev( templ, templMean, templSdv );

    templNorm = templSdv[0]*templSdv[0] + templSdv[1]*templSdv[1] + templSdv[2]*templSdv[2] + templSdv[3]*templSdv[3];

    templSum2 = templNorm + templMean[0]*templMean[0] + templMean[1]*templMean[1] + templMean[2]*templMean[2] + templMean[3]*templMean[3];

    templMean = Scalar::all(0);
    templNorm = templSum2;

    templSum2 /= invArea;
    templNorm = std::sqrt(templNorm);
    templNorm /= std::sqrt(invArea); // care of accuracy here

    q0 = (double*)sqsum.data;
    q1 = q0 + templ.cols*cn;
    q2 = (double*)(sqsum.data + templ.rows*sqsum.step);
    q3 = q2 + templ.cols*cn;

    double* p0 = (double*)sum.data;
    double* p1 = p0 + templ.cols*cn;
    double* p2 = (double*)(sum.data + templ.rows*sum.step);
    double* p3 = p2 + templ.cols*cn;

    int sumstep = sum.data ? (int)(sum.step / sizeof(double)) : 0;
    int sqstep = sqsum.data ? (int)(sqsum.step / sizeof(double)) : 0;

    int i, j, k;

    for( i = 0; i < result.rows; i++ )
    {
        float* rrow = result.ptr<float>(i);
        int idx = i * sumstep;
        int idx2 = i * sqstep;

        for( j = 0; j < result.cols; j++, idx += cn, idx2 += cn )
        {
            double num = rrow[j], t;
            double wndMean2 = 0, wndSum2 = 0;

            for( k = 0; k < cn; k++ )
            {
                t = q0[idx2+k] - q1[idx2+k] - q2[idx2+k] + q3[idx2+k];
                wndSum2 += t;
            }

            num = wndSum2 - 2*num + templSum2;
            num = MAX(num, 0.);

            if( isNormed )
            {
                t = std::sqrt(MAX(wndSum2 - wndMean2,0))*templNorm;
                if( fabs(num) < t )
                    num /= t;
                else if( fabs(num) < t*1.125 )
                    num = num > 0 ? 1 : -1;
                else
                    num = 1;
            }

            rrow[j] = (float)num;
        }
    }
}