diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 27bf522..8509d32 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -72,6 +72,7 @@ private: int clockwise_rotation_init_cnt;//装甲板顺时针旋转次数 int anticlockwise_rotation_init_cnt;//装甲板逆时针旋转次数 int last_mode;//上一帧的能量机关状态 + int manual_delta_x, manual_delta_y;//手动微调量 float target_polar_angle;//待击打装甲板的极坐标角度 float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度 @@ -103,6 +104,9 @@ private: std::vector fans;//图像中所有扇叶 std::vector armors;//图像中所有可能装甲板(可能存在误识别) + std::vector flow_strip_fans;//可能的流动扇叶 + std::vector target_armors;//可能的目标装甲板 + std::vector flow_strips;//可能的流动条 std::vector all_target_armor_centers;//记录全部的装甲板中心,用于风车圆心和半径的计算 std::queue recent_target_armor_centers;//记录最近一段时间的装甲板中心,用于判断大符还是小符 @@ -152,12 +156,13 @@ private: double pointDistance(cv::Point point_1, cv::Point point_2);//计算两点距离 double nonZeroRateOfRotateRect(cv::Mat &bin, const cv::RotatedRect &rotatedRect);//计算旋转矩形内亮点占比 - void writeDownMark();//记录操作手标定的云台初始角度 + void writeDownSlightChange(cv::Mat &src);//记录操作手的手动微调 + void writeDownMark(cv::Mat &src);//记录操作手标定的云台对心初始角度 bool guessTarget();//获得猜测击打点位 bool getOrigin();//获得云台对心所需角度 void changeTarget();//判断目标是否改变 - void multipleMode(cv::Mat gimbal_src);//多模式切换 + void multipleMode(cv::Mat &src);//多模式切换 void getTargetPolarAngle();//获得目标装甲板极坐标角度 void getPredictPoint(cv::Point target_point);//获取预测点位 void getAimPoint(cv::Point target_point);//通过自瞄逻辑计算点位 diff --git a/energy/src/energy/change/multiple_mode.cpp b/energy/src/energy/change/multiple_mode.cpp index 3fdfe2c..bf662ef 100644 --- a/energy/src/energy/change/multiple_mode.cpp +++ b/energy/src/energy/change/multiple_mode.cpp @@ -7,18 +7,18 @@ using namespace std; using namespace cv; -void Energy::multipleMode(cv::Mat gimbal_src) { +void Energy::multipleMode(cv::Mat &src) { if (is_predicting) { getPredictPoint(target_point); getAimPoint(predict_point); judgeShootInGimbal(); sendEnergy(); } else if (is_guessing && stayGuessing()) { - findFans(gimbal_src); - if (show_energy)showFans("fans", gimbal_src); - if (save_mark)writeDownMark(); + findFans(src); + if (show_energy)showFans("fans", src); + if (save_mark)writeDownMark(src); guessTarget(); - if (show_energy)showGuessTarget("guess", gimbal_src); + if (show_energy)showGuessTarget("guess", src); getPredictPoint(guess_point); getAimPoint(predict_point); sendEnergy(); diff --git a/energy/src/energy/clear/clear.cpp b/energy/src/energy/clear/clear.cpp index a40bd6a..8efbdbc 100644 --- a/energy/src/energy/clear/clear.cpp +++ b/energy/src/energy/clear/clear.cpp @@ -15,6 +15,9 @@ using namespace cv; void Energy::clearAll(){ fans.clear(); armors.clear(); + flow_strip_fans.clear(); + target_armors.clear(); + flow_strips.clear(); } diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index 753aeff..832c2ce 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -36,6 +36,8 @@ void Energy::initEnergy() { clockwise_rotation_init_cnt = 0; anticlockwise_rotation_init_cnt = 0; last_mode = -1;//既不是大符也不是小符 + manual_delta_x = 0; + manual_delta_y = 0; target_polar_angle = -1000; last_target_polar_angle = -1000; @@ -59,6 +61,9 @@ void Energy::initEnergy() { fans.clear(); armors.clear(); + flow_strip_fans.clear(); + target_armors.clear(); + flow_strips.clear(); all_target_armor_centers.clear(); while(!recent_target_armor_centers.empty())recent_target_armor_centers.pop(); diff --git a/energy/src/energy/energy.cpp b/energy/src/energy/energy.cpp index 6537d04..60e7b2d 100644 --- a/energy/src/energy/energy.cpp +++ b/energy/src/energy/energy.cpp @@ -32,7 +32,6 @@ Energy::Energy(Serial &u, uint8_t &color) : serial(u), ally_color(color), Energy::~Energy() = default; - //---------------------------------------------------------------------------------------------------------------------- // 此函数为大能量机关再初始化函数 // --------------------------------------------------------------------------------------------------------------------- @@ -58,8 +57,13 @@ void Energy::setBigEnergyInit() { is_small = false; is_gimbal = true; is_chassis = false; -} + FILE *fp = fopen(PROJECT_DIR"/Mark/delta.txt", "r"); + if (fp) { + fscanf(fp, "delta_x: %d, delta_y: %d", &manual_delta_x, &manual_delta_y); + fclose(fp); + } +} //---------------------------------------------------------------------------------------------------------------------- @@ -73,4 +77,10 @@ void Energy::setSmallEnergyInit() { is_small = true; is_gimbal = true; is_chassis = false; + + FILE *fp = fopen(PROJECT_DIR"/Mark/delta.txt", "r"); + if (fp) { + fscanf(fp, "%d %d", &manual_delta_x, &manual_delta_y); + fclose(fp); + } } \ No newline at end of file diff --git a/energy/src/energy/find/energy_finder.cpp b/energy/src/energy/find/energy_finder.cpp index 8fb3400..7f1da0d 100644 --- a/energy/src/energy/find/energy_finder.cpp +++ b/energy/src/energy/find/energy_finder.cpp @@ -181,14 +181,13 @@ bool Energy::findFlowStripFan(const cv::Mat src) { if (!isValidFlowStripFanContour(src_bin, flow_strip_fan_contour)) { continue; } - candidate_flow_strip_fans.emplace_back(cv::minAreaRect(flow_strip_fan_contour)); + flow_strip_fans.emplace_back(cv::minAreaRect(flow_strip_fan_contour)); // RotatedRect cur_rect = minAreaRect(flow_strip_fan_contour); // Size2f cur_size = cur_rect.size; // float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; // float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; // double cur_contour_area = contourArea(flow_strip_fan_contour); -// double non_zero_rate = nonZeroRateOfRotateRect(src_bin, cur_rect); // if (length > 40 && width > 30 && length < 110 && width < 100) { // cout << cur_rect.center<= 2) { - //用锤子筛选仍然有多个候选区,进一步用锤头做筛选 - std::vector candidate_target_fans; - for (auto candidate_flow_strip_fan: candidate_flow_strip_fans) { - flow_strip_fan = candidate_flow_strip_fan; - if (!findTargetInFlowStripFan()) { - continue; - } - candidate_target_fans.emplace_back(candidate_flow_strip_fan); - } - if (candidate_target_fans.size() == 1) { - flow_strip_fan = candidate_target_fans.at(0); - return true; - } else if(candidate_target_fans.empty()){ - cout<<"No candidate target fan contains a target armor!"< mask_rect; - target_armor.points(vertices); - for (int i = 0; i < 4; i++) - line(src_mask, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 0), 20); - if (findFlowStrip(src_mask)) { - flow_strip_fan = candidate_target_fan; - return true; - } - } - cout<<"No candidate target fan contains a flow strip!"< candidate_target_armors = target_armors; + for (auto &candidate_target_armor: candidate_target_armors) { + Point2f vertices[4]; + candidate_target_armor.size.height *= 1.3; + candidate_target_armor.size.width *= 1.3; + candidate_target_armor.points(vertices); //计算矩形的4个顶点 + for (int i = 0; i < 4; i++) { + line(src_bin, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 0), 20); + } + } + + cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件,需转换为单通道 + FlowStripStruct(src_bin);//图像膨胀,防止图像断开并更方便寻找 if (show_process)imshow("flow strip struct", src_bin); std::vector > flow_strip_contours; findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); - for (auto &flow_strip_contour : flow_strip_contours) { - if (!isValidFlowStripContour(flow_strip_contour)) { - continue; - } - flow_strip = cv::minAreaRect(flow_strip_contour); + for (auto candidate_flow_strip_fan: flow_strip_fans) { + for (auto &flow_strip_contour : flow_strip_contours) { + if (!isValidFlowStripContour(flow_strip_contour)) { + continue; + } + std::vector intersection; + RotatedRect cur_rect = minAreaRect(flow_strip_contour); + if (rotatedRectangleIntersection(cur_rect, candidate_flow_strip_fan, intersection) == 0) { + continue; + } else if (contourArea(intersection) > energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) { + flow_strips.emplace_back(cv::minAreaRect(flow_strip_contour)); +// cout << "intersection: " << contourArea(intersection) << '\t' << cur_rect.center << endl; + } + // RotatedRect cur_rect = minAreaRect(flow_strip_contour); // Size2f cur_size = cur_rect.size; // float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; @@ -276,12 +262,39 @@ bool Energy::findFlowStrip(const cv::Mat src) { // cout << "flow strip area: " << length << '\t' << width << endl; // } // cout << cur_rect.center << endl; - return true; + } + } +// cout << "flow strip cnt: " << flow_strips.size() << endl; + if (flow_strips.empty()) { + cout << "flow strip false!" << endl; +// waitKey(0); + return false; + } else if (flow_strips.size() > 1) { + cout << "Too many flow strips!" << endl; +// waitKey(0); + return false; + } else { + flow_strip = flow_strips.at(0); + for (auto &candidate_flow_strip_fan: flow_strip_fans) { + std::vector intersection; + if (rotatedRectangleIntersection(flow_strip, candidate_flow_strip_fan, intersection) == 0) { + continue; + } else if (contourArea(intersection) > energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) { + flow_strip_fan = candidate_flow_strip_fan; + } + } + int i = 0; + for (i = 0; i < target_armors.size(); ++i) { + std::vector intersection; + if (rotatedRectangleIntersection(target_armors.at(i), flow_strip_fan, intersection) == 0) + continue;//返回0表示没有重合面积 + double cur_contour_area = contourArea(intersection); + if (cur_contour_area > energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN) { + target_armor = target_armors.at(i); + target_point = target_armor.center; + } + } } - cout << "flow strip false!" << endl; -// waitKey(0); - return false; - } @@ -289,23 +302,13 @@ bool Energy::findFlowStrip(const cv::Mat src) { // 此函数用于框取中心R的寻找范围 // --------------------------------------------------------------------------------------------------------------------- bool Energy::findCenterROI(const cv::Mat src) { - cv::Mat src_mask = src.clone(); - target_armor.size.height *= 1.3; - target_armor.size.width *= 1.3; - Point2f vertices[4]; - vector mask_rect; - target_armor.points(vertices); //计算矩形的4个顶点 - for (int i = 0; i < 4; i++) - line(src_mask, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 0), 20); -// imshow("fill", src_mask); - if (!findFlowStrip(src_mask))return false; float length = target_armor.size.height > target_armor.size.width ? target_armor.size.height : target_armor.size.width; Point2f p2p(flow_strip.center.x - target_point.x, flow_strip.center.y - target_point.y); p2p = p2p / pointDistance(flow_strip.center, target_point);//单位化 - center_ROI = cv::RotatedRect(cv::Point2f(flow_strip.center + p2p * length * 1.25), + center_ROI = cv::RotatedRect(cv::Point2f(flow_strip.center + p2p * length * 1.7), Size2f(length * 1.4, length * 1.4), -90); return true; diff --git a/energy/src/energy/find/target_finder.cpp b/energy/src/energy/find/target_finder.cpp index 4e5a82c..0703bd2 100644 --- a/energy/src/energy/find/target_finder.cpp +++ b/energy/src/energy/find/target_finder.cpp @@ -9,7 +9,6 @@ using std::endl; using std::vector; - //---------------------------------------------------------------------------------------------------------------------- // 此函数根据矩形重合面积匹配扇叶与装甲板 // --------------------------------------------------------------------------------------------------------------------- @@ -20,32 +19,32 @@ void Energy::findTargetByIntersection() { } } - /* if (fans.empty()) { - target_point = armors.at(0).rect.center; - return; - } - int i = 0, j = 0; - while (i < armors.size()) { - for (j = 0; j < fans.size(); ++j) { - std::vector intersection; - if (rotatedRectangleIntersection(armors.at(i).rect, fans.at(j).rect, intersection) == 0)//返回0表示没有重合面积 - continue; - else - rotatedRectangleIntersection(armors.at(i).rect, fans.at(j).rect, intersection); - double cur_contour_area = contourArea(intersection); - if (cur_contour_area > energy_part_param_.INTERSETION_CONTOUR_AREA_MIN) { -// cout << endl; -// cout << "NO. " << i << " armor and No. " << j << "fans are matched, the intersection area is" -// << cur_contour_area << endl; - break; - } - } - if (j == fans.size()) { - target_point = armors.at(i).rect.center; - break; - } - i++; - }*/ + /* if (fans.empty()) { + target_point = armors.at(0).rect.center; + return; + } + int i = 0, j = 0; + while (i < armors.size()) { + for (j = 0; j < fans.size(); ++j) { + std::vector intersection; + if (rotatedRectangleIntersection(armors.at(i).rect, fans.at(j).rect, intersection) == 0)//返回0表示没有重合面积 + continue; + else + rotatedRectangleIntersection(armors.at(i).rect, fans.at(j).rect, intersection); + double cur_contour_area = contourArea(intersection); + if (cur_contour_area > energy_part_param_.INTERSETION_CONTOUR_AREA_MIN) { + // cout << endl; + // cout << "NO. " << i << " armor and No. " << j << "fans are matched, the intersection area is" + // << cur_contour_area << endl; + break; + } + } + if (j == fans.size()) { + target_point = armors.at(i).rect.center; + break; + } + i++; + }*/ } @@ -53,22 +52,26 @@ void Energy::findTargetByIntersection() { // 此函数在流动条区域内寻找装甲板 // --------------------------------------------------------------------------------------------------------------------- bool Energy::findTargetInFlowStripFan() { - int i = 0; - for (i = 0; i < armors.size(); ++i) { - std::vector intersection; - if (rotatedRectangleIntersection(armors.at(i), flow_strip_fan, intersection) == 0) - continue;//返回0表示没有重合面积 - double cur_contour_area = contourArea(intersection); + for (auto &candidate_flow_strip_fan: flow_strip_fans) { + int i = 0; + for (i = 0; i < armors.size(); ++i) { + std::vector intersection; + if (rotatedRectangleIntersection(armors.at(i), candidate_flow_strip_fan, intersection) == 0) + continue;//返回0表示没有重合面积 + double cur_contour_area = contourArea(intersection); // cout< energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN) { + target_armors.emplace_back(armors.at(i)); + } } } - cout<<"find target false"< &flow_strip_contour // cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl; return false;//轮廓对矩形的面积占有率不合适 } - std::vector intersection; - if (rotatedRectangleIntersection(cur_rect, flow_strip_fan, intersection) == 0) { - return false; - } else if (contourArea(intersection) < energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) { -// cout << "intersection: " << contourArea(intersection) << '\t' << cur_rect.center << endl; - return false; - } return true; } diff --git a/energy/src/energy/mark/mark.cpp b/energy/src/energy/mark/mark.cpp index 889310a..be09f0d 100644 --- a/energy/src/energy/mark/mark.cpp +++ b/energy/src/energy/mark/mark.cpp @@ -7,8 +7,26 @@ using namespace std; using namespace cv; -void Energy::writeDownMark() { - if(fans.size()>=3) { + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于记录操作手的微调dx和dy +// --------------------------------------------------------------------------------------------------------------------- +void Energy::writeDownSlightChange(cv::Mat &src) { + if (findFans(src) == 4){ + FILE *fp_delta = fopen(PROJECT_DIR"/Mark/delta.txt", "w"); + if (fp_delta) { + fprintf(fp_delta, "delta_x: %d, delta_y: %d\n", mcuData.delta_x, mcuData.delta_y); + fclose(fp_delta); + } + } +} + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于记录操作手手动标定的初始对心角度 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::writeDownMark(cv::Mat &src) { + if (findFans(src) >= 3) { FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "w"); if (fp) { fprintf(fp, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch); @@ -20,10 +38,10 @@ void Energy::writeDownMark() { fclose(fp_all); } } - if(fans.size()==4){ + if (findFans(src) == 4) { FILE *fp_best = fopen(PROJECT_DIR"/Mark/mark_best.txt", "a"); - if(fp_best){ - fprintf(fp_best, "yaw: %f, pitch: %f\n",origin_yaw, origin_pitch); + if (fp_best) { + fprintf(fp_best, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch); fclose(fp_best); } } diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 7f6e8c4..baeed89 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -25,6 +25,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) { if (show_energy)showArmors("armor", gimbal_src); if (!findFlowStripFan(gimbal_src))return; if (!findTargetInFlowStripFan()) return; + if (!findFlowStrip(gimbal_src))return; if (!findCenterROI(gimbal_src))return; if (show_energy)showFlowStripFan("strip", gimbal_src); if (!findCenterR(gimbal_src))return; @@ -46,6 +47,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) { if (show_energy)showArmors("armor", chassis_src); if (!findFlowStripFan(chassis_src))return; if (!findTargetInFlowStripFan()) return; + if (!findFlowStrip(gimbal_src))return; if (!findCenterROI(chassis_src))return; if (show_energy)showFlowStripFan("strip", chassis_src); if (!findCenterR(chassis_src))return; @@ -56,6 +58,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) { initRotation(); return; } + if (save_mark)writeDownMark(chassis_src); getPredictPoint(target_point); gimbalRotation(); judgeShootInWorld(); @@ -77,6 +80,7 @@ void Energy::runBig(cv::Mat &gimbal_src) { if (show_energy)showArmors("armor", gimbal_src); if (!findFlowStripFan(gimbal_src))return; if (!findTargetInFlowStripFan()) return; + if (!findFlowStrip(gimbal_src))return; if (!findCenterROI(gimbal_src))return; if (show_energy)showFlowStripFan("strip", gimbal_src); if (!findCenterR(gimbal_src))return; @@ -87,6 +91,7 @@ void Energy::runBig(cv::Mat &gimbal_src) { initRotation(); return; } + if (save_mark)writeDownSlightChange(gimbal_src); getPredictPoint(target_point); getAimPoint(predict_point); judgeShootInGimbal(); @@ -107,8 +112,10 @@ void Energy::runSmall(cv::Mat &gimbal_src) { if (show_energy)showArmors("armor", gimbal_src); if (!findFlowStripFan(gimbal_src))return; if (!findTargetInFlowStripFan()) return; + if (!findFlowStrip(gimbal_src))return; changeTarget(); + if (save_mark)writeDownSlightChange(gimbal_src); getPredictPoint(target_point); getAimPoint(predict_point); judgeShootInGimbal(); diff --git a/main.cpp b/main.cpp index 709bc8c..815376e 100644 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,7 @@ using namespace std; mcu_data mcuData = { // 单片机端回传结构体 0, // 当前云台yaw角 0, // 当前云台pitch角 - ARMOR_STATE, // 当前状态,自瞄-大符-小符 + BIG_ENERGY_STATE, // 当前状态,自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 ENEMY_RED, // 敌方颜色 @@ -120,8 +120,8 @@ int main(int argc, char *argv[]) { if (!from_camera) extract(gimbal_src, chassis_src); if (save_video) saveVideos(gimbal_src, chassis_src);//保存视频 if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像 - energy.runBig(gimbal_src, chassis_src); -// energy.runBig(gimbal_src); +// energy.runBig(gimbal_src, chassis_src); + energy.runBig(gimbal_src); last_state = mcuData.state;//更新上一帧状态 } else if (mcuData.state == SMALL_ENERGY_STATE) { if (mcuData.state != SMALL_ENERGY_STATE) {