VSAG/amadeus_26_fb
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

energy changed

Browse Source
This commit is contained in:
sun
2019-07-11 21:49:44 +08:00
parent 9e8ebe7a32
commit a2f08e9674
24 changed files with 689 additions and 575 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
armor/src/armor_finder/searching_state/find_light_blobs.cpp
View File

@@ -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; auto end = max(linePointX(corners[0], corners[1], r), linePointX(corners[0], corners[2], r)) + 1;
if (start < 0 || end > 640) return 0; if (start < 0 || end > 640) return 0;
for (int c = start; c < end; c++) { for (int c = start; c < end; c++) {
if (bin.at<uint8_t>(c, r)) { if (bin.at<uint8_t>(r, c)) {
cnt++; 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; auto end = max(linePointX(corners[1], corners[3], r), linePointX(corners[2], corners[3], r)) + 1;
if (start < 0 || end > 640) return 0; if (start < 0 || end > 640) return 0;
for (int c = start; c < end; c++) { for (int c = start; c < end; c++) {
if (bin.at<uint8_t>(c, r)) { if (bin.at<uint8_t>(r, c)) {
cnt++; cnt++;
} }
} }
@@ -90,8 +90,8 @@ static bool isValidLightBlob(const cv::Mat &bin, const cv::RotatedRect &rect) {
return (lw_rate(rect) > 1.5) && return (lw_rate(rect) > 1.5) &&
// (rect.size.width*rect.size.height < 3000) && // (rect.size.width*rect.size.height < 3000) &&
(rect.size.width * rect.size.height > 1) && (rect.size.width * rect.size.height > 1) &&
// (nonZeroRateOfRotateRect_opt(bin, rect) > 0.8); (nonZeroRateOfRotateRect_opt(bin, rect) > 0.8);
(nonZeroRateOfRotateRect(bin, rect) > 0.8); // (nonZeroRateOfRotateRect(bin, rect) > 0.8);
} }
void get_blob_color(const cv::Mat &src, std::vector<LightBlob> &blobs) { void get_blob_color(const cv::Mat &src, std::vector<LightBlob> &blobs) {

188
energy/include/energy/energy.h
View File

@@ -23,128 +23,150 @@ using std::vector;
class Energy { class Energy {
public: public:
Energy(Serial &u, uint8_t &color);//构造函数，参数为串口和敌方颜色 Energy(Serial &u, uint8_t &color);//构造函数，参数为串口和敌方颜色
~Energy();//默认析构函数 ~Energy();//默认析构函数
int runBig(cv::Mat &gimble_src, cv::Mat &chassis_src); int runBig(cv::Mat &gimble_src, cv::Mat &chassis_src);
int runBig(cv::Mat &gimble_src); int runBig(cv::Mat &gimble_src);
int runSmall(cv::Mat &gimble_src); int runSmall(cv::Mat &gimble_src);
Serial &serial;//串口
void setEnergyRotationInit();//判断顺逆时针函数 Serial &serial;//串口
void extract(cv::Mat &src);//框取图像中的一块区域进行处理 void setEnergyRotationInit();//判断顺逆时针函数
void sendTarget(Serial& serial, float x, float y, char change, char shoot); void extract(cv::Mat &src);//框取图像中的一块区域进行处理
void sendTarget(Serial &serial, float x, float y, float z);
private: private:
EnergyPartParam energy_part_param_;//能量机关的参数设置 EnergyPartParam energy_part_param_;//能量机关的参数设置
EnergyPartParam gimble_energy_part_param_;//云台摄像头能量机关的参数设置 EnergyPartParam gimble_energy_part_param_;//云台摄像头能量机关的参数设置
EnergyPartParam chassis_energy_part_param_;//底盘摄像头能量机关的参数设置 EnergyPartParam chassis_energy_part_param_;//底盘摄像头能量机关的参数设置
bool isMark;//若操作手正在手动标定，则为true bool isMark;//若操作手正在手动标定，则为true
bool isGimble;//同时具有底盘和云台摄像头时，处于云台摄像头对心过程 bool isGimble;//同时具有底盘和云台摄像头时，处于云台摄像头对心过程
bool isChassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程 bool isChassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程
bool isGuessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标 bool isGuessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标
bool isPredicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打 bool isPredicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打
int fans_cnt;//图像中的扇叶个数 // int fans_cnt;//图像中的扇叶个数
int armors_cnt;//图像中的装甲板个数 // int armors_cnt;//图像中的装甲板个数
int centerRs_cnt;//图像中可能的风车中心字母R选区个数 // int centerRs_cnt;//图像中可能的风车中心字母R选区个数
int flow_strip_fans_cnt;//图像中的含流动条扇叶个数 // int flow_strip_fans_cnt;//图像中的含流动条扇叶个数
int flow_strips_cnt;//图像中的流动条个数 // int flow_strips_cnt;//图像中的流动条个数
int last_fans_cnt;//上一帧的扇叶个数 int last_fans_cnt;//上一帧的扇叶个数
int last_armors_cnt;//上一帧的装甲板个数 int last_flow_strip_fans_cnt;//上一帧的含流动条扇叶个数
int last_flow_strip_fans_cnt;//上一帧的含流动条扇叶个数 int last_flow_strips_cnt;//上一帧的流动条个数
int last_flow_strips_cnt;//上一帧的流动条个数 double radius;//大风车半径
double radius;//大风车半径 float target_polar_angle;//待击打装甲板的极坐标角度
float target_polar_angle;//待击打装甲板的极坐标角度
float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度 float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度
uint8_t &ally_color;//我方颜色 float guess_polar_angle;//猜测的下一个目标装甲板极坐标角度
int energy_rotation_direction;//风车旋转方向 float last_base_angle;//上一帧的各扇叶在0区（0°~72°）的基础角度
float attack_distance;//步兵与风车平面距离 uint8_t &ally_color;//我方颜色
int send_cnt;//向主控板发送的数据总次数 int energy_rotation_direction;//风车旋转方向
float yaw_rotation;//云台yaw轴应该转到的角度 float attack_distance;//步兵与风车平面距离
float pitch_rotation;//云台pitch轴应该转到的角度 int send_cnt;//向主控板发送的数据总次数
uint8_t last_mark;//用于记录上一帧操作手是否进行标定 float yaw_rotation;//云台yaw轴应该转到的角度
float pitch_rotation;//云台pitch轴应该转到的角度
uint8_t last_mark;//用于记录上一帧操作手是否进行标定
double predict_rad;//预测提前角 double predict_rad;//预测提前角
bool energy_rotation_init;//若仍在判断风车旋转方向，则为true bool energy_rotation_init;//若仍在判断风车旋转方向，则为true
int clockwise_rotation_init_cnt;//装甲板顺时针旋转次数 int clockwise_rotation_init_cnt;//装甲板顺时针旋转次数
int anticlockwise_rotation_init_cnt;//装甲板逆时针旋转次数 int anticlockwise_rotation_init_cnt;//装甲板逆时针旋转次数
float red_origin_yaw, red_origin_pitch;//红方的初始云台对心角度设定值 float red_origin_yaw, red_origin_pitch;//红方的初始云台对心角度设定值
float blue_origin_yaw, blue_origin_pitch;//蓝方的初始云台对心角度设定值 float blue_origin_yaw, blue_origin_pitch;//蓝方的初始云台对心角度设定值
float origin_yaw, origin_pitch;//初始的云台角度设定值 float origin_yaw, origin_pitch;//初始的云台角度设定值
float target_cnt;//用于记录寻找到的装甲板总数，该值变化则立即中断主控板发射进程，防止重复击打已点亮的装甲板 float target_cnt;//用于记录寻找到的装甲板总数，该值变化则立即中断主控板发射进程，防止重复击打已点亮的装甲板
bool save_new_mark;//若操作手进行过手动标定，则为true bool save_new_mark;//若操作手进行过手动标定，则为true
bool big_energy_shoot;//大符模式中，若为true，则要求主控板发弹 bool shoot;//若为true，则要求主控板发弹
bool small_energy_shoot;//小符模式中，若为true，则要求主控板发弹 int guess_devide;
std::vector<EnergyPart> fans;//图像中所有扇叶 bool startguessing;
std::vector<EnergyPart> armors;//图像中所有装甲板 timeval time_start_guess;
std::vector<EnergyPart> centerRs;//风车中心字母R的可能候选区 std::vector<cv::RotatedRect> fans;//图像中所有扇叶
std::vector<EnergyPart> flow_strips;//图像中所有流动条（理论上只有一个） std::vector<cv::RotatedRect> armors;//图像中所有可能装甲板（可能存在误识别）
std::vector<EnergyPart> flow_strip_fans;//图像中所有流动条所在扇叶（理论上只有一个） // std::vector<cv::RotatedRect> centerRs;//风车中心字母R的可能候选区
std::vector<EnergyPart> center_ROI;//风车中心候选区 // std::vector<cv::RotatedRect> flow_strips;//图像中所有流动条（理论上只有一个）
std::vector<EnergyPart> target_armor;//目标装甲板（理论上仅一个） // std::vector<cv::RotatedRect> flow_strip_fans;//图像中所有流动条所在扇叶（理论上只有一个）
// std::vector<cv::RotatedRect> center_ROI;//风车中心候选区
// std::vector<cv::RotatedRect> 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 circle_center_point;//风车圆心坐标
cv::Point target_point;//目标装甲板中心坐标 cv::Point target_point;//目标装甲板中心坐标
cv::Point last_target_point;//上一帧目标装甲板中心坐标 cv::Point last_target_point;//上一帧目标装甲板中心坐标
cv::Point predict_point;//预测的击打点坐标 cv::Point guess_point;
std::vector<float>fan_polar_angle;//当前帧所有扇叶的极坐标角度 cv::Point predict_point;//预测的击打点坐标
std::vector<float>armor_polar_angle;//当前帧所有装甲板的极坐标角度 std::vector<float> fan_polar_angle;//当前帧所有扇叶的极坐标角度
std::vector<float> armor_polar_angle;//当前帧所有装甲板的极坐标角度
std::vector<cv::Point> all_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算 std::vector<cv::Point> all_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算
cv::Mat src_blue, src_red, src_green;//通道分离中的三个图像通道 cv::Mat src_blue, src_red, src_green;//通道分离中的三个图像通道
void initEnergy();//能量机关初始化 void initEnergy();//能量机关初始化
void initEnergyPartParam();//能量机关参数初始化 void initEnergyPartParam();//能量机关参数初始化
void initRotation();//顺逆时针初始化 void initRotation();//顺逆时针初始化
void changeMode();//猜测模式与击打模式切换 void clearAll();//清空各vector
bool isGimbleCentered();//判断云台摄像头对心是否完成 void initImage(cv::Mat &src);//图像预处理
int findFan(const cv::Mat src, int &last_fans_cnt);//寻找图中所有扇叶 void startChassis();//从云台摄像头对心状态进入底盘摄像头击打状态
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);//寻找图中的流动条所在扇叶
bool isValidFanContour(const vector<cv::Point> &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<cv::Point> &fan_contour);//扇叶矩形尺寸要求
bool isValidArmorContour(const vector<cv::Point> &armor_contour);//装甲板矩形尺寸要求 bool isValidArmorContour(const vector<cv::Point> &armor_contour);//装甲板矩形尺寸要求
bool isValidCenterRContour(const vector<cv::Point> &center_R_contour);//风车中心选区尺寸要求 bool isValidCenterRContour(const vector<cv::Point> &center_R_contour);//风车中心选区尺寸要求
bool isValidFlowStripContour(const vector<cv::Point> &flow_strip_contour);//流动条扇叶矩形尺寸要求 bool isValidFlowStripContour(const vector<cv::Point> &flow_strip_contour);//流动条扇叶矩形尺寸要求
bool isValidFlowStripFanContour(const vector<cv::Point> &flow_strip_fan_contour);//流动条扇叶矩形尺寸要求 bool isValidFlowStripFanContour(cv::Mat &src, const vector<cv::Point> &flow_strip_fan_contour);//流动条扇叶矩形尺寸要求
void showFanContours(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 showArmorContours(std::string windows_name, const cv::Mat src);//显示装甲板
void showBothContours(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 showCenterRContours(std::string windows_name, const cv::Mat src);//显示风车中心候选区R
void showFlowStripFanContours(std::string windows_name, const cv::Mat src);//显示流动条所在扇叶 void showFlowStripFanContours(std::string windows_name, const cv::Mat src);//显示流动条所在扇叶
void showGuessTarget(std::string windows_name, const cv::Mat src);//显示猜测点位
void getFanPolarAngle();//获取扇叶极坐标角度 void getOrigin();//获得云台对心所需角度
void getArmorPolarAngle();//获取装甲板极坐标角度 void guessTarget();//获得猜测击打点位
void getAllArmorCenters();//记录所有装甲板中心坐标
void getOrigin();//获得云台对心所需角度
void circleLeastFit();//利用所有记录的装甲板中心最小二乘法计算圆心和半径 void circleLeastFit();//利用所有记录的装甲板中心最小二乘法计算圆心和半径
void findTargetByPolar();//通过极坐标角度匹配获取目标装甲板的极坐标角度和装甲板中心坐标 void findTargetByPolar();//通过极坐标角度匹配获取目标装甲板的极坐标角度和装甲板中心坐标
void findTargetByIntersection();//通过面积重合度匹配获取目标装甲板的极坐标角度和装甲板中心坐标 void findTargetByIntersection();//通过面积重合度匹配获取目标装甲板的极坐标角度和装甲板中心坐标
bool findTargetInFlowStripFan();//在已发现的流动条区域中寻找待击打装甲板 bool findTargetInFlowStripFan();//在已发现的流动条区域中寻找待击打装甲板
void rotate();//获取预测点位 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);//计算两点距离 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 getPredictPoint();//获取预测点位
void getAimPoint();//通过自瞄逻辑计算点位 void getAimPoint();//通过自瞄逻辑计算点位
bool changeTarget();//判断目标是否改变 bool changeTarget();//判断目标是否改变
void changeMark();//操作手手动修改标定值 void changeMark();//操作手手动修改标定值
void gimbleRotation();//计算云台旋转角度 void gimbleRotation();//计算云台旋转角度
void splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red);//拜耳阵列分离 void judgeBigShoot();//判断大符模式是否可以发弹
void imagePreprocess(cv::Mat &src);//图像通道分离 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 hsvFilter(cv::Mat &src);//此函数通过HSV颜色空间对图像纯度进行限制，进行滤光
void StructingElementClose(cv::Mat &src,int length, int width);//闭运算 void StructingElementClose(cv::Mat &src, int length, int width);//闭运算
void StructingElementErodeDilate(cv::Mat &src);//腐蚀和膨胀 void StructingElementErodeDilate(cv::Mat &src);//腐蚀和膨胀
void StructingElementDilateErode(cv::Mat &src);//腐蚀和膨胀
}; };

30
energy/include/energy/param_struct_define.h
View File

@@ -12,24 +12,6 @@ using std::vector;
//----------------------------------------------------------------------------------------------------------------------
// 此结构体为能量机关模块的结构体，用于寻找矩形轮廓
// ---------------------------------------------------------------------------------------------------------------------
struct EnergyPart {
cv::RotatedRect rect;
float angle;
vector<cv::Point> contour;
explicit EnergyPart(vector<cv::Point> &c) : contour(c) {
rect = cv::minAreaRect(c);
angle = cv::minAreaRect(c).angle;
};
EnergyPart(cv::RotatedRect rect=cv::RotatedRect(), float angle=0, vector<cv::Point> contour=vector<cv::Point>()):rect(rect),angle(angle),contour(contour){};
};
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此结构体包括能量机关参数 // 此结构体包括能量机关参数
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
@@ -48,6 +30,8 @@ struct EnergyPartParam {
float FAN_CONTOUR_HW_RATIO_MAX;//扇叶长宽比最大值 float FAN_CONTOUR_HW_RATIO_MAX;//扇叶长宽比最大值
float FAN_CONTOUR_HW_RATIO_MIN;//扇叶长宽比最小值 float FAN_CONTOUR_HW_RATIO_MIN;//扇叶长宽比最小值
float FAN_CONTOUR_AREA_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_MAX;//装甲板面积最大值
long ARMOR_CONTOUR_AREA_MIN;//装甲板面积最小值 long ARMOR_CONTOUR_AREA_MIN;//装甲板面积最小值
@@ -78,8 +62,10 @@ struct EnergyPartParam {
long FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX;//流动条扇叶（待击打）宽边长度最大值 long FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX;//流动条扇叶（待击打）宽边长度最大值
float FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX;//流动条扇叶（待击打）长宽比最大值 float FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX;//流动条扇叶（待击打）长宽比最大值
float FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN;//流动条扇叶（待击打）长宽比最小值 float FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN;//流动条扇叶（待击打）长宽比最小值
float FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX;//装甲板轮廓占旋转矩形面积比最小值 float FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX;//流动条扇叶轮廓占旋转矩形面积比最小值
float FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN;//装甲板轮廓占旋转矩形面积比最小值 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_MAX;//流动条（待击打）面积最大值
long FLOW_STRIP_CONTOUR_AREA_MIN;//流动条（待击打）面积最小值 long FLOW_STRIP_CONTOUR_AREA_MIN;//流动条（待击打）面积最小值
@@ -89,10 +75,10 @@ struct EnergyPartParam {
long FLOW_STRIP_CONTOUR_WIDTH_MAX;//流动条（待击打）宽边长度最大值 long FLOW_STRIP_CONTOUR_WIDTH_MAX;//流动条（待击打）宽边长度最大值
float FLOW_STRIP_CONTOUR_HW_RATIO_MAX;//流动条（待击打）长宽比最大值 float FLOW_STRIP_CONTOUR_HW_RATIO_MAX;//流动条（待击打）长宽比最大值
float FLOW_STRIP_CONTOUR_HW_RATIO_MIN;//流动条（待击打）长宽比最小值 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 FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN;//流动条占旋转矩形面积比最小值
float TWIN_ANGEL_MAX;//扇叶和装甲板匹配时极坐标角度差的最大值 float TWIN_ANGEL_MAX;//两个理论上相等的角度在计算时具有的可能最大差值
long INTERSETION_CONTOUR_AREA_MIN;//扇叶与装甲板匹配时的最小重合面积 long INTERSETION_CONTOUR_AREA_MIN;//扇叶与装甲板匹配时的最小重合面积
long TARGET_CHANGE_DISTANCE_MAX;//目标未更改时，目标装甲板中心与原目标装甲板中心的距离变化最大值 long TARGET_CHANGE_DISTANCE_MAX;//目标未更改时，目标装甲板中心与原目标装甲板中心的距离变化最大值

10
energy/src/energy/calibrate/split.cpp
View File

@@ -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)); resize(channels.at(2), src_red, Size(src.size().width/2, src.size().height/2));
if(ally_color == ALLY_RED) if(ally_color == ALLY_RED)
{ {
src = src_red-src_blue; // src = src_red-src_blue;
//src=src_red; src=src_red;
}else if(ally_color == ALLY_BLUE){ }else if(ally_color == ALLY_BLUE){
src = src_blue-src_red; // src = src_blue-src_red;
//src=src_blue; src=src_blue;
} }
} }
cv::resize(src, src, cv::Size(src.size().width * 2, src.size().height * 2), 2); 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);
} }

38
energy/src/energy/calibrate/structing.cpp
View File

@@ -25,27 +25,37 @@ void Energy::StructingElementClose(cv::Mat &src,int length, int width){
// 此函数对图像进行腐蚀与膨胀操作 // 此函数对图像进行腐蚀与膨胀操作
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::StructingElementErodeDilate(cv::Mat &src) { void Energy::StructingElementErodeDilate(cv::Mat &src) {
cv::Mat src_out, src_out_out; Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(8, 6));
Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(3, 3));
// Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(3, 4));
Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 1)); 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_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); erode(src,src, element_erode_1);
// imshow("erode_1", src); // imshow("erode_1", src);
dilate(src, src, element_dilate_1);
// imshow("dilate_1", src);
dilate(src, src, element_dilate_2); dilate(src, src, element_dilate_2);
// imshow("dilate_2", src); // imshow("dilate_2", src);
dilate(src, src, element_dilate_3);
// imshow("dilate_3", src);
erode(src,src, element_erode_2); erode(src,src, element_erode_2);
// imshow("erode_2", src); // 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);
}

28
energy/src/energy/change/mode_change.cpp
View File

@@ -7,31 +7,11 @@ using namespace std;
using namespace cv; 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() { void Energy::startChassis(){
if (centerRs.size() == 1) { isChassis = true;
circle_center_point = centerRs.at(0).rect.center; isGimble = false;
isGimble = false;
isChassis = true;
return true;
}
return false;
} }

163
energy/src/energy/find/energy_finder.cpp
View File

@@ -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; if (src.empty())return 0;
static Mat src_bin; static Mat src_bin;
src_bin = src.clone(); src_bin = src.clone();
// threshold(src, src_bin, energy_part_param_.FAN_GRAY_THRESH, 255, THRESH_BINARY);
if (src.type() == CV_8UC3) { if (src.type() == CV_8UC3) {
cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道 cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
} }
std::vector<vector<Point> > fan_contours; std::vector<vector<Point> > fan_contours;
StructingElementClose(src_bin, 6, 6);//图像膨胀，防止图像断开并更方便寻找 StructingElementErodeDilate(src_bin);//图像膨胀，防止图像断开并更方便寻找
// imshow("fan struct",src_bin); // imshow("fan struct",src_bin);
findContours(src_bin, fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); findContours(src_bin, fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
for (auto &fan_contour : fan_contours) { for (auto &fan_contour : fan_contours) {
if (!isValidFanContour(fan_contour)) { if (!isValidFanContour(src_bin, fan_contour)) {
continue; continue;
} }
fans.emplace_back(fan_contour); fans.emplace_back(cv::minAreaRect(fan_contour));
// RotatedRect cur_rect = minAreaRect(fan_contour); // RotatedRect cur_rect = minAreaRect(fan_contour);
// Size2f cur_size = cur_rect.size; // Size2f cur_size = cur_rect.size;
// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; // 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; // float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
// if(length>20&&width>20){ // if(length>20&&width>20){
// cout<<cur_rect.center; // fans.emplace_back(cv::minAreaRect(fan_contour));
// fans.emplace_back(fan_contour);
// cout<<"fan area: "<<length<<'\t'<<width<<endl; // cout<<"fan area: "<<length<<'\t'<<width<<endl;
// cout<<nonZeroRateOfRotateRect(src_bin,cur_rect)<<endl;
// } // }
} }
if (fans.size() < last_fans_cnt) { if (fans.size() < last_fans_cnt) {
@@ -53,11 +52,10 @@ int Energy::findFan(const cv::Mat src, int &last_fans_cnt) {
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于寻找图像内所有的大风车装甲板模块 // 此函数用于寻找图像内所有的大风车装甲板模块
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
int Energy::findArmor(const cv::Mat src, int &last_armors_cnt) { int Energy::findArmors(const cv::Mat src) {
if (src.empty())return 0; if (src.empty())return 0;
static Mat src_bin; static Mat src_bin;
src_bin = src.clone(); src_bin = src.clone();
// threshold(src, src_bin, energy_part_param_.ARMOR_GRAY_THRESH, 255, THRESH_BINARY);
if (src.type() == CV_8UC3) { if (src.type() == CV_8UC3) {
cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道 cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
} }
@@ -86,23 +84,18 @@ int Energy::findArmor(const cv::Mat src, int &last_armors_cnt) {
if (!isValidArmorContour(armor_contour)) { if (!isValidArmorContour(armor_contour)) {
continue; continue;
} }
armors.emplace_back(armor_contour); armors.emplace_back(cv::minAreaRect(armor_contour));
// RotatedRect cur_rect = minAreaRect(armor_contour); // RotatedRect cur_rect = minAreaRect(armor_contour);
// Size2f cur_size = cur_rect.size; // Size2f cur_size = cur_rect.size;
// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; // 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; // float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
// if(length>10&&width>5){ // if(length>10&&width>5){
// armors.emplace_back(armor_contour); // armors.emplace_back(cv::minAreaRect(armor_contour));
// cout<<"armor area: "<<length<<'\t'<<width<<'\t'<<cur_rect.center<<endl; // cout<<"armor area: "<<length<<'\t'<<width<<'\t'<<cur_rect.center<<endl;
// } // }
} }
if (armors.size() < last_armors_cnt) {
last_armors_cnt = static_cast<int>(armors.size());
return -1;//寻找到的装甲板比上一帧少，说明该帧有误，返回-1
}
last_armors_cnt = static_cast<int>(armors.size());
return static_cast<int>(armors.size()); return static_cast<int>(armors.size());
} }
@@ -110,11 +103,10 @@ int Energy::findArmor(const cv::Mat src, int &last_armors_cnt) {
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于寻找图像内大风车中心字母“R” // 此函数用于寻找图像内大风车中心字母“R”
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
int Energy::findCenterR(const cv::Mat src) { bool Energy::findCenterR(const cv::Mat src) {
if (src.empty())return 0; if (src.empty())return false;
static Mat src_bin; static Mat src_bin;
src_bin = src.clone(); src_bin = src.clone();
// threshold(src, src_bin, energy_part_param_.ARMOR_GRAY_THRESH, 255, THRESH_BINARY);
if (src.type() == CV_8UC3) { if (src.type() == CV_8UC3) {
cvtColor(src_bin, src_bin, CV_BGR2GRAY); cvtColor(src_bin, src_bin, CV_BGR2GRAY);
} }
@@ -126,79 +118,74 @@ int Energy::findCenterR(const cv::Mat src) {
if (!isValidCenterRContour(center_R_contour)) { if (!isValidCenterRContour(center_R_contour)) {
continue; 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); // RotatedRect cur_rect = minAreaRect(center_R_contour);
// Size2f cur_size = cur_rect.size; // Size2f cur_size = cur_rect.size;
// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; // 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; // float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
// if(length>10 && width>5){ // if(length>10 && width>5){
// centerRs.emplace_back(center_R_contour); // centerRs.emplace_back(cv::minAreaRect(center_R_contour));
// cout<<"center R area: "<<length<<'\t'<<width<<'\t'<<cur_rect.center<<endl; // cout<<"center R area: "<<length<<'\t'<<width<<'\t'<<cur_rect.center<<endl;
// } // }
// cout<<"armor area: "<<length<<'\t'<<width<<endl;
} }
if (centerRs.size() == 0) return -1;//未找到合适的中心候选区，说明该帧有误，返回-1 return false;
return static_cast<int>(centerRs.size());
} }
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断找到的矩形候选区是否为含流动条的扇叶 // 此函数用于判断找到的矩形候选区是否为含流动条的扇叶
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
int Energy::findFlowStripFan(const cv::Mat src, int &last_flow_strip_fans_cnt) { bool Energy::findFlowStripFan(const cv::Mat src) {
if (src.empty())return 0; if (src.empty())return false;
static Mat src_bin; static Mat src_bin;
src_bin = src.clone(); src_bin = src.clone();
// threshold(src, src_bin, energy_part_param_.FAN_GRAY_THRESH, 255, THRESH_BINARY);
if (src.type() == CV_8UC3) { if (src.type() == CV_8UC3) {
cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道 cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
} }
std::vector<vector<Point> > flow_strip_fan_contours; std::vector<vector<Point> > flow_strip_fan_contours;
StructingElementClose(src_bin, 6, 6);//图像膨胀，防止图像断开并更方便寻找 StructingElementErodeDilate(src_bin);//图像膨胀，防止图像断开并更方便寻找
// imshow("flow strip fan struct", src_bin); imshow("flow strip fan struct", src_bin);
findContours(src_bin, flow_strip_fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); findContours(src_bin, flow_strip_fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
for (auto &flow_strip_fan_contour : flow_strip_fan_contours) { 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; continue;
} }
flow_strip_fan = cv::minAreaRect(flow_strip_fan_contour);
flow_strip_fans.emplace_back(flow_strip_fan_contour);
// RotatedRect cur_rect = minAreaRect(flow_strip_fan_contour); // RotatedRect cur_rect = minAreaRect(flow_strip_fan_contour);
// Size2f cur_size = cur_rect.size; // Size2f cur_size = cur_rect.size;
// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; // 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; // float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
// if(length>20&&width>20){ // if (length > 10 && width > 10) {
// cout<<cur_rect.center; // cout << cur_rect.center;
// flow_strip_fans.emplace_back(flow_strip_fan_contour); // flow_strip_fan = cv::minAreaRect(flow_strip_fan_contour);
// cout<<"flow strip fan area: "<<length<<'\t'<<width<<endl; // cout << "flow strip fan area: " << length << '\t' << width << endl;
// } // }
}
if (flow_strip_fans.size() < last_flow_strip_fans_cnt) { return true;
last_flow_strip_fans_cnt = static_cast<int>(flow_strip_fans.size());
return -1;//寻找到的流动条扇叶比上一帧少，说明该帧有误，返回-1
} }
last_flow_strip_fans_cnt = static_cast<int>(flow_strip_fans.size()); return false;
return static_cast<int>(flow_strip_fans.size());
} }
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于寻找流动条 // 此函数用于寻找流动条
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
int Energy::findFlowStrip(const cv::Mat src, int &last_flow_strips_cnt) { bool Energy::findFlowStrip(const cv::Mat src) {
imshow("1", src);
if (src.empty())return 0; if (src.empty())return false;
cv::Mat src_bin; cv::Mat src_bin;
src_bin = src.clone(); src_bin = src.clone();
if (src.type() == CV_8UC3) { if (src.type() == CV_8UC3) {
cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道 cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
} }
StructingElementClose(src_bin, 6, 6);//图像膨胀，防止图像断开并更方便寻找 StructingElementErodeDilate(src_bin);//图像膨胀，防止图像断开并更方便寻找
// imshow("flow strip struct", src_bin); // imshow("flow strip struct", src_bin);
std::vector<vector<Point> > flow_strip_contours; std::vector<vector<Point> > flow_strip_contours;
@@ -208,23 +195,19 @@ int Energy::findFlowStrip(const cv::Mat src, int &last_flow_strips_cnt) {
if (!isValidFlowStripContour(flow_strip_contour)) { if (!isValidFlowStripContour(flow_strip_contour)) {
continue; continue;
} }
flow_strips.emplace_back(flow_strip_contour); flow_strip = cv::minAreaRect(flow_strip_contour);
// RotatedRect cur_rect = minAreaRect(flow_strip_contour); // RotatedRect cur_rect = minAreaRect(flow_strip_contour);
// Size2f cur_size = cur_rect.size; // Size2f cur_size = cur_rect.size;
// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; // 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; // float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
// if(length>40&&width>5){ // if (length > 40 && width > 5) {
// cout<<cur_rect.center<<endl; // cout << cur_rect.center << endl;
// flow_strips.emplace_back(flow_strip_contour); // flow_strip = cv::minAreaRect(flow_strip_contour);
// cout<<"flow strip fan area: "<<length<<'\t'<<width<<endl; // cout << "flow strip fan area: " << length << '\t' << width << endl;
// } // }
return true;
} }
if (flow_strips.size() < last_flow_strips_cnt) { return false;
last_flow_strips_cnt = static_cast<int>(flow_strips.size());
return -1;//寻找到的流动条比上一帧少，说明该帧有误，返回-1
}
last_flow_strips_cnt = static_cast<int>(flow_strips.size());
return static_cast<int>(flow_strips.size());
} }
@@ -232,36 +215,34 @@ int Energy::findFlowStrip(const cv::Mat src, int &last_flow_strips_cnt) {
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于框取中心R的寻找范围 // 此函数用于框取中心R的寻找范围
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::findCenterROI(const cv::Mat src) { bool Energy::findCenterROI(const cv::Mat src) {
cv::Mat src_mask = src.clone(); cv::Mat src_mask = src.clone();
target_armor.at(0).rect.size.height *= 1.3; target_armor.size.height *= 1.3;
target_armor.at(0).rect.size.width *= 1.3; target_armor.size.width *= 1.3;
Point2f vertices[4]; Point2f vertices[4];
vector<Point2f> mask_rect; vector<Point2f> mask_rect;
target_armor.at(0).rect.points(vertices); //计算矩形的4个顶点 target_armor.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++) for (int i = 0; i < 4; i++)
line(src_mask, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 0), 15); line(src_mask, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 0), 15);
// imshow("fill", src_mask); // imshow("fill", src_mask);
flow_strips_cnt = findFlowStrip(src_mask, last_flow_strips_cnt); if (!findFlowStrip(src_mask))return false;
float length = target_armor.at(0).rect.size.height > target_armor.at(0).rect.size.width ? float length = target_armor.size.height > target_armor.size.width ?
target_armor.at(0).rect.size.height : target_armor.at(0).rect.size.width; target_armor.size.height : target_armor.size.width;
if (flow_strips_cnt > 0 && target_armor.size() > 0) {
for (auto flow_strip: flow_strips) { Point2f p2p(flow_strip.center.x - target_point.x,
Point2f p2p(flow_strip.rect.center.x - target_point.x, flow_strip.center.y - target_point.y);
flow_strip.rect.center.y - target_point.y); p2p = p2p / pointDistance(flow_strip.center, target_point);//单位化
p2p = p2p / pointDistance(flow_strip.rect.center, target_point);//单位化 center_ROI = cv::RotatedRect(cv::Point2f(flow_strip.center + p2p * length * 1.4),
center_ROI.emplace_back( Size2f(length * 1.2, length * 1.2), -90);
EnergyPart(cv::RotatedRect(cv::Point2f(flow_strip.rect.center + p2p * length * 1.5), return true;
Size2f(length, length), -90)));
}
}
} }
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断找到的矩形候选区是否为扇叶 // 此函数用于判断找到的矩形候选区是否为扇叶
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
bool Energy::isValidFanContour(const vector<cv::Point> &fan_contour) { bool Energy::isValidFanContour(cv::Mat &src, const vector<cv::Point> &fan_contour) {
double cur_contour_area = contourArea(fan_contour); double cur_contour_area = contourArea(fan_contour);
if (cur_contour_area > energy_part_param_.FAN_CONTOUR_AREA_MAX || if (cur_contour_area > energy_part_param_.FAN_CONTOUR_AREA_MAX ||
cur_contour_area < energy_part_param_.FAN_CONTOUR_AREA_MIN) { cur_contour_area < energy_part_param_.FAN_CONTOUR_AREA_MIN) {
@@ -287,8 +268,13 @@ bool Energy::isValidFanContour(const vector<cv::Point> &fan_contour) {
return false; return false;
//长宽比不合适 //长宽比不合适
} }
if (cur_contour_area / cur_size.area() < energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN) // if (cur_contour_area / cur_size.area() < energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN)
return false;//轮廓对矩形的面积占有率不合适 // 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; return true;
} }
@@ -362,7 +348,7 @@ bool Energy::isValidCenterRContour(const vector<cv::Point> &center_R_contour) {
if (cur_contour_area / cur_size.area() < energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN) if (cur_contour_area / cur_size.area() < energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN)
return false;//轮廓对矩形的面积占有率不合适 return false;//轮廓对矩形的面积占有率不合适
std::vector<cv::Point2f> intersection; std::vector<cv::Point2f> 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) { contourArea(intersection) < energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN) {
return false; return false;
} }
@@ -373,7 +359,7 @@ bool Energy::isValidCenterRContour(const vector<cv::Point> &center_R_contour) {
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断找到的矩形候选区是否为流动条扇叶 // 此函数用于判断找到的矩形候选区是否为流动条扇叶
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
bool Energy::isValidFlowStripFanContour(const vector<cv::Point> &flow_strip_fan_contour) { bool Energy::isValidFlowStripFanContour(cv::Mat &src, const vector<cv::Point> &flow_strip_fan_contour) {
double cur_contour_area = contourArea(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 || 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) { cur_contour_area < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN) {
@@ -402,11 +388,15 @@ bool Energy::isValidFlowStripFanContour(const vector<cv::Point> &flow_strip_fan_
return false; return false;
//长宽比不合适 //长宽比不合适
} }
if (cur_contour_area / cur_size.area() < energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN // 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) // || cur_contour_area / cur_size.area() > energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX)
//如果ROI为空、或者选区与ROI无重合面积、或者有重合面积但重合面积太小 // // 轮廓对矩形的面积占有率不合适
// 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 false;
//轮廓对矩形的面积占有率不合适 //亮点占比不合格
return true; return true;
} }
@@ -445,8 +435,7 @@ bool Energy::isValidFlowStripContour(const vector<cv::Point> &flow_strip_contour
if (cur_contour_area / cur_size.area() < energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN) if (cur_contour_area / cur_size.area() < energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN)
return false;//轮廓对矩形的面积占有率不合适 return false;//轮廓对矩形的面积占有率不合适
std::vector<cv::Point2f> intersection; std::vector<cv::Point2f> intersection;
if (flow_strip_fans.size() > 0 && if (rotatedRectangleIntersection(cur_rect, flow_strip_fan, intersection) != 0) {
rotatedRectangleIntersection(cur_rect, flow_strip_fans.at(0).rect, intersection) != 0) {
if (contourArea(intersection) < energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) { if (contourArea(intersection) < energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) {
return false; return false;
} }

30
energy/src/energy/find/target_finder.cpp
View File

@@ -18,7 +18,7 @@ void Energy::findTargetByPolar() {
if (fan_polar_angle.empty()) { if (fan_polar_angle.empty()) {
target_polar_angle = armor_polar_angle.at(0);//视野中没有扇叶，说明在击打第一个装甲板 target_polar_angle = armor_polar_angle.at(0);//视野中没有扇叶，说明在击打第一个装甲板
for (const auto &armor : armors) { for (const auto &armor : armors) {
target_point = armor.rect.center; target_point = armor.center;
} }
return; return;
} }
@@ -33,10 +33,10 @@ void Energy::findTargetByPolar() {
} else { } else {
target_polar_angle = armor_polar_angle.at(j);//无法被匹配到的装甲板为待击打装甲板 target_polar_angle = armor_polar_angle.at(j);//无法被匹配到的装甲板为待击打装甲板
for (const auto &armor : armors) { for (const auto &armor : armors) {
float angle = static_cast<float>(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y), float angle = static_cast<float>(180 / PI * atan2(-1 * (armor.center.y - circle_center_point.y),
(armor.rect.center.x - circle_center_point.x))); (armor.center.x - circle_center_point.x)));
if (target_polar_angle == angle) { if (target_polar_angle == angle) {
target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标 target_point = armor.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标
} }
} }
return; return;
@@ -44,10 +44,10 @@ void Energy::findTargetByPolar() {
} }
target_polar_angle = armor_polar_angle.at(armor_polar_angle.size() - 1);//前几个扇叶都匹配到装甲板，则最后剩下的装甲板为目标 target_polar_angle = armor_polar_angle.at(armor_polar_angle.size() - 1);//前几个扇叶都匹配到装甲板，则最后剩下的装甲板为目标
for (const auto &armor : armors) { for (const auto &armor : armors) {
float angle = static_cast<float>(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y), float angle = static_cast<float>(180 / PI * atan2(-1 * (armor.center.y - circle_center_point.y),
(armor.rect.center.x - circle_center_point.x))); (armor.center.x - circle_center_point.x)));
if (target_polar_angle == angle) { if (target_polar_angle == angle) {
target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标 target_point = armor.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标
} }
} }
} }
@@ -58,8 +58,12 @@ void Energy::findTargetByPolar() {
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::findTargetByIntersection() { void Energy::findTargetByIntersection() {
if (fans.size() >= armors.size()) return;//扇叶多于装甲板，识别错误 if (fans.size() >= armors.size()) return;//扇叶多于装甲板，识别错误
if (armors.empty())return;//找不到扇叶，识别错误 for (const auto &fan : fans) {
if (fans.empty()) { for (const auto &armor : armors) {
}
}
/*if (fans.empty()) {
target_point = armors.at(0).rect.center; target_point = armors.at(0).rect.center;
return; return;
} }
@@ -84,7 +88,7 @@ void Energy::findTargetByIntersection() {
break; break;
} }
i++; i++;
} }*/
} }
@@ -95,14 +99,14 @@ bool Energy::findTargetInFlowStripFan() {
int i = 0; int i = 0;
for (i = 0; i < armors.size(); ++i) { for (i = 0; i < armors.size(); ++i) {
std::vector<cv::Point2f> intersection; std::vector<cv::Point2f> 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表示没有重合面积 continue;//返回0表示没有重合面积
double cur_contour_area = contourArea(intersection); double cur_contour_area = contourArea(intersection);
if (cur_contour_area < energy_part_param_.INTERSETION_CONTOUR_AREA_MIN) if (cur_contour_area < energy_part_param_.INTERSETION_CONTOUR_AREA_MIN)
continue; continue;
else { else {
target_armor.emplace_back(armors.at(i)); target_armor = armors.at(i);
target_point = armors.at(i).rect.center; target_point = armors.at(i).center;
return true; return true;
} }
} }

6
energy/src/energy/get/aim_point_get.cpp
View File

@@ -16,10 +16,4 @@ void Energy::getAimPoint(){
double dy = target_point.y - 240; double dy = target_point.y - 240;
yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI; yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;
pitch_rotation = atan(dy / 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"<<endl;
}
else
small_energy_shoot = false;
} }

7
energy/src/energy/get/gimble_rotation_get.cpp
View File

@@ -46,6 +46,9 @@ void Energy::gimbleRotation() {
pitch_rotation = static_cast<float>(180 / PI * pitch_rotation = static_cast<float>(180 / PI *
atan2((attack_distance * tan(origin_pitch * PI / 180) - real_predict_point.y), atan2((attack_distance * tan(origin_pitch * PI / 180) - real_predict_point.y),
attack_distance)); attack_distance));
if (fabs(yaw_rotation - mcuData.curr_yaw) < 0.5 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.5) if (abs(yaw_rotation - mcuData.curr_yaw) < 0.5 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.5){
big_energy_shoot = true; shoot = true;
isPredicting = false;
isGuessing = true;
}
} }

54
energy/src/energy/get/guess_point_get.cpp Normal file
View File

@@ -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<int> all_fan_angles;
float angle;
for (const auto &fan : fans) {
angle = static_cast<float>(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;
}

49
energy/src/energy/get/polar_angle_get.cpp
View File

@@ -4,54 +4,15 @@
#include "energy/energy.h" #include "energy/energy.h"
using namespace cv; using namespace cv;
using std::cout; using namespace std;
using std::endl;
using std::vector;
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于获得图像中所有扇叶的当前极坐标角度 // 此函数获取目标装甲板极坐标角度
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::getFanPolarAngle() { void Energy::getTargetPolarAngle() {
if (radius == 0)return; target_polar_angle = static_cast<float>(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y),
for (const auto &fan : fans) (target_point.x - circle_center_point.x)));
{
float angle = static_cast<float>(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::getArmorPolarAngle() {
if (radius == 0)return;
for (const auto &armor : armors)
{
float angle = static_cast<float>(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);
}
}

31
energy/src/energy/init/clear.cpp Normal file
View File

@@ -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);
}

118
energy/src/energy/energy_init.cpp → energy/src/energy/init/energy_init.cpp
View File

@@ -10,69 +10,64 @@ using std::endl;
using std::vector; using std::vector;
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数对能量机关成员变量进行初始化 // 此函数对能量机关成员变量进行初始化
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::initEnergy() { void Energy::initEnergy() {
isMark = false; isMark = false;
isPredicting = true; isPredicting = true;
isGuessing = false; isGuessing = false;
fans_cnt = 0; // fans_cnt = 0;
armors_cnt = 0; // armors_cnt = 0;
centerRs_cnt = 0; // centerRs_cnt = 0;
flow_strips_cnt = 0; // flow_strips_cnt = 0;
flow_strip_fans_cnt = 0; // flow_strip_fans_cnt = 0;
circle_center_point = Point(0, 0); circle_center_point = Point(0, 0);
target_point = Point(0, 0); target_point = Point(0, 0);
last_target_point = Point(0, 0); last_target_point = Point(0, 0);
guess_point = Point(0, 0);
predict_point = Point(0, 0); predict_point = Point(0, 0);
target_polar_angle = -1000; target_polar_angle = -1000;
guess_polar_angle = -1000;
last_target_polar_angle = -1000; last_target_polar_angle = -1000;
radius = 0; radius = 0;
energy_rotation_direction = ANTICLOCKWISE; energy_rotation_direction = ANTICLOCKWISE;
attack_distance = ATTACK_DISTANCE; attack_distance = ATTACK_DISTANCE;
last_fans_cnt = 0; last_fans_cnt = 0;
last_armors_cnt = 0;
last_flow_strips_cnt = 0; last_flow_strips_cnt = 0;
last_flow_strip_fans_cnt = 0; last_flow_strip_fans_cnt = 0;
send_cnt = 0; send_cnt = 0;
yaw_rotation = 0; yaw_rotation = 0;
pitch_rotation = 0; pitch_rotation = 0;
last_mark = 0; last_mark = 0;
target_cnt = 0; target_cnt = 0;
big_energy_shoot = false; shoot = false;
small_energy_shoot = false; guess_devide = 0;
predict_rad = 20; startguessing = false;
predict_rad = 20;
fans.clear(); fans.clear();
armors.clear(); armors.clear();
centerRs.clear();
flow_strips.clear();
flow_strip_fans.clear();
center_ROI.clear();
target_armor.clear();
fan_polar_angle.clear(); fan_polar_angle.clear();
armor_polar_angle.clear(); armor_polar_angle.clear();
all_armor_centers.clear(); all_armor_centers.clear();
clockwise_rotation_init_cnt = 0; clockwise_rotation_init_cnt = 0;
anticlockwise_rotation_init_cnt = 0; anticlockwise_rotation_init_cnt = 0;
} }
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数对能量机关参数进行初始化 // 此函数对能量机关参数进行初始化
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::initEnergyPartParam() { void Energy::initEnergyPartParam() {
gimble_energy_part_param_.GRAY_THRESH = 225; gimble_energy_part_param_.GRAY_THRESH = 225;
gimble_energy_part_param_.SPLIT_GRAY_THRESH = 60; gimble_energy_part_param_.SPLIT_GRAY_THRESH = 180;
gimble_energy_part_param_.FAN_GRAY_THRESH = 75; gimble_energy_part_param_.FAN_GRAY_THRESH = 75;
gimble_energy_part_param_.ARMOR_GRAY_THRESH = 80; gimble_energy_part_param_.ARMOR_GRAY_THRESH = 80;
gimble_energy_part_param_.FAN_CONTOUR_AREA_MAX = 17000; gimble_energy_part_param_.FAN_CONTOUR_AREA_MAX = 17000;
gimble_energy_part_param_.FAN_CONTOUR_AREA_MIN = 0; 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_WIDTH_MAX = 60;
gimble_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 4; 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_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_MAX = 100000;
gimble_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0; 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_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_MAX = 0.55;
gimble_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.25; 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_MAX = 100000;
gimble_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 0; 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_AREA_RATIO_MIN = 0.5;
gimble_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 300; gimble_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 300;
gimble_energy_part_param_.TWIN_ANGEL_MAX = 10; gimble_energy_part_param_.TWIN_ANGEL_MAX = 10;
gimble_energy_part_param_.INTERSETION_CONTOUR_AREA_MIN = 60; gimble_energy_part_param_.INTERSETION_CONTOUR_AREA_MIN = 60;
chassis_energy_part_param_.GRAY_THRESH = 225; chassis_energy_part_param_.GRAY_THRESH = 225;
chassis_energy_part_param_.SPLIT_GRAY_THRESH = 60; chassis_energy_part_param_.SPLIT_GRAY_THRESH = 60;
chassis_energy_part_param_.FAN_GRAY_THRESH = 75; 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_LENGTH_MIN = 90;
chassis_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 35; chassis_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 35;
chassis_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 140; 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_MAX = 4;
chassis_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1; 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_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_MAX = 100000;
chassis_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0; 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_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_MAX = 0.55;
chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.25; 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_MAX = 100000;
chassis_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 0; chassis_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 0;
@@ -196,29 +199,26 @@ void Energy::initEnergyPartParam() {
} }
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数对能量机关旋转方向进行初始化 // 此函数对能量机关旋转方向进行初始化
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::initRotation() { void Energy::initRotation() {
if (target_polar_angle >= -180 && last_target_polar_angle >= -180 if (target_polar_angle >= -180 && last_target_polar_angle >= -180
&& fabs(target_polar_angle - last_target_polar_angle) < 30) { && fabs(target_polar_angle - last_target_polar_angle) < 30) {
//target_polar_angle和last_target_polar_angle的初值均为1000，大于-180表示刚开始几帧不要 //target_polar_angle和last_target_polar_angle的初值均为1000，大于-180表示刚开始几帧不要
//若两者比较接近，则说明没有切换目标，因此可以用于顺逆时针的判断 //若两者比较接近，则说明没有切换目标，因此可以用于顺逆时针的判断
if (target_polar_angle < last_target_polar_angle) clockwise_rotation_init_cnt++; 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++; 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次，确定为顺时针 energy_rotation_direction = CLOCKWISE;//顺时针变化30次，确定为顺时针
cout << "rotation: " << energy_rotation_direction << endl; cout << "rotation: " << energy_rotation_direction << endl;
energy_rotation_init = false; energy_rotation_init = false;
} } else if (anticlockwise_rotation_init_cnt == 30) {
else if (anticlockwise_rotation_init_cnt == 30) {
energy_rotation_direction = ANTICLOCKWISE;//逆时针变化30次，确定为顺时针 energy_rotation_direction = ANTICLOCKWISE;//逆时针变化30次，确定为顺时针
cout << "rotation: " << energy_rotation_direction << endl; cout << "rotation: " << energy_rotation_direction << endl;
energy_rotation_init = false; energy_rotation_init = false;
} }
last_target_polar_angle = target_polar_angle; last_target_polar_angle = target_polar_angle;
} }

38
energy/src/energy/judge/judge_shoot.cpp Normal file
View File

@@ -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;
}

21
energy/src/energy/judge/judge_time.cpp Normal file
View File

@@ -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;
};

4
energy/src/energy/mark/mark.cpp
View File

@@ -8,7 +8,7 @@ using namespace std;
using namespace cv; using namespace cv;
void Energy::writeDownMark() { void Energy::writeDownMark() {
if(armors_cnt>=4 && fans_cnt>=3) { if(fans.size()>=3) {
FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "w"); FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "w");
if (fp) { if (fp) {
fprintf(fp, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch); fprintf(fp, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch);
@@ -21,7 +21,7 @@ void Energy::writeDownMark() {
fclose(fp_all); fclose(fp_all);
} }
} }
if(armors_cnt==5){ if(fans.size()==4){
FILE *fp_best = fopen(PROJECT_DIR"/Mark/mark_best.txt", "a"); FILE *fp_best = fopen(PROJECT_DIR"/Mark/mark_best.txt", "a");
if(fp_best){ if(fp_best){
fprintf(fp_best, "yaw: %f, pitch: %f\n",origin_yaw, origin_pitch); fprintf(fp_best, "yaw: %f, pitch: %f\n",origin_yaw, origin_pitch);

200
energy/src/energy/run.cpp
View File

@@ -4,6 +4,7 @@
#include "energy/energy.h" #include "energy/energy.h"
#include "log.h" #include "log.h"
#include "options/options.h" #include "options/options.h"
#include <sys/time.h>
using namespace std; using namespace std;
using namespace cv; using namespace cv;
@@ -16,93 +17,65 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
if (chassis_src.empty()) if (chassis_src.empty())
runBig(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数 runBig(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数
else if (isGimble) { else if (isGimble) {
if (gimble_src.type() == CV_8UC3)cvtColor(gimble_src, gimble_src, COLOR_BGR2GRAY);
energy_part_param_ = chassis_energy_part_param_; energy_part_param_ = chassis_energy_part_param_;
fans.clear(); clearAll();
armors.clear(); initImage(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 (findArmors(gimble_src) < 1)return 0;
if (show_bin)imshow("bin", gimble_src); if (!findFlowStripFan(gimble_src))return 0;
armors_cnt = findArmor(gimble_src, last_armors_cnt); if (!findTargetInFlowStripFan()) return 0;
flow_strip_fans_cnt = findFlowStripFan(gimble_src, last_flow_strip_fans_cnt); if (!findCenterROI(gimble_src))return 0;
if (flow_strip_fans_cnt == 1 && findTargetInFlowStripFan()) { if (show_energy)showFlowStripFanContours("strip", gimble_src);
findCenterROI(gimble_src); if (!findCenterR(gimble_src))return 0;
if (show_strip)showFlowStripFanContours("strip", gimble_src); if (show_energy)showCenterRContours("R", gimble_src);
} else { startChassis();
fans_cnt = findFan(gimble_src, last_fans_cnt); getOrigin();
if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0; initEnergy();
findTargetByIntersection(); destroyAllWindows();
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();
}
} else if (isChassis) { } else if (isChassis) {
if (chassis_src.type() == CV_8UC3)cvtColor(chassis_src, chassis_src, COLOR_BGR2GRAY);
energy_part_param_ = chassis_energy_part_param_; energy_part_param_ = chassis_energy_part_param_;
fans.clear(); clearAll();
armors.clear(); initImage(chassis_src);
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);
changeMark(); changeMark();
if (isMark)return 0;//操作手强制手动标定origin_yaw和origin_pitch 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); findFans(chassis_src);showFanContours("fan",chassis_src);
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();
}
centerRs_cnt = findCenterR(chassis_src); getTargetPolarAngle();
if (show_center && centerRs_cnt > 0)showCenterRContours("R", chassis_src); if (save_mark)writeDownMark();
if (centerRs.size() != 1)return 0;
circle_center_point = centerRs.at(0).rect.center;
target_polar_angle = static_cast<float>(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y),
(target_point.x - circle_center_point.x)));
if (energy_rotation_init) { if (energy_rotation_init) {
initRotation(); initRotation();
return 0; return 0;
} }
if (isPredicting) {
getPredictPoint(); getPredictPoint();
gimbleRotation(); gimbleRotation();
if (changeTarget())target_cnt++; judgeBigShoot();
sendTarget(serial, yaw_rotation, pitch_rotation, target_cnt, big_energy_shoot); sendTarget(serial, yaw_rotation, pitch_rotation, shoot);
} else if (isGuessing) {
return 0; 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; return 0;
} }
@@ -112,60 +85,54 @@ int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src) {
// 此函数为大能量机关模式主控制流函数，且步兵仅拥有云台摄像头 // 此函数为大能量机关模式主控制流函数，且步兵仅拥有云台摄像头
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
int Energy::runBig(cv::Mat &gimble_src) { int Energy::runBig(cv::Mat &gimble_src) {
if (gimble_src.type() == CV_8UC3)cvtColor(gimble_src, gimble_src, COLOR_BGR2GRAY); energy_part_param_ = chassis_energy_part_param_;
energy_part_param_ = gimble_energy_part_param_; clearAll();
fans.clear(); initImage(gimble_src);
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);
changeMark(); changeMark();
if (isMark)return 0;//操作手强制手动标定origin_yaw和origin_pitch if (isMark)return 0;//操作手强制手动标定origin_yaw和origin_pitch
threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY); threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
if (show_bin)imshow("bin", gimble_src); if (show_process)imshow("bin", gimble_src);
armors_cnt = findArmor(gimble_src, last_armors_cnt); if (findArmors(gimble_src) < 1)return 0;
flow_strip_fans_cnt = findFlowStripFan(gimble_src, last_flow_strip_fans_cnt); if (!findFlowStripFan(gimble_src))return 0;
if (flow_strip_fans_cnt == 1 && findTargetInFlowStripFan()) { if (!findTargetInFlowStripFan()) return 0;
findCenterROI(gimble_src); if (!findCenterROI(gimble_src))return 0;
if (show_strip)showFlowStripFanContours("strip", gimble_src); if (show_energy)showFlowStripFanContours("strip", gimble_src);
} else { if (!findCenterR(gimble_src))return 0;
fans_cnt = findFan(gimble_src, last_fans_cnt); if (show_energy)showCenterRContours("R", gimble_src);
if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0; getTargetPolarAngle();
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<float>(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y),
(target_point.x - circle_center_point.x)));
if (energy_rotation_init) { if (energy_rotation_init) {
initRotation(); initRotation();
getOrigin();//一旦确定风车旋向后，就开始移动云台，此时不再更新origin_yaw和origin_pitch getOrigin();//一旦确定风车旋向后，就开始移动云台，此时不再更新origin_yaw和origin_pitch
return 0; return 0;
} }
getPredictPoint(); if (isPredicting) {
gimbleRotation(); getPredictPoint();
if (changeTarget())target_cnt++; gimbleRotation();
sendTarget(serial, yaw_rotation, pitch_rotation, target_cnt, big_energy_shoot); 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; return 0;
} }
//---------------------------------------------------------------------------------------------------------------------- /*//----------------------------------------------------------------------------------------------------------------------
// 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头 // 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
int Energy::runSmall(cv::Mat &gimble_src) { int Energy::runSmall(cv::Mat &gimble_src) {
@@ -176,8 +143,6 @@ int Energy::runSmall(cv::Mat &gimble_src) {
centerRs.clear(); centerRs.clear();
flow_strip_fans.clear(); flow_strip_fans.clear();
flow_strips.clear(); flow_strips.clear();
center_ROI.clear();
target_armor.clear();
threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY); threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
if (show_bin)imshow("bin", gimble_src); if (show_bin)imshow("bin", gimble_src);
armors_cnt = findArmor(gimble_src, last_armors_cnt); 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); if (show_strip)showFlowStripFanContours("strip", gimble_src);
} else { } else {
fans_cnt = findFan(gimble_src, last_fans_cnt); fans_cnt = findFan(gimble_src, last_fans_cnt);
if (fans_cnt == -1 || armors_cnt == -1 || armors_cnt != fans_cnt + 1) return 0;
findTargetByIntersection(); findTargetByIntersection();
if (show_single) { if (show_single) {
if (fans_cnt > 0)showFanContours("fan", gimble_src); if (fans_cnt > 0)showFanContours("fan", gimble_src);
@@ -198,7 +162,7 @@ int Energy::runSmall(cv::Mat &gimble_src) {
centerRs_cnt = findCenterR(gimble_src); centerRs_cnt = findCenterR(gimble_src);
if (show_center && centerRs_cnt > 0)showCenterRContours("R", gimble_src); if (show_center && centerRs_cnt > 0)showCenterRContours("R", gimble_src);
if (centerRs.size() != 1)return 0; 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<float>(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y), target_polar_angle = static_cast<float>(180 / PI * atan2(-1 * (target_point.y - circle_center_point.y),
(target_point.x - circle_center_point.x))); (target_point.x - circle_center_point.x)));
getAimPoint(); getAimPoint();
@@ -206,7 +170,7 @@ int Energy::runSmall(cv::Mat &gimble_src) {
sendTarget(serial, yaw_rotation, pitch_rotation, target_cnt, small_energy_shoot); sendTarget(serial, yaw_rotation, pitch_rotation, target_cnt, small_energy_shoot);
return 0; return 0;
} }*/

9
energy/src/energy/send/send.cpp
View File

@@ -3,6 +3,8 @@
// //
#include "energy/energy.h" #include "energy/energy.h"
#include <iostream> #include <iostream>
#include "log.h"
using namespace std; 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; static short x_tmp, y_tmp, z_tmp;
uint8_t buff[8]; uint8_t buff[8];
x_tmp = static_cast<short>(x * (32768 - 1) / 100); x_tmp = static_cast<short>(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<char>((x_tmp >> 0) & 0xFF); buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF); buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF); buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
buff[5] = change; buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
buff[6] = shoot; buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
buff[7] = 'e'; buff[7] = 'e';
serial.WriteData(buff, sizeof(buff)); serial.WriteData(buff, sizeof(buff));
send_cnt+=1; send_cnt+=1;
// LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send"));
} }

88
energy/src/energy/show/show.cpp
View File

@@ -28,7 +28,7 @@ void Energy::showFanContours(std::string windows_name, const cv::Mat src) {
for (const auto &fan : fans) for (const auto &fan : fans)
{ {
Point2f vertices[4]; //定义矩形的4个顶点 Point2f vertices[4]; //定义矩形的4个顶点
fan.rect.points(vertices); //计算矩形的4个顶点 fan.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++) for (int i = 0; i < 4; i++)
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 2); 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) for (const auto &armor : armors)
{ {
Point2f vertices[4]; //定义矩形的4个顶点 Point2f vertices[4]; //定义矩形的4个顶点
armor.rect.points(vertices); //计算矩形的4个顶点 armor.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++) for (int i = 0; i < 4; i++)
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 2); 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) for (const auto &fan : fans)
{ {
Point2f vertices[4]; //定义矩形的4个顶点 Point2f vertices[4]; //定义矩形的4个顶点
fan.rect.points(vertices); //计算矩形的4个顶点 fan.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++) for (int i = 0; i < 4; i++)
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 4); line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 4);
} }
for (const auto &armor : armors) for (const auto &armor : armors)
{ {
Point2f vertices[4]; //定义矩形的4个顶点 Point2f vertices[4]; //定义矩形的4个顶点
armor.rect.points(vertices); //计算矩形的4个顶点 armor.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++){ for (int i = 0; i < 4; i++){
if(pointDistance(static_cast<cv::Point2f>(armor.rect.center),target_point) < 5) if(pointDistance(static_cast<cv::Point2f>(armor.center),target_point) < 5)
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 4); line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 4);
else else
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 4); 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是圆的半径 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(); image2show = src.clone();
} }
//cvtColor(image2show, image2show, COLOR_GRAY2RGB); //cvtColor(image2show, image2show, COLOR_GRAY2RGB);
for (const auto &center_R : centerRs) Point2f vertices[4]; //定义矩形的4个顶点
{ centerR.points(vertices); //计算矩形的4个顶点
Point2f vertices[4]; //定义矩形的4个顶点 for (int i = 0; i < 4; i++)
center_R.rect.points(vertices); //计算矩形的4个顶点 line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 255), 2);
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;
}
imshow(windows_name, image2show); imshow(windows_name, image2show);
} }
@@ -151,36 +146,55 @@ void Energy::showFlowStripFanContours(std::string windows_name, const cv::Mat sr
{ {
image2show = src.clone(); image2show = src.clone();
} }
for (const auto &flow_strip_fan : flow_strip_fans)
{ Point2f strip_fan_vertices[4]; //定义矩形的4个顶点
Point2f vertices[4]; //定义矩形的4个顶点 flow_strip_fan.points(strip_fan_vertices); //计算矩形的4个顶点for (int i = 0; i < 4; i++)
flow_strip_fan.rect.points(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);
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(127, 127, 255), 2);
}
for (const auto &flow_strip : flow_strips) Point2f strip_vertices[4]; //定义矩形的4个顶点
{ flow_strip.points(strip_vertices); //计算矩形的4个顶点
Point2f vertices[4]; //定义矩形的4个顶点 for (int i = 0; i < 4; i++)
flow_strip.rect.points(vertices); //计算矩形的4个顶点 line(image2show, strip_vertices[i], strip_vertices[(i + 1) % 4], Scalar(0, 255, 0), 2);
for (int i = 0; i < 4; i++)
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 255, 0), 2);
}
for (const auto &armor : armors){ 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个顶点 Point2f vertices[4]; //定义矩形的4个顶点
armor.rect.points(vertices); //计算矩形的4个顶点 armor.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++) for (int i = 0; i < 4; i++)
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 2); line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 2);
} }
} }
Point2f vertices[4]; //定义矩形的4个顶点
if(center_ROI.size() > 0){ Point2f ROI_vertices[4]; //定义矩形的4个顶点
center_ROI.at(0).rect.points(vertices); //计算矩形的4个顶点 center_ROI.points(ROI_vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++) for (int i = 0; i < 4; i++)
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 2); line(image2show, ROI_vertices[i], ROI_vertices[(i + 1) % 4], Scalar(0, 0, 255), 2);
}
imshow(windows_name, image2show); 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);
}

70
energy/src/energy/tool/tool.cpp
View File

@@ -9,10 +9,8 @@
#include <opencv2/highgui/highgui.hpp> #include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp> #include <opencv2/imgproc/imgproc.hpp>
using namespace std;
using namespace cv; 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<uint8_t>(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<uint8_t>(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<uint8_t>(r, c)) {
cnt++;
}
}
}
return double(cnt) / rotatedRect.size.area();
}

12
main.cpp
View File

@@ -27,7 +27,7 @@ using namespace std;
mcu_data mcuData = { // 单片机端回传结构体 mcu_data mcuData = { // 单片机端回传结构体
0, // 当前云台yaw角 0, // 当前云台yaw角
0, // 当前云台pitch角 0, // 当前云台pitch角
ARMOR_STATE, // 当前状态，自瞄-大符-小符 BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符
0, // 云台角度标记位 0, // 云台角度标记位
1, // 是否启用数字识别 1, // 是否启用数字识别
ENEMY_RED, // 敌方颜色 ENEMY_RED, // 敌方颜色
@@ -61,8 +61,8 @@ int main(int argc, char *argv[]) {
video_gimble = new CameraWrapper(0/*, "armor"*/); video_gimble = new CameraWrapper(0/*, "armor"*/);
video_chassis = new CameraWrapper(1/*, "energy"*/); video_chassis = new CameraWrapper(1/*, "energy"*/);
} else { } else {
video_gimble = new VideoWrapper("/home/sun/项目/energy_video/energy_test.avi"); video_gimble = new VideoWrapper("/home/sun/项目/RM2019 能量机关视频/大能量机关（红+开灯）.mov");
video_chassis = new VideoWrapper("/home/sun/项目/energy_video/energy_test.avi"); video_chassis = new VideoWrapper("/home/sun/项目/RM2019 能量机关视频/大能量机关（红+开灯）.mov");
} }
if (video_gimble->init()) { if (video_gimble->init()) {
LOGM("video_gimble source initialization successfully."); LOGM("video_gimble source initialization successfully.");
@@ -100,6 +100,10 @@ int main(int argc, char *argv[]) {
cout << "set" << endl; cout << "set" << endl;
} }
ok = checkReconnect(video_gimble->read(gimble_src), video_chassis->read(chassis_src));//检查有几个摄像头 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 (save_video) saveVideos(gimble_src, chassis_src);//保存视频
if (show_origin) showOrigin(gimble_src, chassis_src);//显示原始图像 if (show_origin) showOrigin(gimble_src, chassis_src);//显示原始图像
energy.runBig(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)); ok = checkReconnect(video_gimble->read(gimble_src));
if (save_video) saveVideos(gimble_src); if (save_video) saveVideos(gimble_src);
if (show_origin) showOrigin(gimble_src); if (show_origin) showOrigin(gimble_src);
energy.runSmall(gimble_src); // energy.runSmall(gimble_src);
} else { // 自瞄模式 } else { // 自瞄模式
if(last_state != ARMOR_STATE){ if(last_state != ARMOR_STATE){
((CameraWrapper*)video_gimble)->changeBrightness(40); ((CameraWrapper*)video_gimble)->changeBrightness(40);

10
others/include/options/options.h
View File

@@ -15,13 +15,9 @@ extern bool run_with_camera;
extern bool save_video; extern bool save_video;
extern bool wait_uart; extern bool wait_uart;
extern bool save_labelled_boxes; extern bool save_labelled_boxes;
extern bool show_bin; extern bool show_process;
extern bool show_split; extern bool show_energy;
extern bool show_single; extern bool save_mark;
extern bool show_both;
extern bool show_center;
extern bool show_strip;
extern bool write_down;
void process_options(int argc, char *argv[]); void process_options(int argc, char *argv[]);

52
others/src/options/options.cpp
View File

@@ -14,13 +14,9 @@ bool run_with_camera = false;
bool save_video = false; bool save_video = false;
bool wait_uart = false; bool wait_uart = false;
bool save_labelled_boxes = false; bool save_labelled_boxes = false;
bool show_bin = false; bool show_process = false;
bool show_split = false; bool show_energy = false;
bool show_single = false; bool save_mark = false;
bool show_both = false;
bool show_center = false;
bool show_strip = false;
bool write_down = false;
void process_options(int argc, char *argv[]) { void process_options(int argc, char *argv[]) {
if (argc >= 2) { if (argc >= 2) {
@@ -54,18 +50,10 @@ void process_options(int argc, char *argv[]) {
LOGM("Enable show light blobs"); LOGM("Enable show light blobs");
show_origin = true; show_origin = true;
LOGM("Enable show origin"); LOGM("Enable show origin");
show_bin = true; show_process = true;
LOGM("Enable show binary image"); LOGM("Enable show processed image");
show_split = true; show_energy = true;
LOGM("Enable show split image"); LOGM("Enable show energy part");
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");
} else if (strcmp(argv[i], "--run-with-camera") == 0) { } else if (strcmp(argv[i], "--run-with-camera") == 0) {
run_with_camera = true; run_with_camera = true;
LOGM("Run with camera!"); LOGM("Run with camera!");
@@ -78,26 +66,14 @@ void process_options(int argc, char *argv[]) {
} else if (strcmp(argv[i], "--save-labelled-boxes") == 0) { } else if (strcmp(argv[i], "--save-labelled-boxes") == 0) {
save_labelled_boxes = true; save_labelled_boxes = true;
LOGM("labelled armor boxes will be saved!"); LOGM("labelled armor boxes will be saved!");
} else if (strcmp(argv[i], "--show-bin") == 0) { } else if (strcmp(argv[i], "--show-process") == 0) {
show_bin = true; show_process = true;
LOGM("Enable show binary image"); LOGM("Enable show processed image!");
} else if (strcmp(argv[i], "--show-split") == 0) { } else if (strcmp(argv[i], "--show-energy") == 0) {
show_split = true; show_energy= true;
LOGM("Enable show split image"); LOGM("Enable show energy part!");
} 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], "--save-mark") == 0) { } else if (strcmp(argv[i], "--save-mark") == 0) {
write_down = true; save_mark = true;
LOGM("Write down mark"); LOGM("Write down mark");
} else { } else {
LOGW("Unknown option: %s. Use --help to see options.", argv[i]); LOGW("Unknown option: %s. Use --help to see options.", argv[i]);