private static double[,] computeCorrelationMatrix(
Bitmap image1, IntPoint[] points1,
Bitmap image2, IntPoint[] points2,
int windowSize, double maxDistance)
{
// Create the initial correlation matrix
double[,] matrix = Matrix.Create(points1.Length, points2.Length, Double.NegativeInfinity);
// Gather some information
int width1 = image1.Width;
int width2 = image2.Width;
int height1 = image1.Height;
int height2 = image2.Height;
int r = (windowSize - 1) / 2; // 'radius' of correlation window
double m = maxDistance * maxDistance; // maximum considered distance
double[,] w1 = new double[windowSize, windowSize]; // first window
double[,] w2 = new double[windowSize, windowSize]; // second window
// Lock the images
BitmapData bitmapData1 = image1.LockBits(new Rectangle(0, 0, width1, height1),
ImageLockMode.ReadOnly, PixelFormat.Format8bppIndexed);
BitmapData bitmapData2 = image2.LockBits(new Rectangle(0, 0, width2, height2),
ImageLockMode.ReadOnly, PixelFormat.Format8bppIndexed);
int stride1 = bitmapData1.Stride;
int stride2 = bitmapData2.Stride;
// We will ignore points at the edge
int[] idx1 = Matrix.Find(points1, p => p.X >= r && p.X < width1 - r &&
p.Y >= r && p.Y < height1 - r);
int[] idx2 = Matrix.Find(points2, p => p.X >= r && p.X < width2 - r &&
p.Y >= r && p.Y < height2 - r);
// For each index in the first set of points
foreach (int n1 in idx1)
{
// Get the current point
IntPoint p1 = points1[n1];
double sum = 0;
unsafe // Create the first window for the current point
{
byte* src = (byte*)bitmapData1.Scan0 + (p1.X - r) + (p1.Y - r) * stride1;
for (int j = 0; j < windowSize; j++)
{
for (int i = 0; i < windowSize; i++)
{
double w = (byte)(*(src + i));
w1[i, j] = w;
sum += w * w;
}
src += stride1;
}
}
// Normalize the window
w1.Divide(System.Math.Sqrt(sum), result: w1);
// Identify the indices of points in p2 that we need to consider.
int[] candidates;
if (maxDistance == 0)
{
// We should consider all points
candidates = idx2;
}
else
{
// We should consider points that are within
// distance maxDistance apart
// Compute distances from the current point
// to all points in the second image.
double[] distances = new double[idx2.Length];
for (int i = 0; i < idx2.Length; i++)
{
double dx = p1.X - points2[idx2[i]].X;
double dy = p1.Y - points2[idx2[i]].Y;
distances[i] = dx * dx + dy * dy;
}
candidates = idx2.Get(Matrix.Find(distances, d => d < m));
}
// Calculate normalized correlation measure
foreach (int n2 in candidates)
{
IntPoint p2 = points2[n2];
unsafe // Generate window in 2nd image
{
byte* src = (byte*)bitmapData2.Scan0 + (p2.X - r) + (p2.Y - r) * stride2;
for (int j = 0; j < windowSize; j++)
{
for (int i = 0; i < windowSize; i++)
w2[i, j] = (byte)(*(src + i));
src += stride2;
}
}
double sum1 = 0, sum2 = 0;
for (int i = 0; i < windowSize; i++)
{
for (int j = 0; j < windowSize; j++)
{
sum1 += w1[i, j] * w2[i, j];
sum2 += w2[i, j] * w2[i, j];
}
}
matrix[n1, n2] = sum1 / Math.Sqrt(sum2);
}
}
// Release the images
image1.UnlockBits(bitmapData1);
image2.UnlockBits(bitmapData2);
return matrix;
}
