diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index cbd5ae9..b821645 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -65,6 +65,7 @@ private: double radius;//大风车半径 + int curr_fps;//帧率 int send_cnt;//向主控板发送的数据总次数 int camera_cnt;//摄像头数量 int fans_cnt;//扇叶个数 @@ -131,6 +132,7 @@ private: int findArmors(const cv::Mat src);//寻找图中所有装甲板 bool findCenterR(const cv::Mat src);//寻找图中可能的风车中心字母R bool findFlowStrip(const cv::Mat src);//寻找图中的流动条 + bool findFlowStripSub(const cv::Mat src);//寻找图中的流动条 bool findCenterROI(const cv::Mat src);//框取中心R候选区 bool findFlowStripFan(const cv::Mat src);//寻找图中的流动条所在扇叶 bool findFlowStripWeak(const cv::Mat src);//弱识别寻找图中的流动条 @@ -191,6 +193,7 @@ private: void ArmorStruct(cv::Mat &src);//腐蚀和膨胀 void FlowStripFanStruct(cv::Mat &src);//腐蚀和膨胀 void FlowStripStruct(cv::Mat &src);//腐蚀和膨胀 + void FlowStripStructSub(cv::Mat &src);//腐蚀和膨胀 void CenterRStruct(cv::Mat &src);//腐蚀和膨胀 }; diff --git a/energy/include/energy/param_struct_define.h b/energy/include/energy/param_struct_define.h index 1636751..13f660d 100644 --- a/energy/include/energy/param_struct_define.h +++ b/energy/include/energy/param_struct_define.h @@ -20,8 +20,6 @@ struct EnergyPartParam { int RED_GRAY_THRESH;//红方二值化阈值 int BLUE_GRAY_THRESH;//蓝方二值化阈值 int SPLIT_GRAY_THRESH;//通道分离二值化阈值 - int FAN_GRAY_THRESH;//扇叶识别二值化阈值 - int ARMOR_GRAY_THRESH;//装甲板识别二值化阈值 long FAN_CONTOUR_AREA_MAX;//扇叶面积最大值 long FAN_CONTOUR_AREA_MIN;//扇叶面积最小值 diff --git a/energy/src/energy/calibrate/structing.cpp b/energy/src/energy/calibrate/structing.cpp index 4cff123..7696660 100644 --- a/energy/src/energy/calibrate/structing.cpp +++ b/energy/src/energy/calibrate/structing.cpp @@ -99,26 +99,51 @@ void Energy::FlowStripFanStruct(cv::Mat &src) { // --------------------------------------------------------------------------------------------------------------------- void Energy::FlowStripStruct(cv::Mat &src) { Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(5, 5)); - Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(5, 5)); - Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(2, 2)); + Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 2)); + Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3)); Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(2 , 2)); + Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(3, 3)); + Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(1 , 1)); + + dilate(src, src, element_dilate_1); +// imshow("dilate_1", src); + erode(src,src, element_erode_1); +// imshow("erode_1", src); + erode(src,src, element_erode_2); +// imshow("erode_2", src); + erode(src,src, element_erode_3); +// imshow("erode_3", src); +// dilate(src, src, element_dilate_2); +// imshow("dilate_2", src); +// dilate(src, src, element_dilate_3); +// imshow("dilate_3", src); +} + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数对图像进行腐蚀与膨胀操作 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::FlowStripStructSub(cv::Mat &src) { + Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(5, 5)); + Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(4, 4)); + Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3)); + Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(3 , 3)); Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(2, 2)); Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(2 , 2)); Mat element_dilate_4 = getStructuringElement(MORPH_RECT, Size(2, 2)); - Mat element_erode_4 = getStructuringElement(MORPH_RECT, Size(1 , 1)); -// dilate(src, src, element_dilate_1); -// imshow("dilate_1", src); erode(src,src, element_erode_1); - imshow("erode_1", src); +// imshow("sub_erode_1", src); // erode(src,src, element_erode_2); // imshow("erode_2", src); -// erode(src,src, element_erode_3); -// imshow("erode_3", src); + erode(src,src, element_erode_3); +// imshow("sub_erode_3", src); +// erode(src,src, element_erode_4); +// imshow("erode_4", src); dilate(src, src, element_dilate_1); -// imshow("dilate_1", src); +// imshow("sub_dilate_1", src); dilate(src, src, element_dilate_2); - imshow("dilate_2", src); +// imshow("sub_dilate_2", src); // dilate(src, src, element_dilate_3); // imshow("dilate_3", src); // dilate(src, src, element_dilate_4); @@ -126,7 +151,6 @@ void Energy::FlowStripStruct(cv::Mat &src) { } - //---------------------------------------------------------------------------------------------------------------------- // 此函数对图像进行腐蚀与膨胀操作 // --------------------------------------------------------------------------------------------------------------------- diff --git a/energy/src/energy/clear/clear.cpp b/energy/src/energy/clear/clear.cpp index cc3affc..6bc8c96 100644 --- a/energy/src/energy/clear/clear.cpp +++ b/energy/src/energy/clear/clear.cpp @@ -28,12 +28,16 @@ void Energy::clearAll() { void Energy::initImage(cv::Mat &src) { // imagePreprocess(src); // if(show_process)imshow("img_preprocess", src); - if (src.type() == CV_8UC3)cvtColor(src, src, COLOR_BGR2GRAY); + if (src.type() == CV_8UC3){ + cvtColor(src, src, COLOR_BGR2GRAY); + } if (mcu_data.enemy_color == ENEMY_BLUE){ threshold(src, src, energy_part_param_.RED_GRAY_THRESH, 255, THRESH_BINARY); } else if(mcu_data.enemy_color == ENEMY_RED){ threshold(src, src, energy_part_param_.BLUE_GRAY_THRESH, 255, THRESH_BINARY); } - if (show_process)imshow("bin", src); + if (show_process){ + imshow("bin", src); + } if (show_energy || show_process)waitKey(1); } diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index 6e0628a..011e500 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -29,6 +29,7 @@ void Energy::initEnergy() { radius = 0; + curr_fps = 0; send_cnt = 0; camera_cnt = 1; fans_cnt = 0; @@ -83,11 +84,9 @@ void Energy::initEnergy() { void Energy::initEnergyPartParam() { // gimbal_energy_part_param_.GRAY_THRESH = 120;//home // gimbal_energy_part_param_.GRAY_THRESH = 200;//official - gimbal_energy_part_param_.RED_GRAY_THRESH = 150;//game + gimbal_energy_part_param_.RED_GRAY_THRESH = 180;//game gimbal_energy_part_param_.BLUE_GRAY_THRESH = 100;//game gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 180; - gimbal_energy_part_param_.FAN_GRAY_THRESH = 75; - gimbal_energy_part_param_.ARMOR_GRAY_THRESH = 80; gimbal_energy_part_param_.FAN_CONTOUR_AREA_MAX = 5000; gimbal_energy_part_param_.FAN_CONTOUR_AREA_MIN = 1500; @@ -143,11 +142,9 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 32; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 55; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 4; -// gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 40; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 7; -// gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 3; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 1; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 3; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.6; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 100; @@ -165,8 +162,6 @@ void Energy::initEnergyPartParam() { // chassis_energy_part_param_.GRAY_THRESH = 200;//official // chassis_energy_part_param_.GRAY_THRESH = 225; chassis_energy_part_param_.SPLIT_GRAY_THRESH = 230; - chassis_energy_part_param_.FAN_GRAY_THRESH = 75; - chassis_energy_part_param_.ARMOR_GRAY_THRESH = 80; chassis_energy_part_param_.FAN_CONTOUR_AREA_MAX = 17000; chassis_energy_part_param_.FAN_CONTOUR_AREA_MIN = 0; diff --git a/energy/src/energy/find/energy_finder.cpp b/energy/src/energy/find/energy_finder.cpp index 8395bc4..af30fa8 100644 --- a/energy/src/energy/find/energy_finder.cpp +++ b/energy/src/energy/find/energy_finder.cpp @@ -315,6 +315,105 @@ bool Energy::findFlowStrip(const cv::Mat src) { } +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于寻找流动条 +// --------------------------------------------------------------------------------------------------------------------- +bool Energy::findFlowStripSub(const cv::Mat src) { + if (src.empty()) { + if (show_info) cout << "empty!" << endl; + return false; + } + cv::Mat src_bin; + src_bin = src.clone(); + + if (src_bin.type() == CV_8UC1) // 黑白图像 + { + cvtColor(src_bin, src_bin, COLOR_GRAY2RGB); + + } + std::vector 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);//若读取三通道视频文件，需转换为单通道 + + FlowStripStructSub(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 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; + } else { + continue; + } + +// 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_contour); +// float length_width_ratio = length / width; +// cout << "area: " << cur_contour_area << '\t' << endl; +// cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl; +// cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl; +// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl; +// cout< 1) { + if (show_info)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; + } + } + } + return true; +} + + //---------------------------------------------------------------------------------------------------------------------- // 此函数用于弱识别寻找流动条 // --------------------------------------------------------------------------------------------------------------------- diff --git a/energy/src/energy/mark/mark.cpp b/energy/src/energy/mark/mark.cpp index bdf6c03..cabe075 100644 --- a/energy/src/energy/mark/mark.cpp +++ b/energy/src/energy/mark/mark.cpp @@ -7,12 +7,14 @@ using namespace std; using namespace cv; +extern McuData mcu_data; + //---------------------------------------------------------------------------------------------------------------------- // 此函数用于记录操作手的微调dx和dy // --------------------------------------------------------------------------------------------------------------------- void Energy::writeDownSlightChange(cv::Mat &src) { - if (fans_cnt >= 4) { + if (fans_cnt >= 2) { FILE *fp_delta = fopen(PROJECT_DIR"/Mark/delta.txt", "w"); if (fp_delta) { fprintf(fp_delta, "delta_x: %d, delta_y: %d\n", mcu_data.delta_x + manual_delta_x, @@ -20,6 +22,27 @@ void Energy::writeDownSlightChange(cv::Mat &src) { fclose(fp_delta); } } + if(fans_cnt>=1){ + FILE *fp_data = fopen(PROJECT_DIR"/Mark/data.txt", "a"); + if (fp_data) { + if(mcu_data.mark == 1){ + fprintf(fp_data, "PID: %s\t", "new"); + } else { + fprintf(fp_data, "PID: %s\t", "default"); + } + if(is_big){ + fprintf(fp_data, "state: %s\t", "big"); + } else if(is_small){ + fprintf(fp_data, "state: %s\t", "small"); + } + fprintf(fp_data, "fps: %d\t", curr_fps); + fprintf(fp_data, "fans_cnt: %d\t", fans_cnt); + fprintf(fp_data, "yaw: %lf , pitch: %lf\t", yaw_rotation, pitch_rotation); + fprintf(fp_data, "delta_x: %d, delta_y: %d\n", mcu_data.delta_x + manual_delta_x, + mcu_data.delta_y + manual_delta_y); + fclose(fp_data); + } + } } diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index cae34fe..41d3056 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -83,7 +83,11 @@ void Energy::runBig(cv::Mat &gimbal_src) { } else { if (show_energy)showFlowStripFan("strip fan", gimbal_src); if (!findTargetInFlowStripFan()) return; - if (!findFlowStrip(gimbal_src))return; + if(!findFlowStrip(gimbal_src)){ + if(!findFlowStripSub(gimbal_src)) { + return; + } + } } if (!findCenterROI(gimbal_src))return; if (show_energy)showFlowStrip("strip", gimbal_src); diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index a0bc804..742ed3a 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -112,6 +112,7 @@ void Energy::sendTarget(Serial &serial, float x, float y, float z) { if (last_time != t) { last_time = t; cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl; + curr_fps = fps; fps = 0; } fps += 1; @@ -148,6 +149,7 @@ void Energy::sendTarget(Serial &serial, float x, float y, float z, uint16_t u) { if (last_time != t) { last_time = t; cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << "," << u << ")" << endl; + curr_fps = fps; fps = 0; } fps += 1; diff --git a/main.cpp b/main.cpp index b1a75f0..488a78c 100644 --- a/main.cpp +++ b/main.cpp @@ -32,7 +32,7 @@ McuData mcu_data = { // 单片机端回传结构体 0, // 当前云台pitch角 ARMOR_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 - 0, // 是否为反陀螺模式 + 1, // 是否启用数字识别 ENEMY_RED, // 敌方颜色 0, // 能量机关x轴补偿量 0, // 能量机关y轴补偿量 @@ -66,8 +66,8 @@ int main(int argc, char *argv[]) { video_gimbal = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2/*, "armor"*/); video_chassis = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2/*, "energy"*/); } else { - video_gimbal = new VideoWrapper(PROJECT_DIR"/8-7-NO7.avi"); - video_chassis = new VideoWrapper(PROJECT_DIR"/8-7-NO7.avi"); + video_gimbal = new VideoWrapper(PROJECT_DIR"/test_video/blue_big.avi"); + video_chassis = new VideoWrapper(PROJECT_DIR"/test_video/blue_big.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully.");