A Mode filter in C#

Machine Vision does not include code for a Mode filter, so I am publishing mine here. It is written in C#. I leaves out some support infrastructure details, but those should be fairly trivial to adapt to another library.

The basic idea here is to treat the neighborhood of pixels around a point as a probability distribution and select the most likely point. This will be the place with the highest density. With a 3x3 neighbor, we only have nine points to sample this distribution but my results turned out surprising well.

First the points are sorted in an array. The array is traversed and the distance between values is examined. The closer two values are, the higher their density. I look for intervals that have two larger intervals on each side. The midpoint of that interval is used as the mode.

If there is more than one interval that meets that criteria, I select the interval whose mid-point is closest to the distribution median.

public static byte Mode(ImageGray8 img, int x, int y) {
    Accessor a = new Accessor(img, x, y);
    byte[] p = new byte[] {
        a.P4, a.P3, a.P2,
        a.P5, a.P0, a.P1,
        a.P6, a.P7, a.P8
        };
    Array.Sort(p);
    byte median = p[4];
    int modeIndex = -1;
    int dp = int.MaxValue;
    for (int i = 0; i < p.Length-1; i++) {
        int dp_i = p[i+1] - p[i];
        if (dp_i < dp) {
            // still increasing to the local maximum density
            dp = dp_i;
        } else {
            // just passed a local maximum
            if (modeIndex < 0) {
                modeIndex = i;
            } else {
                // select the previous interval 
                // which is closest to the median
                byte modeCurrent = (byte) 
                    ((p[modeIndex - 1] + p[modeIndex]) / 2);
                byte modeCandidate = (byte) 
                    ((p[i - 1] + p[i]) / 2);
                if (Math.Abs(median - modeCurrent) > 
                    Math.Abs(median - modeCandidate)
                    ) {
                    modeIndex = i;
                }
            }
        }
    }

    byte modeValue;
    if (modeIndex < 0) {
        // density increased with every step
        modeValue = p[8];
    } else if (modeIndex == 0) {
        // density decreased with every step
        modeValue = p[0];
    } else {
        modeValue = (byte) 
            ((p[modeIndex - 1] + p[modeIndex]) / 2);
    }

    return modeValue;
}

How does this filter look? Detail is flattened into smooth shading, while edges are enhanced (with anti-aliased edges turning to jaggies). I think it looks a bit cartoon like.

The original "Lena" Playboy image commonly used in graphics (click through to see the full size images).

And the image processed multiple times by the mode filter.