public static bool IsNear(this IPoint ip1, IPoint ip2)
{
double dis = GetDistance(ip1, ip2);
Console.WriteLine("Gap:{0}", dis);
//return dis < .25;
return dis < .3;
}
private static double GetDistance(IPoint ip1, IPoint ip2)
{
float sum = 0.0f;
for (int i = 0; i < 64; ++i)
sum += (ip1.descriptor[i] - ip2.descriptor[i]) * (ip1.descriptor[i] - ip2.descriptor[i]);
return Math.Sqrt(sum);
}
private const float FLT_MAX = 3.402823466e+38F; /* max value */
#endregion Fields
#region Methods
public static List<IPoint>[] getMatches(List<IPoint> ipts1, List<IPoint> ipts2)
{
double dist;
double d1, d2;
IPoint match = new IPoint();
List<IPoint>[] matches = new List<IPoint>[2];
matches[0] = new List<IPoint>();
matches[1] = new List<IPoint>();
for (int i = 0; i < ipts1.Count; i++) {
d1 = d2 = FLT_MAX;
for (int j = 0; j < ipts2.Count; j++) {
dist = GetDistance(ipts1[i], ipts2[j]);
if (dist < d1) // if this feature matches better than current best
{
d2 = d1;
d1 = dist;
match = ipts2[j];
} else if (dist < d2) // this feature matches better than second best
{
d2 = dist;
}
}
// If match ha s a d1:d2 ratio < 0.65 ipoints are a match
if (d1 / d2 < 0.77) //越小Match点越少
{
matches[0].Add(ipts1[i]);
matches[1].Add(match);
}
}
return matches;
}
public double subtract(IPoint i1, IPoint i2)
{
double sum = 0.0;
for (int i = 0; i < i1.descriptorLength; ++i)
{
sum += (i1.descriptor[i] - i2.descriptor[i]) * (i1.descriptor[i] - i2.descriptor[i]);
}
return Math.Sqrt(sum);
}
/// <summary>
/// Determine dominant orientation for InterestPoint
/// </summary>
/// <param name="ip"></param>
void GetOrientation(IPoint ip)
{
const byte Responses = 109;
float[] resX = new float[Responses];
float[] resY = new float[Responses];
float[] Ang = new float[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)
{
float 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] = (float)GetAngle(resX[idx], resY[idx]);
++idx;
}
}
}
// calculate the dominant direction
float sumX, sumY, max = 0, orientation = 0;
float ang1, ang2;
float pi = (float)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 = (float)GetAngle(sumX, sumY);
}
}
// assign orientation of the dominant response vector
ip.orientation = (float)orientation;
}
/// <summary>
/// Construct descriptor vector for this interest point
/// </summary>
/// <param name="bUpright"></param>
void GetDescriptor(IPoint 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;
float dx, dy, mdx, mdy, co, si;
float dx_yn, mdx_yn, dy_xn, mdy_xn;
float gauss_s1 = 0f, gauss_s2 = 0f;
float rx = 0f, ry = 0f, rrx = 0f, rry = 0f, len = 0f;
float 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.SetDescriptorLength(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;
}
}
}
/// <summary>
/// Interpolate scale-space extrema to subpixel accuracy to form an image feature
/// </summary>
/// <param name="r"></param>
/// <param name="c"></param>
/// <param name="t"></param>
/// <param name="m"></param>
/// <param name="b"></param>
void interpolateExtremum(int r, int c, ResponseLayer t, ResponseLayer m, ResponseLayer b)
{
Matrix D = Matrix.Create(BuildDerivative(r, c, t, m, b));
Matrix H = Matrix.Create(BuildHessian(r, c, t, m, b));
Matrix Hi = H.Inverse();
Matrix Of = -1 * Hi * D;
// get the offsets from the interpolation
double[] O = { Of[0, 0], Of[1, 0], Of[2, 0] };
// get the step distance between filters
int filterStep = (m.filter - b.filter);
// If point is sufficiently close to the actual extremum
if (Math.Abs(O[0]) < 0.5f && Math.Abs(O[1]) < 0.5f && Math.Abs(O[2]) < 0.5f)
{
IPoint ipt = new IPoint();
ipt.x = (float)((c + O[0]) * t.step);
ipt.y = (float)((r + O[1]) * t.step);
ipt.scale = (float)((0.1333f) * (m.filter + O[2] * filterStep));
ipt.laplacian = (int)(m.getLaplacian(r,c,t));
ipts.Add(ipt);
}
}
public List<IPoint>[] GetMatchesTaskParallal(List<IPoint> ipts1, List<IPoint> ipts2)
{
IPoint match = new IPoint();
List<IPoint>[] matches = new List<IPoint>[2];
matches[0] = new List<IPoint>();
matches[1] = new List<IPoint>();
int pageSize = (ipts1.Count * ipts2.Count) / 60000;
if (pageSize == 0) pageSize = 1;
int pages = (int)Math.Ceiling((float)ipts1.Count / (float)pageSize);
Parallel.For(0, pages, x =>
{
for (int i = x * pageSize; i < ((x * pageSize) + pageSize) && i < ipts1.Count; i++)
{
double dist;
double d1, d2;
d1 = d2 = FLT_MAX;
for (int j = 0; j < ipts2.Count; j++)
{
dist = GetDistance(ipts1[i], ipts2[j]);
if (dist < 0.001)
d1 = d2 = FLT_MAX;
if (dist < d1) // if this feature matches better than current best
{
d2 = d1;
d1 = dist;
match = ipts2[j];
}
else if (dist < d2) // this feature matches better than second best
{
d2 = dist;
}
}
// If match has a d1:d2 ratio < 0.65 ipoints are a match
if (d1 / d2 < 0.77) //Match
{
lock (matches)
{
matches[0].Add(ipts1[i]);
matches[1].Add(match);
}
}
}
});
return matches;
}
public List<IPoint>[] GetMatchesOptimized(List<IPoint> ipts1, List<IPoint> ipts2,int padding=20)
{
bool different = false;
if (ipts1.Count < ipts2.Count)
{
List<IPoint> temp = new List<IPoint>();
temp.AddRange(ipts1);
ipts1 = ipts2;
ipts2 = temp;
different = true;
}
double dist;
double d1, d2;
IPoint match = new IPoint();
List<IPoint>[] matches = new List<IPoint>[2];
matches[0] = new List<IPoint>();
matches[1] = new List<IPoint>();
for (int i = 0; i < ipts1.Count; i++)
{
d1 = d2 = FLT_MAX;
bool flag = false;
for (int j = 0; j < ipts2.Count; j++)
{
if ((ipts1[i].x - padding) < ipts2[j].x && (ipts1[i].x + padding) > ipts2[j].x && (ipts1[i].y - padding) < ipts2[j].y && (ipts1[i].y + padding) > ipts2[j].y)
{
dist = GetDistance(ipts1[i], ipts2[j]);
// Console.WriteLine(ipts2[j].x);
if (dist < d1) // if this feature matches better than current best
{
d2 = d1;
d1 = dist;
match = ipts2[j];
flag = true;
}
else if (dist < d2) // this feature matches better than second best
{
d2 = dist;
flag = true;
}
}
//else d1 = d2 = FLT_MAX;
}
// If match has a d1:d2 ratio < 0.65 ipoints are a match
if (d1 / d2 < 0.77 && flag) //Match
{
matches[0].Add(ipts1[i]);
matches[1].Add(match);
}
}
matches = GetDistinctMatches(matches);
matches = GetMatches(matches[0], matches[1]);
if (different)
{
List<IPoint> temp = new List<IPoint>();
temp.AddRange(matches[0]);
matches[0] = matches[1];
matches[1] = temp;
}
return matches;
}
public SurfIndentifier(int n, IPoint fea)
: base(n)
{
_ipoint=fea;
}
