1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
|
/********************************
Author: Sravanthi Kota Venkata
********************************/
#include "tracking.h"
/** ANMS - Adaptive Non-Maximal Suppression
This function takes features as input and suppresses those which
are close to each other (within the SUPPRESSION_RADIUS) and have
similar feature strength
**/
F2D *getANMS (F2D *points, float r)
{
float MAX_LIMIT = 100000;
F2D *suppressR;
float C_ROBUST = 1.0;
F2D *srtdPnts;
int n;
I2D *srtdVIdx, *supId;
float t, t1, r_sq;
F2D *tempF, *srtdV, *interestPnts;
int i, j, validCount, cnt, end, k;
int iter, rows, cols;
F2D *temp;
int supIdPtr = 0;
/** Concatenate x,y,v to form points matrix **/
r_sq = r * r;
n = points->height;
srtdVIdx = iMallocHandle(points->height, 1);
for (i = 0; i < srtdVIdx->height; i++) {
asubsref(srtdVIdx,i) = i;
}
srtdPnts = fMallocHandle (srtdVIdx->height, points->width);
for (i = 0; i < srtdVIdx->height; i++) {
for(j=0; j<points->width; j++) {
subsref(srtdPnts,i,j) = subsref(points, asubsref(srtdVIdx,i), j);
}
}
temp = fSetArray (1, 3, 0);
suppressR = fSetArray(n, 1, MAX_LIMIT);
validCount = n;
iter = 0;
/** Allocate supId for #validCount and fill the values of
supId with the indices where suppressR>r_sq **/
k = 0;
supId = iMallocHandle(validCount, 1);
for (i = 0; i < (suppressR->height*suppressR->width); i++)
{
if ( asubsref(suppressR,i) > r_sq)
{
asubsref(supId,k++) = i;
}
}
/** While number of features not-inspected is >0, **/
while (validCount > 0)
{
F2D *tempp, *temps;
/** Inspect the strongest feature point in srtdPnts
The index of that feature is in supId and the
index values in supId are arranged in descending order **/
asubsref(temp,0) = subsref(srtdPnts, asubsref(supId,0), 0);
asubsref(temp,1) = subsref(srtdPnts, asubsref(supId,0), 1);
asubsref(temp,2) = subsref(srtdPnts, asubsref(supId,0), 2);
/** Stacking up the interestPnts matrix with top features
post suppression **/
if(iter == 0)
interestPnts = fDeepCopy(temp);
else
{
tempp = fDeepCopy(interestPnts);
fFreeHandle(interestPnts);
interestPnts = ffVertcat(tempp, temp);
fFreeHandle(tempp);
}
iter++;
tempp = fDeepCopy(srtdPnts);
temps = fDeepCopy(suppressR);
fFreeHandle(srtdPnts);
fFreeHandle(suppressR);
/** Remove the feature that has been added to interestPnts **/
srtdPnts = fMallocHandle(supId->height-1, 3);
suppressR = fMallocHandle(supId->height-1, 1);
k=0;
for(i=1; i<(supId->height); i++) /** Filling srtdPnts after removing the feature that was added to interestPnts**/
{
subsref(srtdPnts,(i-1),0) = subsref(tempp, asubsref(supId,i) ,0);
subsref(srtdPnts,(i-1),1) = subsref(tempp, asubsref(supId,i) ,1);
subsref(srtdPnts,(i-1),2) = subsref(tempp, asubsref(supId,i) ,2);
subsref(suppressR,(i-1),0) = subsref(temps, asubsref(supId,i) ,0);
}
fFreeHandle(tempp);
fFreeHandle(temps);
rows = interestPnts->height-1;
cols = interestPnts->width;
/** For each feature, find how robust it is compared to the one in interestPnts **/
for (i = 0; i < srtdPnts->height; i++)
{
t = 0;
t1 = 0;
if ((C_ROBUST * subsref(interestPnts,rows,2)) >= subsref(srtdPnts, i,2))
{
t = subsref(srtdPnts, i,0) - subsref(interestPnts,rows,0);
t1 = subsref(srtdPnts, i,1) - subsref(interestPnts,rows,1);
t = t * t + t1 * t1;
t1 = 0;
}
if ((C_ROBUST * subsref(interestPnts,rows,2)) < subsref(srtdPnts, i,2))
t1 = 1 * MAX_LIMIT;
if ( asubsref(suppressR, i) > (t + t1))
{
asubsref(suppressR, i) = t + t1;
}
}
/** Inspect the new suppressR to find how many valid features left **/
validCount=0;
for (i = 0; i < suppressR->height; i++) {
if ( asubsref(suppressR,i) > r_sq) {
validCount++;
}
}
k = 0;
iFreeHandle(supId);
/** Allocate supId for #validCount and fill the values of
supId with the indices where suppressR>r_sq **/
supId = iMallocHandle(validCount, 1);
for (i = 0; i < suppressR->height*suppressR->width; i++) {
if ( asubsref(suppressR,i) > r_sq)
asubsref(supId,k++) = i;
}
}
iFreeHandle(supId);
iFreeHandle(srtdVIdx);
fFreeHandle(srtdPnts);
fFreeHandle(temp);
fFreeHandle(suppressR);
return interestPnts;
}
|
