Lock_Armor

本函数的功能：通过传入最终装甲板和间隔时间,寻找跟踪装甲板

这个函数也是整个跟踪最根本的逻辑，用来找寻跟踪装甲板

需要注意的是，这个函数是包含了整车观测的部分内容的，这个部分不会在此展开讲解，将在 "整车观测篇" 展开论述

下面则是我对跟踪部分代码的提炼，请对照代码：

    //=========================卡尔曼初始化=========================
    KF.setF(dt);                                             // 设置状态转移矩阵时间间隔
    predicted = KF.predict();                                   // 预测值矩阵
    //==========================跟踪装甲板==========================
    SpinHeading spin_status;                                    // 记录陀螺状态（未知，顺时针，逆时针）
    Armor matched_armor;                                        // 匹配装甲板
    bool match = false;                                         // 匹配状态

    //=========================锁定目标装甲板========================
    /** 装甲板不为空的情况,更新了匹配状态 */
    if(!Armors.empty()){
        double min_position_diff = DBL_MAX;                     // 初始化最小距离(DBL_MAX为double类型最大值)
        int Armor_number = 0;                                   // 当前锁定装甲板数量
        std::vector<Armor> Track_Armor;                         // 存放锁定装甲板
        /** 选择距离最近的装甲板 */
        for (auto &armor: Armors) {
            armor.world_position = AS.pixel2imu(armor);                 // 装甲板转换世界坐标系
            Eigen::Vector3d pre = predicted.head(3);                    // 预测的世界坐标系
            double position_diff = (pre - armor.world_position).norm();    // 计算坐标偏移量
            /** 判断最小距离 */
            if (position_diff < min_position_diff) {
                min_position_diff = position_diff;
                matched_armor = armor;
            }
            /** 记录跟踪装甲板ID */
            if(armor.id == tracking_id) {
                Armor_number++;
                Track_Armor.push_back(armor);
            }
        }
        /** 判断是否为锁定装甲板 */
        if (min_position_diff < new_old_threshold && matched_armor.id == tracking_id) {
            /** 匹配状态 */
            match = true;                                                   // 匹配成功
            /** 卡尔曼预测 */
            predicted = KF.update(matched_armor.world_position);        // 更新预测值 返回的是校正后的结果
        }
        /** 本帧相同装甲板 */
        else{
            /** 匹配相同ID的装甲板 */
            for (auto &armor: Armors){
                if (armor.id == tracking_id){
                    /** 匹配状态 */
                    match = true;                                                                   // 匹配成功
                    /** 卡尔曼重置 */
                    Eigen::Matrix<double, 6, 1> pos_speed;
                    KF.Initial();                                                                   // 卡尔曼初始化
                    pos_speed << armor.world_position, predicted.tail(3);                        // 保留速度变量
                    KF.setPosAndSpeed(armor.world_position, predicted.tail(3));   // 设置坐标点和3个速度变量
                    predicted = pos_speed;                                                          // 更新预测值
                    matched_armor = armor;
                    break;
                }
            }
        }

    //=========================跟踪状态处理===========================
    // 更新最终装甲板
    if (match) enemy_armor = matched_armor;

    /** 处理跟踪状态(MISSING,DETECTING,LOSING,TRACKING) */
    State_solve(match);

    /** LOSING 丢失目标卡尔曼继续预测 */
    if(tracker_state == LOSING)
    {
        enemy_armor.world_position = predicted.head(3);
        KF.setPosAndSpeed(enemy_armor.world_position, predicted.tail(3));
    }
    /** MISSING 没有目标状态 */
    if (tracker_state == MISSING)                   // 跟踪器处于没有目标状态
    {
        Reset();                                    // 重新初始化
        return false;
    }

上面就是跟踪的整体逻辑，接下来一个一个部分进行讲解