跟踪器选择模块
int armor_id = tracking_id; // 获取跟踪ID
Armor detect_armor = enemy_armor; // 检测装甲板
bool new_armors = false; // 判断是否有新增装甲板
auto trackers_map_size = trackers_map.count(armor_id); // 已存在类型的跟踪器数量
auto是一个自动推导类型，不必多说
std::multimap<int, SpinTracker> trackers_map; // 陀螺跟踪器Map
multimap是一个关联式容器，int自然是存放装甲板的ID
multimap和map的唯一区别是multimap中的key可以重复，而map的key是唯一的
// 陀螺状态的跟踪器
class SpinTracker {
public:
Armor last_armor; // 本次装甲板
bool is_initialized; // 是否完成初始化
chrono_time last_timestamp; // 本次装甲板时间戳
Axes_State axesState; // 当前跟踪装甲板长短轴状态
Axes_State HeightState; // 当前跟踪装甲板高低轴状态
SpinTracker(){is_initialized = false;axesState = UNAWARE;HeightState = UNAWARE;}
explicit SpinTracker(const Armor& armor, chrono_time armor_timestamp);
bool update_tracker(const Armor& new_armor, chrono_time timestamp);
};
这个是陀螺的核心内部类，陀螺的判断条件主要是时间和位置两个变量
接下来先看简单的情况
int armor_id = tracking_id; // 获取跟踪ID
Armor detect_armor = enemy_armor; // 检测装甲板
bool new_armors = false; // 判断是否有新增装甲板
auto trackers_map_size = trackers_map.count(armor_id); // 已存在类型的跟踪器数量
/** 跟踪器只有1个 */
if(trackers_map_size == 1){
auto candidate = trackers_map.find(armor_id); // 原有的同ID装甲板
auto delta_dist = (detect_armor.world_position - (*candidate).second.last_armor.world_position).norm(); // 计算装甲板距离
/** 如果距离小于阈值 */
if (delta_dist < new_old_threshold) {
(*candidate).second.update_tracker(detect_armor, t); // 更新陀螺跟踪器
}
/** 跳变情况(为新增装甲板设置跟踪器 1->2 )或速度超出阈值 */
else{
SpinTracker spinTracker(detect_armor, t); // 重新创建跟踪器
trackers_map.insert(std::make_pair(armor_id, spinTracker)); // 预测器Map添加装甲板
new_armors = true; // 判断为有新增装甲板
}
}
multimap_name插入函数如下：
iterator multimap_name.insert({key, element})
make_pair函数的定义如下：
template pair make_pair(T1 a, T2 b) { return pair(a, b); }
std::pair主要的作用是将两个数据组合成一个数据，两个数据可以是同一类型或者不同类型。
这里与multimap容器结合使用
在只有单个跟踪器的时候，我们先在容器中查询当前锁定ID的跟踪器，并获取"迭代器"iterator，将其理解为指针即可
随后计算当前跟踪装甲板和跟踪器内部装甲板的世界距离（理想情况下就是上下两帧装甲板的位移）
根据阈值判断是否是陀螺，
如果小于阈值：更新跟踪器的内部信息（装甲板和时间）
如果超过阈值：因为陀螺状态下会有一个装甲板的跳变，在这么短的时间（15ms，一帧时间）左右，装甲板不可能有这么大的位移，所以我们判断发生了一次旋转，同时创建一个新的跟踪器
注意：multimap键值是允许重复的，所以这个时候发生跳变就会让跟踪器内部存在两个相同ID的跟踪器，在后续中进行判断
接下来就是复杂一点的情况
/** 没有跟踪器或多个跟踪器(多个跟踪器中选择跟踪目标) */
else{
// 1e9无实际意义，仅用于非零初始化
double min_delta_dist = 1e9; // 最小距离
double min_delta_t = 1e9; // 最小时间
bool is_best_candidate_exist = false; // 判断最优装甲板
std::multimap<int, SpinTracker>::iterator best_candidate; // 记录最优的装甲板
auto candidates = trackers_map.equal_range(armor_id); // 获取所有当前跟踪ID跟踪器
/** 跟踪ID相同的跟踪器(在所有跟踪目标中选择与当前锁定目标最接近的) */
for (auto iter = candidates.first; iter != candidates.second; ++iter) {
auto delta_dist = (detect_armor.world_position - (*iter).second.last_armor.world_position).norm(); // 计算距离
auto delta_t = milliseconds_duration (t - (*iter).second.last_timestamp).count(); // 计算时间间隔
/** 在同一位置存在过装甲板且时间最接近设为最高优先级(寻找到与当前锁定目标最接近的装甲板) */
if (delta_dist <= new_old_threshold && delta_dist <= min_delta_dist && /*时间*/delta_t < min_delta_t) // 距离需要调试
{
min_delta_dist = delta_dist;
min_delta_t = delta_t;
best_candidate = iter;
is_best_candidate_exist = true;
}
}
/** 存在最优装甲板 */
if (is_best_candidate_exist) (*best_candidate).second.update_tracker(detect_armor, t); // 更新跟踪装甲板
/** 初始化过程或跟踪丢失(跳变2->1) */
else{
/** 为当前锁定装甲板添加跟踪器 */
SpinTracker spinTracker(detect_armor, t);
trackers_map.insert(std::make_pair(static_cast<int&&>(armor_id), static_cast<SpinTracker&&>(spinTracker)));
}
}
此时就具备多个跟踪器的情况，通过equal_range获取当前锁定ID的所有跟踪器，通过位置和时间进行判断，哪个是正在跟踪的装甲板，然后更新跟踪器的信息
static_cast
equal_range()函数主要是求在multimap中有多少个重复的数，即取出相同ID的跟踪器
要想深入了解，自行查询用法
看到这里你可能会有一点疑惑，为什么这个部分还需要再添加一次跟踪器，下面给出详细的解释
一、初始化
第一次为锁定的装甲板的添加跟踪器的时候，这个时候 if 判断是进不去的，所以在这里会进入并添加跟踪器
起到一个初始化的作用
二、跳变(2->1)
在陀螺发生跳变的时候，会超出我们设置的 new_old_threshold 阈值，这个时候就会判断没有最优装甲板，就会为当前的装甲板添加跟踪器，所以此时发生了
装甲板的跳变，这个时候事实上应当算作一次新增装甲板（new_armors = true），但是同时这里也包含了初始化的功能，需要再优化和测试一下，旧的跟踪器会
在跟踪器维护的部分删除
当然，这里的判断也并不是很好，还有很大的优化空间
主要问题在于我们想要让现实情况接近理想情况，这里 if(trackers_map_size == 1) 的进入条件实际上比较苛刻
难于预料会出现什么特殊情况，这个需要足够的测试
关于时间：
在robot_status.h里面有着chrono_time定义的详细介绍
/**
* 重命名模块名
* std::chrono::duration<intmax_t N, intmax_t D = 1> N表示分子，D表示分母
*
* 使用示例:
* auto start = std::chrono::high_resolution_clock::now();
* for (int i = 0; i < 1000000000; ++i) {}
* auto end = std::chrono::high_resolution_clock::now();
* auto duration = milliseconds_duration(end-start).count();
* std::cout << duration;
*/
using seconds_duration = std::chrono::duration<double>; //秒
using milliseconds_duration = std::chrono::duration<double,std::milli>; //毫秒
using microseconds_duration = std::chrono::duration<double,std::micro>; //微秒
using chrono_time = decltype(std::chrono::high_resolution_clock::now()); //当前时间 decltype()获取返回变量类型
里面的容器使用方法就请自行了解
multimap简单使用 可以看下面的示例代码：
#include <iostream>
#include <map>
using namespace std;
int main()
{
std::multimap<char, int> mymm;
mymm.insert(std::pair<char, int>('a', 10));
mymm.insert(std::pair<char, int>('b', 20));
mymm.insert(std::pair<char, int>('b', 30));
mymm.insert(std::pair<char, int>('b', 40));
mymm.insert(std::pair<char, int>('c', 50));
mymm.insert(std::pair<char, int>('c', 60));
mymm.insert(std::pair<char, int>('d', 60));
std::cout << "mymm contains:\n";
for (char ch = 'a'; ch <= 'd'; ch++)
{
std::pair <std::multimap<char, int>::iterator, std::multimap<char, int>::iterator> ret;
ret = mymm.equal_range(ch);
std::cout << ch << " =>";
for (std::multimap<char, int>::iterator it = ret.first; it != ret.second; ++it)
{
std::cout << ' ' << it->second;
}
std::cout << '\n';
}
return 0;
}