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

energy change

Browse Source
This commit is contained in:
sun
2019-07-24 11:15:21 +08:00
parent b3fc773222
commit 11f7c8b469
12 changed files with 183 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
energy/include/energy/energy.h
View File

@@ -72,6 +72,7 @@ private:
int clockwise_rotation_init_cnt;//装甲板顺时针旋转次数
int anticlockwise_rotation_init_cnt;//装甲板逆时针旋转次数
int last_mode;//上一帧的能量机关状态
int manual_delta_x, manual_delta_y;//手动微调量
float target_polar_angle;//待击打装甲板的极坐标角度
float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度
@@ -103,6 +104,9 @@ private:
std::vector<cv::RotatedRect> fans;//图像中所有扇叶
std::vector<cv::RotatedRect> armors;//图像中所有可能装甲板（可能存在误识别)
std::vector<cv::RotatedRect> flow_strip_fans;//可能的流动扇叶
std::vector<cv::RotatedRect> target_armors;//可能的目标装甲板
std::vector<cv::RotatedRect> flow_strips;//可能的流动条
std::vector<cv::Point> all_target_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算
std::queue<float> recent_target_armor_centers;//记录最近一段时间的装甲板中心，用于判断大符还是小符
@@ -152,12 +156,13 @@ private:
double pointDistance(cv::Point point_1, cv::Point point_2);//计算两点距离
double nonZeroRateOfRotateRect(cv::Mat &bin, const cv::RotatedRect &rotatedRect);//计算旋转矩形内亮点占比
void writeDownMark();//记录操作手标定的云台初始角度
void writeDownSlightChange(cv::Mat &src);//记录操作手的手动微调
void writeDownMark(cv::Mat &src);//记录操作手标定的云台对心初始角度
bool guessTarget();//获得猜测击打点位
bool getOrigin();//获得云台对心所需角度
void changeTarget();//判断目标是否改变
void multipleMode(cv::Mat gimbal_src);//多模式切换
void multipleMode(cv::Mat &src);//多模式切换
void getTargetPolarAngle();//获得目标装甲板极坐标角度
void getPredictPoint(cv::Point target_point);//获取预测点位
void getAimPoint(cv::Point target_point);//通过自瞄逻辑计算点位

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

@@ -7,18 +7,18 @@
using namespace std;
using namespace cv;
void Energy::multipleMode(cv::Mat gimbal_src) {
void Energy::multipleMode(cv::Mat &src) {
if (is_predicting) {
getPredictPoint(target_point);
getAimPoint(predict_point);
judgeShootInGimbal();
sendEnergy();
} else if (is_guessing && stayGuessing()) {
findFans(gimbal_src);
if (show_energy)showFans("fans", gimbal_src);
if (save_mark)writeDownMark();
findFans(src);
if (show_energy)showFans("fans", src);
if (save_mark)writeDownMark(src);
guessTarget();
if (show_energy)showGuessTarget("guess", gimbal_src);
if (show_energy)showGuessTarget("guess", src);
getPredictPoint(guess_point);
getAimPoint(predict_point);
sendEnergy();

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

@@ -15,6 +15,9 @@ using namespace cv;
void Energy::clearAll(){
fans.clear();
armors.clear();
flow_strip_fans.clear();
target_armors.clear();
flow_strips.clear();
}

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

@@ -36,6 +36,8 @@ void Energy::initEnergy() {
clockwise_rotation_init_cnt = 0;
anticlockwise_rotation_init_cnt = 0;
last_mode = -1;//既不是大符也不是小符
manual_delta_x = 0;
manual_delta_y = 0;
target_polar_angle = -1000;
last_target_polar_angle = -1000;
@@ -59,6 +61,9 @@ void Energy::initEnergy() {
fans.clear();
armors.clear();
flow_strip_fans.clear();
target_armors.clear();
flow_strips.clear();
all_target_armor_centers.clear();
while(!recent_target_armor_centers.empty())recent_target_armor_centers.pop();

14
energy/src/energy/energy.cpp
View File

@@ -32,7 +32,6 @@ Energy::Energy(Serial &u, uint8_t &color) : serial(u), ally_color(color),
Energy::~Energy() = default;
//----------------------------------------------------------------------------------------------------------------------
// 此函数为大能量机关再初始化函数
// ---------------------------------------------------------------------------------------------------------------------
@@ -58,8 +57,13 @@ void Energy::setBigEnergyInit() {
is_small = false;
is_gimbal = true;
is_chassis = false;
}
FILE *fp = fopen(PROJECT_DIR"/Mark/delta.txt", "r");
if (fp) {
fscanf(fp, "delta_x: %d, delta_y: %d", &manual_delta_x, &manual_delta_y);
fclose(fp);
}
}
//----------------------------------------------------------------------------------------------------------------------
@@ -73,4 +77,10 @@ void Energy::setSmallEnergyInit() {
is_small = true;
is_gimbal = true;
is_chassis = false;
FILE *fp = fopen(PROJECT_DIR"/Mark/delta.txt", "r");
if (fp) {
fscanf(fp, "%d %d", &manual_delta_x, &manual_delta_y);
fclose(fp);
}
}

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

@@ -181,14 +181,13 @@ bool Energy::findFlowStripFan(const cv::Mat src) {
if (!isValidFlowStripFanContour(src_bin, flow_strip_fan_contour)) {
continue;
}
candidate_flow_strip_fans.emplace_back(cv::minAreaRect(flow_strip_fan_contour));
flow_strip_fans.emplace_back(cv::minAreaRect(flow_strip_fan_contour));
// RotatedRect cur_rect = minAreaRect(flow_strip_fan_contour);
// Size2f cur_size = cur_rect.size;
// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;
// float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
// double cur_contour_area = contourArea(flow_strip_fan_contour);
// double non_zero_rate = nonZeroRateOfRotateRect(src_bin, cur_rect);
// if (length > 40 && width > 30 && length < 110 && width < 100) {
// cout << cur_rect.center<<endl;
// flow_strip_fan = cv::minAreaRect(flow_strip_fan_contour);
@@ -196,51 +195,13 @@ bool Energy::findFlowStripFan(const cv::Mat src) {
// cout << "non zero: " << non_zero_rate << endl;
// cout<<cur_contour_area / cur_size.area()<<endl;
// }
// cout << cur_rect.center << endl;
}
if (candidate_flow_strip_fans.size() == 1) {
flow_strip_fan = candidate_flow_strip_fans.at(0);
return true;
} else if (candidate_flow_strip_fans.size() >= 2) {
//用锤子筛选仍然有多个候选区，进一步用锤头做筛选
std::vector<cv::RotatedRect> candidate_target_fans;
for (auto candidate_flow_strip_fan: candidate_flow_strip_fans) {
flow_strip_fan = candidate_flow_strip_fan;
if (!findTargetInFlowStripFan()) {
continue;
}
candidate_target_fans.emplace_back(candidate_flow_strip_fan);
}
if (candidate_target_fans.size() == 1) {
flow_strip_fan = candidate_target_fans.at(0);
return true;
} else if(candidate_target_fans.empty()){
cout<<"No candidate target fan contains a target armor!"<<endl;
return false;
} else { //用锤子+锤头筛选仍然有多个候选区，进一步用锤柄做筛选
for (auto candidate_target_fan: candidate_target_fans) {
flow_strip_fan = candidate_target_fan;
findTargetInFlowStripFan();
cv::Mat src_mask = src.clone();
target_armor.size.height *= 1.3;
target_armor.size.width *= 1.3;
Point2f vertices[4];
vector<Point2f> mask_rect;
target_armor.points(vertices);
for (int i = 0; i < 4; i++)
line(src_mask, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 0), 20);
if (findFlowStrip(src_mask)) {
flow_strip_fan = candidate_target_fan;
return true;
}
}
cout<<"No candidate target fan contains a flow strip!"<<endl;
return false;
}
} else {
// cout << "flow_strip_fans_cnt: " << flow_strip_fans.size() << endl;
if (flow_strip_fans.empty()) {
cout << "flow strip fan false!" << endl;
// waitKey(0);
return false;
} else {
return true;
}
}
@@ -252,20 +213,45 @@ bool Energy::findFlowStrip(const cv::Mat src) {
if (src.empty())return false;
cv::Mat src_bin;
src_bin = src.clone();
if (src.type() == CV_8UC3) {
cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
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);//若读取三通道视频文件，需转换为单通道
FlowStripStruct(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;
}
flow_strip = cv::minAreaRect(flow_strip_contour);
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;
}
// RotatedRect cur_rect = minAreaRect(flow_strip_contour);
// Size2f cur_size = cur_rect.size;
// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;
@@ -276,12 +262,39 @@ bool Energy::findFlowStrip(const cv::Mat src) {
// cout << "flow strip area: " << length << '\t' << width << endl;
// }
// cout << cur_rect.center << endl;
return true;
}
}
// cout << "flow strip cnt: " << flow_strips.size() << endl;
if (flow_strips.empty()) {
cout << "flow strip false!" << endl;
// waitKey(0);
return false;
} else if (flow_strips.size() > 1) {
cout << "Too many flow strips!" << endl;
// waitKey(0);
return false;
} else {
flow_strip = flow_strips.at(0);
for (auto &candidate_flow_strip_fan: flow_strip_fans) {
std::vector<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;
}
}
}
}
@@ -289,23 +302,13 @@ bool Energy::findFlowStrip(const cv::Mat src) {
// 此函数用于框取中心R的寻找范围
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::findCenterROI(const cv::Mat src) {
cv::Mat src_mask = src.clone();
target_armor.size.height *= 1.3;
target_armor.size.width *= 1.3;
Point2f vertices[4];
vector<Point2f> mask_rect;
target_armor.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++)
line(src_mask, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 0), 20);
// imshow("fill", src_mask);
if (!findFlowStrip(src_mask))return false;
float length = target_armor.size.height > target_armor.size.width ?
target_armor.size.height : target_armor.size.width;
Point2f p2p(flow_strip.center.x - target_point.x,
flow_strip.center.y - target_point.y);
p2p = p2p / pointDistance(flow_strip.center, target_point);//单位化
center_ROI = cv::RotatedRect(cv::Point2f(flow_strip.center + p2p * length * 1.25),
center_ROI = cv::RotatedRect(cv::Point2f(flow_strip.center + p2p * length * 1.7),
Size2f(length * 1.4, length * 1.4), -90);
return true;

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

@@ -9,7 +9,6 @@ using std::endl;
using std::vector;
//----------------------------------------------------------------------------------------------------------------------
// 此函数根据矩形重合面积匹配扇叶与装甲板
// ---------------------------------------------------------------------------------------------------------------------
@@ -34,9 +33,9 @@ void Energy::findTargetByIntersection() {
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;
// cout << endl;
// cout << "NO. " << i << " armor and No. " << j << "fans are matched, the intersection area is"
// << cur_contour_area << endl;
break;
}
}
@@ -53,22 +52,26 @@ void Energy::findTargetByIntersection() {
// 此函数在流动条区域内寻找装甲板
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::findTargetInFlowStripFan() {
for (auto &candidate_flow_strip_fan: flow_strip_fans) {
int i = 0;
for (i = 0; i < armors.size(); ++i) {
std::vector<cv::Point2f> intersection;
if (rotatedRectangleIntersection(armors.at(i), flow_strip_fan, intersection) == 0)
if (rotatedRectangleIntersection(armors.at(i), candidate_flow_strip_fan, intersection) == 0)
continue;//返回0表示没有重合面积
double cur_contour_area = contourArea(intersection);
// cout<<cur_contour_area<<endl;
if (cur_contour_area < energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN)
continue;
else {
target_armor = armors.at(i);
target_point = armors.at(i).center;
if (cur_contour_area > energy_part_param_.TARGET_INTERSETION_CONTOUR_AREA_MIN) {
target_armors.emplace_back(armors.at(i));
}
}
}
// cout << "target armor cnt: " << target_armors.size() << endl;
if (target_armors.empty()) {
cout << "find target armor false" << endl;
return false;
} else {
return true;
}
}
cout<<"find target false"<<endl;
// cout<<armors.size()<<'\t'<<armors.at(0).size<<endl;
return false;
}

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

@@ -15,11 +15,11 @@ extern mcu_data mcuData;
// 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击
// ---------------------------------------------------------------------------------------------------------------------
void Energy::getAimPoint(cv::Point target_point) {
double dx = -(target_point.x - 320 - COMPENSATE_YAW - mcuData.delta_x);
double dy = -(target_point.y - 240 - COMPENSATE_PITCH - mcuData.delta_y);
double dx = -(target_point.x - 320 - COMPENSATE_YAW - mcuData.delta_x - manual_delta_x);
double dy = -(target_point.y - 240 - COMPENSATE_PITCH - mcuData.delta_y - manual_delta_y);
yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;
pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;
// cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;
cout << "mcuData.delta_x: " << mcuData.delta_x << '\t' << "mcuData.delta_y: " << mcuData.delta_y << endl;
// cout << "mcuData.delta_x: " << mcuData.delta_x << '\t' << "mcuData.delta_y: " << mcuData.delta_y << endl;
}

7
energy/src/energy/judge/judge_contour.cpp
View File

@@ -233,12 +233,5 @@ bool Energy::isValidFlowStripContour(const vector<cv::Point> &flow_strip_contour
// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl;
return false;//轮廓对矩形的面积占有率不合适
}
std::vector<cv::Point2f> intersection;
if (rotatedRectangleIntersection(cur_rect, flow_strip_fan, intersection) == 0) {
return false;
} else if (contourArea(intersection) < energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) {
// cout << "intersection: " << contourArea(intersection) << '\t' << cur_rect.center << endl;
return false;
}
return true;
}

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

@@ -7,8 +7,26 @@
using namespace std;
using namespace cv;
void Energy::writeDownMark() {
if(fans.size()>=3) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于记录操作手的微调dx和dy
// ---------------------------------------------------------------------------------------------------------------------
void Energy::writeDownSlightChange(cv::Mat &src) {
if (findFans(src) == 4){
FILE *fp_delta = fopen(PROJECT_DIR"/Mark/delta.txt", "w");
if (fp_delta) {
fprintf(fp_delta, "delta_x: %d, delta_y: %d\n", mcuData.delta_x, mcuData.delta_y);
fclose(fp_delta);
}
}
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于记录操作手手动标定的初始对心角度
// ---------------------------------------------------------------------------------------------------------------------
void Energy::writeDownMark(cv::Mat &src) {
if (findFans(src) >= 3) {
FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "w");
if (fp) {
fprintf(fp, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch);
@@ -20,10 +38,10 @@ void Energy::writeDownMark() {
fclose(fp_all);
}
}
if(fans.size()==4){
if (findFans(src) == 4) {
FILE *fp_best = fopen(PROJECT_DIR"/Mark/mark_best.txt", "a");
if(fp_best){
fprintf(fp_best, "yaw: %f, pitch: %f\n",origin_yaw, origin_pitch);
if (fp_best) {
fprintf(fp_best, "yaw: %f, pitch: %f\n", origin_yaw, origin_pitch);
fclose(fp_best);
}
}

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

@@ -25,6 +25,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
if (show_energy)showArmors("armor", gimbal_src);
if (!findFlowStripFan(gimbal_src))return;
if (!findTargetInFlowStripFan()) return;
if (!findFlowStrip(gimbal_src))return;
if (!findCenterROI(gimbal_src))return;
if (show_energy)showFlowStripFan("strip", gimbal_src);
if (!findCenterR(gimbal_src))return;
@@ -46,6 +47,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
if (show_energy)showArmors("armor", chassis_src);
if (!findFlowStripFan(chassis_src))return;
if (!findTargetInFlowStripFan()) return;
if (!findFlowStrip(gimbal_src))return;
if (!findCenterROI(chassis_src))return;
if (show_energy)showFlowStripFan("strip", chassis_src);
if (!findCenterR(chassis_src))return;
@@ -56,6 +58,7 @@ void Energy::runBig(cv::Mat &gimbal_src, cv::Mat &chassis_src) {
initRotation();
return;
}
if (save_mark)writeDownMark(chassis_src);
getPredictPoint(target_point);
gimbalRotation();
judgeShootInWorld();
@@ -77,6 +80,7 @@ void Energy::runBig(cv::Mat &gimbal_src) {
if (show_energy)showArmors("armor", gimbal_src);
if (!findFlowStripFan(gimbal_src))return;
if (!findTargetInFlowStripFan()) return;
if (!findFlowStrip(gimbal_src))return;
if (!findCenterROI(gimbal_src))return;
if (show_energy)showFlowStripFan("strip", gimbal_src);
if (!findCenterR(gimbal_src))return;
@@ -87,6 +91,7 @@ void Energy::runBig(cv::Mat &gimbal_src) {
initRotation();
return;
}
if (save_mark)writeDownSlightChange(gimbal_src);
getPredictPoint(target_point);
getAimPoint(predict_point);
judgeShootInGimbal();
@@ -107,8 +112,10 @@ void Energy::runSmall(cv::Mat &gimbal_src) {
if (show_energy)showArmors("armor", gimbal_src);
if (!findFlowStripFan(gimbal_src))return;
if (!findTargetInFlowStripFan()) return;
if (!findFlowStrip(gimbal_src))return;
changeTarget();
if (save_mark)writeDownSlightChange(gimbal_src);
getPredictPoint(target_point);
getAimPoint(predict_point);
judgeShootInGimbal();

6
main.cpp
View File

@@ -30,7 +30,7 @@ using namespace std;
mcu_data mcuData = { // 单片机端回传结构体
0, // 当前云台yaw角
0, // 当前云台pitch角
ARMOR_STATE, // 当前状态，自瞄-大符-小符
BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符
0, // 云台角度标记位
1, // 是否启用数字识别
ENEMY_RED, // 敌方颜色
@@ -120,8 +120,8 @@ int main(int argc, char *argv[]) {
if (!from_camera) extract(gimbal_src, chassis_src);
if (save_video) saveVideos(gimbal_src, chassis_src);//保存视频
if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像
energy.runBig(gimbal_src, chassis_src);
// energy.runBig(gimbal_src);
// energy.runBig(gimbal_src, chassis_src);
energy.runBig(gimbal_src);
last_state = mcuData.state;//更新上一帧状态
} else if (mcuData.state == SMALL_ENERGY_STATE) {
if (mcuData.state != SMALL_ENERGY_STATE) {