1 #include "objects_tracker.h"
2 #include "track_link.h"
3
4 #define max(a,b) (((a) > (b)) ? (a) : (b))
5 #define min(a,b) (((a) < (b)) ? (a) : (b))
6
ObjectsTracker()7 ObjectsTracker::ObjectsTracker() :
8 parameters(),
9 numTrackedSteps(0)
10 {
11
12 }
13
~ObjectsTracker()14 ObjectsTracker::~ObjectsTracker()
15 {
16
17 }
18
Parameters()19 ObjectsTracker::Parameters::Parameters()
20 {
21 maxTrackLifetime=6;
22 numLastPositionsToTrack=4;
23 numDetectedToWaitBeforeFirstShow=0;
24 numStepsToWaitBeforeFirstShow=6;
25 numStepsToTrackWithoutDetectingIfObjectHasNotBeenShown=4;
26 numStepsToShowWithoutDetecting=5;
27 }
28
CalculateIOU(Rect_T r1,Rect_T r2)29 float CalculateIOU(Rect_T r1, Rect_T r2) {
30 int xmin0 = r1.x;
31 int ymin0 = r1.y;
32 int xmax0 = r1.x +r1.width;
33 int ymax0 = r1.y +r1.height;
34 int xmin1 = r2.x;
35 int ymin1 = r2.y;
36 int xmax1 = r2.x +r2.width;
37 int ymax1 = r2.y +r2.height;
38 float w = max(0.f, min(xmax0, xmax1) - max(xmin0, xmin1));
39 float h = max(0.f, min(ymax0, ymax1) - max(ymin0, ymin1));
40 float i = w * h;
41 float u = (xmax0 - xmin0) * (ymax0 - ymin0) + (xmax1 - xmin1) * (ymax1 - ymin1) - i;
42 return u <= 0.f ? 0.f : (i / u);
43 }
44
CalculateArea(Rect_T r)45 float CalculateArea(Rect_T r) {
46 float i = r.width * r.height;
47 return i;
48 }
49
predict_loctation(TrackedObject & curObject,int image_width,int image_height,float & pre_x,float & pre_y,float & v_x,float & v_y)50 void ObjectsTracker::predict_loctation(TrackedObject& curObject, int image_width,int image_height, float& pre_x, float& pre_y, float& v_x, float& v_y){
51 int numpositions = (int)curObject.lastPositions.size();
52 Rect_T prevRect = curObject.lastPositions[numpositions-1];
53 Rect_T tmpRect=curObject.lastPositions[numpositions-2];
54 float vx_1 = (prevRect.x -tmpRect.x)/(1.0f +curObject.preNumFramesNotDetected);
55 float vx_2 = (prevRect.x +prevRect.width -tmpRect.x -tmpRect.width)/(1.0f +curObject.preNumFramesNotDetected);
56 float vx_ = min(vx_1, vx_2);
57 float vy_1 = (prevRect.y -tmpRect.y)/(1.0f +curObject.preNumFramesNotDetected);
58 float vy_2 = (prevRect.y +prevRect.height -tmpRect.y -tmpRect.height)/(1.0f +curObject.preNumFramesNotDetected);
59 float vy_ = min(vy_1, vy_2);
60 v_x = 0.5f*vx_ + 0.5f*curObject.vx;
61 v_y = 0.5f*vy_ + 0.5f*curObject.vy;
62 int x = (int)(prevRect.x + v_x*(1 ) *0.7f +0.5f);
63 int y = (int)(prevRect.y + v_y*(1 ) *0.7f +0.5f);
64 x = x >= 0 ? x : 0;
65 y = y >= 0 ? y : 0;
66 if(x + prevRect.width >= image_width)
67 x -= (x + prevRect.width -image_width +1);
68 if(y + prevRect.height >= image_height)
69 y -= (y + prevRect.height -image_height +1);
70 x = x >= 0 ? x : 0;
71 y = y >= 0 ? y : 0;
72 pre_x = x;
73 pre_y = y;
74
75 }
76
getObjects(std::vector<ExtObject> & result)77 void ObjectsTracker::getObjects(std::vector<ExtObject>& result)
78 {
79 result.clear();
80 for(size_t i=0; i < trackedObjects.size(); i++) {
81 ObjectStatus status;
82 Rect_T r=calcTrackedObjectPositionToShow((int)i, status);
83
84 if (CalculateArea(r)==0.f || trackedObjects[i].numFramesDetected < 2){
85 continue;
86 }
87
88 result.push_back(ExtObject(trackedObjects[i].id, r, trackedObjects[i].predict_loc_when_miss, trackedObjects[i].smooth_Positionn, status,
89 trackedObjects[i].numFramesDetected, trackedObjects[i].numFramesNotDetected, trackedObjects[i].obj_class, trackedObjects[i].score, trackedObjects[i].miss));
90
91 }
92 }
93
updateTrackedObjects(const std::vector<Rect_T> & detectedObjects,const std::vector<int> objects_class,const std::vector<float> objects_score,int maxTrackLifetime,int image_width,int image_height)94 void ObjectsTracker::updateTrackedObjects(const std::vector<Rect_T>& detectedObjects, const std::vector<int> objects_class
95 , const std::vector<float> objects_score, int maxTrackLifetime, int image_width,int image_height)
96 {
97 enum {
98 NEW_RECTANGLE=-1,
99 INTERSECTED_RECTANGLE=-2
100 };
101
102 int N1=(int)trackedObjects.size();
103 int N2=(int)detectedObjects.size();
104
105 for(int i=0; i < N1; i++) {
106 trackedObjects[i].numDetectedFrames++;
107 }
108
109 std::vector<int> correspondence(detectedObjects.size(), NEW_RECTANGLE);
110 std::vector<float> correspondenceScore(detectedObjects.size(), 0);
111
112 for(int i=0; i < N1; i++) {
113 TrackedObject& curObject=trackedObjects[i];
114 int bestIndex=-1;
115 float bestArea=-1;
116 int numpositions=(int)curObject.lastPositions.size();
117 Rect_T prevRect=curObject.lastPositions[numpositions-1];
118
119 //save predict loctation
120 if(numpositions>1)
121 {
122 float pre_x, pre_y, vx, vy;
123 predict_loctation(curObject, image_width, image_height, pre_x, pre_y, vx, vy);
124 curObject.vx = vx;
125 curObject.vy = vy;
126 prevRect.x = (int)pre_x;
127 prevRect.y = (int)pre_y;
128 curObject.predict_loc_when_miss = prevRect;
129 }
130
131 //search track loaction
132 for(int j=0; j < N2; j++) {
133
134 float percentage_IOU = CalculateIOU(prevRect, detectedObjects[j]);
135 if ( percentage_IOU > 0.1f ) {//&& objects_class[j] == curObject.obj_class
136
137 float trackScore = percentage_IOU *1.f / (curObject.numFramesNotDetected + 1);
138 if ( percentage_IOU > bestArea && correspondenceScore[j] < trackScore) {
139 bestIndex = j;
140 bestArea = percentage_IOU;
141 correspondenceScore[j] = trackScore;
142 }
143 }
144
145 }
146 if (bestIndex >= 0) {
147 correspondence[bestIndex] = i;
148 }
149 }
150
151 //select track loaction
152 for (int i = 0; i < N1; i++) {
153 TrackedObject& curObject = trackedObjects[i];
154 int bestIndex = -1;
155 for (int j = 0; j < N2; j++) {
156 if (correspondence[j] == i){
157 bestIndex = j;
158 break;
159 }
160 }
161 if (bestIndex >= 0) {
162 correspondence[bestIndex] = i;
163 for (int j = 0; j < N2; j++) {
164 if (correspondence[j] >= 0)
165 continue;
166
167 float percentage_IOU = CalculateIOU(detectedObjects[j], detectedObjects[bestIndex]);
168 if (percentage_IOU > 0.45f ){//&& objects_class[j] == curObject.obj_class
169 correspondence[j] = INTERSECTED_RECTANGLE;
170 }
171 }
172 curObject.numFramesDetected++;
173 }
174 else {
175
176 curObject.numFramesNotDetected++;
177 curObject.miss = 1;
178 }
179
180 }
181
182 //allocate new detected location
183 for(int j=0; j < N2; j++) {
184 int i = correspondence[j];
185 if (i >= 0) {//add position
186 trackedObjects[i].lastPositions.push_back(detectedObjects[j]);
187 while ((int)trackedObjects[i].lastPositions.size() > (int) parameters.numLastPositionsToTrack) {
188 trackedObjects[i].lastPositions.erase(trackedObjects[i].lastPositions.begin());
189 }
190 trackedObjects[i].preNumFramesNotDetected = trackedObjects[i].numFramesNotDetected;
191 trackedObjects[i].numFramesNotDetected = 0;
192 trackedObjects[i].score = objects_score[j];
193 trackedObjects[i].obj_class = objects_class[j];
194 trackedObjects[i].miss = 0;
195 //smooth rect
196 trackedObjects[i].smooth_Positionn.width = (trackedObjects[i].smooth_Positionn.width *1 +detectedObjects[j].width )/(2);
197 trackedObjects[i].smooth_Positionn.height = (trackedObjects[i].smooth_Positionn.height *1 +detectedObjects[j].height)/(2);
198 Rect_T r_smooth = trackedObjects[i].smooth_Positionn;
199 float weight_p = 0.5f, weight_n = 1.f;
200 trackedObjects[i].smooth_Positionn.x = (int)(((r_smooth.x +r_smooth.width*0.5f) *weight_p +(detectedObjects[j].x +detectedObjects[j].width*0.5f)*weight_n)/(weight_p +weight_n)
201 +trackedObjects[i].vx *weight_p /(weight_p +weight_n) -r_smooth.width *0.5f);
202
203 trackedObjects[i].smooth_Positionn.y = (int)(((r_smooth.y +r_smooth.height*0.5f) *weight_p +(detectedObjects[j].y +detectedObjects[j].height*0.5f)*weight_n)/(weight_p +weight_n)
204 +trackedObjects[i].vy *weight_p /(weight_p +weight_n)-r_smooth.height *0.5f);
205 } else if (i==NEW_RECTANGLE){ //new object
206
207 trackedObjects.push_back(detectedObjects[j]);
208 int _N2 = (int)trackedObjects.size();
209 trackedObjects[_N2-1].obj_class = objects_class[j];
210 trackedObjects[_N2-1].score = objects_score[j];
211 correspondence[j] = _N2 -1;
212 trackedObjects[_N2-1].smooth_Positionn.width = detectedObjects[j].width;
213 trackedObjects[_N2-1].smooth_Positionn.height = detectedObjects[j].height;
214 trackedObjects[_N2-1].smooth_Positionn.x = (detectedObjects[j].x);
215 trackedObjects[_N2-1].smooth_Positionn.y = (detectedObjects[j].y);
216 }
217 }
218
219 std::vector<TrackedObject>::iterator it=trackedObjects.begin();
220 while( it != trackedObjects.end() ) {
221 if ( (it->numFramesNotDetected > maxTrackLifetime)
222 #if 0
223 ||
224 (
225 (it->numDetectedFrames <= parameters.numStepsToWaitBeforeFirstShow)
226 &&
227 (it->numFramesNotDetected > parameters.numStepsToTrackWithoutDetectingIfObjectHasNotBeenShown)
228 )
229 #endif
230 )
231 {
232 it=trackedObjects.erase(it);
233
234 } else {
235 it++;
236 }
237
238 }
239
240 }
241
calcTrackedObjectPositionToShow(int i,ObjectStatus & status) const242 Rect_T ObjectsTracker::calcTrackedObjectPositionToShow(int i, ObjectStatus& status) const
243 {
244 Rect_T r;
245 r.x = 0;
246 r.y = 0;
247 r.width = 0;
248 r.height = 0;
249
250 if ( (i < 0) || (i >= (int)trackedObjects.size()) ) {
251 status = WRONG_OBJECT;
252
253 return r;
254 }
255
256 #if 0
257 if (trackedObjects[i].numDetectedFrames <= parameters.numStepsToWaitBeforeFirstShow) {
258 status = DETECTED_NOT_SHOWN_YET;
259 return r;
260 }
261
262
263 if (trackedObjects[i].numFramesDetected <= parameters.numDetectedToWaitBeforeFirstShow) {
264 status = DETECTED_NOT_SHOWN_YET;
265 return r;
266 }
267
268
269 if (trackedObjects[i].numFramesNotDetected > parameters.numStepsToShowWithoutDetecting) {
270 status = DETECTED_TEMPORARY_LOST;
271
272 return r;
273 }
274 #endif
275
276 const TrackedObject::PositionsVector& lastPositions=trackedObjects[i].lastPositions;
277
278 int N=(int)lastPositions.size();
279 if (N<=0) {
280 status = WRONG_OBJECT;
281 return r;
282 }
283
284 return lastPositions[N-1];
285
286 }
287
288
289