| 0,0 → 1,87 |
|---|
| #include "MovingPointTracker.h" |
| |
| MovingPointTracker::MovingPointTracker() { |
| active_objects = 0; |
| nextID = 0; |
| } |
| |
| |
| MovingPointTracker::~MovingPointTracker() { |
| |
| } |
| |
| float MovingPointTracker::pointDistance(cv::Point2f &pt1, cv::Point2f &pt2) { |
| cv::Point2f diff = pt1 - pt2; |
| return cv::sqrt(diff.x * diff.x + diff.y * diff.y); |
| } |
| |
| std::vector<MOVING_OBJECT> MovingPointTracker::update(std::vector<cv::Point2f> points) { |
| // Mark each existing object as invalid |
| for (int i = 0; i < objects.size(); i++) { |
| objects[i].valid = false; |
| } |
| |
| std::vector<MOVING_OBJECT> newObjects; |
| |
| // For each point, determine if it is an extension of an existing object |
| for (int i = 0; i < points.size(); i++) { |
| bool found = false; |
| int index; |
| for (index = 0; index < objects.size(); index++) { |
| // Find the first object within the distance threshold |
| // TODO: calculate all distances and use the closest one |
| // TODO: handle case where multiple objects are merged into one |
| if (pointDistance(points[i], objects[index].last_known_pt) < OBJECT_DISTANCE_THRESHOLD) { |
| found = true; |
| break; |
| } |
| } |
| if (found) { |
| // If a match was found, update the object |
| objects[index].valid = true; |
| // Predict the next point using the Kalman filter |
| objects[index].filter->predict(); |
| objects[index].last_known_pt = points[i]; |
| objects[index].predicted_pt = objects[index].filter->correct(points[i].x, points[i].y); |
| // Calculate the running average |
| objects[index].history.push_front(objects[index].predicted_pt); |
| if (objects[index].history.size() > HISTORY_DEPTH) |
| objects[index].history.pop_back(); |
| float avgX = 0, avgY = 0; |
| for (std::list<cv::Point2f>::iterator it = objects[index].history.begin(); it != objects[index].history.end(); it++) { |
| avgX += (*it).x; |
| avgY += (*it).y; |
| } |
| avgX /= objects[index].history.size(); |
| avgY /= objects[index].history.size(); |
| objects[index].historyAvg = cv::Point2f(avgX, avgY); |
| // Calculate the updated velocity and angle |
| objects[index].velocity = pointDistance(objects[index].predicted_pt, objects[index].historyAvg); |
| cv::Point2f diff = objects[index].predicted_pt - objects[index].historyAvg; |
| objects[index].angle = atan2(diff.y, diff.x); |
| } else { |
| // If no matches were found, create a new object |
| MOVING_OBJECT newObject; |
| newObject.ID = nextID; |
| nextID++; |
| newObject.last_known_pt = points[i]; |
| newObject.predicted_pt = points[i]; |
| newObject.velocity = 0; |
| newObject.angle = 0; |
| newObject.filter = new KalmanFilter(points[i].x, points[i].y); |
| newObject.historyAvg = cv::Point2f(0, 0); |
| newObjects.push_back(newObject); |
| } |
| } |
| |
| // If an object is invalid at this point, remove it |
| //std::vector<MOVING_OBJECT> newObjects; |
| for (int i = 0; i < objects.size(); i++) { |
| if (objects[i].valid) { |
| newObjects.push_back(objects[i]); |
| } |
| } |
| objects = newObjects; |
| |
| return objects; |
| } |