реализация медианного фильтра

Bitmap MedianPhilter(Picture orig_img, int rad)
         {
             Picture result_img = new Picture(orig_img.bmp.Width, orig_img.bmp.Height,
                                             orig_img.bmp.PixelFormat, orig_img.typeOfPixel);
             for (int i = 0; i < orig_img.bmp.Height; i++)
                 for (int j = 0; j < orig_img.bmp.Width; j++)
                 {
                     if (i >= rad && j >= rad && j <= (orig_img.bmp.Width - rad) && i <= (orig_img.bmp.Height - rad))//проверка на границы
                     {
                         List<int> PixelsValues = new List<int>();//список соседних пикселей
                         for (int k = -rad / 2; k < rad / 2 + 1; k++)
                         {
                             for (int l = -rad / 2; l < rad / 2 + 1; l++)
                             {
                                 PixelsValues.Add((int)orig_img.GetPixel(j - l, i - k)[0] + (int)orig_img.GetPixel(j - l, i - k)[1]
                                     + (int)orig_img.GetPixel(j - l, i - k)[2]);//добовляем соседние пиксели в массив и ссумируем цвета (ВОТ ОТ СЮДА НАДО ПЕРЕДЕЛАТЬ, ЧТОБ НЕ СКЛАДЫВАЛО ЦВЕТА)
                             }
                         }
                         PixelsValues.Sort();
                         int pValue = PixelsValues[PixelsValues.Count / 2];
                         for (int k = -rad / 2; k < rad / 2 + 1; k++)
                         {
                             bool flag = false;
                             for (int l = -rad / 2; l < rad / 2 + 1; l++)
                             {
                                 int Value = (int)orig_img.GetPixel(j - l, i - k)[0] + (int)orig_img.GetPixel(j - l, i - k)[1]
                                     + (int)orig_img.GetPixel(j - l, i - k)[2];
                                 if (Value == pValue) { result_img.SetPixel(j, i, orig_img.GetPixel(j - l, i - k)); flag = true; }
                             }
                             if (flag) break;
                         }
                     }
                     else result_img.SetPixel(j,i,orig_img.GetPixel(j,i));
                 }       
             return result_img.bmp;
         }

class Picture
     {
         public Bitmap bmp;
         byte[] Pixels;
         public int typeOfPixel; 
         BitmapData bmpData;
         IntPtr ptr;
         Rectangle rectngl;
 
         public Picture(Image orig_img)//конструктор если имеется исходное изображение
         {
             bmp = new Bitmap(orig_img);// orig_img;
             rectngl = new Rectangle(0, 0, orig_img.Width, orig_img.Height);
             bmpData = bmp.LockBits(rectngl, ImageLockMode.ReadWrite, PixelFormat.Format32bppArgb);//orig_img.PixelFormat);
             ptr = bmpData.Scan0;
             int numBytes = orig_img.Height * orig_img.Width * 4; //bmpData.Stride * orig_img.Height; //размерность массива
             typeOfPixel = 4;// bmpData.Stride / orig_img.Width; //количество байт на пиксель
             Pixels = new byte[numBytes];
             Marshal.Copy(ptr, Pixels, 0, numBytes);
             //FreeMemory();
         }
 
         public Picture(int width, int height, PixelFormat format, int type) //конструктор если изображение создается
         {
             bmp = new Bitmap(width, height, format);
             rectngl = new Rectangle(0, 0, width, height);
             typeOfPixel = type; //количество байт на пиксель
             bmpData = bmp.LockBits(rectngl, ImageLockMode.ReadWrite, bmp.PixelFormat);
             ptr = bmpData.Scan0;
             int numBytes = width * height* typeOfPixel; //размерность массива
             Pixels = new byte[numBytes];
         }
 
         public void SetPixel(int x, int y, byte[] color)
         {
             int offset = (y * bmp.Width + x) * typeOfPixel;
             Marshal.Copy(color, 0, ptr+offset, typeOfPixel);
         }
 
         public byte[] GetPixel(int x, int y)
         {
             byte[] color = new byte[typeOfPixel];
             int offset = (y * bmp.Width + x) * typeOfPixel;
             Marshal.Copy(ptr + offset, color, 0, typeOfPixel);
             return color;
         }
}