Autofocus algorithm for USB microscope

Question

I'm trying to design an auto-focus system for a low cost USB microscope. I have been developing the hardware side with a precision PAP motor that is able to adjust the focus knob in the microscope, and now I'm in the hard part.

I have been thinking about how to implement the software. The hardware have two USB ports, one for the microscope camera and another for the motor. My initial idea is to write an application in C# that is able to get the image from the microscope and to move the motor forth and backwards, so far so good :)

Now I need a bit of help with the auto-focus, how to implement it? There is any good algorithm for this? Or maybe an image processing library that will help my in my task?

I have been googleling but with no success... I'll appreciate any help/idea/recommendation!

Many thanks :)

EDIT: Thanks guys for your answers, i'll try all the options and get back here with the results (or maybe more questions).

Answer 1

The most important piece is code which tells you how much out of focus the image is. Since an unfocused image loses high frequency data I'd try something like the following:

long CalculateFocusQuality(byte[,] pixels)
{
  long sum = 0;
  for(int y = 0; y<height-1; y++)
    for(int x=0; x<width-1; x++)
    {
      sum += Square(pixels[x+1, y] - pixels[x, y]);
      sum += Square(pixels[x, y] - pixels[x, y+1]);
    }
  return sum;
}

int Square(int x)
{
  return x*x;
}

This algorithm doesn't work well if the image is noisy. In that case you could to downsample it, or use a more complex algorithm.

Or another idea is calculating the variation of the pixel values:

long CalculateFocusQuality(byte[,] pixels)
{
  long sum = 0;
  long sumOfSquares = 0;
  for(int y=0; y<height; y++)
    for(int x=0; x<width; x++)
    {
      byte pixel=pixels[x,y];
      sum+=pixel;
      sumofSquares+=pixel*pixel;
    }
  return sumOfSquares*width*height - sum*sum;
}

These functions work on monochromatic images, for RGB images just sum the values for the channels.

Using this function change the focus trying to maximize CalculateFocusQuality. Increase the stepsize if several attempts in a row improved the quality, and decrease it and reverse the direction if the step reduced the quality.