| 287 |
Kevin |
1 |
#include "MovingPointTracker.h"
|
|
|
2 |
|
|
|
3 |
MovingPointTracker::MovingPointTracker() {
|
|
|
4 |
active_objects = 0;
|
|
|
5 |
nextID = 0;
|
|
|
6 |
}
|
|
|
7 |
|
|
|
8 |
|
|
|
9 |
MovingPointTracker::~MovingPointTracker() {
|
|
|
10 |
|
|
|
11 |
}
|
|
|
12 |
|
|
|
13 |
float MovingPointTracker::pointDistance(cv::Point2f &pt1, cv::Point2f &pt2) {
|
|
|
14 |
cv::Point2f diff = pt1 - pt2;
|
|
|
15 |
return cv::sqrt(diff.x * diff.x + diff.y * diff.y);
|
|
|
16 |
}
|
|
|
17 |
|
|
|
18 |
std::vector<MOVING_OBJECT> MovingPointTracker::update(std::vector<cv::Point2f> points) {
|
|
|
19 |
// Mark each existing object as invalid
|
|
|
20 |
for (int i = 0; i < objects.size(); i++) {
|
|
|
21 |
objects[i].valid = false;
|
|
|
22 |
}
|
|
|
23 |
|
|
|
24 |
std::vector<MOVING_OBJECT> newObjects;
|
|
|
25 |
|
|
|
26 |
// For each point, determine if it is an extension of an existing object
|
|
|
27 |
for (int i = 0; i < points.size(); i++) {
|
|
|
28 |
bool found = false;
|
|
|
29 |
int index;
|
|
|
30 |
for (index = 0; index < objects.size(); index++) {
|
|
|
31 |
// Find the first object within the distance threshold
|
|
|
32 |
// TODO: calculate all distances and use the closest one
|
|
|
33 |
// TODO: handle case where multiple objects are merged into one
|
|
|
34 |
if (pointDistance(points[i], objects[index].last_known_pt) < OBJECT_DISTANCE_THRESHOLD) {
|
|
|
35 |
found = true;
|
|
|
36 |
break;
|
|
|
37 |
}
|
|
|
38 |
}
|
|
|
39 |
if (found) {
|
|
|
40 |
// If a match was found, update the object
|
|
|
41 |
objects[index].valid = true;
|
|
|
42 |
// Predict the next point using the Kalman filter
|
|
|
43 |
objects[index].filter->predict();
|
|
|
44 |
objects[index].last_known_pt = points[i];
|
|
|
45 |
objects[index].predicted_pt = objects[index].filter->correct(points[i].x, points[i].y);
|
|
|
46 |
// Calculate the running average
|
|
|
47 |
objects[index].history.push_front(objects[index].predicted_pt);
|
|
|
48 |
if (objects[index].history.size() > HISTORY_DEPTH)
|
|
|
49 |
objects[index].history.pop_back();
|
|
|
50 |
float avgX = 0, avgY = 0;
|
|
|
51 |
for (std::list<cv::Point2f>::iterator it = objects[index].history.begin(); it != objects[index].history.end(); it++) {
|
|
|
52 |
avgX += (*it).x;
|
|
|
53 |
avgY += (*it).y;
|
|
|
54 |
}
|
|
|
55 |
avgX /= objects[index].history.size();
|
|
|
56 |
avgY /= objects[index].history.size();
|
|
|
57 |
objects[index].historyAvg = cv::Point2f(avgX, avgY);
|
|
|
58 |
// Calculate the updated velocity and angle
|
|
|
59 |
objects[index].velocity = pointDistance(objects[index].predicted_pt, objects[index].historyAvg);
|
|
|
60 |
cv::Point2f diff = objects[index].predicted_pt - objects[index].historyAvg;
|
|
|
61 |
objects[index].angle = atan2(diff.y, diff.x);
|
|
|
62 |
} else {
|
|
|
63 |
// If no matches were found, create a new object
|
|
|
64 |
MOVING_OBJECT newObject;
|
|
|
65 |
newObject.ID = nextID;
|
|
|
66 |
nextID++;
|
|
|
67 |
newObject.last_known_pt = points[i];
|
|
|
68 |
newObject.predicted_pt = points[i];
|
|
|
69 |
newObject.velocity = 0;
|
|
|
70 |
newObject.angle = 0;
|
|
|
71 |
newObject.filter = new KalmanFilter(points[i].x, points[i].y);
|
|
|
72 |
newObject.historyAvg = cv::Point2f(0, 0);
|
|
|
73 |
newObjects.push_back(newObject);
|
|
|
74 |
}
|
|
|
75 |
}
|
|
|
76 |
|
|
|
77 |
// If an object is invalid at this point, remove it
|
|
|
78 |
//std::vector<MOVING_OBJECT> newObjects;
|
|
|
79 |
for (int i = 0; i < objects.size(); i++) {
|
|
|
80 |
if (objects[i].valid) {
|
|
|
81 |
newObjects.push_back(objects[i]);
|
|
|
82 |
}
|
|
|
83 |
}
|
|
|
84 |
objects = newObjects;
|
|
|
85 |
|
|
|
86 |
return objects;
|
|
|
87 |
}
|