From a2f08e9674c036006f733438bea35516f10e1231 Mon Sep 17 00:00:00 2001 From: sun Date: Thu, 11 Jul 2019 21:49:44 +0800 Subject: [PATCH] energy changed --- .../searching_state/find_light_blobs.cpp | 8 +- energy/include/energy/energy.h | 188 ++++++++-------- energy/include/energy/param_struct_define.h | 30 +-- energy/src/energy/calibrate/split.cpp | 10 +- energy/src/energy/calibrate/structing.cpp | 38 ++-- energy/src/energy/change/mode_change.cpp | 28 +-- energy/src/energy/find/energy_finder.cpp | 163 +++++++------- energy/src/energy/find/target_finder.cpp | 30 +-- energy/src/energy/get/aim_point_get.cpp | 6 - energy/src/energy/get/gimble_rotation_get.cpp | 7 +- energy/src/energy/get/guess_point_get.cpp | 54 +++++ energy/src/energy/get/polar_angle_get.cpp | 49 +---- energy/src/energy/init/clear.cpp | 31 +++ energy/src/energy/{ => init}/energy_init.cpp | 118 +++++------ energy/src/energy/judge/judge_shoot.cpp | 38 ++++ energy/src/energy/judge/judge_time.cpp | 21 ++ energy/src/energy/mark/mark.cpp | 4 +- energy/src/energy/run.cpp | 200 +++++++----------- energy/src/energy/send/send.cpp | 9 +- energy/src/energy/show/show.cpp | 88 ++++---- energy/src/energy/tool/tool.cpp | 70 +++++- main.cpp | 12 +- others/include/options/options.h | 10 +- others/src/options/options.cpp | 52 ++--- 24 files changed, 689 insertions(+), 575 deletions(-) create mode 100644 energy/src/energy/get/guess_point_get.cpp create mode 100644 energy/src/energy/init/clear.cpp rename energy/src/energy/{ => init}/energy_init.cpp (77%) create mode 100644 energy/src/energy/judge/judge_shoot.cpp create mode 100644 energy/src/energy/judge/judge_time.cpp diff --git a/armor/src/armor_finder/searching_state/find_light_blobs.cpp b/armor/src/armor_finder/searching_state/find_light_blobs.cpp index 1f2d798..be845f6 100644 --- a/armor/src/armor_finder/searching_state/find_light_blobs.cpp +++ b/armor/src/armor_finder/searching_state/find_light_blobs.cpp @@ -58,7 +58,7 @@ static double nonZeroRateOfRotateRect_opt(const cv::Mat &bin, const cv::RotatedR auto end = max(linePointX(corners[0], corners[1], r), linePointX(corners[0], corners[2], r)) + 1; if (start < 0 || end > 640) return 0; for (int c = start; c < end; c++) { - if (bin.at(c, r)) { + if (bin.at(r, c)) { cnt++; } } @@ -78,7 +78,7 @@ static double nonZeroRateOfRotateRect_opt(const cv::Mat &bin, const cv::RotatedR auto end = max(linePointX(corners[1], corners[3], r), linePointX(corners[2], corners[3], r)) + 1; if (start < 0 || end > 640) return 0; for (int c = start; c < end; c++) { - if (bin.at(c, r)) { + if (bin.at(r, c)) { cnt++; } } @@ -90,8 +90,8 @@ static bool isValidLightBlob(const cv::Mat &bin, const cv::RotatedRect &rect) { return (lw_rate(rect) > 1.5) && // (rect.size.width*rect.size.height < 3000) && (rect.size.width * rect.size.height > 1) && - // (nonZeroRateOfRotateRect_opt(bin, rect) > 0.8); - (nonZeroRateOfRotateRect(bin, rect) > 0.8); + (nonZeroRateOfRotateRect_opt(bin, rect) > 0.8); +// (nonZeroRateOfRotateRect(bin, rect) > 0.8); } void get_blob_color(const cv::Mat &src, std::vector &blobs) { diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 0090c2b..6e2dd12 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -23,128 +23,150 @@ using std::vector; class Energy { public: - Energy(Serial &u, uint8_t &color);//构造函数，参数为串口和敌方颜色 - ~Energy();//默认析构函数 - int runBig(cv::Mat &gimble_src, cv::Mat &chassis_src); + Energy(Serial &u, uint8_t &color);//构造函数，参数为串口和敌方颜色 + ~Energy();//默认析构函数 + int runBig(cv::Mat &gimble_src, cv::Mat &chassis_src); + int runBig(cv::Mat &gimble_src); + int runSmall(cv::Mat &gimble_src); - Serial &serial;//串口 - void setEnergyRotationInit();//判断顺逆时针函数 - void extract(cv::Mat &src);//框取图像中的一块区域进行处理 - void sendTarget(Serial& serial, float x, float y, char change, char shoot); + + Serial &serial;//串口 + void setEnergyRotationInit();//判断顺逆时针函数 + void extract(cv::Mat &src);//框取图像中的一块区域进行处理 + void sendTarget(Serial &serial, float x, float y, float z); private: EnergyPartParam energy_part_param_;//能量机关的参数设置 - EnergyPartParam gimble_energy_part_param_;//云台摄像头能量机关的参数设置 + EnergyPartParam gimble_energy_part_param_;//云台摄像头能量机关的参数设置 EnergyPartParam chassis_energy_part_param_;//底盘摄像头能量机关的参数设置 - bool isMark;//若操作手正在手动标定，则为true - bool isGimble;//同时具有底盘和云台摄像头时，处于云台摄像头对心过程 - bool isChassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程 - bool isGuessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标 - bool isPredicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打 - int fans_cnt;//图像中的扇叶个数 - int armors_cnt;//图像中的装甲板个数 - int centerRs_cnt;//图像中可能的风车中心字母R选区个数 - int flow_strip_fans_cnt;//图像中的含流动条扇叶个数 - int flow_strips_cnt;//图像中的流动条个数 - int last_fans_cnt;//上一帧的扇叶个数 - int last_armors_cnt;//上一帧的装甲板个数 - int last_flow_strip_fans_cnt;//上一帧的含流动条扇叶个数 - int last_flow_strips_cnt;//上一帧的流动条个数 - double radius;//大风车半径 - float target_polar_angle;//待击打装甲板的极坐标角度 + bool isMark;//若操作手正在手动标定，则为true + bool isGimble;//同时具有底盘和云台摄像头时，处于云台摄像头对心过程 + bool isChassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程 + bool isGuessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标 + bool isPredicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打 +// int fans_cnt;//图像中的扇叶个数 +// int armors_cnt;//图像中的装甲板个数 +// int centerRs_cnt;//图像中可能的风车中心字母R选区个数 +// int flow_strip_fans_cnt;//图像中的含流动条扇叶个数 +// int flow_strips_cnt;//图像中的流动条个数 + int last_fans_cnt;//上一帧的扇叶个数 + int last_flow_strip_fans_cnt;//上一帧的含流动条扇叶个数 + int last_flow_strips_cnt;//上一帧的流动条个数 + double radius;//大风车半径 + float target_polar_angle;//待击打装甲板的极坐标角度 float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度 - uint8_t &ally_color;//我方颜色 - int energy_rotation_direction;//风车旋转方向 - float attack_distance;//步兵与风车平面距离 - int send_cnt;//向主控板发送的数据总次数 - float yaw_rotation;//云台yaw轴应该转到的角度 - float pitch_rotation;//云台pitch轴应该转到的角度 - uint8_t last_mark;//用于记录上一帧操作手是否进行标定 + float guess_polar_angle;//猜测的下一个目标装甲板极坐标角度 + float last_base_angle;//上一帧的各扇叶在0区（0°~72°）的基础角度 + uint8_t &ally_color;//我方颜色 + int energy_rotation_direction;//风车旋转方向 + float attack_distance;//步兵与风车平面距离 + int send_cnt;//向主控板发送的数据总次数 + float yaw_rotation;//云台yaw轴应该转到的角度 + float pitch_rotation;//云台pitch轴应该转到的角度 + uint8_t last_mark;//用于记录上一帧操作手是否进行标定 double predict_rad;//预测提前角 - bool energy_rotation_init;//若仍在判断风车旋转方向，则为true - int clockwise_rotation_init_cnt;//装甲板顺时针旋转次数 - int anticlockwise_rotation_init_cnt;//装甲板逆时针旋转次数 - float red_origin_yaw, red_origin_pitch;//红方的初始云台对心角度设定值 - float blue_origin_yaw, blue_origin_pitch;//蓝方的初始云台对心角度设定值 - float origin_yaw, origin_pitch;//初始的云台角度设定值 - float target_cnt;//用于记录寻找到的装甲板总数，该值变化则立即中断主控板发射进程，防止重复击打已点亮的装甲板 + bool energy_rotation_init;//若仍在判断风车旋转方向，则为true + int clockwise_rotation_init_cnt;//装甲板顺时针旋转次数 + int anticlockwise_rotation_init_cnt;//装甲板逆时针旋转次数 + float red_origin_yaw, red_origin_pitch;//红方的初始云台对心角度设定值 + float blue_origin_yaw, blue_origin_pitch;//蓝方的初始云台对心角度设定值 + float origin_yaw, origin_pitch;//初始的云台角度设定值 + float target_cnt;//用于记录寻找到的装甲板总数，该值变化则立即中断主控板发射进程，防止重复击打已点亮的装甲板 bool save_new_mark;//若操作手进行过手动标定，则为true - bool big_energy_shoot;//大符模式中，若为true，则要求主控板发弹 - bool small_energy_shoot;//小符模式中，若为true，则要求主控板发弹 - std::vector fans;//图像中所有扇叶 - std::vector armors;//图像中所有装甲板 - std::vector centerRs;//风车中心字母R的可能候选区 - std::vector flow_strips;//图像中所有流动条（理论上只有一个） - std::vector flow_strip_fans;//图像中所有流动条所在扇叶（理论上只有一个） - std::vector center_ROI;//风车中心候选区 - std::vector target_armor;//目标装甲板（理论上仅一个） + bool shoot;//若为true，则要求主控板发弹 + int guess_devide; + bool startguessing; + timeval time_start_guess; + std::vector fans;//图像中所有扇叶 + std::vector armors;//图像中所有可能装甲板（可能存在误识别） +// std::vector centerRs;//风车中心字母R的可能候选区 +// std::vector flow_strips;//图像中所有流动条（理论上只有一个） +// std::vector flow_strip_fans;//图像中所有流动条所在扇叶（理论上只有一个） +// std::vector center_ROI;//风车中心候选区 +// std::vector target_armor;//目标装甲板（理论上仅一个） + cv::RotatedRect centerR;//风车中心字母R的可能候选区 + cv::RotatedRect flow_strip;//图像中所有流动条（理论上只有一个） + cv::RotatedRect flow_strip_fan;//图像中所有流动条所在扇叶（理论上只有一个） + cv::RotatedRect center_ROI;//风车中心候选区 + cv::RotatedRect target_armor;//目标装甲板（理论上仅一个） - cv::Point circle_center_point;//风车圆心坐标 - cv::Point target_point;//目标装甲板中心坐标 - cv::Point last_target_point;//上一帧目标装甲板中心坐标 - cv::Point predict_point;//预测的击打点坐标 - std::vectorfan_polar_angle;//当前帧所有扇叶的极坐标角度 - std::vectorarmor_polar_angle;//当前帧所有装甲板的极坐标角度 + cv::Point circle_center_point;//风车圆心坐标 + cv::Point target_point;//目标装甲板中心坐标 + cv::Point last_target_point;//上一帧目标装甲板中心坐标 + cv::Point guess_point; + cv::Point predict_point;//预测的击打点坐标 + std::vector fan_polar_angle;//当前帧所有扇叶的极坐标角度 + std::vector armor_polar_angle;//当前帧所有装甲板的极坐标角度 std::vector all_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算 - cv::Mat src_blue, src_red, src_green;//通道分离中的三个图像通道 + cv::Mat src_blue, src_red, src_green;//通道分离中的三个图像通道 - void initEnergy();//能量机关初始化 - void initEnergyPartParam();//能量机关参数初始化 - void initRotation();//顺逆时针初始化 + void initEnergy();//能量机关初始化 + void initEnergyPartParam();//能量机关参数初始化 + void initRotation();//顺逆时针初始化 - void changeMode();//猜测模式与击打模式切换 - bool isGimbleCentered();//判断云台摄像头对心是否完成 + void clearAll();//清空各vector + void initImage(cv::Mat &src);//图像预处理 - int findFan(const cv::Mat src, int &last_fans_cnt);//寻找图中所有扇叶 - int findArmor(const cv::Mat src, int &last_armors_cnt);//寻找图中所有装甲板 - int findCenterR(const cv::Mat src);//寻找图中可能的风车中心字母R - int findFlowStrip(const cv::Mat src, int &last_flow_strips_cnt);//寻找图中的流动条 - void findCenterROI(const cv::Mat src);//框取中心R候选区 - int findFlowStripFan(const cv::Mat src, int &last_flow_strip_fans_cnt);//寻找图中的流动条所在扇叶 + void startChassis();//从云台摄像头对心状态进入底盘摄像头击打状态 - bool isValidFanContour(const vector &fan_contour);//扇叶矩形尺寸要求 + int findFans(const cv::Mat src);//寻找图中所有扇叶 + int findArmors(const cv::Mat src);//寻找图中所有装甲板 + bool findCenterR(const cv::Mat src);//寻找图中可能的风车中心字母R + bool findFlowStrip(const cv::Mat src);//寻找图中的流动条 + bool findCenterROI(const cv::Mat src);//框取中心R候选区 + bool findFlowStripFan(const cv::Mat src);//寻找图中的流动条所在扇叶 + + bool isValidFanContour(cv::Mat &src, const vector &fan_contour);//扇叶矩形尺寸要求 bool isValidArmorContour(const vector &armor_contour);//装甲板矩形尺寸要求 bool isValidCenterRContour(const vector ¢er_R_contour);//风车中心选区尺寸要求 bool isValidFlowStripContour(const vector &flow_strip_contour);//流动条扇叶矩形尺寸要求 - bool isValidFlowStripFanContour(const vector &flow_strip_fan_contour);//流动条扇叶矩形尺寸要求 + bool isValidFlowStripFanContour(cv::Mat &src, const vector &flow_strip_fan_contour);//流动条扇叶矩形尺寸要求 - void showFanContours(std::string windows_name, const cv::Mat src);//显示扇叶 - void showArmorContours(std::string windows_name, const cv::Mat src);//显示装甲板 - void showBothContours(std::string windows_name, const cv::Mat src);//显示扇叶和装甲板 + void showFanContours(std::string windows_name, const cv::Mat src);//显示扇叶 + void showArmorContours(std::string windows_name, const cv::Mat src);//显示装甲板 + void showBothContours(std::string windows_name, const cv::Mat src);//显示扇叶和装甲板 void showCenterRContours(std::string windows_name, const cv::Mat src);//显示风车中心候选区R void showFlowStripFanContours(std::string windows_name, const cv::Mat src);//显示流动条所在扇叶 + void showGuessTarget(std::string windows_name, const cv::Mat src);//显示猜测点位 - void getFanPolarAngle();//获取扇叶极坐标角度 - void getArmorPolarAngle();//获取装甲板极坐标角度 - void getAllArmorCenters();//记录所有装甲板中心坐标 - void getOrigin();//获得云台对心所需角度 + void getOrigin();//获得云台对心所需角度 + void guessTarget();//获得猜测击打点位 void circleLeastFit();//利用所有记录的装甲板中心最小二乘法计算圆心和半径 - void findTargetByPolar();//通过极坐标角度匹配获取目标装甲板的极坐标角度和装甲板中心坐标 + void findTargetByPolar();//通过极坐标角度匹配获取目标装甲板的极坐标角度和装甲板中心坐标 void findTargetByIntersection();//通过面积重合度匹配获取目标装甲板的极坐标角度和装甲板中心坐标 bool findTargetInFlowStripFan();//在已发现的流动条区域中寻找待击打装甲板 void rotate();//获取预测点位 - void stretch(cv::Point point_1, cv::Point2f &point_2);//将像素差转换为实际距离差 + void stretch(cv::Point point_1, cv::Point2f &point_2);//将像素差转换为实际距离差 double pointDistance(cv::Point point_1, cv::Point point_2);//计算两点距离 + int devide(float angle);//将极坐标分为五个区域，判断一个角度处于哪个区域 + int linePointX(const cv::Point2f &p1, const cv::Point2f &p2, int y);//计算直线上一点横坐标 + double nonZeroRateOfRotateRect(const cv::Mat &bin, const cv::RotatedRect &rotatedRect);//计算旋转矩形内亮点占比 - void writeDownMark();//记录操作手标定的云台初始角度 + void writeDownMark();//记录操作手标定的云台初始角度 + void getTargetPolarAngle(); void getPredictPoint();//获取预测点位 void getAimPoint();//通过自瞄逻辑计算点位 - bool changeTarget();//判断目标是否改变 - void changeMark();//操作手手动修改标定值 + bool changeTarget();//判断目标是否改变 + void changeMark();//操作手手动修改标定值 void gimbleRotation();//计算云台旋转角度 - void splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red);//拜耳阵列分离 - void imagePreprocess(cv::Mat &src);//图像通道分离 + void judgeBigShoot();//判断大符模式是否可以发弹 + void judgeSmallShoot();//判断小符模式是否可以发弹 + bool is_guessing_timeout();//判断猜测模式是否超时（没打中） + + void splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red);//拜耳阵列分离 + void imagePreprocess(cv::Mat &src);//图像通道分离 void hsvFilter(cv::Mat &src);//此函数通过HSV颜色空间对图像纯度进行限制，进行滤光 - void StructingElementClose(cv::Mat &src,int length, int width);//闭运算 - void StructingElementErodeDilate(cv::Mat &src);//腐蚀和膨胀 + void StructingElementClose(cv::Mat &src, int length, int width);//闭运算 + void StructingElementErodeDilate(cv::Mat &src);//腐蚀和膨胀 + void StructingElementDilateErode(cv::Mat &src);//腐蚀和膨胀 }; diff --git a/energy/include/energy/param_struct_define.h b/energy/include/energy/param_struct_define.h index 1062c75..8ebeec2 100644 --- a/energy/include/energy/param_struct_define.h +++ b/energy/include/energy/param_struct_define.h @@ -12,24 +12,6 @@ using std::vector; -//---------------------------------------------------------------------------------------------------------------------- -// 此结构体为能量机关模块的结构体，用于寻找矩形轮廓 -// --------------------------------------------------------------------------------------------------------------------- -struct EnergyPart { - cv::RotatedRect rect; - float angle; - vector contour; - - explicit EnergyPart(vector &c) : contour(c) { - rect = cv::minAreaRect(c); - angle = cv::minAreaRect(c).angle; - }; - - EnergyPart(cv::RotatedRect rect=cv::RotatedRect(), float angle=0, vector contour=vector()):rect(rect),angle(angle),contour(contour){}; -}; - - - //---------------------------------------------------------------------------------------------------------------------- // 此结构体包括能量机关参数 // --------------------------------------------------------------------------------------------------------------------- @@ -48,6 +30,8 @@ struct EnergyPartParam { float FAN_CONTOUR_HW_RATIO_MAX;//扇叶长宽比最大值 float FAN_CONTOUR_HW_RATIO_MIN;//扇叶长宽比最小值 float FAN_CONTOUR_AREA_RATIO_MIN;//装甲板轮廓占旋转矩形面积比最小值 + double FAN_NON_ZERO_RATE_MAX;//流动条扇叶中亮点占旋转矩形的比值最大值 + double FAN_NON_ZERO_RATE_MIN;//流动条扇叶中亮点占旋转矩形的比值最大值 long ARMOR_CONTOUR_AREA_MAX;//装甲板面积最大值 long ARMOR_CONTOUR_AREA_MIN;//装甲板面积最小值 @@ -78,8 +62,10 @@ struct EnergyPartParam { long FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX;//流动条扇叶（待击打）宽边长度最大值 float FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX;//流动条扇叶（待击打）长宽比最大值 float FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN;//流动条扇叶（待击打）长宽比最小值 - float FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX;//装甲板轮廓占旋转矩形面积比最小值 - float FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN;//装甲板轮廓占旋转矩形面积比最小值 + float FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX;//流动条扇叶轮廓占旋转矩形面积比最小值 + float FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN;//流动条扇叶占旋转矩形面积比最小值 + double FLOW_STRIP_FAN_NON_ZERO_RATE_MAX;//流动条扇叶中亮点占旋转矩形的比值最大值 + double FLOW_STRIP_FAN_NON_ZERO_RATE_MIN;//流动条扇叶中亮点占旋转矩形的比值最大值 long FLOW_STRIP_CONTOUR_AREA_MAX;//流动条（待击打）面积最大值 long FLOW_STRIP_CONTOUR_AREA_MIN;//流动条（待击打）面积最小值 @@ -89,10 +75,10 @@ struct EnergyPartParam { long FLOW_STRIP_CONTOUR_WIDTH_MAX;//流动条（待击打）宽边长度最大值 float FLOW_STRIP_CONTOUR_HW_RATIO_MAX;//流动条（待击打）长宽比最大值 float FLOW_STRIP_CONTOUR_HW_RATIO_MIN;//流动条（待击打）长宽比最小值 - float FLOW_STRIP_CONTOUR_AREA_RATIO_MIN;//装甲板轮廓占旋转矩形面积比最小值 + float FLOW_STRIP_CONTOUR_AREA_RATIO_MIN;//流动条占旋转矩形面积比最小值 float FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN;//流动条占旋转矩形面积比最小值 - float TWIN_ANGEL_MAX;//扇叶和装甲板匹配时极坐标角度差的最大值 + float TWIN_ANGEL_MAX;//两个理论上相等的角度在计算时具有的可能最大差值 long INTERSETION_CONTOUR_AREA_MIN;//扇叶与装甲板匹配时的最小重合面积 long TARGET_CHANGE_DISTANCE_MAX;//目标未更改时，目标装甲板中心与原目标装甲板中心的距离变化最大值 diff --git a/energy/src/energy/calibrate/split.cpp b/energy/src/energy/calibrate/split.cpp index cdafbfb..c9a9dda 100644 --- a/energy/src/energy/calibrate/split.cpp +++ b/energy/src/energy/calibrate/split.cpp @@ -57,14 +57,14 @@ void Energy::imagePreprocess(cv::Mat &src) { resize(channels.at(2), src_red, Size(src.size().width/2, src.size().height/2)); if(ally_color == ALLY_RED) { - src = src_red-src_blue; - //src=src_red; +// src = src_red-src_blue; + src=src_red; }else if(ally_color == ALLY_BLUE){ - src = src_blue-src_red; - //src=src_blue; +// src = src_blue-src_red; + src=src_blue; } } cv::resize(src, src, cv::Size(src.size().width * 2, src.size().height * 2), 2); - threshold(src, src, energy_part_param_.SPLIT_GRAY_THRESH, 255, THRESH_BINARY); +// threshold(src, src, energy_part_param_.SPLIT_GRAY_THRESH, 255, THRESH_BINARY); } diff --git a/energy/src/energy/calibrate/structing.cpp b/energy/src/energy/calibrate/structing.cpp index 7c6004d..845608c 100644 --- a/energy/src/energy/calibrate/structing.cpp +++ b/energy/src/energy/calibrate/structing.cpp @@ -25,27 +25,37 @@ void Energy::StructingElementClose(cv::Mat &src,int length, int width){ // 此函数对图像进行腐蚀与膨胀操作 // --------------------------------------------------------------------------------------------------------------------- void Energy::StructingElementErodeDilate(cv::Mat &src) { - cv::Mat src_out, src_out_out; - Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(3, 3)); -// Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(3, 4)); + Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(8, 6)); Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 1)); - Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(8, 6)); + Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3)); Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(4 , 4)); - Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(3, 3)); - - -// dilate(src, src, element_dilate_1); -// imshow("dilate_1", src); erode(src,src, element_erode_1); // imshow("erode_1", src); - + dilate(src, src, element_dilate_1); +// imshow("dilate_1", src); dilate(src, src, element_dilate_2); // imshow("dilate_2", src); - - dilate(src, src, element_dilate_3); -// imshow("dilate_3", src); - erode(src,src, element_erode_2); // imshow("erode_2", src); } + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数对图像进行腐蚀与膨胀操作 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::StructingElementDilateErode(cv::Mat &src) { + Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(3, 3)); + Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 1)); + Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(8, 6)); + Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(4 , 4)); + + dilate(src, src, element_dilate_2); +// imshow("dilate_1", src); +// erode(src,src, element_erode_1); +// imshow("erode_1", src); +// erode(src,src, element_erode_2); +// imshow("erode_2", src); +// dilate(src, src, element_dilate_2); +// imshow("dilate_2", src); +} \ No newline at end of file diff --git a/energy/src/energy/change/mode_change.cpp b/energy/src/energy/change/mode_change.cpp index b331f5f..e123d6b 100644 --- a/energy/src/energy/change/mode_change.cpp +++ b/energy/src/energy/change/mode_change.cpp @@ -7,31 +7,11 @@ using namespace std; using namespace cv; -//---------------------------------------------------------------------------------------------------------------------- -// 此函数用于切换猜测模式（发弹——新目标出现）和击打模式（新目标出现——发弹） -// --------------------------------------------------------------------------------------------------------------------- -void Energy::changeMode() { - if (changeTarget()) { - isGuessing = false; - isPredicting = true; - target_cnt++;//若云台移动过程中发现有新装甲板亮起，需改变target_cnt值，以及时告知主控板中断进程，防止重复打击 - } - if (big_energy_shoot || small_energy_shoot){ - isGuessing = true; - isPredicting = false; - } -} - //---------------------------------------------------------------------------------------------------------------------- -// 此函数用于判断云台对心是否完成 +// 此函数用于从云台摄像头对心状态切换到底盘摄像头击打状态 // --------------------------------------------------------------------------------------------------------------------- -bool Energy::isGimbleCentered() { - if (centerRs.size() == 1) { - circle_center_point = centerRs.at(0).rect.center; - isGimble = false; - isChassis = true; - return true; - } - return false; +void Energy::startChassis(){ + isChassis = true; + isGimble = false; } \ 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 74e1086..1dbe137 100644 --- a/energy/src/energy/find/energy_finder.cpp +++ b/energy/src/energy/find/energy_finder.cpp @@ -12,33 +12,32 @@ using std::vector; //---------------------------------------------------------------------------------------------------------------------- // 此函数用于寻找图像内所有的大风车扇叶 // --------------------------------------------------------------------------------------------------------------------- -int Energy::findFan(const cv::Mat src, int &last_fans_cnt) { +int Energy::findFans(const cv::Mat src) { if (src.empty())return 0; static Mat src_bin; src_bin = src.clone(); -// threshold(src, src_bin, energy_part_param_.FAN_GRAY_THRESH, 255, THRESH_BINARY); if (src.type() == CV_8UC3) { cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道 } std::vector > fan_contours; - StructingElementClose(src_bin, 6, 6);//图像膨胀，防止图像断开并更方便寻找 + StructingElementErodeDilate(src_bin);//图像膨胀，防止图像断开并更方便寻找 // imshow("fan struct",src_bin); findContours(src_bin, fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); for (auto &fan_contour : fan_contours) { - if (!isValidFanContour(fan_contour)) { + if (!isValidFanContour(src_bin, fan_contour)) { continue; } - fans.emplace_back(fan_contour); + fans.emplace_back(cv::minAreaRect(fan_contour)); // RotatedRect cur_rect = minAreaRect(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; // if(length>20&&width>20){ -// cout< cur_size.width ? cur_size.height : cur_size.width; // float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; // if(length>10&&width>5){ -// armors.emplace_back(armor_contour); +// armors.emplace_back(cv::minAreaRect(armor_contour)); // cout<<"armor area: "<(armors.size()); - return -1;//寻找到的装甲板比上一帧少，说明该帧有误，返回-1 - } - last_armors_cnt = static_cast(armors.size()); return static_cast(armors.size()); } @@ -110,11 +103,10 @@ int Energy::findArmor(const cv::Mat src, int &last_armors_cnt) { //---------------------------------------------------------------------------------------------------------------------- // 此函数用于寻找图像内大风车中心字母“R” // --------------------------------------------------------------------------------------------------------------------- -int Energy::findCenterR(const cv::Mat src) { - if (src.empty())return 0; +bool Energy::findCenterR(const cv::Mat src) { + if (src.empty())return false; static Mat src_bin; src_bin = src.clone(); -// threshold(src, src_bin, energy_part_param_.ARMOR_GRAY_THRESH, 255, THRESH_BINARY); if (src.type() == CV_8UC3) { cvtColor(src_bin, src_bin, CV_BGR2GRAY); } @@ -126,79 +118,74 @@ int Energy::findCenterR(const cv::Mat src) { if (!isValidCenterRContour(center_R_contour)) { continue; } - centerRs.emplace_back(center_R_contour); + centerR = cv::minAreaRect(center_R_contour); + circle_center_point = centerR.center; + return true; // RotatedRect cur_rect = minAreaRect(center_R_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; // if(length>10 && width>5){ -// centerRs.emplace_back(center_R_contour); +// centerRs.emplace_back(cv::minAreaRect(center_R_contour)); // cout<<"center R area: "<(centerRs.size()); + return false; + } //---------------------------------------------------------------------------------------------------------------------- // 此函数用于判断找到的矩形候选区是否为含流动条的扇叶 // --------------------------------------------------------------------------------------------------------------------- -int Energy::findFlowStripFan(const cv::Mat src, int &last_flow_strip_fans_cnt) { - if (src.empty())return 0; +bool Energy::findFlowStripFan(const cv::Mat src) { + if (src.empty())return false; static Mat src_bin; src_bin = src.clone(); -// threshold(src, src_bin, energy_part_param_.FAN_GRAY_THRESH, 255, THRESH_BINARY); if (src.type() == CV_8UC3) { cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道 } std::vector > flow_strip_fan_contours; - StructingElementClose(src_bin, 6, 6);//图像膨胀，防止图像断开并更方便寻找 -// imshow("flow strip fan struct", src_bin); + StructingElementErodeDilate(src_bin);//图像膨胀，防止图像断开并更方便寻找 + imshow("flow strip fan struct", src_bin); findContours(src_bin, flow_strip_fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); for (auto &flow_strip_fan_contour : flow_strip_fan_contours) { - if (!isValidFlowStripFanContour(flow_strip_fan_contour)) { + if (!isValidFlowStripFanContour(src_bin, flow_strip_fan_contour)) { continue; } - - flow_strip_fans.emplace_back(flow_strip_fan_contour); + flow_strip_fan = 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; -// if(length>20&&width>20){ -// cout< 10 && width > 10) { +// cout << cur_rect.center; +// flow_strip_fan = cv::minAreaRect(flow_strip_fan_contour); +// cout << "flow strip fan area: " << length << '\t' << width << endl; +// } - if (flow_strip_fans.size() < last_flow_strip_fans_cnt) { - last_flow_strip_fans_cnt = static_cast(flow_strip_fans.size()); - return -1;//寻找到的流动条扇叶比上一帧少，说明该帧有误，返回-1 + return true; } - last_flow_strip_fans_cnt = static_cast(flow_strip_fans.size()); - return static_cast(flow_strip_fans.size()); + return false; } //---------------------------------------------------------------------------------------------------------------------- // 此函数用于寻找流动条 // --------------------------------------------------------------------------------------------------------------------- -int Energy::findFlowStrip(const cv::Mat src, int &last_flow_strips_cnt) { - - if (src.empty())return 0; +bool Energy::findFlowStrip(const cv::Mat src) { + imshow("1", src); + if (src.empty())return false; cv::Mat src_bin; src_bin = src.clone(); if (src.type() == CV_8UC3) { cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道 } - StructingElementClose(src_bin, 6, 6);//图像膨胀，防止图像断开并更方便寻找 + StructingElementErodeDilate(src_bin);//图像膨胀，防止图像断开并更方便寻找 // imshow("flow strip struct", src_bin); std::vector > flow_strip_contours; @@ -208,23 +195,19 @@ int Energy::findFlowStrip(const cv::Mat src, int &last_flow_strips_cnt) { if (!isValidFlowStripContour(flow_strip_contour)) { continue; } - flow_strips.emplace_back(flow_strip_contour); + flow_strip = cv::minAreaRect(flow_strip_contour); // 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; // float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; -// if(length>40&&width>5){ -// cout< 40 && width > 5) { +// cout << cur_rect.center << endl; +// flow_strip = cv::minAreaRect(flow_strip_contour); +// cout << "flow strip fan area: " << length << '\t' << width << endl; +// } + return true; } - if (flow_strips.size() < last_flow_strips_cnt) { - last_flow_strips_cnt = static_cast(flow_strips.size()); - return -1;//寻找到的流动条比上一帧少，说明该帧有误，返回-1 - } - last_flow_strips_cnt = static_cast(flow_strips.size()); - return static_cast(flow_strips.size()); + return false; } @@ -232,36 +215,34 @@ int Energy::findFlowStrip(const cv::Mat src, int &last_flow_strips_cnt) { //---------------------------------------------------------------------------------------------------------------------- // 此函数用于框取中心R的寻找范围 // --------------------------------------------------------------------------------------------------------------------- -void Energy::findCenterROI(const cv::Mat src) { +bool Energy::findCenterROI(const cv::Mat src) { cv::Mat src_mask = src.clone(); - target_armor.at(0).rect.size.height *= 1.3; - target_armor.at(0).rect.size.width *= 1.3; + target_armor.size.height *= 1.3; + target_armor.size.width *= 1.3; Point2f vertices[4]; vector mask_rect; - target_armor.at(0).rect.points(vertices); //计算矩形的4个顶点 + 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), 15); // imshow("fill", src_mask); - flow_strips_cnt = findFlowStrip(src_mask, last_flow_strips_cnt); - float length = target_armor.at(0).rect.size.height > target_armor.at(0).rect.size.width ? - target_armor.at(0).rect.size.height : target_armor.at(0).rect.size.width; - if (flow_strips_cnt > 0 && target_armor.size() > 0) { - for (auto flow_strip: flow_strips) { - Point2f p2p(flow_strip.rect.center.x - target_point.x, - flow_strip.rect.center.y - target_point.y); - p2p = p2p / pointDistance(flow_strip.rect.center, target_point);//单位化 - center_ROI.emplace_back( - EnergyPart(cv::RotatedRect(cv::Point2f(flow_strip.rect.center + p2p * length * 1.5), - Size2f(length, length), -90))); - } - } + 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.4), + Size2f(length * 1.2, length * 1.2), -90); + return true; + } //---------------------------------------------------------------------------------------------------------------------- // 此函数用于判断找到的矩形候选区是否为扇叶 // --------------------------------------------------------------------------------------------------------------------- -bool Energy::isValidFanContour(const vector &fan_contour) { +bool Energy::isValidFanContour(cv::Mat &src, const vector &fan_contour) { double cur_contour_area = contourArea(fan_contour); if (cur_contour_area > energy_part_param_.FAN_CONTOUR_AREA_MAX || cur_contour_area < energy_part_param_.FAN_CONTOUR_AREA_MIN) { @@ -287,8 +268,13 @@ bool Energy::isValidFanContour(const vector &fan_contour) { return false; //长宽比不合适 } - if (cur_contour_area / cur_size.area() < energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN) - return false;//轮廓对矩形的面积占有率不合适 +// if (cur_contour_area / cur_size.area() < energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN) +// return false;//轮廓对矩形的面积占有率不合适 + double non_zero_rate = nonZeroRateOfRotateRect(src, cur_rect); + if (non_zero_rate > energy_part_param_.FAN_NON_ZERO_RATE_MAX || + non_zero_rate < energy_part_param_.FAN_NON_ZERO_RATE_MIN) + return false; + //亮点占比不合格 return true; } @@ -362,7 +348,7 @@ bool Energy::isValidCenterRContour(const vector ¢er_R_contour) { if (cur_contour_area / cur_size.area() < energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN) return false;//轮廓对矩形的面积占有率不合适 std::vector intersection; - if (center_ROI.empty() || rotatedRectangleIntersection(cur_rect, center_ROI.at(0).rect, intersection) == 0 || + if (rotatedRectangleIntersection(cur_rect, center_ROI, intersection) == 0 || contourArea(intersection) < energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN) { return false; } @@ -373,7 +359,7 @@ bool Energy::isValidCenterRContour(const vector ¢er_R_contour) { //---------------------------------------------------------------------------------------------------------------------- // 此函数用于判断找到的矩形候选区是否为流动条扇叶 // --------------------------------------------------------------------------------------------------------------------- -bool Energy::isValidFlowStripFanContour(const vector &flow_strip_fan_contour) { +bool Energy::isValidFlowStripFanContour(cv::Mat &src, const vector &flow_strip_fan_contour) { double cur_contour_area = contourArea(flow_strip_fan_contour); if (cur_contour_area > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX || cur_contour_area < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN) { @@ -402,11 +388,15 @@ bool Energy::isValidFlowStripFanContour(const vector &flow_strip_fan_ return false; //长宽比不合适 } - if (cur_contour_area / cur_size.area() < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN - || cur_contour_area / cur_size.area() > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX) - //如果ROI为空、或者选区与ROI无重合面积、或者有重合面积但重合面积太小 +// if (cur_contour_area / cur_size.area() < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN +// || cur_contour_area / cur_size.area() > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX) +// // 轮廓对矩形的面积占有率不合适 +// return false; + double non_zero_rate = nonZeroRateOfRotateRect(src, cur_rect); + if (non_zero_rate > energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX || + non_zero_rate < energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN) return false; - //轮廓对矩形的面积占有率不合适 + //亮点占比不合格 return true; } @@ -445,8 +435,7 @@ bool Energy::isValidFlowStripContour(const vector &flow_strip_contour if (cur_contour_area / cur_size.area() < energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN) return false;//轮廓对矩形的面积占有率不合适 std::vector intersection; - if (flow_strip_fans.size() > 0 && - rotatedRectangleIntersection(cur_rect, flow_strip_fans.at(0).rect, intersection) != 0) { + if (rotatedRectangleIntersection(cur_rect, flow_strip_fan, intersection) != 0) { if (contourArea(intersection) < energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) { return false; } diff --git a/energy/src/energy/find/target_finder.cpp b/energy/src/energy/find/target_finder.cpp index 92a9c43..309c834 100644 --- a/energy/src/energy/find/target_finder.cpp +++ b/energy/src/energy/find/target_finder.cpp @@ -18,7 +18,7 @@ void Energy::findTargetByPolar() { if (fan_polar_angle.empty()) { target_polar_angle = armor_polar_angle.at(0);//视野中没有扇叶，说明在击打第一个装甲板 for (const auto &armor : armors) { - target_point = armor.rect.center; + target_point = armor.center; } return; } @@ -33,10 +33,10 @@ void Energy::findTargetByPolar() { } else { target_polar_angle = armor_polar_angle.at(j);//无法被匹配到的装甲板为待击打装甲板 for (const auto &armor : armors) { - float angle = static_cast(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y), - (armor.rect.center.x - circle_center_point.x))); + float angle = static_cast(180 / PI * atan2(-1 * (armor.center.y - circle_center_point.y), + (armor.center.x - circle_center_point.x))); if (target_polar_angle == angle) { - target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标 + target_point = armor.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标 } } return; @@ -44,10 +44,10 @@ void Energy::findTargetByPolar() { } target_polar_angle = armor_polar_angle.at(armor_polar_angle.size() - 1);//前几个扇叶都匹配到装甲板，则最后剩下的装甲板为目标 for (const auto &armor : armors) { - float angle = static_cast(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y), - (armor.rect.center.x - circle_center_point.x))); + float angle = static_cast(180 / PI * atan2(-1 * (armor.center.y - circle_center_point.y), + (armor.center.x - circle_center_point.x))); if (target_polar_angle == angle) { - target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标 + target_point = armor.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标 } } } @@ -58,8 +58,12 @@ void Energy::findTargetByPolar() { // --------------------------------------------------------------------------------------------------------------------- void Energy::findTargetByIntersection() { if (fans.size() >= armors.size()) return;//扇叶多于装甲板，识别错误 - if (armors.empty())return;//找不到扇叶，识别错误 - if (fans.empty()) { + for (const auto &fan : fans) { + for (const auto &armor : armors) { + + } + } + /*if (fans.empty()) { target_point = armors.at(0).rect.center; return; } @@ -84,7 +88,7 @@ void Energy::findTargetByIntersection() { break; } i++; - } + }*/ } @@ -95,14 +99,14 @@ bool Energy::findTargetInFlowStripFan() { int i = 0; for (i = 0; i < armors.size(); ++i) { std::vector intersection; - if (rotatedRectangleIntersection(armors.at(i).rect, flow_strip_fans.at(0).rect, intersection) == 0) + if (rotatedRectangleIntersection(armors.at(i), flow_strip_fan, intersection) == 0) continue;//返回0表示没有重合面积 double cur_contour_area = contourArea(intersection); if (cur_contour_area < energy_part_param_.INTERSETION_CONTOUR_AREA_MIN) continue; else { - target_armor.emplace_back(armors.at(i)); - target_point = armors.at(i).rect.center; + target_armor = armors.at(i); + target_point = armors.at(i).center; return true; } } diff --git a/energy/src/energy/get/aim_point_get.cpp b/energy/src/energy/get/aim_point_get.cpp index 82d501a..6814977 100644 --- a/energy/src/energy/get/aim_point_get.cpp +++ b/energy/src/energy/get/aim_point_get.cpp @@ -16,10 +16,4 @@ void Energy::getAimPoint(){ double dy = target_point.y - 240; yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI; pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI; - if(fabs(yaw_rotation) < 0.5 && fabs(pitch_rotation) < 0.5){ - small_energy_shoot = true; - cout<<"send"<(180 / PI * atan2((attack_distance * tan(origin_pitch * PI / 180) - real_predict_point.y), attack_distance)); - if (fabs(yaw_rotation - mcuData.curr_yaw) < 0.5 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.5) - big_energy_shoot = true; + if (abs(yaw_rotation - mcuData.curr_yaw) < 0.5 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.5){ + shoot = true; + isPredicting = false; + isGuessing = true; + } } \ No newline at end of file diff --git a/energy/src/energy/get/guess_point_get.cpp b/energy/src/energy/get/guess_point_get.cpp new file mode 100644 index 0000000..358a077 --- /dev/null +++ b/energy/src/energy/get/guess_point_get.cpp @@ -0,0 +1,54 @@ +// +// Created by sun on 19-7-11. +// +#include "energy/energy.h" + +using namespace std; +using namespace cv; + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数获得猜测的击打点 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::guessTarget() { + vector all_fan_angles; + float angle; + for (const auto &fan : fans) { + angle = static_cast(180 / PI * atan2(-1 * (fan.center.y - circle_center_point.y), + (fan.center.x - circle_center_point.x))); + if (angle < 0)angle += 360; + all_fan_angles.emplace_back(angle); + } + angle = target_polar_angle; + if (angle < 0)angle += 360; + all_fan_angles.emplace_back(angle); + + sort(all_fan_angles.begin(), all_fan_angles.end()); + float min_angle = all_fan_angles.at(0); + float max_angle = all_fan_angles.at(all_fan_angles.size() - 1); + float base_angle = min_angle; + while (base_angle > 72)base_angle -= 72; + if (startguessing) { + int i = 0; + for (i = 1; i < all_fan_angles.size(); ++i) { + if (abs(min_angle + 72 * i - all_fan_angles.at(i)) > energy_part_param_.TWIN_ANGEL_MAX) { + guess_polar_angle = min_angle + 72 * i; + break; + } + } + if (i == all_fan_angles.size()) guess_polar_angle = max_angle + 72; + startguessing = false; + guess_devide = devide(guess_polar_angle); + } else if (abs(base_angle - last_base_angle) > 20) { + if (energy_rotation_direction == CLOCKWISE)guess_devide = (guess_devide + 4) % 5; + else guess_devide = (guess_devide + 1) % 5; + guess_polar_angle = base_angle + guess_devide * 72; + } else { + guess_polar_angle = base_angle + guess_devide * 72; + } + if (guess_polar_angle > 180)guess_polar_angle -= 360; + radius = pointDistance(target_point, circle_center_point); + guess_point.x = circle_center_point.x + radius * cos(PI / 180.0 * guess_polar_angle); + guess_point.y = circle_center_point.y - radius * sin(PI / 180.0 * guess_polar_angle); + last_base_angle = base_angle; +} diff --git a/energy/src/energy/get/polar_angle_get.cpp b/energy/src/energy/get/polar_angle_get.cpp index 33d59d3..a30b85d 100644 --- a/energy/src/energy/get/polar_angle_get.cpp +++ b/energy/src/energy/get/polar_angle_get.cpp @@ -4,54 +4,15 @@ #include "energy/energy.h" using namespace cv; -using std::cout; -using std::endl; -using std::vector; - +using namespace std; //---------------------------------------------------------------------------------------------------------------------- -// 此函数用于获得图像中所有扇叶的当前极坐标角度 +// 此函数获取目标装甲板极坐标角度 // --------------------------------------------------------------------------------------------------------------------- -void Energy::getFanPolarAngle() { - if (radius == 0)return; - for (const auto &fan : fans) - { - float angle = static_cast(180 / PI * atan2(-1 * (fan.rect.center.y - circle_center_point.y), - (fan.rect.center.x - circle_center_point.x))); - fan_polar_angle.push_back(angle); - } -// cout << "fanPosition.size() = " << fanPosition.size() << '\t' << endl; +void Energy::getTargetPolarAngle() { + target_polar_angle = static_cast(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y), + (target_point.x - circle_center_point.x))); } - - - -//---------------------------------------------------------------------------------------------------------------------- -// 此函数用于获得图像中所有装甲板的当前极坐标角度 -// --------------------------------------------------------------------------------------------------------------------- -void Energy::getArmorPolarAngle() { - if (radius == 0)return; - for (const auto &armor : armors) - { - float angle = static_cast(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y), - (armor.rect.center.x - circle_center_point.x))); - armor_polar_angle.push_back(angle); - - } -// cout << "armorPosition.size() = " << armorPosition.size() << '\t' << endl; -} - - - - -//---------------------------------------------------------------------------------------------------------------------- -// 此函数用于存储图像中所有装甲板的中心坐标，以便后续最小二乘法计算圆心和半径 -// --------------------------------------------------------------------------------------------------------------------- -void Energy::getAllArmorCenters() -{ - for (const auto &armor : armors) { - all_armor_centers.push_back(armor.rect.center); - } -} \ No newline at end of file diff --git a/energy/src/energy/init/clear.cpp b/energy/src/energy/init/clear.cpp new file mode 100644 index 0000000..ccfe220 --- /dev/null +++ b/energy/src/energy/init/clear.cpp @@ -0,0 +1,31 @@ +// +// Created by sun on 19-7-11. +// + +#include "energy/energy.h" + +using namespace std; +using namespace cv; + + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于清空各vector +// --------------------------------------------------------------------------------------------------------------------- +void Energy::clearAll(){ + fans.clear(); + armors.clear(); +} + + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于图像预处理 +// --------------------------------------------------------------------------------------------------------------------- +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); + threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY); + if (show_process)imshow("bin", src); +} \ No newline at end of file diff --git a/energy/src/energy/energy_init.cpp b/energy/src/energy/init/energy_init.cpp similarity index 77% rename from energy/src/energy/energy_init.cpp rename to energy/src/energy/init/energy_init.cpp index 358a89d..12aa6d9 100644 --- a/energy/src/energy/energy_init.cpp +++ b/energy/src/energy/init/energy_init.cpp @@ -10,69 +10,64 @@ using std::endl; using std::vector; - //---------------------------------------------------------------------------------------------------------------------- // 此函数对能量机关成员变量进行初始化 // --------------------------------------------------------------------------------------------------------------------- void Energy::initEnergy() { - isMark = false; - isPredicting = true; - isGuessing = false; - fans_cnt = 0; - armors_cnt = 0; - centerRs_cnt = 0; - flow_strips_cnt = 0; - flow_strip_fans_cnt = 0; + isMark = false; + isPredicting = true; + isGuessing = false; +// fans_cnt = 0; +// armors_cnt = 0; +// centerRs_cnt = 0; +// flow_strips_cnt = 0; +// flow_strip_fans_cnt = 0; circle_center_point = Point(0, 0); - target_point = Point(0, 0); + target_point = Point(0, 0); last_target_point = Point(0, 0); + guess_point = Point(0, 0); predict_point = Point(0, 0); target_polar_angle = -1000; + guess_polar_angle = -1000; last_target_polar_angle = -1000; - radius = 0; + radius = 0; energy_rotation_direction = ANTICLOCKWISE; - attack_distance = ATTACK_DISTANCE; - last_fans_cnt = 0; - last_armors_cnt = 0; + attack_distance = ATTACK_DISTANCE; + last_fans_cnt = 0; last_flow_strips_cnt = 0; last_flow_strip_fans_cnt = 0; - send_cnt = 0; - yaw_rotation = 0; - pitch_rotation = 0; - last_mark = 0; + send_cnt = 0; + yaw_rotation = 0; + pitch_rotation = 0; + last_mark = 0; - target_cnt = 0; - big_energy_shoot = false; - small_energy_shoot = false; - predict_rad = 20; + target_cnt = 0; + shoot = false; + guess_devide = 0; + startguessing = false; + predict_rad = 20; - fans.clear(); - armors.clear(); - centerRs.clear(); - flow_strips.clear(); - flow_strip_fans.clear(); - center_ROI.clear(); - target_armor.clear(); + fans.clear(); + armors.clear(); - fan_polar_angle.clear(); - armor_polar_angle.clear(); + fan_polar_angle.clear(); + armor_polar_angle.clear(); - all_armor_centers.clear(); + all_armor_centers.clear(); - clockwise_rotation_init_cnt = 0; - anticlockwise_rotation_init_cnt = 0; + clockwise_rotation_init_cnt = 0; + anticlockwise_rotation_init_cnt = 0; } - //---------------------------------------------------------------------------------------------------------------------- // 此函数对能量机关参数进行初始化 // --------------------------------------------------------------------------------------------------------------------- void Energy::initEnergyPartParam() { - gimble_energy_part_param_.GRAY_THRESH = 225; - gimble_energy_part_param_.SPLIT_GRAY_THRESH = 60; - gimble_energy_part_param_.FAN_GRAY_THRESH = 75; - gimble_energy_part_param_.ARMOR_GRAY_THRESH = 80; + gimble_energy_part_param_.GRAY_THRESH = 225; + gimble_energy_part_param_.SPLIT_GRAY_THRESH = 180; + gimble_energy_part_param_.FAN_GRAY_THRESH = 75; + gimble_energy_part_param_.ARMOR_GRAY_THRESH = 80; gimble_energy_part_param_.FAN_CONTOUR_AREA_MAX = 17000; gimble_energy_part_param_.FAN_CONTOUR_AREA_MIN = 0; @@ -82,6 +77,8 @@ void Energy::initEnergyPartParam() { gimble_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 60; gimble_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 4; gimble_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1; + gimble_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.8; + gimble_energy_part_param_.FAN_NON_ZERO_RATE_MIN = 0.48; gimble_energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 100000; gimble_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0; @@ -113,6 +110,8 @@ void Energy::initEnergyPartParam() { gimble_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1; gimble_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.55; gimble_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.25; + gimble_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.48; + gimble_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.25; gimble_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MAX = 100000; gimble_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 0; @@ -125,11 +124,11 @@ void Energy::initEnergyPartParam() { gimble_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.5; gimble_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 300; - - gimble_energy_part_param_.TWIN_ANGEL_MAX = 10; gimble_energy_part_param_.INTERSETION_CONTOUR_AREA_MIN = 60; + + chassis_energy_part_param_.GRAY_THRESH = 225; chassis_energy_part_param_.SPLIT_GRAY_THRESH = 60; chassis_energy_part_param_.FAN_GRAY_THRESH = 75; @@ -140,10 +139,12 @@ void Energy::initEnergyPartParam() { chassis_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 90; chassis_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 35; chassis_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 140; - chassis_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 60; + chassis_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 65; chassis_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 4; chassis_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1; chassis_energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN = 0.6; + chassis_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.8; + chassis_energy_part_param_.FAN_NON_ZERO_RATE_MIN = 0.48; chassis_energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 100000; chassis_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0; @@ -176,6 +177,8 @@ void Energy::initEnergyPartParam() { chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1; chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.55; chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.25; + chassis_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.48; + chassis_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.25; chassis_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MAX = 100000; chassis_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 0; @@ -196,29 +199,26 @@ void Energy::initEnergyPartParam() { } - - //---------------------------------------------------------------------------------------------------------------------- // 此函数对能量机关旋转方向进行初始化 // --------------------------------------------------------------------------------------------------------------------- void Energy::initRotation() { - if (target_polar_angle >= -180 && last_target_polar_angle >= -180 - && fabs(target_polar_angle - last_target_polar_angle) < 30) { - //target_polar_angle和last_target_polar_angle的初值均为1000，大于-180表示刚开始几帧不要 - //若两者比较接近，则说明没有切换目标，因此可以用于顺逆时针的判断 - if (target_polar_angle < last_target_polar_angle) clockwise_rotation_init_cnt++; - else if (target_polar_angle > last_target_polar_angle) anticlockwise_rotation_init_cnt++; - } + if (target_polar_angle >= -180 && last_target_polar_angle >= -180 + && fabs(target_polar_angle - last_target_polar_angle) < 30) { + //target_polar_angle和last_target_polar_angle的初值均为1000，大于-180表示刚开始几帧不要 + //若两者比较接近，则说明没有切换目标，因此可以用于顺逆时针的判断 + if (target_polar_angle < last_target_polar_angle) clockwise_rotation_init_cnt++; + else if (target_polar_angle > last_target_polar_angle) anticlockwise_rotation_init_cnt++; + } //由于刚开始圆心判断不准，角度变化可能计算有误，因此需要在角度正向或逆向变化足够大时才可确定是否为顺逆时针 - if (clockwise_rotation_init_cnt == 30) { + if (clockwise_rotation_init_cnt == 30) { energy_rotation_direction = CLOCKWISE;//顺时针变化30次，确定为顺时针 - cout << "rotation: " << energy_rotation_direction << endl; - energy_rotation_init = false; - } - else if (anticlockwise_rotation_init_cnt == 30) { + cout << "rotation: " << energy_rotation_direction << endl; + energy_rotation_init = false; + } else if (anticlockwise_rotation_init_cnt == 30) { energy_rotation_direction = ANTICLOCKWISE;//逆时针变化30次，确定为顺时针 - cout << "rotation: " << energy_rotation_direction << endl; - energy_rotation_init = false; - } + cout << "rotation: " << energy_rotation_direction << endl; + energy_rotation_init = false; + } last_target_polar_angle = target_polar_angle; } diff --git a/energy/src/energy/judge/judge_shoot.cpp b/energy/src/energy/judge/judge_shoot.cpp new file mode 100644 index 0000000..3d3a6db --- /dev/null +++ b/energy/src/energy/judge/judge_shoot.cpp @@ -0,0 +1,38 @@ +// +// Created by sun on 19-7-11. +// + +#include "energy/energy.h" +#include "log.h" + +using namespace std; +using namespace cv; + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于判断大符模式是否可以发弹 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::judgeBigShoot() { + if (abs(yaw_rotation - mcuData.curr_yaw) < 0.3 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.3) { + shoot = true; + isPredicting = false; + isGuessing = true; + startguessing = true; + gettimeofday(&time_start_guess, NULL); + LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!")); + } else + shoot = false; +} + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于判断小符模式是否可以发弹 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::judgeSmallShoot() { + if (fabs(yaw_rotation) < 0.3 && fabs(pitch_rotation) < 0.3) { + shoot = true; + isPredicting = false; + isGuessing = true; + } else + shoot = false; +} \ No newline at end of file diff --git a/energy/src/energy/judge/judge_time.cpp b/energy/src/energy/judge/judge_time.cpp new file mode 100644 index 0000000..ef9168d --- /dev/null +++ b/energy/src/energy/judge/judge_time.cpp @@ -0,0 +1,21 @@ +// +// Created by sun on 19-7-11. +// + + +#include "energy/energy.h" +#include "log.h" + +using namespace std; +using namespace cv; + + +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于判断guess模式是否超时 +// --------------------------------------------------------------------------------------------------------------------- +bool Energy::is_guessing_timeout() { + timeval cur_time; + gettimeofday(&cur_time, NULL); + return (cur_time.tv_sec - time_start_guess.tv_sec) * 1000.0 + + (cur_time.tv_usec - time_start_guess.tv_usec) / 1000.0 > 500; +}; \ No newline at end of file diff --git a/energy/src/energy/mark/mark.cpp b/energy/src/energy/mark/mark.cpp index 9280858..41293d5 100644 --- a/energy/src/energy/mark/mark.cpp +++ b/energy/src/energy/mark/mark.cpp @@ -8,7 +8,7 @@ using namespace std; using namespace cv; void Energy::writeDownMark() { - if(armors_cnt>=4 && fans_cnt>=3) { + if(fans.size()>=3) { FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "w"); if (fp) { fprintf(fp, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch); @@ -21,7 +21,7 @@ void Energy::writeDownMark() { fclose(fp_all); } } - if(armors_cnt==5){ + if(fans.size()==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); diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 2e46392..2343f5d 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -4,6 +4,7 @@ #include "energy/energy.h" #include "log.h" #include "options/options.h" +#include using namespace std; using namespace cv; @@ -16,93 +17,65 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) { if (chassis_src.empty()) runBig(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数 else if (isGimble) { - if (gimble_src.type() == CV_8UC3)cvtColor(gimble_src, gimble_src, COLOR_BGR2GRAY); energy_part_param_ = chassis_energy_part_param_; - fans.clear(); - armors.clear(); - centerRs.clear(); - flow_strip_fans.clear(); - flow_strips.clear(); - center_ROI.clear(); - target_armor.clear(); + clearAll(); + initImage(gimble_src); - threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY); - if (show_bin)imshow("bin", gimble_src); - armors_cnt = findArmor(gimble_src, last_armors_cnt); - flow_strip_fans_cnt = findFlowStripFan(gimble_src, last_flow_strip_fans_cnt); - if (flow_strip_fans_cnt == 1 && findTargetInFlowStripFan()) { - findCenterROI(gimble_src); - if (show_strip)showFlowStripFanContours("strip", gimble_src); - } else { - fans_cnt = findFan(gimble_src, last_fans_cnt); - if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0; - findTargetByIntersection(); - if (show_single) { - if (fans_cnt > 0)showFanContours("fan", gimble_src); - if (armors_cnt > 0)showArmorContours("armor", gimble_src); - } - if (show_both && (fans_cnt > 0 || armors_cnt > 0))showBothContours("both", gimble_src); - } - centerRs_cnt = findCenterR(gimble_src); - if (show_center && centerRs_cnt > 0)showCenterRContours("R", gimble_src); - if (isGimbleCentered()) { - getOrigin(); - initEnergy(); - destroyAllWindows(); - } + if (findArmors(gimble_src) < 1)return 0; + if (!findFlowStripFan(gimble_src))return 0; + if (!findTargetInFlowStripFan()) return 0; + if (!findCenterROI(gimble_src))return 0; + if (show_energy)showFlowStripFanContours("strip", gimble_src); + if (!findCenterR(gimble_src))return 0; + if (show_energy)showCenterRContours("R", gimble_src); + startChassis(); + getOrigin(); + initEnergy(); + destroyAllWindows(); } else if (isChassis) { - if (chassis_src.type() == CV_8UC3)cvtColor(chassis_src, chassis_src, COLOR_BGR2GRAY); energy_part_param_ = chassis_energy_part_param_; - fans.clear(); - armors.clear(); - centerRs.clear(); - flow_strip_fans.clear(); - flow_strips.clear(); - center_ROI.clear(); - target_armor.clear(); - -// imagePreprocess(chassis_src); -// if(show_split)imshow("img_preprocess", chassis_src); - + clearAll(); + initImage(chassis_src); changeMark(); if (isMark)return 0;//操作手强制手动标定origin_yaw和origin_pitch - threshold(chassis_src, chassis_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY); + if (show_process)imshow("bin", chassis_src); + if (findArmors(chassis_src) < 1)return 0; + if (!findFlowStripFan(chassis_src))return 0; + if (!findTargetInFlowStripFan()) return 0; + if (!findCenterROI(chassis_src))return 0; + if (show_energy)showFlowStripFanContours("strip", chassis_src); + if (!findCenterR(chassis_src))return 0; + if (show_energy)showCenterRContours("R", chassis_src); - armors_cnt = findArmor(chassis_src, last_armors_cnt); - flow_strip_fans_cnt = findFlowStripFan(chassis_src, last_flow_strip_fans_cnt); - if (flow_strip_fans_cnt == 1 && findTargetInFlowStripFan()) { - findCenterROI(chassis_src); - if (show_strip)showFlowStripFanContours("strip", chassis_src); - } else { - fans_cnt = findFan(chassis_src, last_fans_cnt); - if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0; - findTargetByIntersection(); - if (show_single) { - if (fans_cnt > 0)showFanContours("fan", gimble_src); - if (armors_cnt > 0)showArmorContours("armor", gimble_src); - } - if (show_both && (fans_cnt > 0 || armors_cnt > 0))showBothContours("both", gimble_src); - if (write_down)writeDownMark(); - } + findFans(chassis_src);showFanContours("fan",chassis_src); - centerRs_cnt = findCenterR(chassis_src); - if (show_center && centerRs_cnt > 0)showCenterRContours("R", chassis_src); - if (centerRs.size() != 1)return 0; - circle_center_point = centerRs.at(0).rect.center; - target_polar_angle = static_cast(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y), - (target_point.x - circle_center_point.x))); + getTargetPolarAngle(); + if (save_mark)writeDownMark(); if (energy_rotation_init) { initRotation(); return 0; } - - getPredictPoint(); - gimbleRotation(); - if (changeTarget())target_cnt++; - sendTarget(serial, yaw_rotation, pitch_rotation, target_cnt, big_energy_shoot); - - return 0; + if (isPredicting) { + getPredictPoint(); + gimbleRotation(); + judgeBigShoot(); + sendTarget(serial, yaw_rotation, pitch_rotation, shoot); + } else if (isGuessing) { + if (changeTarget() || is_guessing_timeout()) { + isPredicting = true; + isGuessing = false; + LOGM(STR_CTR(WORD_LIGHT_YELLOW, "Start Predicting!")); + } else { + findFans(chassis_src); + guessTarget(); + if (show_energy)showGuessTarget("guess", chassis_src); + target_point = guess_point; + getPredictPoint(); + gimbleRotation(); + sendTarget(serial, yaw_rotation, pitch_rotation, false); + } + } } return 0; } @@ -112,60 +85,54 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) { // 此函数为大能量机关模式主控制流函数，且步兵仅拥有云台摄像头 // --------------------------------------------------------------------------------------------------------------------- int Energy::runBig(cv::Mat &gimble_src) { - if (gimble_src.type() == CV_8UC3)cvtColor(gimble_src, gimble_src, COLOR_BGR2GRAY); - energy_part_param_ = gimble_energy_part_param_; - fans.clear(); - armors.clear(); - centerRs.clear(); - flow_strip_fans.clear(); - flow_strips.clear(); - center_ROI.clear(); - target_armor.clear(); -// imagePreprocess(gimble_src); -// if(show_split)imshow("img_preprocess", gimble_src); + energy_part_param_ = chassis_energy_part_param_; + clearAll(); + initImage(gimble_src); changeMark(); if (isMark)return 0;//操作手强制手动标定origin_yaw和origin_pitch threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY); - if (show_bin)imshow("bin", gimble_src); - armors_cnt = findArmor(gimble_src, last_armors_cnt); - flow_strip_fans_cnt = findFlowStripFan(gimble_src, last_flow_strip_fans_cnt); - if (flow_strip_fans_cnt == 1 && findTargetInFlowStripFan()) { - findCenterROI(gimble_src); - if (show_strip)showFlowStripFanContours("strip", gimble_src); - } else { - fans_cnt = findFan(gimble_src, last_fans_cnt); - if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0; - findTargetByIntersection(); - if (show_single) { - if (fans_cnt > 0)showFanContours("fan", gimble_src); - if (armors_cnt > 0)showArmorContours("armor", gimble_src); - } - if (show_both && (fans_cnt > 0 || armors_cnt > 0))showBothContours("both", gimble_src); - if (write_down)writeDownMark(); - } - centerRs_cnt = findCenterR(gimble_src); - if (show_center && centerRs_cnt > 0)showCenterRContours("R", gimble_src); - if (centerRs.size() != 1)return 0; - circle_center_point = centerRs.at(0).rect.center; - target_polar_angle = static_cast(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y), - (target_point.x - circle_center_point.x))); + if (show_process)imshow("bin", gimble_src); + if (findArmors(gimble_src) < 1)return 0; + if (!findFlowStripFan(gimble_src))return 0; + if (!findTargetInFlowStripFan()) return 0; + if (!findCenterROI(gimble_src))return 0; + if (show_energy)showFlowStripFanContours("strip", gimble_src); + if (!findCenterR(gimble_src))return 0; + if (show_energy)showCenterRContours("R", gimble_src); + getTargetPolarAngle(); if (energy_rotation_init) { initRotation(); getOrigin();//一旦确定风车旋向后，就开始移动云台，此时不再更新origin_yaw和origin_pitch return 0; } - getPredictPoint(); - gimbleRotation(); - if (changeTarget())target_cnt++; - sendTarget(serial, yaw_rotation, pitch_rotation, target_cnt, big_energy_shoot); + if (isPredicting) { + getPredictPoint(); + gimbleRotation(); + judgeBigShoot(); + sendTarget(serial, yaw_rotation, pitch_rotation, shoot); + } else if (isGuessing) { + if (changeTarget() || is_guessing_timeout()){ + isPredicting = true; + isGuessing = false; + LOGM(STR_CTR(WORD_LIGHT_YELLOW, "Start Predicting!")); + } else { + findFans(gimble_src); + guessTarget(); + if (show_energy)showGuessTarget("guess", gimble_src); + target_point = guess_point; + getPredictPoint(); + gimbleRotation(); + sendTarget(serial, yaw_rotation, pitch_rotation, false); + } + } return 0; } -//---------------------------------------------------------------------------------------------------------------------- +/*//---------------------------------------------------------------------------------------------------------------------- // 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头 // --------------------------------------------------------------------------------------------------------------------- int Energy::runSmall(cv::Mat &gimble_src) { @@ -176,8 +143,6 @@ int Energy::runSmall(cv::Mat &gimble_src) { centerRs.clear(); flow_strip_fans.clear(); flow_strips.clear(); - center_ROI.clear(); - target_armor.clear(); threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY); if (show_bin)imshow("bin", gimble_src); armors_cnt = findArmor(gimble_src, last_armors_cnt); @@ -187,7 +152,6 @@ int Energy::runSmall(cv::Mat &gimble_src) { if (show_strip)showFlowStripFanContours("strip", gimble_src); } else { fans_cnt = findFan(gimble_src, last_fans_cnt); - if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0; findTargetByIntersection(); if (show_single) { if (fans_cnt > 0)showFanContours("fan", gimble_src); @@ -198,7 +162,7 @@ int Energy::runSmall(cv::Mat &gimble_src) { centerRs_cnt = findCenterR(gimble_src); if (show_center && centerRs_cnt > 0)showCenterRContours("R", gimble_src); if (centerRs.size() != 1)return 0; - circle_center_point = centerRs.at(0).rect.center; + circle_center_point = centerRs.at(0).center; target_polar_angle = static_cast(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y), (target_point.x - circle_center_point.x))); getAimPoint(); @@ -206,7 +170,7 @@ int Energy::runSmall(cv::Mat &gimble_src) { sendTarget(serial, yaw_rotation, pitch_rotation, target_cnt, small_energy_shoot); return 0; -} +}*/ diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index c6ad954..51b9825 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -3,6 +3,8 @@ // #include "energy/energy.h" #include +#include "log.h" + using namespace std; @@ -10,7 +12,7 @@ using namespace std; //---------------------------------------------------------------------------------------------------------------------- // 此函数用于发送数据给主控板 // --------------------------------------------------------------------------------------------------------------------- -void Energy::sendTarget(Serial& serial, float x, float y, char change, char shoot){ +void Energy::sendTarget(Serial& serial, float x, float y, float z){ static short x_tmp, y_tmp, z_tmp; uint8_t buff[8]; x_tmp = static_cast(x * (32768 - 1) / 100); @@ -20,9 +22,10 @@ void Energy::sendTarget(Serial& serial, float x, float y, char change, char shoo buff[2] = static_cast((x_tmp >> 0) & 0xFF); buff[3] = static_cast((y_tmp >> 8) & 0xFF); buff[4] = static_cast((y_tmp >> 0) & 0xFF); - buff[5] = change; - buff[6] = shoot; + buff[5] = static_cast((z_tmp >> 8) & 0xFF); + buff[6] = static_cast((z_tmp >> 0) & 0xFF); buff[7] = 'e'; serial.WriteData(buff, sizeof(buff)); send_cnt+=1; +// LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send")); } \ No newline at end of file diff --git a/energy/src/energy/show/show.cpp b/energy/src/energy/show/show.cpp index 8a8bd97..9d85364 100644 --- a/energy/src/energy/show/show.cpp +++ b/energy/src/energy/show/show.cpp @@ -28,7 +28,7 @@ void Energy::showFanContours(std::string windows_name, const cv::Mat src) { for (const auto &fan : fans) { Point2f vertices[4]; //定义矩形的4个顶点 - fan.rect.points(vertices); //计算矩形的4个顶点 + fan.points(vertices); //计算矩形的4个顶点 for (int i = 0; i < 4; i++) line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 2); } @@ -55,7 +55,7 @@ void Energy::showArmorContours(std::string windows_name, const cv::Mat src) { for (const auto &armor : armors) { Point2f vertices[4]; //定义矩形的4个顶点 - armor.rect.points(vertices); //计算矩形的4个顶点 + armor.points(vertices); //计算矩形的4个顶点 for (int i = 0; i < 4; i++) line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 2); } @@ -81,21 +81,21 @@ void Energy::showBothContours(std::string windows_name, const cv::Mat src) { for (const auto &fan : fans) { Point2f vertices[4]; //定义矩形的4个顶点 - fan.rect.points(vertices); //计算矩形的4个顶点 + fan.points(vertices); //计算矩形的4个顶点 for (int i = 0; i < 4; i++) line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 4); } for (const auto &armor : armors) { Point2f vertices[4]; //定义矩形的4个顶点 - armor.rect.points(vertices); //计算矩形的4个顶点 + armor.points(vertices); //计算矩形的4个顶点 for (int i = 0; i < 4; i++){ - if(pointDistance(static_cast(armor.rect.center),target_point) < 5) + if(pointDistance(static_cast(armor.center),target_point) < 5) line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 4); else line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 4); } - cv::Point2f point = armor.rect.center; + cv::Point2f point = armor.center; cv::circle(image2show, point, 2, cv::Scalar(0, 0, 255));//在图像中画出特征点，2是圆的半径 } @@ -121,15 +121,10 @@ void Energy::showCenterRContours(std::string windows_name, const cv::Mat src) { image2show = src.clone(); } //cvtColor(image2show, image2show, COLOR_GRAY2RGB); - for (const auto ¢er_R : centerRs) - { - Point2f vertices[4]; //定义矩形的4个顶点 - center_R.rect.points(vertices); //计算矩形的4个顶点 - for (int i = 0; i < 4; i++) - line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 255), 2); - //cout << armor.rect.center << '\t' << armor.rect.angle << '\t'; - //cout << endl; - } + Point2f vertices[4]; //定义矩形的4个顶点 + centerR.points(vertices); //计算矩形的4个顶点 + for (int i = 0; i < 4; i++) + line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 255), 2); imshow(windows_name, image2show); } @@ -151,36 +146,55 @@ void Energy::showFlowStripFanContours(std::string windows_name, const cv::Mat sr { image2show = src.clone(); } - for (const auto &flow_strip_fan : flow_strip_fans) - { - Point2f vertices[4]; //定义矩形的4个顶点 - flow_strip_fan.rect.points(vertices); //计算矩形的4个顶点 - for (int i = 0; i < 4; i++) - line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(127, 127, 255), 2); - } - for (const auto &flow_strip : flow_strips) - { - Point2f vertices[4]; //定义矩形的4个顶点 - flow_strip.rect.points(vertices); //计算矩形的4个顶点 - for (int i = 0; i < 4; i++) - line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 255, 0), 2); - } + + Point2f strip_fan_vertices[4]; //定义矩形的4个顶点 + flow_strip_fan.points(strip_fan_vertices); //计算矩形的4个顶点for (int i = 0; i < 4; i++) + for (int i = 0; i < 4; i++) + line(image2show, strip_fan_vertices[i], strip_fan_vertices[(i + 1) % 4], Scalar(127, 127, 255), 2); + + + Point2f strip_vertices[4]; //定义矩形的4个顶点 + flow_strip.points(strip_vertices); //计算矩形的4个顶点 + for (int i = 0; i < 4; i++) + line(image2show, strip_vertices[i], strip_vertices[(i + 1) % 4], Scalar(0, 255, 0), 2); + for (const auto &armor : armors){ - if(pointDistance(armor.rect.center, target_point) < energy_part_param_.TWIN_POINT_MAX){ + if(pointDistance(armor.center, target_point) < energy_part_param_.TWIN_POINT_MAX){ Point2f vertices[4]; //定义矩形的4个顶点 - armor.rect.points(vertices); //计算矩形的4个顶点 + armor.points(vertices); //计算矩形的4个顶点 for (int i = 0; i < 4; i++) line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 2); } } - Point2f vertices[4]; //定义矩形的4个顶点 - if(center_ROI.size() > 0){ - center_ROI.at(0).rect.points(vertices); //计算矩形的4个顶点 - for (int i = 0; i < 4; i++) - line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 2); - } + + Point2f ROI_vertices[4]; //定义矩形的4个顶点 + center_ROI.points(ROI_vertices); //计算矩形的4个顶点 + for (int i = 0; i < 4; i++) + line(image2show, ROI_vertices[i], ROI_vertices[(i + 1) % 4], Scalar(0, 0, 255), 2); imshow(windows_name, image2show); } +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于显示猜测的下一个目标点 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::showGuessTarget(std::string windows_name, const cv::Mat src) { + if (src.empty())return; + static Mat image2show; + + if(src.type() == CV_8UC1) // 黑白图像 + { + cvtColor(src, image2show, COLOR_GRAY2RGB); + + } else if (src.type() == CV_8UC3) //RGB 彩色 + { + image2show = src.clone(); + } + cv::Point2f point = guess_point; + cv::circle(image2show, point, 4, cv::Scalar(0, 0, 255));//在图像中画出特征点，2是圆的半径 + + imshow(windows_name, image2show); +} + + diff --git a/energy/src/energy/tool/tool.cpp b/energy/src/energy/tool/tool.cpp index 601562c..9a8ee6d 100644 --- a/energy/src/energy/tool/tool.cpp +++ b/energy/src/energy/tool/tool.cpp @@ -9,10 +9,8 @@ #include #include +using namespace std; using namespace cv; -using std::cout; -using std::endl; -using std::vector; //---------------------------------------------------------------------------------------------------------------------- @@ -73,3 +71,69 @@ double Energy::pointDistance(cv::Point point_1, cv::Point point_2) { } +//---------------------------------------------------------------------------------------------------------------------- +// 计算角度所处的极坐标分区，共五个 +// --------------------------------------------------------------------------------------------------------------------- +int Energy::devide(float angle) { + if (angle < 0)angle += 360;//若angle小于0，说明当前角度范围为-180°~180° + int i = 0; + while (angle - 72 * i > 72)i++; + return i; +} + + +//---------------------------------------------------------------------------------------------------------------------- +// 计算直线上一点的横坐标 +// --------------------------------------------------------------------------------------------------------------------- +int Energy::linePointX(const cv::Point2f &p1, const cv::Point2f &p2, int y) { + return (p2.x - p1.x) / (p2.y - p1.y) * (y - p1.y) + p1.x; +} + + + +//---------------------------------------------------------------------------------------------------------------------- +// 计算旋转矩形内的两点占比 +// --------------------------------------------------------------------------------------------------------------------- + +double Energy::nonZeroRateOfRotateRect(const cv::Mat &bin, const cv::RotatedRect &rotatedRect) { + int cnt = 0; + cv::Point2f corners[4]; + rotatedRect.points(corners); + sort(corners, &corners[4], [](cv::Point2f p1, cv::Point2f p2) { + return p1.y < p2.y; + }); + for (int r = corners[0].y; r < corners[1].y; r++) { + auto start = min(linePointX(corners[0], corners[1], r), linePointX(corners[0], corners[2], r)) - 1; + auto end = max(linePointX(corners[0], corners[1], r), linePointX(corners[0], corners[2], r)) + 1; + if (start < 0)start = 0; + if (end > 640)end = 640; + for (int c = start; c < end; c++) { + if (bin.at(r, c)) { + cnt++; + } + } + } + for (int r = corners[1].y; r < corners[2].y; r++) { + auto start = min(linePointX(corners[0], corners[2], r), linePointX(corners[1], corners[3], r)) - 1; + auto end = max(linePointX(corners[0], corners[2], r), linePointX(corners[1], corners[3], r)) + 1; + if (start < 0)start = 0; + if (end > 640)end = 640; + for (int c = start; c < end; c++) { + if (bin.at(r, c)) { + cnt++; + } + } + } + for (int r = corners[2].y; r < corners[3].y; r++) { + auto start = min(linePointX(corners[1], corners[3], r), linePointX(corners[2], corners[3], r)) - 1; + auto end = max(linePointX(corners[1], corners[3], r), linePointX(corners[2], corners[3], r)) + 1; + if (start < 0)start = 0; + if (end > 640)end = 640; + for (int c = start; c < end; c++) { + if (bin.at(r, c)) { + cnt++; + } + } + } + return double(cnt) / rotatedRect.size.area(); +} \ No newline at end of file diff --git a/main.cpp b/main.cpp index 32fdd94..bf8b52e 100644 --- a/main.cpp +++ b/main.cpp @@ -27,7 +27,7 @@ using namespace std; mcu_data mcuData = { // 单片机端回传结构体 0, // 当前云台yaw角 0, // 当前云台pitch角 - ARMOR_STATE, // 当前状态，自瞄-大符-小符 + BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 ENEMY_RED, // 敌方颜色 @@ -61,8 +61,8 @@ int main(int argc, char *argv[]) { video_gimble = new CameraWrapper(0/*, "armor"*/); video_chassis = new CameraWrapper(1/*, "energy"*/); } else { - video_gimble = new VideoWrapper("/home/sun/项目/energy_video/energy_test.avi"); - video_chassis = new VideoWrapper("/home/sun/项目/energy_video/energy_test.avi"); + video_gimble = new VideoWrapper("/home/sun/项目/RM2019 能量机关视频/大能量机关（红+开灯）.mov"); + video_chassis = new VideoWrapper("/home/sun/项目/RM2019 能量机关视频/大能量机关（红+开灯）.mov"); } if (video_gimble->init()) { LOGM("video_gimble source initialization successfully."); @@ -100,6 +100,10 @@ int main(int argc, char *argv[]) { cout << "set" << endl; } ok = checkReconnect(video_gimble->read(gimble_src), video_chassis->read(chassis_src));//检查有几个摄像头 + resize(gimble_src,gimble_src,cv::Size(853,480)); + resize(chassis_src,chassis_src,cv::Size(853,480)); + gimble_src = gimble_src(Rect(106, 0, 640, 480)); + chassis_src = chassis_src(Rect(106, 0, 640, 480)); if (save_video) saveVideos(gimble_src, chassis_src);//保存视频 if (show_origin) showOrigin(gimble_src, chassis_src);//显示原始图像 energy.runBig(gimble_src, chassis_src);//击打大符 @@ -113,7 +117,7 @@ int main(int argc, char *argv[]) { ok = checkReconnect(video_gimble->read(gimble_src)); if (save_video) saveVideos(gimble_src); if (show_origin) showOrigin(gimble_src); - energy.runSmall(gimble_src); +// energy.runSmall(gimble_src); } else { // 自瞄模式 if(last_state != ARMOR_STATE){ ((CameraWrapper*)video_gimble)->changeBrightness(40); diff --git a/others/include/options/options.h b/others/include/options/options.h index a6e1c43..ed49689 100644 --- a/others/include/options/options.h +++ b/others/include/options/options.h @@ -15,13 +15,9 @@ extern bool run_with_camera; extern bool save_video; extern bool wait_uart; extern bool save_labelled_boxes; -extern bool show_bin; -extern bool show_split; -extern bool show_single; -extern bool show_both; -extern bool show_center; -extern bool show_strip; -extern bool write_down; +extern bool show_process; +extern bool show_energy; +extern bool save_mark; void process_options(int argc, char *argv[]); diff --git a/others/src/options/options.cpp b/others/src/options/options.cpp index 64cc585..0b7c7a5 100644 --- a/others/src/options/options.cpp +++ b/others/src/options/options.cpp @@ -14,13 +14,9 @@ bool run_with_camera = false; bool save_video = false; bool wait_uart = false; bool save_labelled_boxes = false; -bool show_bin = false; -bool show_split = false; -bool show_single = false; -bool show_both = false; -bool show_center = false; -bool show_strip = false; -bool write_down = false; +bool show_process = false; +bool show_energy = false; +bool save_mark = false; void process_options(int argc, char *argv[]) { if (argc >= 2) { @@ -54,18 +50,10 @@ void process_options(int argc, char *argv[]) { LOGM("Enable show light blobs"); show_origin = true; LOGM("Enable show origin"); - show_bin = true; - LOGM("Enable show binary image"); - show_split = true; - LOGM("Enable show split image"); - show_single = true; - LOGM("Enable show single image"); - show_both = true; - LOGM("Enable show both image"); - show_center = true; - LOGM("Enable show circle center"); - show_strip = true; - LOGM("Enable show strip"); + show_process = true; + LOGM("Enable show processed image"); + show_energy = true; + LOGM("Enable show energy part"); } else if (strcmp(argv[i], "--run-with-camera") == 0) { run_with_camera = true; LOGM("Run with camera!"); @@ -78,26 +66,14 @@ void process_options(int argc, char *argv[]) { } else if (strcmp(argv[i], "--save-labelled-boxes") == 0) { save_labelled_boxes = true; LOGM("labelled armor boxes will be saved!"); - } else if (strcmp(argv[i], "--show-bin") == 0) { - show_bin = true; - LOGM("Enable show binary image"); - } else if (strcmp(argv[i], "--show-split") == 0) { - show_split = true; - LOGM("Enable show split image"); - } else if (strcmp(argv[i], "--show-single") == 0) { - show_single = true; - LOGM("Enable show single image"); - } else if (strcmp(argv[i], "--show-both") == 0) { - show_both = true; - LOGM("Enable show both image"); - } else if (strcmp(argv[i], "--show-center") == 0) { - show_center = true; - LOGM("Enable show circle center"); - } else if (strcmp(argv[i], "--show-strip") == 0) { - show_strip = true; - LOGM("Enable show strip"); + } else if (strcmp(argv[i], "--show-process") == 0) { + show_process = true; + LOGM("Enable show processed image!"); + } else if (strcmp(argv[i], "--show-energy") == 0) { + show_energy= true; + LOGM("Enable show energy part!"); } else if (strcmp(argv[i], "--save-mark") == 0) { - write_down = true; + save_mark = true; LOGM("Write down mark"); } else { LOGW("Unknown option: %s. Use --help to see options.", argv[i]);