Initial

函数功能：通过传入最终装甲板,选择锁定装甲板,进入初始化状态

bool ArmorTrack::Initial(std::vector<Armor> &Armors) {
    if(Armors.empty())  return false;   //如果为空直接返回

    //====================预测初始化====================

    /** 根据装甲板的分数从大到小排序 */
    sort(Armors.begin(),Armors.end(),
         [](Armor &armor1,Armor &armor2){
             return armor1.grade > armor2.grade;});

    /** 锁定装甲板流程 */
    enemy_armor = Armors[0];                                        // 选择锁定装甲板
    enemy_armor.world_position = AS.pixel2imu(enemy_armor);      // 最终装甲板的世界坐标系为像素转换为imu坐标系
    tracking_id = enemy_armor.id;                                   // 设置跟踪id，根据最优装甲板选择
    tracker_state = DETECTING;                                      // 跟踪状态设置为初始化状态

    /** 初始化卡尔曼 */
    KF.Initial();    //卡尔曼初始化
    // 传入世界坐标系,设置初始速度为0
    KF.setPosAndSpeed(enemy_armor.world_position,Eigen::Vector3d(0,0,0));
    return true;
}

初始化函数，这里的逻辑便是根据装甲板的分数进行排序，然后选择分数最高的一个装甲板作为最终装甲板进行跟踪

同时记录下装甲板ID、坐标信息，进行卡尔曼的初始化，为预测做准备

tracker_state 在 Armor_Track.h 中，共有4个状态，在后续会有更加详细的介绍

/** 跟踪状态 */
enum TrackerState {
    MISSING,            // 没有目标，跳过该部分
    DETECTING,          // 还未开始跟踪，作为跟踪第一帧的切换，初始化好卡尔曼
    LOSING,             // 处于丢失状态，还会保留预测
    TRACKING,           // 处于跟踪状态
};

关于上述代码中 AS.pixel2imu(enemy_armor) 、 KF.Initial() 的部分不会在本篇进行详细的展开

像素转换为imu坐标系部分 将会在 "空间位姿解算篇" 进行详细展开

卡尔曼滤波部分 将会在 "算法篇" 进行详细展开

在本篇使用的时候将会进行简单的介绍