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

energy changed a lot

Browse Source
This commit is contained in:
sun
2019-08-13 22:36:26 +08:00
parent 05527de9e2
commit 9d94de939b
32 changed files with 282 additions and 1363 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
energy/src/energy/calculate/circle_calculate.cpp
View File

@@ -1,64 +0,0 @@
//
// Created by xixiliadorabarry on 1/24/19.
//
#include "energy/energy.h"
using namespace cv;
using std::cout;
using std::endl;
using std::vector;
//----------------------------------------------------------------------------------------------------------------------
// 此函数通过最小二乘法计算大风车圆心和半径
// ---------------------------------------------------------------------------------------------------------------------
void Energy::circleLeastFit()
{
circle_center_point.x = 0;
circle_center_point.y = 0;
radius = 0.0f;
if (all_target_armor_centers.size() < 3)
{
// cout<<"Cannot calculate a circle"<<endl;
return;
}
double sum_x = 0.0f, sum_y = 0.0f;
double sum_x2 = 0.0f, sum_y2 = 0.0f;
double sum_x3 = 0.0f, sum_y3 = 0.0f;
double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f;
int N = static_cast<int>(all_target_armor_centers.size());
for (int i = 0; i < N; i++)
{
double x = all_target_armor_centers.at(i).x;
double y = all_target_armor_centers.at(i).y;
double x2 = x * x;
double y2 = y * y;
sum_x += x;
sum_y += y;
sum_x2 += x2;
sum_y2 += y2;
sum_x3 += x2 * x;
sum_y3 += y2 * y;
sum_xy += x * y;
sum_x1y2 += x * y2;
sum_x2y1 += x2 * y;
}
double C, D, E, G, H;
double a, b, c;
C = N * sum_x2 - sum_x * sum_x;
D = N * sum_xy - sum_x * sum_y;
E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
G = N * sum_y2 - sum_y * sum_y;
H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y;
a = (H * D - E * G) / (C * G - D * D);
b = (H * C - E * D) / (D * D - G * C);
c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;
circle_center_point.x = static_cast<int>(a / (-2));
circle_center_point.y = static_cast<int>(b / (-2));
radius = sqrt(a * a + b * b - 4 * c) / 2;
// cout << "The cycle center is: " << cycle_center << endl;
// cout << "The radius is: " << radius << endl;
}

28
energy/src/energy/calibrate/hsv_filter.cpp
View File

@@ -1,28 +0,0 @@
//
// Created by sun on 19-7-7.
//
#include "energy/energy.h"
using namespace std;
using namespace cv;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于利用HSV颜色空间对图像进行预处理
// ---------------------------------------------------------------------------------------------------------------------
void Energy::hsvFilter(cv::Mat &src){
Mat hsv, hsv_in_range;
cvtColor(src, hsv, CV_BGR2HSV);
// imshow("hsv", hsv);
vector<Mat> hsv_channels;
split(hsv, hsv_channels);
Mat H = hsv_channels.at(0);
Mat S = hsv_channels.at(1);
Mat V = hsv_channels.at(2);
// imshow("S", S);
inRange(hsv,Scalar(0,150,0),Scalar(180,255,255),hsv_in_range);
// imshow("in range", hsv_in_range);
}

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

@@ -17,24 +17,15 @@ using std::vector;
void Energy::FanStruct(cv::Mat &src) {
Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(5, 5));
Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 2));
Mat element_dilate_2 = getStructuringElement(MORPH_RECT,
Size(3, 3));
Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3));
Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(2 , 2));
Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(3, 3));
Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(1 , 1));
dilate(src, src, element_dilate_1);
// imshow("dilate_1", src);
erode(src,src, element_erode_1);
// imshow("erode_1", src);
erode(src,src, element_erode_2);
// imshow("erode_2", src);
erode(src,src, element_erode_3);
// imshow("erode_3", src);
// dilate(src, src, element_dilate_2);
// imshow("dilate_2", src);
// dilate(src, src, element_dilate_3);
// imshow("dilate_3", src);
}
@@ -51,19 +42,7 @@ void Energy::ArmorStruct(cv::Mat &src) {
Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(1 , 1));
dilate(src, src, element_dilate_1);
// imshow("dilate_1", src);
erode(src,src, element_erode_1);
// imshow("erode_1", src);
// erode(src,src, element_erode_2);
// imshow("erode_2", src);
// erode(src,src, element_erode_3);
// imshow("erode_3", src);
// dilate(src, src, element_dilate_2);
// imshow("dilate_2", src);
// dilate(src, src, element_dilate_3);
// imshow("dilate_3", src);
}
@@ -80,17 +59,9 @@ void Energy::FlowStripFanStruct(cv::Mat &src) {
Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(1 , 1));
dilate(src, src, element_dilate_1);
// imshow("dilate_1", src);
erode(src,src, element_erode_1);
// imshow("erode_1", src);
erode(src,src, element_erode_2);
// imshow("erode_2", src);
erode(src,src, element_erode_3);
// imshow("erode_3", src);
// dilate(src, src, element_dilate_2);
// imshow("dilate_2", src);
// dilate(src, src, element_dilate_3);
// imshow("dilate_3", src);
}
@@ -107,48 +78,9 @@ void Energy::FlowStripStruct(cv::Mat &src) {
Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(1 , 1));
dilate(src, src, element_dilate_1);
// imshow("dilate_1", src);
erode(src,src, element_erode_1);
// imshow("erode_1", src);
erode(src,src, element_erode_2);
// imshow("erode_2", src);
erode(src,src, element_erode_3);
// imshow("erode_3", src);
// dilate(src, src, element_dilate_2);
// imshow("dilate_2", src);
// dilate(src, src, element_dilate_3);
// imshow("dilate_3", src);
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数对图像进行腐蚀与膨胀操作
// ---------------------------------------------------------------------------------------------------------------------
void Energy::FlowStripStructSub(cv::Mat &src) {
Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(5, 5));
Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(4, 4));
Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3));
Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(3 , 3));
Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(2, 2));
Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(2 , 2));
Mat element_dilate_4 = getStructuringElement(MORPH_RECT, Size(2, 2));
erode(src,src, element_erode_1);
// imshow("sub_erode_1", src);
// erode(src,src, element_erode_2);
// imshow("erode_2", src);
erode(src,src, element_erode_3);
// imshow("sub_erode_3", src);
// erode(src,src, element_erode_4);
// imshow("erode_4", src);
dilate(src, src, element_dilate_1);
// imshow("sub_dilate_1", src);
dilate(src, src, element_dilate_2);
// imshow("sub_dilate_2", src);
// dilate(src, src, element_dilate_3);
// imshow("dilate_3", src);
// dilate(src, src, element_dilate_4);
// imshow("dilate_4", src);
}
@@ -162,11 +94,5 @@ void Energy::CenterRStruct(cv::Mat &src) {
Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(4 , 4));
erode(src,src, element_erode_1);
// imshow("erode_1", src);
dilate(src, src, element_dilate_1);
// imshow("dilate_1", src);
// dilate(src, src, element_dilate_2);
// imshow("dilate_2", src);
// erode(src,src, element_erode_2);
// imshow("erode_2", src);
}

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

@@ -8,18 +8,6 @@ using namespace std;
using namespace cv;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于从云台摄像头对心状态切换到底盘摄像头击打状态
// ---------------------------------------------------------------------------------------------------------------------
void Energy::startChassis(){
is_chassis = true;
is_gimbal = false;
LOGM(STR_CTR(WORD_YELLOW, "Start Chassis!"));
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断是否应当继续保持猜测模式
// ---------------------------------------------------------------------------------------------------------------------

6
energy/src/energy/change/multiple_mode.cpp
View File

@@ -7,11 +7,15 @@
using namespace std;
using namespace cv;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于切换预测模式和猜测模式，但最终未使用
// ---------------------------------------------------------------------------------------------------------------------
void Energy::multipleMode(cv::Mat &src) {
if (is_predicting) {
getPredictPoint(target_point);
getAimPoint(predict_point);
judgeShootInGimbal();
judgeShoot();
sendEnergy();
} else if (is_guessing && stayGuessing()) {
findFans(src);

6
energy/src/energy/change/target_change.cpp
View File

@@ -11,11 +11,11 @@ using namespace cv;
// 此函数用于判断目标是否切换
// ---------------------------------------------------------------------------------------------------------------------
void Energy::changeTarget() {
if (pointDistance(target_point, last_target_point) < energy_part_param_.TARGET_CHANGE_DISTANCE_MAX) {
if (abs(last_target_polar_angle_judge_change - target_polar_angle) < 20 ||
abs(last_target_polar_angle_judge_change - target_polar_angle) > 340) {
change_target = false;
} else {
change_target = true;
}
last_target_point = target_point;
// cout << change_target << endl;
last_target_polar_angle_judge_change = target_polar_angle;
}

7
energy/src/energy/clear/clear.cpp
View File

@@ -26,8 +26,6 @@ void Energy::clearAll() {
// 此函数用于图像预处理
// ---------------------------------------------------------------------------------------------------------------------
void Energy::initImage(cv::Mat &src) {
// imagePreprocess(src);
// if(show_process)imshow("img_preprocess", src);
if (src.type() == CV_8UC3){
cvtColor(src, src, COLOR_BGR2GRAY);
}
@@ -36,8 +34,7 @@ void Energy::initImage(cv::Mat &src) {
} else if(mcu_data.enemy_color == ENEMY_RED){
threshold(src, src, energy_part_param_.BLUE_GRAY_THRESH, 255, THRESH_BINARY);
}
if (show_process){
imshow("bin", src);
}
if (show_process) imshow("bin", src);
if (show_energy || show_process)waitKey(1);
}

222
energy/src/energy/clear/energy_init.cpp
View File

@@ -14,24 +14,15 @@ using std::vector;
// 此函数对能量机关成员变量进行初始化
// ---------------------------------------------------------------------------------------------------------------------
void Energy::initEnergy() {
is_mark = false;
is_guessing = false;
is_predicting = true;
is_find_target = false;
energy_mode_init = true;
energy_rotation_init = true;
manual_mark = false;
auto_mark = false;
start_guess = false;
change_target = false;
last_mark = 0;
radius = 0;
curr_fps = 0;
send_cnt = 0;
camera_cnt = 1;
fans_cnt = 0;
last_fans_cnt = 0;
guess_devide = 0;
@@ -45,10 +36,11 @@ void Energy::initEnergy() {
extra_delta_x = 0;
target_polar_angle = -1000;
last_target_polar_angle = -1000;
last_target_polar_angle_judge_change = -1000;
last_target_polar_angle_judge_rotation = -1000;
guess_polar_angle = -1000;
last_base_angle = -1000;
predict_rad = 25;
predict_rad = 0;
predict_rad_norm = 25;
attack_distance = ATTACK_DISTANCE;
center_delta_yaw = 1000;
@@ -63,7 +55,6 @@ void Energy::initEnergy() {
circle_center_point = Point(0, 0);
target_point = Point(0, 0);
last_target_point = Point(0, 0);
guess_point = Point(0, 0);
predict_point = Point(0, 0);
@@ -82,154 +73,69 @@ void Energy::initEnergy() {
// 此函数对能量机关参数进行初始化
// ---------------------------------------------------------------------------------------------------------------------
void Energy::initEnergyPartParam() {
// gimbal_energy_part_param_.GRAY_THRESH = 120;//home
// gimbal_energy_part_param_.GRAY_THRESH = 200;//official
gimbal_energy_part_param_.RED_GRAY_THRESH = 180;//game
gimbal_energy_part_param_.BLUE_GRAY_THRESH = 100;//game
gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 180;
energy_part_param_.RED_GRAY_THRESH = 180;//game
energy_part_param_.BLUE_GRAY_THRESH = 100;//game
energy_part_param_.SPLIT_GRAY_THRESH = 180;
gimbal_energy_part_param_.FAN_CONTOUR_AREA_MAX = 5000;
gimbal_energy_part_param_.FAN_CONTOUR_AREA_MIN = 1500;
gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 45;
gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 100;
gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 10;
gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 52;
gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 3.5;
gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1.2;
gimbal_energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN = 0.6;
gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.8;
gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MIN = 0.48;
// gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.3;
// gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MIN = 0.16;
energy_part_param_.FAN_CONTOUR_AREA_MAX = 5000;
energy_part_param_.FAN_CONTOUR_AREA_MIN = 1500;
energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 45;
energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 100;
energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 10;
energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 52;
energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 3.5;
energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1.2;
energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN = 0.6;
gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 500;
gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 180;
gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 10;
gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 50;
gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 0;
gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 30;
gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;
energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 500;
energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 180;
energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 10;
energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 50;
energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 0;
energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 30;
energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;
gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 200;
gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 40;
gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 6;
gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 20;
gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 6;
gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 20;
gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 2;
gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1;
gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.6;
gimbal_energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN = 10;
energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 200;
energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 40;
energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 6;
energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 20;
energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 6;
energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 20;
energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 2;
energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1;
energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.6;
energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN = 10;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 2000;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 500;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MIN = 60;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX = 100;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN = 20;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 52;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 2.8;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1.2;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.58;
gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.34;
gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.58;
gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.34;
// gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.2;
// gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.08;
energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 2000;
energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 500;
energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MIN = 60;
energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX = 100;
energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN = 20;
energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 52;
energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 2.8;
energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1.2;
energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.58;
energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.34;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MAX = 700;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 100;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 32;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 55;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 4;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 7;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 3;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.6;
gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 100;
energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MAX = 700;
energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 100;
energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 32;
energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 55;
energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 4;
energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20;
energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 7;
energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 3;
energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.6;
energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 100;
gimbal_energy_part_param_.TWIN_ANGEL_MAX = 10;
gimbal_energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN = 40;
energy_part_param_.TWIN_ANGEL_MAX = 10;
energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN = 40;
gimbal_energy_part_param_.TARGET_CHANGE_DISTANCE_MAX = 20;
gimbal_energy_part_param_.TWIN_POINT_MAX = 20;
energy_part_param_.TWIN_POINT_MAX = 20;
gimbal_energy_part_param_.STRIP_ARMOR_DISTANCE_MIN = 28;
gimbal_energy_part_param_.STRIP_ARMOR_DISTANCE_MAX = 52;
chassis_energy_part_param_.GRAY_THRESH = 120;//home
// chassis_energy_part_param_.GRAY_THRESH = 200;//official
// chassis_energy_part_param_.GRAY_THRESH = 225;
chassis_energy_part_param_.SPLIT_GRAY_THRESH = 230;
chassis_energy_part_param_.FAN_CONTOUR_AREA_MAX = 17000;
chassis_energy_part_param_.FAN_CONTOUR_AREA_MIN = 0;
chassis_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 90;
chassis_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 140;
chassis_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 35;
chassis_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 65;
chassis_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 4;
chassis_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1;
chassis_energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN = 0.6;
chassis_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.8;
chassis_energy_part_param_.FAN_NON_ZERO_RATE_MIN = 0.48;
// chassis_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.3;
// chassis_energy_part_param_.FAN_NON_ZERO_RATE_MIN = 0.16;
chassis_energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 100000;
chassis_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0;
chassis_energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 30;
chassis_energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 50;
chassis_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 13;
chassis_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 33;
chassis_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
chassis_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;
chassis_energy_part_param_.ARMOR_CONTOUR_AREA_RATIO_MIN = 0.7;
chassis_energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 100000;
chassis_energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 0;
chassis_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 10;
chassis_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 30;
chassis_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 10;
chassis_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 30;
chassis_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 3;
chassis_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1;
chassis_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.7;
chassis_energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN = 10;
chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 17000;
chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 0;
chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MIN = 90;
chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX = 140;
chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN = 35;
chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 65;
chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 3;
chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1;
chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.55;
chassis_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.25;
chassis_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.48;
chassis_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.25;
// chassis_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.2;
// chassis_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.08;
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_LENGTH_MIN = 55;
chassis_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 95;
chassis_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 8;
chassis_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 25;
chassis_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 12;
// chassis_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 4;
chassis_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 2.7;
chassis_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.5;
chassis_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 300;
chassis_energy_part_param_.TWIN_ANGEL_MAX = 10;
chassis_energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN = 60;
chassis_energy_part_param_.TARGET_CHANGE_DISTANCE_MAX = 20;
chassis_energy_part_param_.TWIN_POINT_MAX = 20;
energy_part_param_.STRIP_ARMOR_DISTANCE_MIN = 28;
energy_part_param_.STRIP_ARMOR_DISTANCE_MAX = 52;
}
@@ -237,12 +143,12 @@ void Energy::initEnergyPartParam() {
// 此函数对能量机关旋转方向进行初始化
// ---------------------------------------------------------------------------------------------------------------------
void Energy::initRotation() {
if (target_polar_angle >= -180 && last_target_polar_angle >= -180
&& fabs(target_polar_angle - last_target_polar_angle) < 30) {
//target_polar_angle和last_target_polar_angle的初值均为1000，大于-180表示刚开始几帧不要
if (target_polar_angle >= -180 && last_target_polar_angle_judge_rotation >= -180
&& fabs(target_polar_angle - last_target_polar_angle_judge_rotation) < 30) {
//target_polar_angle和last_target_polar_angle_judge_rotation的初值均为1000，大于-180表示刚开始几帧不要
//若两者比较接近，则说明没有切换目标，因此可以用于顺逆时针的判断
if (target_polar_angle < last_target_polar_angle) clockwise_rotation_init_cnt++;
else if (target_polar_angle > last_target_polar_angle) anticlockwise_rotation_init_cnt++;
if (target_polar_angle < last_target_polar_angle_judge_rotation) clockwise_rotation_init_cnt++;
else if (target_polar_angle > last_target_polar_angle_judge_rotation) anticlockwise_rotation_init_cnt++;
}
//由于刚开始圆心判断不准，角度变化可能计算有误，因此需要在角度正向或逆向变化足够大时才可确定是否为顺逆时针
if (clockwise_rotation_init_cnt == 15) {
@@ -254,5 +160,5 @@ void Energy::initRotation() {
cout << "rotation: " << energy_rotation_direction << endl;
energy_rotation_init = false;
}
last_target_polar_angle = target_polar_angle;
last_target_polar_angle_judge_rotation = target_polar_angle;
}

41
energy/src/energy/energy.cpp
View File

@@ -21,8 +21,6 @@ Energy::Energy(Serial &u, uint8_t &color) : serial(u), ally_color(color),
is_big = false;
is_small = false;
is_gimbal = true;
is_chassis = false;
}
@@ -39,48 +37,9 @@ void Energy::setEnergyInit() {
initEnergy();
initEnergyPartParam();
is_big = false;
is_small = false;
is_gimbal = true;
is_chassis = false;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数为大能量机关再初始化函数
// ---------------------------------------------------------------------------------------------------------------------
void Energy::setBigEnergyInit() {
initEnergy();
initEnergyPartParam();
is_big = true;
is_small = false;
is_gimbal = true;
is_chassis = false;
FILE *fp = fopen(PROJECT_DIR"/Mark/delta.txt", "r");
if (fp) {
fscanf(fp, "delta_x: %d, delta_y: %d", &manual_delta_x, &manual_delta_y);
fclose(fp);
}
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数为大能量机关再初始化函数
// ---------------------------------------------------------------------------------------------------------------------
void Energy::setSmallEnergyInit() {
initEnergy();
initEnergyPartParam();
is_big = false;
is_small = true;
is_gimbal = true;
is_chassis = false;
FILE *fp = fopen(PROJECT_DIR"/Mark/delta.txt", "r");
if (fp) {
fscanf(fp, "delta_x: %d, delta_y: %d", &manual_delta_x, &manual_delta_y);
fclose(fp);
}
}

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

@@ -13,7 +13,7 @@ using std::vector;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于寻找图像内所有的大风车扇叶
// ---------------------------------------------------------------------------------------------------------------------
int Energy::findFans(const cv::Mat src) {
int Energy::findFans(const cv::Mat &src) {
if (src.empty()) {
if (show_info) cout << "empty!" << endl;
return 0;
@@ -33,21 +33,7 @@ int Energy::findFans(const cv::Mat src) {
continue;
}
fans.emplace_back(cv::minAreaRect(fan_contour));
// RotatedRect cur_rect = minAreaRect(fan_contour);
// Size2f cur_size = cur_rect.size;
// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;
// float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
// double cur_contour_area = contourArea(fan_contour);
// float length_width_ratio = length / width;
// cout << "area: " << cur_contour_area << '\t' << endl;
// cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
// cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
// cout<<endl;
}
// showFans("fan", src_bin);
if (fans.size() < last_fans_cnt) {
last_fans_cnt = static_cast<int>(fans.size());
return -1;//寻找到的扇叶比上一帧少，说明该帧有误，返回-1
@@ -60,7 +46,7 @@ int Energy::findFans(const cv::Mat src) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于寻找图像内所有的大风车装甲板模块
// ---------------------------------------------------------------------------------------------------------------------
int Energy::findArmors(const cv::Mat src) {
int Energy::findArmors(const cv::Mat &src) {
if (src.empty()) {
if (show_info) cout << "empty!" << endl;
return 0;
@@ -96,19 +82,6 @@ int Energy::findArmors(const cv::Mat src) {
continue;
}
armors.emplace_back(cv::minAreaRect(armor_contour));
// RotatedRect cur_rect = minAreaRect(armor_contour);
// Size2f cur_size = cur_rect.size;
// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;
// float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
// double cur_contour_area = contourArea(armor_contour);
// float length_width_ratio = length / width;
// cout << "area: " << cur_contour_area << '\t' << endl;
// cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
// cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
// cout<<endl;
}
if (show_info) {
@@ -122,7 +95,7 @@ int Energy::findArmors(const cv::Mat src) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于寻找图像内大风车中心字母“R”
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::findCenterR(const cv::Mat src) {
bool Energy::findCenterR(const cv::Mat &src) {
if (src.empty()) {
if (show_info) cout << "empty!" << endl;
return false;
@@ -145,23 +118,9 @@ bool Energy::findCenterR(const cv::Mat src) {
target_armor.size.height > target_armor.size.width ? target_armor.size.height : target_armor.size.width;
circle_center_point = centerR.center;
circle_center_point.y += target_length / 7.5;//实际最小二乘得到的中心在R的下方
// RotatedRect cur_rect = minAreaRect(center_R_contour);
// Size2f cur_size = cur_rect.size;
// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;
// float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
// double cur_contour_area = contourArea(center_R_contour);
// float length_width_ratio = length / width;
// cout << "area: " << cur_contour_area << '\t' << endl;
// cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
// cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
// cout<<endl;
return true;
}
if (show_info)cout << "find center R false!" << endl;
// cv::waitKey(0);
return false;
}
@@ -170,7 +129,7 @@ bool Energy::findCenterR(const cv::Mat src) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断找到的矩形候选区是否为含流动条的扇叶
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::findFlowStripFan(const cv::Mat src) {
bool Energy::findFlowStripFan(const cv::Mat &src) {
if (src.empty()) {
if (show_info) cout << "empty!" << endl;
return false;
@@ -194,22 +153,9 @@ bool Energy::findFlowStripFan(const cv::Mat src) {
continue;
}
flow_strip_fans.emplace_back(cv::minAreaRect(flow_strip_fan_contour));
// RotatedRect cur_rect = minAreaRect(flow_strip_fan_contour);
// Size2f cur_size = cur_rect.size;
// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;
// float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
// double cur_contour_area = contourArea(flow_strip_fan_contour);
// float length_width_ratio = length / width;
// cout << "area: " << cur_contour_area << '\t' << endl;
// cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
// cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
// cout<<endl;
}
if (flow_strip_fans.empty()) {
if (show_info)cout << "flow strip fan false!" << endl;\
// waitKey(0);
return false;
}
return true;
@@ -219,7 +165,7 @@ bool Energy::findFlowStripFan(const cv::Mat src) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于寻找流动条
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::findFlowStrip(const cv::Mat src) {
bool Energy::findFlowStrip(const cv::Mat &src) {
if (src.empty()) {
if (show_info) cout << "empty!" << endl;
return false;
@@ -268,125 +214,13 @@ bool Energy::findFlowStrip(const cv::Mat src) {
} else {
continue;
}
// Size2f cur_size = cur_rect.size;
// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;
// float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
// double cur_contour_area = contourArea(flow_strip_contour);
// float length_width_ratio = length / width;
// cout << "area: " << cur_contour_area << '\t' << endl;
// cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
// cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
// cout<<endl;
}
}
if (flow_strips.empty()) {
if (show_info)cout << "flow strip false!" << endl;
// waitKey(0);
return false;
} else if (flow_strips.size() > 1) {
if (show_info)cout << "Too many flow strips!" << endl;
// waitKey(0);
return false;
} else {
flow_strip = flow_strips.at(0);
for (auto &candidate_flow_strip_fan: flow_strip_fans) {
std::vector<cv::Point2f> intersection;
if (rotatedRectangleIntersection(flow_strip, candidate_flow_strip_fan, intersection) == 0) {
continue;
} else if (contourArea(intersection) > energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) {
flow_strip_fan = candidate_flow_strip_fan;
}
}
int i = 0;
for (i = 0; i < target_armors.size(); ++i) {
std::vector<cv::Point2f> intersection;
if (rotatedRectangleIntersection(target_armors.at(i), flow_strip_fan, intersection) == 0)
continue;//返回0表示没有重合面积
double cur_contour_area = contourArea(intersection);
if (cur_contour_area > energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN) {
target_armor = target_armors.at(i);
target_point = target_armor.center;
}
}
}
return true;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于寻找流动条
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::findFlowStripSub(const cv::Mat src) {
if (src.empty()) {
if (show_info) cout << "empty!" << endl;
return false;
}
cv::Mat src_bin;
src_bin = src.clone();
if (src_bin.type() == CV_8UC1) // 黑白图像
{
cvtColor(src_bin, src_bin, COLOR_GRAY2RGB);
}
std::vector<cv::RotatedRect> candidate_target_armors = target_armors;
for (auto &candidate_target_armor: candidate_target_armors) {
Point2f vertices[4];
candidate_target_armor.size.height *= 1.3;
candidate_target_armor.size.width *= 1.3;
candidate_target_armor.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++) {
line(src_bin, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 0), 20);
}
}
cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
FlowStripStructSub(src_bin);//图像膨胀，防止图像断开并更方便寻找
if (show_process)imshow("flow strip struct", src_bin);
std::vector<vector<Point> > flow_strip_contours;
findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
for (auto candidate_flow_strip_fan: flow_strip_fans) {
for (auto &flow_strip_contour : flow_strip_contours) {
if (!isValidFlowStripContour(flow_strip_contour)) {
continue;
}
std::vector<cv::Point2f> intersection;
RotatedRect cur_rect = minAreaRect(flow_strip_contour);
if (rotatedRectangleIntersection(cur_rect, candidate_flow_strip_fan, intersection) == 0) {
continue;
} else if (contourArea(intersection) > energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) {
flow_strips.emplace_back(cv::minAreaRect(flow_strip_contour));
// cout << "intersection: " << contourArea(intersection) << '\t' << cur_rect.center << endl;
} else {
continue;
}
// Size2f cur_size = cur_rect.size;
// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;
// float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
// double cur_contour_area = contourArea(flow_strip_contour);
// float length_width_ratio = length / width;
// cout << "area: " << cur_contour_area << '\t' << endl;
// cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
// cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
// cout<<endl;
}
}
if (flow_strips.empty()) {
if (show_info)cout << "flow strip false!" << endl;
// waitKey(0);
return false;
} else if (flow_strips.size() > 1) {
if (show_info)cout << "Too many flow strips!" << endl;
// waitKey(0);
return false;
} else {
flow_strip = flow_strips.at(0);
@@ -417,7 +251,7 @@ bool Energy::findFlowStripSub(const cv::Mat src) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于弱识别寻找流动条
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::findFlowStripWeak(const cv::Mat src) {
bool Energy::findFlowStripWeak(const cv::Mat &src) {
if (src.empty()) {
if (show_info) cout << "empty!" << endl;
return false;
@@ -445,7 +279,6 @@ bool Energy::findFlowStripWeak(const cv::Mat src) {
FlowStripStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找
if (show_process)imshow("weak struct", src_bin);
// waitKey(0);
std::vector<vector<Point> > flow_strip_contours;
findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
@@ -454,24 +287,8 @@ bool Energy::findFlowStripWeak(const cv::Mat src) {
if (!isValidFlowStripContour(flow_strip_contour)) {
continue;
}
flow_strips.emplace_back(cv::minAreaRect(flow_strip_contour));
// RotatedRect cur_rect = minAreaRect(flow_strip_contour);
// Size2f cur_size = cur_rect.size;
// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;
// float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
// double cur_contour_area = contourArea(flow_strip_contour);
// float length_width_ratio = length / width;
// cout << "area: " << cur_contour_area << '\t' << endl;
// cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl;
// cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl;
// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
// cout << endl;
}
// cout << "size: " << flow_strips.size() << endl;
if (flow_strips.empty()) {
if (show_info)cout << "weak flow strip false!" << endl;
// waitKey(0);
@@ -500,7 +317,6 @@ bool Energy::findFlowStripWeak(const cv::Mat src) {
}
}
if (show_info)cout << "weak flow strip false!" << endl;
// waitKey(0);
return false;
}
}
@@ -508,7 +324,7 @@ bool Energy::findFlowStripWeak(const cv::Mat src) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于框取中心R的寻找范围
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::findCenterROI(const cv::Mat src) {
bool Energy::findCenterROI(const cv::Mat &src) {
float length = target_armor.size.height > target_armor.size.width ?
target_armor.size.height : target_armor.size.width;

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

@@ -11,43 +11,6 @@ using std::endl;
using std::vector;
//----------------------------------------------------------------------------------------------------------------------
// 此函数根据矩形重合面积匹配扇叶与装甲板
// ---------------------------------------------------------------------------------------------------------------------
void Energy::findTargetByIntersection() {
if (fans.size() >= armors.size()) return;//扇叶多于装甲板，识别错误
for (const auto &fan : fans) {
for (const auto &armor : armors) {
}
}
/* if (fans.empty()) {
target_point = armors.at(0).rect.center;
return;
}
int i = 0, j = 0;
while (i < armors.size()) {
for (j = 0; j < fans.size(); ++j) {
std::vector<cv::Point2f> intersection;
if (rotatedRectangleIntersection(armors.at(i).rect, fans.at(j).rect, intersection) == 0)//返回0表示没有重合面积
continue;
else
rotatedRectangleIntersection(armors.at(i).rect, fans.at(j).rect, intersection);
double cur_contour_area = contourArea(intersection);
if (cur_contour_area > energy_part_param_.INTERSETION_CONTOUR_AREA_MIN) {
// cout << endl;
// cout << "NO. " << i << " armor and No. " << j << "fans are matched, the intersection area is"
// << cur_contour_area << endl;
break;
}
}
if (j == fans.size()) {
target_point = armors.at(i).rect.center;
break;
}
i++;
}*/
}
//----------------------------------------------------------------------------------------------------------------------

0
energy/src/energy/get/aim_point_get.cpp → energy/src/energy/get/get_aim_point.cpp
View File

34
energy/src/energy/get/get_center.cpp Normal file
View File

@@ -0,0 +1,34 @@
//
// Created by sun on 19-7-10.
//
#include "energy/energy.h"
#include "log.h"
#include "config/setconfig.h"
using namespace std;
using namespace cv;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于对心
// ---------------------------------------------------------------------------------------------------------------------
void Energy::getCenter(){
int compensate_yaw = 0, compensate_pitch = 0;
if (mcu_data.enemy_color == ENEMY_BLUE) {
compensate_yaw = RED_COMPENSATE_YAW;
compensate_pitch = RED_COMPENSATE_PITCH;
} else if (mcu_data.enemy_color == ENEMY_RED) {
compensate_yaw = BLUE_COMPENSATE_YAW;
compensate_pitch = BLUE_COMPENSATE_PITCH;
}
double dx = -(circle_center_point.x - 320 - compensate_yaw);
double dy = -(circle_center_point.y - 240 - compensate_pitch);
yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;
pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;
if (abs(yaw_rotation) < 0.7 && abs(pitch_rotation) < 0.7) {
shoot = 2;
} else
shoot = 1;
}

2
energy/src/energy/get/guess_point_get.cpp → energy/src/energy/get/get_guess_point.cpp
View File

@@ -52,7 +52,7 @@ bool Energy::guessTarget() {
guess_polar_angle = base_angle + guess_devide * 72;
}
if (guess_polar_angle > 180)guess_polar_angle -= 360;
radius = pointDistance(target_point, circle_center_point);
double 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;

7
energy/src/energy/get/polar_angle_get.cpp → energy/src/energy/get/get_polar_angle.cpp
View File

@@ -16,13 +16,6 @@ void Energy::getTargetPolarAngle() {
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于存储图像中所有装甲板的中心坐标，以便后续最小二乘法计算圆心和半径
// ---------------------------------------------------------------------------------------------------------------------
void Energy::getAllTargetArmorCenters() {
all_target_armor_centers.emplace_back(target_point);
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于存储近30帧图像中所有装甲板的中心坐标，用于判断小符和大符

0
energy/src/energy/get/predict_point_get.cpp → energy/src/energy/get/get_predict_point.cpp
View File

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

@@ -1,50 +0,0 @@
//
// Created by xixiliadorabarry on 19-3-23.
//
#include "energy/energy.h"
#include <cmath>
#include "log.h"
using namespace cv;
using std::cout;
using std::endl;
using std::vector;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于操作手手动标定
// ---------------------------------------------------------------------------------------------------------------------
void Energy::changeMark() {
if (mcu_data.mark == 0 && last_mark == 1) {//完成标定
last_mark = mcu_data.mark;
origin_yaw = mcu_data.curr_yaw;
origin_pitch = mcu_data.curr_pitch;
is_mark = false;
manual_mark = true;
// LOGM(STR_CTR(WORD_LIGHT_YELLOW, "IsMark"));
} else if (mcu_data.mark == 1) {//正在标定
last_mark = mcu_data.mark;
is_mark = true;
// LOGM(STR_CTR(WORD_BLUE,"Marking..."));
} else {//未在标定
last_mark = mcu_data.mark;
is_mark = false;
}
//cout<<"mark: "<<int(mcu_data.mark)<<endl;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于计算云台应当转到的角度
// ---------------------------------------------------------------------------------------------------------------------
void Energy::gimbalRotation() {
cv::Point2f real_predict_point;//计算在真实世界中的预测点位，进而计算云台的旋转角度
stretch(predict_point, real_predict_point);
yaw_rotation = static_cast<float>(180 / PI *
atan2((attack_distance * tan(origin_yaw * PI / 180) - real_predict_point.x),
attack_distance));
pitch_rotation = static_cast<float>(180 / PI *
atan2((attack_distance * tan(origin_pitch * PI / 180) - real_predict_point.y),
attack_distance));
}

51
energy/src/energy/get/origin_get.cpp
View File

@@ -1,51 +0,0 @@
//
// Created by sun on 19-7-10.
//
#include "energy/energy.h"
#include "log.h"
#include "config/setconfig.h"
using namespace std;
using namespace cv;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于对心
// ---------------------------------------------------------------------------------------------------------------------
void Energy::getCenter(){
double dx = -(circle_center_point.x - 320 - COMPENSATE_YAW);
double dy = -(circle_center_point.y - 240 - COMPENSATE_PITCH);
yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;
pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;
if (abs(yaw_rotation) < 0.5 && abs(pitch_rotation) < 0.5) {
shoot = 4;
} else
shoot = 2;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于获得云台对心得到的初始yaw和pitch（即以该yaw和pitch发射子弹，可以击中风车中心）
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::getOrigin() {
if (!auto_mark && !manual_mark) {
double dx = -(circle_center_point.x - 320 - COMPENSATE_YAW);
double dy = -(circle_center_point.y - 240 - COMPENSATE_PITCH);
center_delta_yaw = static_cast<float>(atan(dx / FOCUS_PIXAL) * 180 / PI);
center_delta_pitch = static_cast<float>(atan(dy / FOCUS_PIXAL) * 180 / PI);
shoot = 1;
sendEnergy();
if (abs(center_delta_yaw) > 0.3 || abs(center_delta_pitch) > 0.3) {
return false;
} else {
origin_yaw = mcu_data.curr_yaw;
origin_pitch = mcu_data.curr_pitch;
auto_mark = true;
LOGM(STR_CTR(WORD_BLUE_CODE, "auto mark success!"));
return true;
}
}
return true;
}

22
energy/src/energy/judge/judge_shoot.cpp
View File

@@ -10,34 +10,18 @@ using namespace std;
using namespace cv;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断世界坐标系下是否可以发弹
// ---------------------------------------------------------------------------------------------------------------------
void Energy::judgeShootInWorld() {
if (abs(yaw_rotation - mcu_data.curr_yaw) < 0.5 && abs(pitch_rotation - mcu_data.curr_pitch) < 0.5) {
shoot = 4;
// is_predicting = false;
// is_guessing = true;
// start_guess = true;
// getsystime(time_start_guess);
LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!"));
} else
shoot = 2;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断云台坐标系下是否可以发弹
// ---------------------------------------------------------------------------------------------------------------------
void Energy::judgeShootInGimbal() {
void Energy::judgeShoot() {
if (abs(yaw_rotation) < 0.7 && abs(pitch_rotation) < 0.7) {
shoot = 4;
shoot = 2;
// is_predicting = false;
// is_guessing = true;
// start_guess = true;
// getsystime(time_start_guess);
// LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!"));
} else
shoot = 2;
// cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;
shoot = 1;
}

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

@@ -13,7 +13,7 @@ extern McuData mcu_data;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于记录操作手的微调dx和dy
// ---------------------------------------------------------------------------------------------------------------------
void Energy::writeDownSlightChange(cv::Mat &src) {
void Energy::writeDownMark(cv::Mat &src) {
if (fans_cnt >= 2) {
FILE *fp_delta = fopen(PROJECT_DIR"/Mark/delta.txt", "w");
if (fp_delta) {
@@ -43,29 +43,3 @@ void Energy::writeDownSlightChange(cv::Mat &src) {
fclose(fp_data);
}
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于记录操作手手动标定的初始对心角度
// ---------------------------------------------------------------------------------------------------------------------
void Energy::writeDownMark(cv::Mat &src) {
if (findFans(src) >= 3) {
FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "w");
if (fp) {
fprintf(fp, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch);
fclose(fp);
}
FILE *fp_all = fopen(PROJECT_DIR"/Mark/mark_all.txt", "a");
if (fp_all) {
fprintf(fp_all, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch);
fclose(fp_all);
}
}
if (findFans(src) == 4) {
FILE *fp_best = fopen(PROJECT_DIR"/Mark/mark_best.txt", "a");
if (fp_best) {
fprintf(fp_best, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch);
fclose(fp_best);
}
}
}

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

@@ -9,150 +9,43 @@
using namespace std;
using namespace cv;
//----------------------------------------------------------------------------------------------------------------------
// 此函数为能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头
// ---------------------------------------------------------------------------------------------------------------------
void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
if (chassis_src.empty())
runBig(gimbal_src);//仅拥有云台摄像头则调用单摄像头的run函数
else if (is_gimbal) {
energy_part_param_ = gimbal_energy_part_param_;
clearAll();
initImage(gimbal_src);
camera_cnt = 2;
if (findArmors(gimbal_src) < 1)return;
if (show_energy)showArmors("armor", gimbal_src);
if (!findFlowStripFan(gimbal_src))return;
if (!findTargetInFlowStripFan()) return;
if (!findFlowStrip(gimbal_src))return;
if (!findCenterROI(gimbal_src))return;
if (show_energy)showFlowStripFan("strip", gimbal_src);
if (!findCenterR(gimbal_src))return;
if (show_energy)showCenterR("R", gimbal_src);
if (!getOrigin())return;
startChassis();
initEnergy();
destroyAllWindows();
} else if (is_chassis) {
energy_part_param_ = chassis_energy_part_param_;
clearAll();
initImage(chassis_src);
changeMark();
if (is_mark)return;//操作手强制手动标定origin_yaw和origin_pitch
if (findArmors(chassis_src) < 1)return;
if (show_energy)showArmors("armor", chassis_src);
if (!findFlowStripFan(chassis_src))return;
if (!findTargetInFlowStripFan()) return;
if (!findFlowStrip(gimbal_src))return;
if (!findCenterROI(chassis_src))return;
if (show_energy)showFlowStripFan("strip", chassis_src);
if (!findCenterR(chassis_src))return;
if (show_energy)showCenterR("R", chassis_src);
getTargetPolarAngle();
changeTarget();
if (is_big && energy_rotation_init) {
initRotation();
return;
}
if (save_mark)writeDownMark(chassis_src);
getPredictPoint(target_point);
gimbalRotation();
judgeShootInWorld();
sendEnergy();
}
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数为能量机关模式主控制流函数，且步兵仅拥有云台摄像头
// ---------------------------------------------------------------------------------------------------------------------
void Energy::runBig(cv::Mat &gimbal_src) {
energy_part_param_ = gimbal_energy_part_param_;
void Energy::run(cv::Mat &src) {
clearAll();
initImage(gimbal_src);
initImage(src);
if (show_process)imshow("bin", gimbal_src);
if (findArmors(gimbal_src) < 1)return;
if (show_energy)showArmors("armor", gimbal_src);
if (!findFlowStripFan(gimbal_src)) {
if (!findFlowStripWeak(gimbal_src)) return;
if (show_process)imshow("bin", src);
if (findArmors(src) < 1)return;
if (show_energy)showArmors("armor", src);
if (!findFlowStripFan(src)) {
if (!findFlowStripWeak(src)) return;
} else {
if (show_energy)showFlowStripFan("strip fan", gimbal_src);
if (show_energy)showFlowStripFan("strip fan", src);
if (!findTargetInFlowStripFan()) return;
if(!findFlowStrip(gimbal_src)){
if(!findFlowStripSub(gimbal_src)) {
return;
}
}
if (!findFlowStrip(src)) return;
}
if (!findCenterROI(gimbal_src))return;
if (show_energy)showFlowStrip("strip", gimbal_src);
if (!findCenterR(gimbal_src))return;
if (show_energy)showCenterR("R", gimbal_src);
fans_cnt = findFans(gimbal_src);
if (show_energy)showFans("fans", gimbal_src);
// getCenter();
// sendEnergy();
// return;
findCenterROI(src);
if (show_energy)showFlowStrip("strip", src);
if (!findCenterR(src)) return;
if (show_energy)showCenterR("R", src);
fans_cnt = findFans(src);
if (show_energy)showFans("fans", src);
changeTarget();
getTargetPolarAngle();
if (energy_rotation_init) {
if (is_big && energy_rotation_init) {
initRotation();
return;
}
getPredictPoint(target_point);
getAimPoint(predict_point);
// cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << '\t' << "shoot: " << shoot << endl;
// waitKey(0);
judgeShootInGimbal();
judgeShoot();
sendEnergy();
if (save_mark)writeDownSlightChange(gimbal_src);
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头
// ---------------------------------------------------------------------------------------------------------------------
void Energy::runSmall(cv::Mat &gimbal_src) {
energy_part_param_ = gimbal_energy_part_param_;
clearAll();
initImage(gimbal_src);
if (show_process)imshow("bin", gimbal_src);
if (findArmors(gimbal_src) < 1)return;
if (show_energy)showArmors("armor", gimbal_src);
if (!findFlowStripFan(gimbal_src)) {
if (!findFlowStripWeak(gimbal_src)) return;
} else {
if (show_energy)showFlowStripFan("strip fan", gimbal_src);
if (!findTargetInFlowStripFan()) return;
if (!findFlowStrip(gimbal_src)) {
if(!findFlowStripSub(gimbal_src)) {
return;
}
}
}
if (show_energy)showTarget("target", gimbal_src);
fans_cnt = findFans(gimbal_src);
if (show_energy)showFans("fans", gimbal_src);
// getCenter();
// sendEnergy();
// return;
changeTarget();
getPredictPoint(target_point);
getAimPoint(predict_point);
judgeShootInGimbal();
sendEnergy();
if (save_mark)writeDownSlightChange(gimbal_src);
if (save_mark)writeDownMark(src);
}

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

@@ -14,127 +14,34 @@ using namespace std;
// 此函数用于发送能量机关数据
// ---------------------------------------------------------------------------------------------------------------------
void Energy::sendEnergy() {
if (is_big) {
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
if (ROBOT_ID == 4) {
MINMAX(sum_yaw, -100, 100);
MINMAX(sum_yaw, -100, 100);
} else if (ROBOT_ID == 3 || ROBOT_ID == 7 || ROBOT_ID == 8) {
float yaw_I_component = BIG_YAW_AIM_KI * sum_yaw;
float pitch_I_component = BIG_PITCH_AIM_KI * sum_pitch;
MINMAX(yaw_I_component, -2, 2);
MINMAX(pitch_I_component, -2, 2);
}
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
float yaw_I_component = YAW_AIM_KI * sum_yaw;
float pitch_I_component = PITCH_AIM_KI * sum_pitch;
MINMAX(yaw_I_component, -2, 2);
MINMAX(pitch_I_component, -2, 2);
double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_rotation;
if (mcu_data.mark == 1) {
yaw_rotation = TRY_BIG_YAW_AIM_KP * yaw_rotation + TRY_BIG_YAW_AIM_KI * sum_yaw +
TRY_BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = TRY_BIG_PITCH_AIM_KP * pitch_rotation + TRY_BIG_PITCH_AIM_KI * sum_pitch +
TRY_BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
} else {
yaw_rotation = BIG_YAW_AIM_KP * yaw_rotation + BIG_YAW_AIM_KI * sum_yaw +
BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = BIG_PITCH_AIM_KP * pitch_rotation + BIG_PITCH_AIM_KI * sum_pitch +
BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
}
// cout << "yaw: "<<BIG_YAW_AIM_KP * yaw_rotation << '\t' << BIG_YAW_AIM_KI * sum_yaw << '\t'
// << BIG_YAW_AIM_KD * (yaw_rotation - last_yaw) << endl;
// cout << "pitch: "<<BIG_PITCH_AIM_KP * pitch_rotation << '\t' << BIG_PITCH_AIM_KI * sum_pitch << '\t'
// << BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch) << endl;
if (ROBOT_ID == 3 || ROBOT_ID == 7 || ROBOT_ID == 8) {
MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6);
}
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
} else if (is_small) {
sum_yaw += yaw_rotation;
sum_pitch += pitch_rotation;
if (ROBOT_ID == 4) {
MINMAX(sum_yaw, -100, 100);
MINMAX(sum_yaw, -100, 100);
} else if (ROBOT_ID == 3 || ROBOT_ID == 7 || ROBOT_ID == 8) {
float yaw_I_component = SMALL_YAW_AIM_KI * sum_yaw;
float pitch_I_component = SMALL_PITCH_AIM_KI * sum_pitch;
MINMAX(yaw_I_component, -2, 2);
MINMAX(pitch_I_component, -2, 2);
}
double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_rotation;
if (mcu_data.mark == 1) {
yaw_rotation = TRY_SMALL_YAW_AIM_KP * yaw_rotation + TRY_SMALL_YAW_AIM_KI * sum_yaw +
TRY_SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation =
TRY_SMALL_PITCH_AIM_KP * pitch_rotation + TRY_SMALL_PITCH_AIM_KI * sum_pitch +
TRY_SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch);
} else {
yaw_rotation = SMALL_YAW_AIM_KP * yaw_rotation + SMALL_YAW_AIM_KI * sum_yaw +
SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = SMALL_PITCH_AIM_KP * pitch_rotation + SMALL_PITCH_AIM_KI * sum_pitch +
SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch);
}
if (ROBOT_ID == 3 || ROBOT_ID == 7 || ROBOT_ID == 8) {
MINMAX(yaw_rotation, -6, 6);
MINMAX(pitch_rotation, -6, 6);
}
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
}
double tmp_yaw = yaw_rotation;
double tmp_pitch = pitch_rotation;
yaw_rotation = YAW_AIM_KP * yaw_rotation + YAW_AIM_KI * sum_yaw +
YAW_AIM_KD * (yaw_rotation - last_yaw);
pitch_rotation = PITCH_AIM_KP * pitch_rotation + PITCH_AIM_KI * sum_pitch +
PITCH_AIM_KD * (pitch_rotation - last_pitch);
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
if (change_target) {
sendTarget(serial, yaw_rotation, pitch_rotation, 5, 0);
sendTarget(serial, yaw_rotation, pitch_rotation, 3, 0);//表示目标切换
} else if (is_guessing) {
sendTarget(serial, yaw_rotation, pitch_rotation, 6, 0);
sendTarget(serial, yaw_rotation, pitch_rotation, 4, 0);//表示猜测模式
} else {
sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0);
sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0);//跟随或发弹
}
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于发送数据给主控板
// ---------------------------------------------------------------------------------------------------------------------
void Energy::sendTarget(Serial &serial, float x, float y, float z) {
short x_tmp, y_tmp, z_tmp;
uint8_t buff[8];
#ifdef WITH_COUNT_FPS
static auto last_time = time(nullptr);
static int fps = 0;
time_t t = time(nullptr);
if (last_time != t) {
last_time = t;
cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
curr_fps = fps;
fps = 0;
}
fps += 1;
#endif
x_tmp = static_cast<short>(x * (32768 - 1) / 100);
y_tmp = static_cast<short>(y * (32768 - 1) / 100);
z_tmp = static_cast<short>(z * (32768 - 1) / 100);
buff[0] = 's';
buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
buff[7] = 'e';
serial.WriteData(buff, sizeof(buff));
send_cnt += 1;
// LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send"));
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于发送数据给主控板
// ---------------------------------------------------------------------------------------------------------------------

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

@@ -13,7 +13,7 @@ using std::vector;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中所有扇叶
// ---------------------------------------------------------------------------------------------------------------------
void Energy::showFans(std::string windows_name, const cv::Mat src) {
void Energy::showFans(std::string windows_name, const cv::Mat &src) {
if (src.empty())return;
static Mat image2show;
@@ -38,7 +38,7 @@ void Energy::showFans(std::string windows_name, const cv::Mat src) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中所有装甲板
// ---------------------------------------------------------------------------------------------------------------------
void Energy::showArmors(std::string windows_name, const cv::Mat src) {
void Energy::showArmors(std::string windows_name, const cv::Mat &src) {
if (src.empty())return;
static Mat image2show;
@@ -60,74 +60,11 @@ void Energy::showArmors(std::string windows_name, const cv::Mat src) {
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中所有扇叶和装甲板,并框出待击打装甲板
// ---------------------------------------------------------------------------------------------------------------------
void Energy::showBoth(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();
}
for (const auto &fan : fans) {
Point2f vertices[4]; //定义矩形的4个顶点
fan.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++)
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 4);
}
for (const auto &armor : armors) {
Point2f vertices[4]; //定义矩形的4个顶点
armor.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++) {
if (pointDistance(static_cast<cv::Point2f>(armor.center), target_point) < 5)
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 4);
else
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 4);
}
cv::Point2f point = armor.center;
cv::circle(image2show, point, 2, cv::Scalar(0, 0, 255));//在图像中画出特征点，2是圆的半径
}
imshow(windows_name, image2show);
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中目标装甲板
// ---------------------------------------------------------------------------------------------------------------------
void Energy::showTarget(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();
}
for (const auto &armor : armors) {
if (pointDistance(armor.center, target_point) < energy_part_param_.TWIN_POINT_MAX) {
Point2f vertices[4]; //定义矩形的4个顶点
armor.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++)
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 2);
}
}
imshow(windows_name, image2show);
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中所有可能的风车中心候选区R
// ---------------------------------------------------------------------------------------------------------------------
void Energy::showCenterR(std::string windows_name, const cv::Mat src) {
void Energy::showCenterR(std::string windows_name, const cv::Mat &src) {
if (src.empty())return;
static Mat image2show;
@@ -154,7 +91,7 @@ void Energy::showCenterR(std::string windows_name, const cv::Mat src) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中流动条扇叶
// ---------------------------------------------------------------------------------------------------------------------
void Energy::showFlowStripFan(std::string windows_name, const cv::Mat src) {
void Energy::showFlowStripFan(std::string windows_name, const cv::Mat &src) {
if (src.empty())return;
static Mat image2show;
@@ -176,7 +113,7 @@ void Energy::showFlowStripFan(std::string windows_name, const cv::Mat src) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中流动条扇叶
// ---------------------------------------------------------------------------------------------------------------------
void Energy::showFlowStrip(std::string windows_name, const cv::Mat src) {
void Energy::showFlowStrip(std::string windows_name, const cv::Mat &src) {
if (src.empty())return;
static Mat image2show;
@@ -214,7 +151,7 @@ void Energy::showFlowStrip(std::string windows_name, const cv::Mat src) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示猜测的下一个目标点
// ---------------------------------------------------------------------------------------------------------------------
void Energy::showGuessTarget(std::string windows_name, const cv::Mat src) {
void Energy::showGuessTarget(std::string windows_name, const cv::Mat &src) {
if (src.empty())return;
static Mat image2show;

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

@@ -32,22 +32,6 @@ void Energy::rotate(cv::Point target_point) {
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数将像素差转换到实际距离差
// ---------------------------------------------------------------------------------------------------------------------
void Energy::stretch(cv::Point point_1, cv::Point2f &point_2) {
if (point_1 == circle_center_point) {
// cout<<"stretch wrong!"<<endl;
return;
}
double x_0 = point_1.x - circle_center_point.x;
double y_0 = point_1.y - circle_center_point.y;
double r_0 = sqrt(pow(x_0, 2) + pow(y_0, 2));
point_2.x = static_cast<float >( ARMOR_CENTER_TO_CYCLE_CENTER * x_0 / r_0);
point_2.y = static_cast<float >( ARMOR_CENTER_TO_CYCLE_CENTER * y_0 / r_0);
}
//----------------------------------------------------------------------------------------------------------------------