Выравнивание освещения (Retinex)

Mat Retinex(const Mat &img)
{
    Mat im;
    Mat t1,t2,t3,t4,t5,res,tm1,tm2;
 
    cvtColor(img,im,CV_BGR2YCrCb);
    
    Mat i3[3];
    split(im,i3);
    Mat ims1 = i3[0].clone();
    Mat ims;
    ims1.convertTo(ims,CV_64FC1,1.0);
 
    //
    for(int i=0; i<ims.rows; i++)
    {
        for(int j=0; j<ims.cols; j++)
        {
            double d = ims.at<double>(i,j); 
            if(d==0)
            {
                ims.at<double>(i,j) +=0.00001;
            }
        }
    }
    //
 
    int sm1 = MIN(ims.size().height,ims.size().width);
    int sm2 = sm1/2;
    int sm3=sm1/4;
 
    if(!(sm1%2))sm1--;
    if(!(sm2%2))sm2--;
    if(!(sm3%2))sm3--;
 
    if(sm1==0)sm1=1;
    if(sm2==0)sm2=1;
    if(sm3==0)sm3=1;
 
    GaussianBlur(ims,t1,Size(sm1,sm1),sm1);
    GaussianBlur(ims,t2,Size(sm2,sm2),sm2);
    GaussianBlur(ims,t3,Size(sm3,sm3),sm3);
 
    Mat l1,l2,l3,l4;
    log(ims,l1);log(t1,l2);log(t2,l3);log(t3,l4);
    Mat tr = (3*l1 - l2-l3-l4)/3;
 
    Mat etr;
    exp(tr,etr);
 
    double dm=10000000,dx=-100000000;
 
    for(int i=0; i<etr.rows; i++)
    {
        for(int j=0; j<etr.cols; j++)
        {
            double d = etr.at<double>(i,j);
            if(d<dm)dm=d;
            if(d>dx)dx=d;
        }
    }       
 
    etr = etr - Mat::ones(etr.size(),CV_64FC1)*dm;
    etr = etr/(dx-dm);
    etr = etr*255;
    //etr+=127.5;
 
    etr.convertTo(tm2,CV_8UC1,1.0);
    i3[2]=tm2.clone();
    
    merge(i3,3,tm1);
    cvtColor(tm1,res,CV_YCrCb2BGR);
 
    return res;
}

#region Single Scale Retinex
 
        public static Image<Bgr, byte> SingleScaleRetinex(this Image<Bgr, byte> img, int gaussianKernelSize, double sigma)
        {
            var radius = gaussianKernelSize / 2;
            var kernelSize = 2 * radius + 1;
 
            var ycc = img.Convert<Ycc, byte>();
 
            // Формируем ядро Гауссиана
            var sum = 0f;
            var gaussKernel = new float[kernelSize * kernelSize];
            for (int i = -radius, k = 0; i <= radius; i++, k++)
            {
                for (int j = -radius; j <= radius; j++)
                {
                    var val = (float)Math.Exp(-(i * i + j * j) / (sigma * sigma));
                    gaussKernel[k] = val;
                    sum += val;
                }
            }
            for (int i = 0; i < gaussKernel.Length; i++)
                gaussKernel[i] /= sum;
 
            // Работаем только с яркостным каналом
            var gray = new Image<Gray, byte>(ycc.Size);
            CvInvoke.cvSetImageCOI(ycc, 1);
            CvInvoke.cvCopy(ycc, gray, IntPtr.Zero);
 
            // Размеры изображения
            var width = img.Width;
            var height = img.Height;
 
            var bmp = gray.Bitmap;
            var bitmapData = bmp.LockBits(new Rectangle(Point.Empty, gray.Size), ImageLockMode.ReadWrite, PixelFormat.Format8bppIndexed);
 
            unsafe
            {
                for (var y = 0; y < height; y++)
                {
                    var row = (byte*)bitmapData.Scan0 + y * bitmapData.Stride;
                    for (var x = 0; x < width; x++)
                    {
                        var color = row + x;
 
                        float val = 0;
 
                        for (int i = -radius, k = 0; i <= radius; i++, k++)
                        {
                            var ii = y + i;
                            if (ii < 0) ii = 0; if (ii >= height) ii = height - 1;
 
                            var row2 = (byte*)bitmapData.Scan0 + ii * bitmapData.Stride;
                            for (int j = -radius; j <= radius; j++)
                            {
                                var jj = x + j;
                                if (jj < 0) jj = 0; if (jj >= width) jj = width - 1;
 
                                val += *(row2 + jj) * gaussKernel[k];
 
                            }
                        }
 
                        var newColor = 127.5 + 255 * Math.Log(*color / val);
                        if (newColor > 255)
                            newColor = 255;
                        else if (newColor < 0)
                            newColor = 0;
                        *color = (byte)newColor;
                    }
                }
            }
            bmp.UnlockBits(bitmapData);
 
            CvInvoke.cvCopy(gray, ycc, IntPtr.Zero);
            CvInvoke.cvSetImageCOI(ycc, 0);
 
            return ycc.Convert<Bgr, byte>();
 
        }
 
        #endregion