public static Bitmap DrawTwoImages(Bitmap img1, Bitmap img2, List <ValueTuple <Descriptor, Descriptor> > match)
{
var offset = 20;
var width = img1.Width + img2.Width + offset;
var height = Math.Max(img1.Height, img2.Height) + offset;
var result = new Bitmap(width + 2 * offset, height + 2 * offset);
using (var g = Graphics.FromImage(result))
{
g.DrawImage(img1,
offset,
offset,
img1.Width,
img1.Height);
g.DrawImage(img2,
2 * offset + img1.Width,
2 * offset,
img2.Width,
img2.Height);
}
foreach (var(d1, d2) in match)
{
InterestingPoint
from = d1.Point,
to = d2.Point;
DrawLine(result,
offset + from.getX(),
offset + from.getY(),
(int)from.Radius,
2 * offset + to.getX() + img1.Width,
2 * offset + to.getY(),
(int)to.Radius
);
}
return(result);
}
private static List <int> GetInliers(List <ValueTuple <Descriptor, Descriptor> > matches, List <double> homography)
{
var inliers = new List <int>();
for (var i = 0; i < matches.Count; i++)
{
var match = matches[i];
InterestingPoint
a = match.Item1.Point,
b = match.Item2.Point;
var point = Transform(homography, a.getX(), a.getY());
var distance = MathHelper.SqrtOfSqrSum(point.Item1 - b.getX(), point.Item2 - b.getY());
if (distance < Threshold)
{
inliers.Add(i);
}
}
return(inliers);
}
private static List <double> FindCurrentHomography(List <ValueTuple <Descriptor, Descriptor> > matches,
List <int> choices, bool reverse)
{
double[,] Homography = new double[2 * choices.Count, MatrixWidth];
var index = 0;
foreach (var value in choices)
{
var match = matches[value];
InterestingPoint a = match.Item1.Point, b = match.Item2.Point;
if (reverse)
{
(a, b) = (b, a);
}
int i1 = 2 * index, i2 = 2 * index + 1;
Homography[i1, 0] = a.getX();
Homography[i1, 1] = a.getY();
Homography[i1, 2] = 1;
Homography[i1, 3] = 0;
Homography[i1, 4] = 0;
Homography[i1, 5] = 0;
Homography[i1, 6] = -b.getX() * a.getX();
Homography[i1, 7] = -b.getX() * a.getY();
Homography[i1, 8] = -b.getX();
Homography[i2, 0] = 0;
Homography[i2, 1] = 0;
Homography[i2, 2] = 0;
Homography[i2, 3] = a.getX();
Homography[i2, 4] = a.getY();
Homography[i2, 5] = 1;
Homography[i2, 6] = -b.getY() * a.getX();
Homography[i2, 7] = -b.getY() * a.getY();
Homography[i2, 8] = -b.getY();
index++;
}
var m = Homography.GetLength(1);
var n = Homography.GetLength(1);
var k = Homography.GetLength(0);
var mat = new double[m, n];
alglib.rmatrixgemm(m, n, k, 1,
Homography, 0, 0, 1,
Homography, 0, 0, 0,
0,
ref mat, 0, 0
);
// SV decomposition. A = U * S * V^T
m = mat.GetLength(0);
n = mat.GetLength(1);
alglib.rmatrixsvd(mat, m, n, 2, 0, 2, out var w, out var u, out _);
var minIndex = Array.IndexOf(w, w.Min());
var scale = 1 / u[MatrixWidth - 1, minIndex];
var result = new List <double>();
for (var i = 0; i < MatrixWidth; i++)
{
result.Add(scale * u[i, minIndex]);
}
return(result);
}
private static Vector CalculateForPointWithRotation(Mat gradient, Mat dx, Mat dy, double expScale,
int gridSize, double radius, int binsCount, InterestingPoint center, double alpha, bool affine)
{
var centerX = center.getX() / (1 << center.Octave);
var centerY = center.getY() / (1 << center.Octave);
var size = (int)(radius * 2 + 1);
var blockSize = size / gridSize;
var step = 2 * Math.PI / binsCount;
double cos = Math.Cos(-alpha),
sin = Math.Sin(-alpha);
var bins = new Dictionary <ValueTuple <int, int>, List <double> >();
for (var u = -radius; u < radius; u++)
{
for (var v = -radius; v < radius; v++)
{
int x = (int)(centerX + u),
y = (int)(centerY + v);
var magnitude = gradient.GetPixel(x, y, BorderWrapType.Wrap);
var theta = Math.Atan2(dy.GetPixel(x, y, BorderWrapType.Wrap), dx.GetPixel(x, y, BorderWrapType.Wrap));
if (theta < 0)
{
theta += Math.PI * 2;
}
var rotatedU = u * cos + v * sin;
var rotatedV = v * cos - u * sin;
var row = (rotatedV + size / 2D) / blockSize;
var column = (rotatedU + size / 2D) / blockSize;
if (column < 0 || column >= gridSize || row < 0 || row >= gridSize)
{
continue;
}
magnitude *= Math.Exp(expScale * (rotatedU * rotatedU + rotatedV * rotatedV));
var rotatedTheta = theta + alpha;
if (rotatedTheta > Math.PI * 2)
{
rotatedTheta -= Math.PI * 2;
}
var ratio = rotatedTheta % step / step;
var leftBin = Math.Min((int)Math.Floor(rotatedTheta / step), binsCount - 1);
var rightBin = (leftBin + 1) % binsCount;
if (!bins.ContainsKey(((int)row, (int)column)))
{
bins.Add(((int)row, (int)column), Enumerable.Repeat(0D, binsCount).ToList());
}
if (!affine)
{
PutToBins(bins, (int)row, (int)column, 1, leftBin, rightBin, ratio, magnitude);
}
else
{
var i = (int)row;
if (row - i <= 0.5)
{
i--;
}
var j = (int)column;
if (column - j <= 0.5)
{
j--;
}
var ki = 1 - Math.Abs(column - (j + 0.5));
var kj = 1 - Math.Abs(row - (i + 0.5));
PutToBins(bins, i, j, ki * kj, leftBin, rightBin, ratio, magnitude);
PutToBins(bins, i, j + 1, (1 - ki) * kj, leftBin, rightBin, ratio, magnitude);
PutToBins(bins, i + 1, j, ki * (1 - kj), leftBin, rightBin, ratio, magnitude);
PutToBins(bins, i + 1, j + 1, (1 - ki) * (1 - kj), leftBin, rightBin, ratio, magnitude);
}
}
}
var result = new Vector(gridSize * gridSize * binsCount);
for (var i = 0; i < gridSize; i++)
{
for (var j = 0; j < gridSize; j++)
{
for (var k = 0; k < binsCount; k++)
{
double value = 0;
if (bins.ContainsKey((i, j)))
{
value = bins[(i, j)][k];
public static Vector CalculateForPointWithRotation(Mat gradient, Mat dx, Mat dy, double sigma,
int gridSize, int binsCount, InterestingPoint center, double alpha)
{
var expScale = -1 / (2 * sigma * sigma);
var radius = (int)Math.Round(3 * sigma);
return(CalculateForPointWithRotation(gradient, dx, dy, expScale, gridSize, radius, binsCount, center,
alpha, true));
}
void GetDescriptor(InterestingPoint ip, bool bUpright, bool bExtended)
{
int sample_x, sample_y, count = 0;
int i = 0, ix = 0, j = 0, jx = 0, xs = 0, ys = 0;
double dx, dy, mdx, mdy, co, si;
double dx_yn, mdx_yn, dy_xn, mdy_xn;
double gauss_s1 = 0f, gauss_s2 = 0f;
double rx = 0f, ry = 0f, rrx = 0f, rry = 0f, len = 0f;
double cx = -0.5f, cy = 0f; //Subregion centers for the 4x4 gaussian weighting
// Get rounded InterestPoint data
int X = (int)Math.Round(ip.X, 0);
int Y = (int)Math.Round(ip.Y, 0);
int S = (int)Math.Round(ip.Scale, 0);
// Allocate descriptor memory
ip.DescriptorLength = 64;
if (bUpright)
{
co = 1;
si = 0;
}
else
{
co = (float)Math.Cos(ip.Orientation);
si = (float)Math.Sin(ip.Orientation);
}
//Calculate descriptor for this interest point
i = -8;
while (i < 12)
{
j = -8;
i = i - 4;
cx += 1f;
cy = -0.5f;
while (j < 12)
{
cy += 1f;
j = j - 4;
ix = i + 5;
jx = j + 5;
dx = dy = mdx = mdy = 0f;
dx_yn = mdx_yn = dy_xn = mdy_xn = 0f;
xs = (int)Math.Round(X + (-jx * S * si + ix * S * co), 0);
ys = (int)Math.Round(Y + (jx * S * co + ix * S * si), 0);
// zero the responses
dx = dy = mdx = mdy = 0f;
dx_yn = mdx_yn = dy_xn = mdy_xn = 0f;
for (int k = i; k < i + 9; ++k)
{
for (int l = j; l < j + 9; ++l)
{
//Get coords of sample point on the rotated axis
sample_x = (int)Math.Round(X + (-l * S * si + k * S * co), 0);
sample_y = (int)Math.Round(Y + (l * S * co + k * S * si), 0);
//Get the gaussian weighted x and y responses
gauss_s1 = Gaussian(xs - sample_x, ys - sample_y, 2.5f * S);
rx = (float)img.HaarX(sample_y, sample_x, 2 * S);
ry = (float)img.HaarY(sample_y, sample_x, 2 * S);
//Get the gaussian weighted x and y responses on rotated axis
rrx = gauss_s1 * (-rx * si + ry * co);
rry = gauss_s1 * (rx * co + ry * si);
if (bExtended)
{
// split x responses for different signs of y
if (rry >= 0)
{
dx += rrx;
mdx += Math.Abs(rrx);
}
else
{
dx_yn += rrx;
mdx_yn += Math.Abs(rrx);
}
// split y responses for different signs of x
if (rrx >= 0)
{
dy += rry;
mdy += Math.Abs(rry);
}
else
{
dy_xn += rry;
mdy_xn += Math.Abs(rry);
}
}
else
{
dx += rrx;
dy += rry;
mdx += Math.Abs(rrx);
mdy += Math.Abs(rry);
}
}
}
//Add the values to the descriptor vector
gauss_s2 = Gaussian(cx - 2f, cy - 2f, 1.5f);
ip.Descriptor[count++] = dx * gauss_s2;
ip.Descriptor[count++] = dy * gauss_s2;
ip.Descriptor[count++] = mdx * gauss_s2;
ip.Descriptor[count++] = mdy * gauss_s2;
// add the extended components
if (bExtended)
{
ip.Descriptor[count++] = dx_yn * gauss_s2;
ip.Descriptor[count++] = dy_xn * gauss_s2;
ip.Descriptor[count++] = mdx_yn * gauss_s2;
ip.Descriptor[count++] = mdy_xn * gauss_s2;
}
len += (dx * dx + dy * dy + mdx * mdx + mdy * mdy
+ dx_yn + dy_xn + mdx_yn + mdy_xn) * gauss_s2 * gauss_s2;
j += 9;
}
i += 9;
}
//Convert to Unit Vector
len = (float)Math.Sqrt((double)len);
if (len > 0)
{
for (int d = 0; d < ip.DescriptorLength; ++d)
{
ip.Descriptor[d] /= len;
}
}
}
void GetOrientation(InterestingPoint ip)
{
const byte Responses = 109;
double[] resX = new double[Responses];
double[] resY = new double[Responses];
double[] Ang = new double[Responses];
int idx = 0;
int[] id = { 6, 5, 4, 3, 2, 1, 0, 1, 2, 3, 4, 5, 6 };
// Get rounded InterestPoint data
int X = (int)Math.Round(ip.X, 0);
int Y = (int)Math.Round(ip.Y, 0);
int S = (int)Math.Round(ip.Scale, 0);
// calculate haar responses for points within radius of 6*scale
for (int i = -6; i <= 6; ++i)
{
for (int j = -6; j <= 6; ++j)
{
if (i * i + j * j < 36)
{
double gauss = gauss25[id[i + 6], id[j + 6]];
resX[idx] = gauss * img.HaarX(Y + j * S, X + i * S, 4 * S);
resY[idx] = gauss * img.HaarY(Y + j * S, X + i * S, 4 * S);
Ang[idx] = GetAngle(resX[idx], resY[idx]);
++idx;
}
}
}
// calculate the dominant direction
double sumX, sumY, max = 0, orientation = 0;
double ang1, ang2;
double pi = Math.PI;
// loop slides pi/3 window around feature point
for (ang1 = 0; ang1 < 2 * pi; ang1 += 0.15f)
{
ang2 = (ang1 + pi / 3f > 2 * pi ? ang1 - 5 * pi / 3f : ang1 + pi / 3f);
sumX = sumY = 0;
for (int k = 0; k < Responses; ++k)
{
// determine whether the point is within the window
if (ang1 < ang2 && ang1 < Ang[k] && Ang[k] < ang2)
{
sumX += resX[k];
sumY += resY[k];
}
else if (ang2 < ang1 &&
((Ang[k] > 0 && Ang[k] < ang2) || (Ang[k] > ang1 && Ang[k] < pi)))
{
sumX += resX[k];
sumY += resY[k];
}
}
// if the vector produced from this window is longer than all
// previous vectors then this forms the new dominant direction
if (sumX * sumX + sumY * sumY > max)
{
// store largest orientation
max = sumX * sumX + sumY * sumY;
orientation = GetAngle(sumX, sumY);
}
}
// assign orientation of the dominant response vector
ip.Orientation = orientation;
}
