角速度计算
在进行整车观测过程中，我们有一个非常重要的值，就是角速度，只有计算出对方的角速度，我们才可以对接下来的预测进行操作
首先先看一下大致的一个思路
/** 识别到两个装甲板且处于跟踪状态 */
if(OB_Track[tracking_id].is_initialized && tracker_state == TRACKING){
// 判断跳变
double diff = abs(OB_Track[tracking_id].last_armor.camera_position[0] - enemy_armor.camera_position[0]);
if(diff > new_old_threshold && matched_armor.id == tracking_id ){
}
else{
// 计算角速度
double old_angle,new_angle,diff_angle;
double angle = 180.0f/CV_PI;
old_angle = OB_Track[tracking_id].last_armor.R[1]*angle;
new_angle = enemy_armor.R[1]*angle;
diff_angle = abs(new_angle-old_angle);
// 记录结果
Angle_Speed_recore.emplace_back(diff_angle); // 记录所有角度,同时判断帧数
test_dt += dt;
}
}
这个部分是什么意思呢，首先最上面的条件代表是，如果你当前处于跟踪状态，同时上一帧的装甲板是一个已经初始化过(也就是之前一直在跟踪的装甲板)，事实上这个条件其实相当于没有，算是历史遗留
注意，这里不要被混淆了，这里有两个概念，一个是跟踪装甲板（OB_Track[tracking_id]），一个是当前装甲板（enemy_armor），跟踪装甲板是永远落后当前装甲板一帧的，所以，如果在陀螺过程中，出现了装甲板的跳变，这个时候假设当前装甲板从左侧变到右侧，这个时候，跟踪装甲板还是在左侧的，通过两个装甲板的一个x轴距离变化，我们可以判断出跳变，这个时候我们在里面去计算角速度
这里如果判断装甲板并没有发生一个跳变的话，我们就会去记录上下两帧之间的一个角度差值，这样我们就可以很好的在跳变的时候利用这些记录来计算角速度
如上图所见，你可能会有疑问，为什么不用世界坐标系计算两点之间的距离呢？因为不稳定，首先测距就会有一定的偏差，三轴累加的一个误差是比较恐怖的，如果不使用滤波一类的东西，数据跳变回比较大，但是单使用x轴的数据，虽然比较简陋，但是不需要再引入复杂的计算，同时数据上又具有不错的可信度（x轴和z轴数据是比较可信的）
通过上面的逻辑，我们已经得到了一次陀螺所得到的所有角度记录，那都不需要我说明，接下来我们对数据进行累加计算就行
double All_angle = 0.0;
// 暂时不需要这个方法
for (int i = 0; i < Angle_Speed_recore.size(); ++i) {
All_angle += Angle_Speed_recore[i];
}
// 记录角度数据 可能不需要了(暂弃)
int max_size = 3;
if(Angle_Speed_smooth.size() < max_size){
Angle_Speed_smooth.push_back(All_angle/Angle_Speed_recore.size());
} else{
Angle_Speed_smooth.pop_front();
Angle_Speed_smooth.push_back(All_angle/Angle_Speed_recore.size());
}
if(Angle_Speed_smooth.size() == max_size) {
Angle_Speed_OK = true;
double temp = 0.0;
for (int i = 0; i < max_size; ++i) {
temp += Angle_Speed_smooth[i];
}
temp /= max_size;
Angle_Speed = temp;
}
// 平滑帧数试试 基本没作用
if(Angle_Speed_FPS.size() < max_size){
Angle_Speed_FPS.push_back(Angle_Speed_recore.size());
}else{
Angle_Speed_FPS.pop_front();
Angle_Speed_FPS.push_back(Angle_Speed_recore.size());
}
double FPS_size = 0;
for (int i = 0; i < Angle_Speed_FPS.size(); ++i) {
FPS_size += Angle_Speed_FPS[i];
}
FPS_size /= Angle_Speed_FPS.size();
Angle_Speed = All_angle/FPS_size;
// 清空记录
std::cout << "FPS_size:" << FPS_size << std::endl;
std::cout << All_angle << std::endl;
Angle_Speed_recore.clear();
// std::cout << "test_dt:" << test_dt << std::endl;
std::cout << "dt:" << dt << std::endl;
// 计算ms的角速度 TODO:钉死图像处理速度为15ms？可能需要调整
Angle_Speed /= 15;
Angle_Speed = 0.26; //固定转速(拍视频使用)
std::cout << "Angle_Speed:" << Angle_Speed << std::endl;
std::cout << "-----------------------" << std::endl;
test_dt = 0;
这里可以看到，我尝试过平缓帧数等等一系列的方法，很可惜，数据还是有一定的波动，这里或许用卡尔曼滤波会更好，可以尝试，这里开发比较赶没有机会做过多尝试，不过在曝光正确，相机参数正确的情况，这个计算出来的角速度还是比较可观的，偶尔会出现一两个过大的数据，用卡尔曼可以不错的滤掉这部分的错误数据，这里可以看到有个拍视频用的速度，可以说，用固定的转速拍出来的基本是近100%的命中率，所以一个稳定数据的是相当重要的，如何才能得到稳定可靠的数据，就是算法的核心了
现在我们得到了陀螺状态下的角速度，接下来就可以进行整车观测了