Subversion Repositories Code-Repo

#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;
}