手搓目标跟踪器

跟踪器顾名思义，就是跟踪用的。

据个例子，你眼睛盯着一个美女看，当你眨眼的时候，下一秒大概率还是这个美女。这个过程中，美女在你心中建立了一个唯一的ID。当你看这个美女的时候她在你心里建立了一个专属特征，当你眨眼再睁开的时候，你就会用这个专属特征去和当前这个美女去比对，如果比对成功，那么当前这个美女仍然属于这个已有的ID，同时这个ID下的状态也就更新成当前美女的特征，对于这个ID可以形成轨迹，这样美女从哪来可一直追溯下去。

下图中检测框上面有个ID，这个ID会在不同帧之间更新，这样计算机就具备一种类似人类跟踪目标的能力。

说到这里你已经理解：跟踪器是用来解决前后帧目标的关联性问题。

上面的例子中眼睛可以替换成：毫米波雷达、激光雷达、相机等传感器，同时赋予传感器一个工作频率，然后让检测到的结果输入到跟踪器中，这样就开始工作了。

1、object

传感器开始工作以后会检测到很多目标，这些目标是由object这个类来维护的，对于object里面只能告诉我们当前时刻下存在目标：位置、速度等等。（见下图）

class Position {
    public:
    float x;
    float y;
    // float z;
};

class Velocity {
    public:
    float x;
    float y;
    // float z;
};

class Object {
    public:
    ~Object(){};
    Object();
    int obj_id;
    Position* obj_pos;
    Velocity* obj_vel;
    float timestamp;
    Eigen::VectorXf state;
};

2、Frame

当然仅仅知道一个目标不行，一帧中有可能有多个目标（美女）：（见下图）

class Frame {
    public:
    float timestamp;
    std::vector<Object*> Frame_obj;
};

3、Track

知道很多个美女不行，还需要把她们特征记录下来，不然一眨眼就对不上号了是吧！

这里要把目标（美女）记录成轨迹，记录历史状态。（见下图）

class Track {
    public:
    float timestamp;
    ~Track(){};
    Track(Object* objptr);
    Object* obj;
    Eigen::VectorXf state;//x,y,vx,vy
    Eigen::MatrixXf P;
    bool isdead;
    int lost_times;
};

4、Tracker

最关键的来了，这里建立了一个叫跟踪器的东西，由他来管理这些轨迹：假如美女离开了你的视线，你应该保留她的轨迹吗？应该保留一会儿，万一她又回来了呢，但不能一直保留，等她个10秒如果她没出现就把她的轨迹删了把。（见下图）

class Tracker {
    public:
    std::vector<Track*> tracks;//要维护的轨迹
    Tracker(){};
    ~Tracker(){};
    
    void track_match(Frame* objs);
    //跟踪器的核心函数，由它来决定跟踪器的前后帧的目标关联性。他输入一个Frame的东西这个是当前帧的数据也就是最新的，同时类内还维护一个tracker,这个就是历史的。
看到这里就清楚了吧。
    void update_matched_tracks(std::vector<std::pair<int,int>>& matched_pair, Frame* objs);
    //track_match的配套函数，对匹配后结果进行处理。输入一个对组，对组是当前帧和历史帧中配上对后的index，同时再输入一个当前帧，这里面干什么应该都猜到了吧：更新美女的状态。
    void creat_new_tracks(Frame* objs, std::vector<int> unmatch_obj);
    //如果有没匹配上的就创建一个新的对吧（新来了一个没有见过的美女），那就需要知道当前帧中哪一个是需要创建新轨迹。
    void delete_dead_tracks(std::vector<int> unmatch_track);
    //如果类中已有的轨迹没有匹配上，那就要考虑是不是要给她删除。同样unmatch_track中维护的是tracks的index。
};

5、看看Tracker里面

void Tracker::track_match(Frame* objs) {
    std::vector<int> unmatch_track;
    std::vector<int> unmatch_obj;
    std::vector<std::pair<int,int>> matched_pair;
    match(tracks, objs, unmatch_track, unmatch_obj, matched_pair);
    //用到一个匹配函数，接收当前帧和历史帧，输出未匹配轨迹、未匹配目标、匹配的对组；
    update_matched_tracks(matched_pair, objs);
    creat_new_tracks(objs,unmatch_obj);
    delete_dead_tracks(unmatch_track);
    //上面三步，按照流程处理match函数给出的结果。
}

6、继续展开match函数

void match(std::vector<Track*> tracks, Frame* objs, 
           std::vector<int>& unmatch_track,
           std::vector<int>& unmatch_obj,
           std::vector<std::pair<int,int>>& matched_pair) {

    std::vector<bool> track_assist(tracks.size(),false);
    std::vector<bool> obj_assist(objs->Frame_obj.size(),false);
    
    //这里很简单！！就是两个for循环搞定，计算一下前后帧目标之间的距离，然后把结果分配一下！
    //同时这里也很灵活！！有大量的可操作性，怎么匹配，什么尺度，怎么度量，什么算法，都可以在这里DIY,很有意思！
    
    for (int i = 0; i < tracks.size(); i++) {
        Track* track = tracks[i];
        for (int j = 0; j < objs->Frame_obj.size(); j++) {
            Object* obj = objs->Frame_obj[j];
            float dis = eculid_dis(track->obj, obj);//欧式距离
            if (dis < MAX_DIS) {
                std::pair<int,int> pair(i,j);
                matched_pair.push_back(pair);
                track_assist[i] = true;//辅助记录一下index
                obj_assist[i] = true;
            }
        }
    }
    for (int i = 0; i < track_assist.size(); i++) {
        if (track_assist[i]) {
            unmatch_track.push_back(i);//再反馈一下结果index就可以了
        }
    }
    for (int i = 0; i < obj_assist.size(); i++) {
        if (obj_assist[i]) {
            unmatch_obj.push_back(i);
        }
    }
}

7、展开eculid_dis函数

inline float eculid_dis(Object* obj1, Object* obj2) {
    float x_1 = obj1->obj_pos->x;
    float y_1 = obj1->obj_pos->y;
    float x_2 = obj2->obj_pos->x;
    float y_2 = obj2->obj_pos->y;
    float dis = sqrt(pow(fabs(fabs(x_1) - fabs(x_2)),2) + pow(fabs(fabs(y_1) - fabs(y_2)),2)); 
    //计算一下欧式距离
    return dis;
}

8、展开三个track管理函数

void Tracker::update_matched_tracks(std::vector<std::pair<int,int>>& matched_pair, Frame* objs) {
    KalmanFilter kalman_filter;
    //卡尔曼滤波的工作原理，需要知道历史状态，再结合当前帧的状态，修正当前的位置，嘿嘿，放在这里有没有一种很巧妙的感觉。
    for(int i = 0; i < matched_pair.size(); i++) {
        Track* track = tracks[matched_pair[i].first];
        Object* obj = objs->Frame_obj[matched_pair[i].second];
        kalman_filter.predict_update(track, obj);
    }
}

void Tracker::creat_new_tracks(Frame* objs, std::vector<int> unmatch_obj) {
    //当前帧没有匹配对象就新建一个。
    for(int i = 0; i < unmatch_obj.size(); i++) {
        Object* obj = objs->Frame_obj[unmatch_obj[i]];
        Track* new_track = new Track(obj);
        tracks.push_back(new_track);
    }
}

void Tracker::delete_dead_tracks(std::vector<int> unmatch_track) {
    //当轨迹没有可匹配目标，就删除他
    for(int i = 0; i < unmatch_track.size(); i++) {
        if (tracks[unmatch_track[i]]->lost_times > MAX_HOLD_TIMES) {
            tracks[unmatch_track[i]]->isdead = true;
        }
    }
    delete_track(tracks);
}

9、滤波器

要明白两个问题，滤波器运行的几个参数，哪个是跟着滤波器走的（静态），哪些是跟着目标走的（属于目标的）

//滤波器初始化，初始化用的Q矩阵（过程矩阵）R矩阵（噪声矩阵）这两个设定是不变的。
KalmanFilter::KalmanFilter() {
    Q<< 1,0,0,0,
        0,1,0,0,
        0,0,1,0,
        0,0,0,1; 
    R<< 1,0,0,0,
        0,1,0,0;                          
}



void KalmanFilter::predict_update(Track* track, Object* obj) {
    Eigen::VectorXf state = track->state;
    Eigen::MatrixXf P = track->P;
    float track_time = track->timestamp;
    float cur_time = obj->timestamp;
    float T = cur_time - track_time;
    Eigen::VectorXf cur_state = obj->state;
    Eigen::MatrixXf F;
    F<< 1,0,T,0,
        0,1,0,T,
        0,0,1,0,
        0,0,0,1; 
    Eigen::VectorXf prior_state = state * F;//先验结果
    Eigen::MatrixXf Prior_P = F * P * F.transpose() + Q;//更新P矩阵

    Eigen::MatrixXf H;
    H<< 1,0,0,0,
        0,1,0,0;
    Eigen::MatrixXf S = H * Prior_P * H.transpose() + R;
    Eigen::MatrixXf K = Prior_P * H.transpose() * S.inverse();//更新K矩阵
    Eigen::VectorXf Posteriori_state = prior_state + K * (cur_state - H * prior_state);//后验估计
    
    Eigen::MatrixXf I = Eigen::MatrixXf::Identity(4,4);
    Eigen::MatrixXf Posteriori_P = (I - K * H) * Prior_P;//更新后验P矩阵

    track->P = Posteriori_P;//P随着目标走
    track->state = Posteriori_state;//更新结果

}

10、删除函数

inline void delete_track(std::vector<Track*>& tracks) {
    //这个像不像删除数组中的指定元素？去力扣看一看
    int j = 0;
    for(int i = 0; i < tracks.size(); i++) {
        if (!tracks[i]->isdead) {
            tracks[j] = tracks[i];
            j++;
        } else {
            tracks[j] = tracks[i];
        }
    }
    int count = tracks.size() - j + 1;
    while(count--) {
        tracks.pop_back();
    }
}