cnstream/modules/locus/include/trajectory.hpp

#pragma once

#include <iostream>
#include <string>
#include <vector>
#include <map>
#include <list>

#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/dnn.hpp"
#include "opencv2/opencv.hpp"
#include "opencv2/highgui.hpp"
#if (CV_MAJOR_VERSION >= 3)
#include "opencv2/imgcodecs/imgcodecs.hpp"
#endif


using namespace std;
using namespace cv;


class Trajectory
{
private:
    std::map<int, std::vector<cv::Point>> m_trajectoryData_map;

    Point pointAdd(Point p, Point q){
        p.x += q.x;
        p.y += q.y;          
        return p;  
    }

    Point pointTimes(float c, Point p){
        p.x *= c;
        p.y *= c;   
        return p;  
    }

    Point Bernstein(float u, cv::Point qi,cv::Point mid,cv::Point mo){
        Point a, b, c, r;  
        a = pointTimes(pow(u, 2), mo);  
        b = pointTimes(pow((1 - u), 2), qi);  
        c = pointTimes(2 * u*(1 - u), mid);  

        r = pointAdd(pointAdd(a, b), c);  

        return r;  
    }

public:
    Trajectory(){};
    ~Trajectory(){};
    void addTraData(int track_id, cv::Point center){
        if(find(track_id)){
            if(m_trajectoryData_map[track_id].size() >= 30){
                m_trajectoryData_map[track_id].erase(m_trajectoryData_map[track_id].begin());
            }
        }
        m_trajectoryData_map[track_id].push_back(center);
    }

    bool find(int track_id){
        std::map<int, std::vector<cv::Point>>::iterator iter = m_trajectoryData_map.find(track_id);
        if(iter != m_trajectoryData_map.end()){
            return true;
        }
        return false;
    }
    
    void delTraData(int track_id){
        if(find(track_id)){
            m_trajectoryData_map.erase(track_id);
        }
    }

    void updataTraData(std::list<int> track_id){
        std::map<int, std::vector<cv::Point>> trajectoryData_map;
        std::map<int, std::vector<cv::Point>>::iterator map_iter;
        for (map_iter = m_trajectoryData_map.begin(); map_iter != m_trajectoryData_map.end(); map_iter++)
        {
            for (std::list<int>::iterator iter = track_id.begin(); iter != track_id.end(); iter++)
            {
                if(map_iter->first == *iter){
                    trajectoryData_map[map_iter->first] = map_iter->second;
                }
            }
        }
        m_trajectoryData_map = trajectoryData_map;
    }

    void dramTrajectory(cv::Mat img){
        std::map<int, std::vector<cv::Point>>::iterator iter;
        for (iter = m_trajectoryData_map.begin(); iter != m_trajectoryData_map.end(); iter++)
        {
            std::vector<cv::Point> Points = iter->second;
            for (size_t i = 2; i < Points.size(); i += 2)
            {
                cv::Point pre, last, mid;
                pre = Points[i - 2];
                mid = Points[i - 1];
                last = Points[i];

                cv::Point pt_pre = Points[i - 2];
                cv::Point pt_now;
                for (size_t k = 0; k <= 10; k++)
                {
                    float u = (float)k / 10;
                    cv::Point new_point = Bernstein(u, pre, mid, last);
                    pt_now.x = (int)new_point.x;
                    pt_now.y = (int)new_point.y;
                    cv::line(img, pt_pre, pt_now, Scalar(0, 255, 0), 2, 16, 0);//绘制直线
                    pt_pre = pt_now;
                }
            }
        }
    }
};