圆心拟合
终于到了这一步，这里是整车观测的一个核心，在这里你会知道我们如果去计算所有装甲板的位置，以及我们如何去选择击打装甲板，当然这里的功能还不够完善和稳定，同时消耗大部分的时间资源，期待你们的优化
ArmorObserve::Center_fitting 函数如下：
center_position = armor.camera_position; // 圆心坐标
center_position = {center_position[0],center_position[2],-center_position[1]}; //世界坐标系
Armor_distance = AS.World_projection(armor,0); // 获取装甲板4点距离差
#define A
#ifdef A
yaw = armor.R[1];
#else
yaw = AS.Armor_Angle(armor); // 获取装甲板yaw角度
yaw = yaw*CV_PI/180.0f; // 转换为弧度
#endif //A
#ifdef CENTER_FIT
cv::putText(_src,"yaw:"+ std::to_string(yaw*180.0f/CV_PI),cv::Point(0,40),cv::FONT_HERSHEY_SIMPLEX, 1,cv::Scalar(255, 255, 0),2,3);
cv::putText(_src,"Armor_Distance:"+ std::to_string(Armor_distance),cv::Point(0,80),cv::FONT_HERSHEY_SIMPLEX, 1,cv::Scalar(255, 255, 0),2,3);
#endif //CENTER_FIT
需要注意的是，这里我们做了一个坐标转换的操作，我们整车观测的所有操作都是基于相机坐标系的，相信在你学完坐标系转换的部分，应该已经可以深刻的理解坐标系转换的必要和魅力，如果我们在世界坐标系下，是固定的，我们的计算复杂度和难度都会非常高，你可以自己手动推导一下
这一段代码是在保存装甲板的Yaw朝向角，这里我们获取了装甲板的4点距离差，我们得到Yaw的方法有两种，一种是pnp自带的朝向角解算，另一种就世界反投影法，关于世界反投影法，就是将一个虚拟的装甲板的投影到当前实际装甲板的位置上，然后我们通过从-90°~90°的一个三分查找，找到匹配度最高的装甲板，那么当前投影角度就是实际装甲板的角度，关于世界反投影的方法，可以仔细看看 2023年上交的青工会分享，赞美方佬！
接下来是重头戏
/** 高低装甲板和长短轴参数 */
double OF_length; // 前后装甲板高度
double RL_length; // 左右装甲板高度
double OF_height; // 前后装甲板高度
double RL_height; // 左右装甲板高度
// 判断当前装甲板是什么轴
if(OB_Track.axesState == LONG) { // 长轴
OF_length = OB.Long_axes;
RL_length = OB.Short_axes;
}
else if(OB_Track.axesState == SHORT) { // 短轴
OF_length = OB.Short_axes;
RL_length = OB.Long_axes;
}
if(OB_Track.HeightState == HIGH){ // 高轴
RL_height = -OB.High_LOW_difference;
}
else if(OB_Track.HeightState == LOW) { // 矮轴
RL_height = OB.High_LOW_difference;
}
/** 计算装甲板中心点坐标参数 */
double OF_cos_r = cos(abs(spin_angle))*OF_length; // 半径在x轴方向投影
double RL_cos_r = cos(abs(spin_angle))*RL_length; // 半径在x轴方向投影
double OF_sin_r = sin(abs(spin_angle))*OF_length; // 半径在y轴方向投影
double RL_sin_r = sin(abs(spin_angle))*RL_length; // 半径在y轴方向投影
/** 计算圆心坐标 */ // TODO: 需要将在相机坐标系下操作再转换到世界坐标系
center_position[0] += sin(yaw)*axes_length; // 三角函数解圆心
center_position[1] += cos(abs(yaw))*axes_length; // 三角函数解圆心
center_position = {center_position[0],-center_position[2],center_position[1]}; //相机坐标系
center_position = AS.cam2imu(center_position);
Smooth_Filter.update(center_position,Smooth_position); // 更新数据,进行平滑
// 平滑
// Smooth_position = {Smooth_position[0],-Smooth_position[2],Smooth_position[1]}; //相机坐标系
// Smooth_position = AS.cam2imu(Smooth_position);
// Smooth_position[2] = z;
Smooth_position[2] = OB.center_Z;
cir = AS.imu2pixel(Smooth_position); // 转换像素坐标
计算装甲板中心点坐标参数这里有点小问题，这里的逻辑需要放到spin_angel计算的后面，不然会导致解算出来的其实是上一帧的位置，而不是当前的位置，我现在手里的代码是老旧的，实际代码已经修改了，这里的计算我不相信展开说明了，你可以自己拿上纸笔，画出4个装甲板的俯视图，然后手动推导一下，便可以知道如何进行计算，最后转换回像素坐标是为了可以调试可视化，可以删除
接下来是火控预测和数据平滑，数据平滑写的比较简陋，只是一个所有数据的平均滤波，可以考虑用其他更好的方法
if(Smooth_Filter.fit){
// TODO：注意顺逆时针 这里都是逆时针
Eigen::Vector3d center_temp; // 圆心世界坐标(temp)
cv::Point2f spin_Aromor_cir; // 陀螺装甲板
double FPS_size = 1; // 帧率倍率(待调整) TODO:不能调过大,会导致跳过角度检测,试试计算(距离/运动时间*角速度)进行补偿 慢速:10(展示)(已弃)
// 已进入调参阶段
double compensate; // 弹道时间角度补偿
double Dial_time; // 拨盘转动时间 (新增)（待调整）
double time; // 距离使用车中心试试？还是使用装甲板的距离？ 现使用车中心看看
double Diff = sqrt(pow(Smooth_position[0],2)+pow(Smooth_position[1],2));
double bullet_speed = 25;
double compensate_size = 1; // 倍率 TODO:测试可行,有上升空间(待测试)
time = Diff/bullet_speed; // 单位: s
Dial_time = 80; // 单位: ms
compensate = compensate_size*(time*1000)*(Angle_Speed) + Dial_time*(Angle_Speed); // 计算补偿角度 (等待测试看效果)
std::cout << "compensate:" << compensate << std::endl;
// std::cout << "anglespend:" << Angle_Speed << std::endl;
// std::cout << "time:" << time << std::endl;
cv::putText(_src,"compensate:"+ std::to_string(compensate),cv::Point(0,600),cv::FONT_HERSHEY_SIMPLEX, 1,cv::Scalar(255, 255, 0),2,3);
// 计算预测角度 (帧数补偿设置为15ms)
spin_angle = yaw*(180.0f/CV_PI) - FPS_size*(Angle_Speed*15) - compensate; // 陀螺装甲板所在角度(逆时针)
spin_angle *= (CV_PI/180.0f);
center_temp = AS.imu2cam(Smooth_position);
center_temp = {center_temp[0],center_temp[2],-center_temp[1]}; // 世界坐标系
spin_Aromor[0] = center_temp[0] - sin(spin_angle)*OF_length;
spin_Aromor[1] = center_temp[1] - cos(spin_angle)*OF_length;
spin_Aromor[2] = center_temp[2]-(RL_height/2);
spin_Aromor = {spin_Aromor[0],-spin_Aromor[2],spin_Aromor[1]}; //相机坐标系
spin_Aromor = AS.cam2imu(spin_Aromor);
spin_Aromor_cir = AS.imu2pixel(spin_Aromor);
// 计算其他点位
Left_Armor_angle = spin_angle*(180.0f/CV_PI) + 90.0;
Right_Armor_angle = spin_angle*(180.0f/CV_PI) - 90.0;
std::cout << spin_angle*(180.0f/CV_PI) << std::endl;
std::cout << Left_Armor_angle << std::endl;
if(yaw > 0){
// Left_Armor = {center_temp[0] - RL_cos_r,center_temp[1] + RL_sin_r,center_temp[2]+(RL_height/2)};
Left_Armor = {center_temp[0] - RL_cos_r,center_temp[1] - RL_sin_r,center_temp[2]};//test
Right_Armor = {center_temp[0] + RL_cos_r,center_temp[1] - RL_sin_r,center_temp[2]+(RL_height/2)};
O_Armor = {center_temp[0] + OF_sin_r,center_temp[1] + OF_cos_r,center_temp[2]-(RL_height/2)};
}else{
// Left_Armor = {center_temp[0] - RL_cos_r,center_temp[1] - RL_sin_r,center_temp[2]+(RL_height/2)};
Left_Armor = {center_temp[0] - RL_cos_r,center_temp[1] - RL_sin_r,center_temp[2]};//test
Right_Armor = {center_temp[0] + RL_cos_r,center_temp[1] + RL_sin_r,center_temp[2]+(RL_height/2)};
O_Armor = {center_temp[0] - OF_sin_r,center_temp[1] + OF_cos_r,center_temp[2]-(RL_height/2)};
}
// 转换部分,待优化
Left_Armor = {Left_Armor[0],-Left_Armor[2],Left_Armor[1]}; //相机坐标系
Left_Armor = AS.cam2imu(Left_Armor);
Left_Armor_cir = AS.imu2pixel(Left_Armor);
// 转换部分,待优化
Right_Armor = {Right_Armor[0],-Right_Armor[2],Right_Armor[1]}; //相机坐标系
Right_Armor = AS.cam2imu(Right_Armor);
Right_Armor_cir = AS.imu2pixel(Right_Armor);
// 转换部分,待优化
O_Armor = {O_Armor[0],-O_Armor[2],O_Armor[1]}; //相机坐标系
O_Armor = AS.cam2imu(O_Armor);
O_Armor_cir = AS.imu2pixel(O_Armor);
// 计算左右30度(暂定,可调) TODO:长短轴高低装甲板等待计算处理
cv::Point2f left_target_Armor; // 左侧装甲板点位
cv::Point2f right_target_Armor; // 右侧装甲板点位
left_target[0] = center_temp[0] - sin(30*(CV_PI/180.0f))*RL_length;
left_target[1] = center_temp[1] - cos(30*(CV_PI/180.0f))*RL_length;
left_target[2] = center_temp[2]+(RL_height/2);
right_target[0] = center_temp[0] + sin(30*(CV_PI/180.0f))*RL_length;
right_target[1] = center_temp[1] - cos(30*(CV_PI/180.0f))*RL_length;
right_target[2] = center_temp[2]+(RL_height/2);
// 转换部分,待优化
left_target = {left_target[0],-left_target[2],left_target[1]}; //相机坐标系
left_target = AS.cam2imu(left_target);
left_target_Armor = AS.imu2pixel(left_target);
// 转换部分,待优化
right_target = {right_target[0],-right_target[2],right_target[1]}; //相机坐标系
right_target = AS.cam2imu(right_target);
right_target_Armor = AS.imu2pixel(right_target);
// 更新拟合状态
// Fit_OK = Smooth_Filter.fit && CKF.Track_OK;
Fit_OK = Smooth_Filter.fit;
// Fit_OK = true;
cv::Point2f Armor_cir;
cv::Point2f pre_Armor_cir;
Armor_cir = AS.imu2pixel(armor.world_position);
pre_Armor = {pre_Armor[0],-pre_Armor[2],pre_Armor[1]}; //相机坐标系
pre_Armor = AS.cam2imu(pre_Armor);
pre_Armor_cir = AS.imu2pixel(pre_Armor);
circle(_src,cir,5,cv::Scalar(0,0,255),-1);
// circle(_src,Armor_cir,5,cv::Scalar(0,255,255),-1);
circle(_src,spin_Aromor_cir,5,cv::Scalar(255,0,255),-1);
// 左右预测点位
// circle(_src,Left_Armor_cir,5,cv::Scalar(255,0,255),-1);
// circle(_src,Right_Armor_cir,5,cv::Scalar(255,0,255),-1);
// circle(_src,O_Armor_cir,5,cv::Scalar(255,0,255),-1);
// 移动点位
// circle(_src,left_target_Armor,5,cv::Scalar(0,255,0),-1);
circle(_src,right_target_Armor,5,cv::Scalar(0,255,0),-1);
// circle(_src,pre_Armor_cir,5,cv::Scalar(255,0,255),-1);
// circle(_src,pre_cir,5,cv::Scalar(0,255,0),-1);
// std::cout << pre_cir << std::endl;
// cv::imshow("Observe_src",_src);
}
这一段就有很明显的调试和开发痕迹了，比较凌乱，注意不要以这个代码为主，这个代码版本已经落后，只是为了辅助学习原初的逻辑和想法，很多地方后续都是优化过的
首先就是一个角度预测，这里固定15ms不是个好的选择，要根据实际处理时间调整，然后根据当前一个圆心，计算出所有的装甲板位置，计算方法比较简单，就是一些几何和三角函数，存在一定的优化空间，比如可以考虑用矩阵计算，这样同时可以同时计算多个数据，速度上矩阵也比自己写的计算要快
得到所有的装甲板位置和当前的一个预测角度，我们很自然的可以选择出我们需要击打的装甲板，发送开火命令，完成火控流程