/// <summary>
/// Сравнение двух дескрипторов
/// </summary>
/// <param name="pt1">Ключевая точка 1</param>
/// <param name="pt2">Ключевая точка 2</param>
/// <param name="best">Максимально возможное расстояние</param>
/// <returns></returns>
public static double compareDescriptors(InterestPoint pt1, InterestPoint pt2, double best)
{
double total_cost = 0;
for (int i = 0; i < pt1.descriptorLength; i += 4)
{
double t0 = pt1.descriptor[i] - pt2.descriptor[i];
double t1 = pt1.descriptor[i + 1] - pt2.descriptor[i + 1];
double t2 = pt1.descriptor[i + 2] - pt2.descriptor[i + 2];
double t3 = pt1.descriptor[i + 3] - pt2.descriptor[i + 3];
total_cost += t0 * t0 + t1 * t1 + t2 * t2 + t3 * t3;
if (total_cost > best)
break;
}
return total_cost;
}
/// <summary>
/// Сравнение двух дескрипторов
/// </summary>
/// <param name="pt1">Ключевая точка 1</param>
/// <param name="pt2">Ключевая точка 2</param>
/// <param name="best">Максимально возможное расстояние</param>
/// <returns></returns>
public static double compareDescriptors(InterestPoint pt1, InterestPoint pt2, double best)
{
double total_cost = 0;
for (int i = 0; i < pt1.descriptorLength; i += 4)
{
double t0 = pt1.descriptor[i] - pt2.descriptor[i];
double t1 = pt1.descriptor[i + 1] - pt2.descriptor[i + 1];
double t2 = pt1.descriptor[i + 2] - pt2.descriptor[i + 2];
double t3 = pt1.descriptor[i + 3] - pt2.descriptor[i + 3];
total_cost += t0 * t0 + t1 * t1 + t2 * t2 + t3 * t3;
if (total_cost > best)
{
break;
}
}
return(total_cost);
}
/// <summary>
/// Интерполирование найденных Гессианов соседей 3x3x3
/// </summary>
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;
double[] O = { Of[0, 0], Of[1, 0], Of[2, 0] };
// шаг между фильтрами
int filterStep = (m.filter - b.filter);
// если точка достаточно близка к фактическому экстремуму
if (Math.Abs(O[0]) < 0.5f && Math.Abs(O[1]) < 0.5f && Math.Abs(O[2]) < 0.5f)
{
InterestPoint iPoint = new InterestPoint();
iPoint.x = (float)((c + O[0]) * t.step);
iPoint.y = (float)((r + O[1]) * t.step);
iPoint.scale = (float)((0.1333f) * (m.filter + O[2] * filterStep));
iPoint.laplacian = (int)(m.getLaplacian(r, c, t));
iPoints.Add(iPoint);
}
}
/// <summary>
/// Определение преобладающего направления перепада яркости в окрестности ключевой точки
/// </summary>
/// <param name="ip">Ключевая точка</param>
void GetOrientation(InterestPoint 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 };
//Округление
int X = (int)Math.Round(ip.x, 0);
int Y = (int)Math.Round(ip.y, 0);
int S = (int)Math.Round(ip.scale, 0);
//Вычислить значение вейвлетов Хаара для точек внутри окружности 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;
}
}
}
//Вычислить приоритетное направление
float sumX, sumY, max = 0, orientation = 0;
float ang1, ang2;
float pi = (float)Math.PI;
// вращение окна размером pi/3 вокруг ключевой точки
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)
{
// определим, какие точки находятся внутри окна
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 (sumX * sumX + sumY * sumY > max)
{
//Запомним приоритетное заправление
max = sumX * sumX + sumY * sumY;
orientation = (float)GetAngle(sumX, sumY);
}
}
//Присвоим значение ориентации
ip.orientation = (float)orientation;
}
/// <summary>
/// Вычисление дескриптора для указанной точки
/// </summary>
void GetDescriptor(InterestPoint ip)
{
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;
//Округление
int X = (int)Math.Round(ip.x, 0);
int Y = (int)Math.Round(ip.y, 0);
int S = (int)Math.Round(ip.scale, 0);
ip.SetDescriptorLength(64);
co = (float)Math.Cos(ip.orientation);
si = (float)Math.Sin(ip.orientation);
//Вычислить дескриптор ключевой точки
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);
// обнулим значения
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)
{
//Получим координаты точки после вращения
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);
//Получим значение гауссиана
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);
//он же для повернутой
rrx = gauss_s1 * (-rx * si + ry * co);
rry = gauss_s1 * (rx * co + ry * si);
dx += rrx;
dy += rry;
mdx += Math.Abs(rrx);
mdy += Math.Abs(rry);
}
}
//Добавим значения в вектор дескриптора
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;
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;
}
len = (float)Math.Sqrt((double)len);
if (len > 0)
{
for (int d = 0; d < ip.descriptorLength; ++d)
{
ip.descriptor[d] /= len;
}
}
}
/// <summary>
/// Получает на вход два списка ключевых точек и составляет из них соответствующие пары путем сравнения дескрипторов
/// </summary>
/// <param name="CurIpts">Первый список</param>
/// <param name="lastIpts">Второй список</param>
/// <returns>Пары точек</returns>
ConcurrentBag<InterestPointPair> CreatePairs(List<InterestPoint> CurIpts, List<InterestPoint> lastIpts)
{
ConcurrentBag<InterestPointPair> matched = new ConcurrentBag<InterestPointPair>();
Parallel.ForEach(CurIpts, ip =>
{
InterestPoint neighbour = new InterestPoint();
double dist1 = double.MaxValue;
double dist2 = double.MaxValue;
foreach (InterestPoint ip2 in lastIpts)
{
if (ip.laplacian != ip2.laplacian) continue;
double d = InterestPoint.compareDescriptors(ip, ip2, dist2);
if (d < dist1)
{
dist2 = dist1;
dist1 = d;
neighbour = ip2;
}
else if (d < dist2)
dist2 = d;
}
if (dist1 < 0.2 * dist2)
{
matched.Add(new InterestPointPair { p1 = ip, p2 = neighbour, dist = dist1 }); //Пары одинаковых ключевых точек из соседних кадров
}
});
return matched;
}
/// <summary>
/// Интерполирование найденных Гессианов соседей 3x3x3
/// </summary>
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;
double[] O = { Of[0, 0], Of[1, 0], Of[2, 0] };
// шаг между фильтрами
int filterStep = (m.filter - b.filter);
// если точка достаточно близка к фактическому экстремуму
if (Math.Abs(O[0]) < 0.5f && Math.Abs(O[1]) < 0.5f && Math.Abs(O[2]) < 0.5f)
{
InterestPoint iPoint = new InterestPoint();
iPoint.x = (float)((c + O[0]) * t.step);
iPoint.y = (float)((r + O[1]) * t.step);
iPoint.scale = (float)((0.1333f) * (m.filter + O[2] * filterStep));
iPoint.laplacian = (int)(m.getLaplacian(r, c, t));
iPoints.Add(iPoint);
}
}
/// <summary>
/// Определение преобладающего направления перепада яркости в окрестности ключевой точки
/// </summary>
/// <param name="ip">Ключевая точка</param>
void GetOrientation(InterestPoint 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 };
//Округление
int X = (int)Math.Round(ip.x, 0);
int Y = (int)Math.Round(ip.y, 0);
int S = (int)Math.Round(ip.scale, 0);
//Вычислить значение вейвлетов Хаара для точек внутри окружности 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;
}
}
}
//Вычислить приоритетное направление
float sumX, sumY, max = 0, orientation = 0;
float ang1, ang2;
float pi = (float)Math.PI;
// вращение окна размером pi/3 вокруг ключевой точки
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)
{
// определим, какие точки находятся внутри окна
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 (sumX * sumX + sumY * sumY > max)
{
//Запомним приоритетное заправление
max = sumX * sumX + sumY * sumY;
orientation = (float)GetAngle(sumX, sumY);
}
}
//Присвоим значение ориентации
ip.orientation = (float)orientation;
}
/// <summary>
/// Вычисление дескриптора для указанной точки
/// </summary>
void GetDescriptor(InterestPoint ip)
{
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;
//Округление
int X = (int)Math.Round(ip.x, 0);
int Y = (int)Math.Round(ip.y, 0);
int S = (int)Math.Round(ip.scale, 0);
ip.SetDescriptorLength(64);
co = (float)Math.Cos(ip.orientation);
si = (float)Math.Sin(ip.orientation);
//Вычислить дескриптор ключевой точки
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);
// обнулим значения
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)
{
//Получим координаты точки после вращения
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);
//Получим значение гауссиана
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);
//он же для повернутой
rrx = gauss_s1 * (-rx * si + ry * co);
rry = gauss_s1 * (rx * co + ry * si);
dx += rrx;
dy += rry;
mdx += Math.Abs(rrx);
mdy += Math.Abs(rry);
}
}
//Добавим значения в вектор дескриптора
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;
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;
}
len = (float)Math.Sqrt((double)len);
if (len > 0)
{
for (int d = 0; d < ip.descriptorLength; ++d)
{
ip.descriptor[d] /= len;
}
}
}