VSAG/vision_sjtu_19
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-05 14:06:40 +08:00
parent f9fcde55ac
commit 1a20133ca1
17 changed files with 325 additions and 796 deletions
Download Patch File Download Diff File Expand all files Collapse all files

177
energy/include/energy/energy.h
View File

@@ -23,130 +23,93 @@ using std::vector;
class Energy { class Energy {
public: public:
Energy(Serial &u, uint8_t &color); Energy(Serial &u, uint8_t &color);//构造函数，参数为串口和敌方颜色
~Energy(); ~Energy();//默认析构函数
int run(cv::Mat &src); int run(cv::Mat &src);
Serial &serial;//串口
cv::Point2f uart_hit_point; void setEnergyRotationInit();//判断顺逆时针函数
clock_t start; void extract(cv::Mat &src);//框取图像中的一块区域进行处理
Serial &serial; void sendTargetByUart(float x, float y, float z);//向主控板发送数据
// void setAllyColor(int color);
void setRotation(int rotation);
void setEnergyRotationInit();
void extract(cv::Mat &src);
void sendTargetByUart(float x, float y, float z);
private: private:
EnergyPartParam energy_part_param_;//能量机关的参数设置
bool isMark;//若操作手正在手动标定，则为true
int fans_cnt;//图像中的扇叶个数
int armors_cnt;//图像中的装甲板个数
int centerRs_cnt;//图像中可能的风车中心字母R选区个数
int last_fans_cnt;//上一帧的扇叶个数
int last_armors_cnt;//上一帧的装甲板个数
double radius;//大风车半径
float target_polar_angle;//待击打装甲板的极坐标角度
float last_target_polar_angle;//上一帧待击打装甲板的极坐标角度
uint8_t &ally_color;//我方颜色
int energy_rotation_direction;//风车旋转方向
float attack_distance;//步兵与风车平面距离
int send_cnt;//向主控板发送的数据总次数
float yaw_rotation;//云台yaw轴应该转到的角度
float pitch_rotation;//云台pitch轴应该转到的角度
uint8_t last_mark;//用于记录上一帧操作手是否进行标定
double predict_rad;//预测提前角
bool energy_rotation_init;//若仍在判断风车旋转方向，则为true
int clockwise_rotation_init_cnt;//装甲板顺时针旋转次数
int anticlockwise_rotation_init_cnt;//装甲板逆时针旋转次数
float red_origin_yaw, red_origin_pitch;//红方的初始云台对心角度设定值
float blue_origin_yaw, blue_origin_pitch;//蓝方的初始云台对心角度设定值
float origin_yaw, origin_pitch;//初始的云台角度设定值
float target_cnt;//用于记录寻找到的装甲板总数，该值变化则立即中断主控板发射进程，防止重复击打已点亮的装甲板
bool save_new_mark;//若操作手进行过手动标定，则为true
EnergyPartParam energy_part_param_; std::vector<EnergyPart> fans;//图像中所有扇叶
LiftHeight lift_height_; std::vector<EnergyPart> armors;//图像中所有装甲板
bool isSendTarget; std::vector<EnergyPart> centerRs;//风车中心字母R的可能候选区
bool isMark;
int fans_cnt;
int armors_cnt;
int centerRs_cnt;
int count;
int last_fans_cnt;
int last_armors_cnt;
double radius;
double target_position;
double last_target_position;
double last_hit_position;
float target_armor;
float last_target_armor;
uint8_t &ally_color;
int energy_part_rotation;
float attack_distance;
int send_cnt;
double rectified_focal_length;
double theta;//电机pitch轴应旋转的角度
double phi;//电机yaw轴应旋转的角度
float yaw_rotation;
float pitch_rotation;
uint8_t last_mark;
int position_mode;
int last_position_mode;
int isLeftVertexFound, isTopVertexFound, isRightVertexFound, isBottomVertexFound;
bool energy_rotation_init;
int clockwise_rotation_init_cnt;
int anticlockwise_rotation_init_cnt;
float red_origin_yaw, red_origin_pitch;
float blue_origin_yaw, blue_origin_pitch;
float origin_yaw, origin_pitch;
float target_cnt;
bool target_cnt_flag;
bool save_new_mark;
std::vector<EnergyPart> fans; cv::Point circle_center_point;//风车圆心坐标
std::vector<EnergyPart> armors; cv::Point target_point;//目标装甲板中心坐标
std::vector<EnergyPart> centerRs; cv::Point predict_point;//预测的击打点坐标
// std::vector<EnergyPart> gimble_zero_points; std::vector<float>fan_polar_angle;//当前帧所有扇叶的极坐标角度
std::vector<float>armor_polar_angle;//当前帧所有装甲板的极坐标角度
std::vector<cv::Point> all_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算
cv::Mat src_blue, src_red, src_green;//通道分离中的三个图像通道
cv::Point cycle_center; void initEnergy();//能量机关初始化
cv::Point target_center; void initEnergyPartParam();//能量机关参数初始化
cv::Point last_target_center; void initRotation();//顺逆时针初始化
cv::Point hit_point;
std::vector<float>fanPosition;
std::vector<float>armorPosition;
std::vector<cv::Point> Armor_center;
std::vector<cv::Point> first_armor_centers;
std::vector<cv::Point> all_armor_centers;
cv::Point left, right, top, bottom;
cv::Mat src_blue, src_red, src_green;
void initEnergy(); int findFan(const cv::Mat src, int &last_fans_cnt);//寻找图中所有扇叶
void initEnergyPartParam(); int findArmor(const cv::Mat src, int &last_armors_cnt);//寻找图中所有装甲板
void initRotation(); int findCenterR(const cv::Mat src);//寻找图中可能的风车中心字母R
int findFan(const cv::Mat src, int &last_fans_cnt); bool isValidFanContour(const vector<cv::Point> fan_contour);//扇叶矩形尺寸要求
int findArmor(const cv::Mat src, int &last_armors_cnt); bool isValidArmorContour(const vector<cv::Point> armor_contour);//装甲板矩形尺寸要求
int findCenterR(const cv::Mat src); bool isValidCenterRContour(const vector<cv::Point> center_R_contour);//风车中心选区尺寸要求
void showFanContours(std::string windows_name, const cv::Mat &src, const std::vector<EnergyPart> &fans); void showFanContours(std::string windows_name, const cv::Mat src);//显示扇叶
void showArmorContours(std::string windows_name, const cv::Mat &src, const std::vector<EnergyPart> &armors); 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); void showCenterRContours(std::string windows_name, const cv::Mat src);//显示风车中心候选区R
bool isValidFanContour(const vector<cv::Point> fan_contour); void getFanPosition();//获取扇叶极坐标角度
bool isValidArmorContour(const vector<cv::Point> armor_contour); void getArmorPosition();//获取装甲板极坐标角度
bool isValidCenterRContour(const vector<cv::Point> center_R_contour); void getAllArmorCenters();//记录所有装甲板中心坐标
void getFanPosition(std::vector<float> &fanPosition, const std::vector<EnergyPart> &fans, cv::Point cycle_center, double radius); void cycleLeastFit();//利用所有记录的装甲板中心最小二乘法计算圆心和半径
void getArmorPosition(std::vector<float> &armorPosition, const std::vector<EnergyPart> &armors, cv::Point cycle_center, double radius);
void getFirstArmorCenters(vector<EnergyPart> &armors, std::vector<cv::Point> &first_armor_centers);
void getAllArmorCenters();
void getPosition(cv::Point point, double &angle);
void cycleQuickCalculate(std::vector<cv::Point> &first_armor_centers, cv::Point &cycle_center, double &radius); void findTarget();//获取目标装甲板的极坐标角度和装甲板中心坐标
void cycleDefaultCalculateConst(cv::Point &cycle_center, double &radius);
void cycleCalculate();
void cycleLeastFit();
void findTarget(const std::vector<float>fanPosition, const std::vector<float>armorPosition, float &target_armor); void rotate();//获取预测点位
void stretch(cv::Point point_1, cv::Point2f &point_2);//将像素差转换为实际距离差
void findWholeCycle(const std::vector<cv::Point>&first_armor_centers); void getPredictPoint();//获取预测点位
bool changeTarget();//判断目标是否改变
void changeMark();//操作手手动修改标定值
void gimbleRotation();//计算云台旋转角度
void saveFourPoints(std::vector<cv::Point> &FourPoints, cv::Point point_1, cv::Point point_2, cv::Point point_3, cv::Point point_4); void splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red);//拜耳阵列分离
void savePoint2f(std::vector<cv::Point2f> &point_save, cv::Point point); void imagePreprocess(cv::Mat &src);//图像通道分离
double pointDistance(cv::Point point_1, cv::Point point_2);
void rotate(double rad, double radius, cv::Point center, cv::Point point_old, cv::Point &point_new);
void stretch(cv::Point point_1, cv::Point2f &point_2);
void cycle(cv::Point p1, cv::Point p2, cv::Point p3, cv::Point &center, double &radius);
void getHitPoint(); void StructingElementClose(cv::Mat &src,int length, int width);//闭运算
bool changeTarget(); void StructingElementErodeDilate(cv::Mat &src);//腐蚀和膨胀
void changeMark();
void gimbleRotation();
void splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red);
void imagePreprocess(cv::Mat &src);
void StructingElementClose(cv::Mat &src,int length, int width);
void StructingElementErodeDilate(cv::Mat &src);
}; };

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

@@ -60,16 +60,6 @@ struct EnergyPartParam {
float TWIN_ANGEL_MAX; float TWIN_ANGEL_MAX;
}; };
struct LiftHeight{
float LIFT_0;
float LIFT_30;
float LIFT_60;
float LIFT_90;
float LIFT_minus_30;
float LIFT_minus_60;
float LIFT_minus_90;
};
typedef struct GMAngle_t{ typedef struct GMAngle_t{
float yaw; float yaw;
float pitch; float pitch;

57
energy/src/energy/calculate/cycle_calculate.cpp
View File

@@ -8,65 +8,26 @@ using std::cout;
using std::endl; using std::endl;
using std::vector; using std::vector;
void Energy::cycleQuickCalculate(std::vector<cv::Point> &first_armor_centers, cv::Point &cycle_center, double &radius) {
//if (isCalibrated()) return;
int cur_size = static_cast<int>(first_armor_centers.size());
if (cur_size < 3)return;
cv::Point point_1, point_2, point_3;
point_1 = first_armor_centers.at(0);
point_2 = first_armor_centers.at(static_cast<unsigned long>(cur_size - 1));
point_3 = first_armor_centers.at(static_cast<unsigned long>(cur_size / 2));
//cout << point_1 << '\t' << point_2 << endl;
//cout << first_armor_centers.at(point_1) << '\t' << first_armor_centers.at(point_2) << endl;
cycle(point_1, point_2, point_3, cycle_center, radius);
cout << "The cycle center is: " << cycle_center << endl;
cout << "The radius is: " << radius << endl;
}
void Energy::cycleDefaultCalculateConst(cv::Point &cycle_center, double &radius) {
if (count >= 5)
return;
clock_t end;
double time_duration = 1.0, dt;
if (Armor_center.size() < 3) {
end = clock();
dt = (end - start) / 1000000.00;
if (dt >= time_duration * count) {
getFirstArmorCenters(armors, Armor_center);
count++;
}
}
else {
cycleQuickCalculate(Armor_center, cycle_center, radius);
count++;
}
}
void Energy::cycleCalculate(){
cycle_center.x = (top.x + bottom.x)/2;
cycle_center.y = (left.y + right.y)/2;
radius = (right.x - left.x)/2;
cout << "The cycle center is: " << cycle_center << endl;
cout << "The radius is: " << radius << endl;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数通过最小二乘法计算大风车圆心和半径
// ---------------------------------------------------------------------------------------------------------------------
void Energy::cycleLeastFit() void Energy::cycleLeastFit()
{ {
cycle_center.x = 0; circle_center_point.x = 0;
cycle_center.y = 0; circle_center_point.y = 0;
radius = 0.0f; radius = 0.0f;
if (all_armor_centers.size() < 3) if (all_armor_centers.size() < 3)
{ {
// cout<<"Cannot calculate a circle"<<endl; // cout<<"Cannot calculate a circle"<<endl;
return; return;
} }
double sum_x = 0.0f, sum_y = 0.0f; double sum_x = 0.0f, sum_y = 0.0f;
double sum_x2 = 0.0f, sum_y2 = 0.0f; double sum_x2 = 0.0f, sum_y2 = 0.0f;
double sum_x3 = 0.0f, sum_y3 = 0.0f; double sum_x3 = 0.0f, sum_y3 = 0.0f;
double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f; double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f;
int N = static_cast<int>(all_armor_centers.size()); int N = static_cast<int>(all_armor_centers.size());
for (int i = 0; i < N; i++) for (int i = 0; i < N; i++)
{ {
@@ -84,10 +45,8 @@ void Energy::cycleLeastFit()
sum_x1y2 += x * y2; sum_x1y2 += x * y2;
sum_x2y1 += x2 * y; sum_x2y1 += x2 * y;
} }
double C, D, E, G, H; double C, D, E, G, H;
double a, b, c; double a, b, c;
C = N * sum_x2 - sum_x * sum_x; C = N * sum_x2 - sum_x * sum_x;
D = N * sum_xy - sum_x * sum_y; D = N * sum_xy - sum_x * sum_y;
E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x; E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
@@ -96,11 +55,9 @@ void Energy::cycleLeastFit()
a = (H * D - E * G) / (C * G - D * D); a = (H * D - E * G) / (C * G - D * D);
b = (H * C - E * D) / (D * D - G * C); b = (H * C - E * D) / (D * D - G * C);
c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N; c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;
circle_center_point.x = static_cast<int>(a / (-2));
cycle_center.x = static_cast<int>(a / (-2)); circle_center_point.y = static_cast<int>(b / (-2));
cycle_center.y = static_cast<int>(b / (-2));
radius = sqrt(a * a + b * b - 4 * c) / 2; radius = sqrt(a * a + b * b - 4 * c) / 2;
// cout << "The cycle center is: " << cycle_center << endl; // cout << "The cycle center is: " << cycle_center << endl;
// cout << "The radius is: " << radius << endl; // cout << "The radius is: " << radius << endl;
} }

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

@@ -8,7 +8,7 @@ using std::cout;
using std::endl; using std::endl;
using std::vector; using std::vector;
void Energy::splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red) { void Energy::splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red) {
uchar* data; uchar* data;
uchar* bayer_data[2]; uchar* bayer_data[2];
for (int i = 0; i < src.rows; ++i) { for (int i = 0; i < src.rows; ++i) {

15
energy/src/energy/energy.cpp
View File

@@ -20,11 +20,11 @@ Energy::Energy(Serial &u, uint8_t &color):serial(u),ally_color(color),
save_new_mark = true; save_new_mark = true;
if(ally_color==ALLY_RED){ if(ally_color == ALLY_RED){
origin_yaw = red_origin_yaw; origin_yaw = red_origin_yaw;
origin_pitch = red_origin_pitch; origin_pitch = red_origin_pitch;
} }
else if(ally_color==ALLY_BLUE){ else if(ally_color == ALLY_BLUE){
origin_yaw = blue_origin_yaw; origin_yaw = blue_origin_yaw;
origin_pitch = blue_origin_pitch; origin_pitch = blue_origin_pitch;
} }
@@ -35,22 +35,11 @@ Energy::Energy(Serial &u, uint8_t &color):serial(u),ally_color(color),
Energy::~Energy() = default; Energy::~Energy() = default;
//
//void Energy::setAllyColor(int color)
//{
// ally_color_ = color;
//}
void Energy::setRotation(int rotation){
energy_part_rotation = rotation;
}
void Energy::setEnergyRotationInit() { void Energy::setEnergyRotationInit() {
initEnergy(); initEnergy();
initEnergyPartParam(); initEnergyPartParam();
energy_rotation_init = true; energy_rotation_init = true;
if(!save_new_mark){ if(!save_new_mark){
FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "r"); FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "r");
if(fp){ if(fp){

89
energy/src/energy/find/cycle_finder.cpp
View File

@@ -1,89 +0,0 @@
//
// Created by xixiliadorabarry on 19-3-23.
//
#include "energy/energy.h"
#include <iostream>
using namespace cv;
using std::cout;
using std::endl;
using std::vector;
void Energy::findWholeCycle(const std::vector<cv::Point>&first_armor_centers) {
int cur_size = static_cast<int>(first_armor_centers.size());
//cout << "first armor centers' size: " << first_armor_centers.size() << endl;
if (cur_size == 0)return;
int vertex = 0;
for (int i = 1; i < cur_size - 1; ++i)
{
if (vertex == 4)break;
if (first_armor_centers.at(i).x >= first_armor_centers.at(i - 1).x && first_armor_centers.at(i).x >= first_armor_centers.at(i + 1).x)
{
if (isRightVertexFound == -1) {
vertex += 1;
isRightVertexFound = 1;
right = first_armor_centers.at(i);
cout << "right vertex: " << right << endl;
continue;
}
else if (right.x > first_armor_centers.at(i).x)continue;
else {
right = first_armor_centers.at(i);
continue;
}
}
if (first_armor_centers.at(i).x <= first_armor_centers.at(i - 1).x && first_armor_centers.at(i).x <= first_armor_centers.at(i + 1).x)
{
if (isLeftVertexFound == -1) {
vertex += 1;
isLeftVertexFound = 1;
left = first_armor_centers.at(i);
cout << "left vertex: " << left << endl;
continue;
}
else if (left.x < first_armor_centers.at(i).x)continue;
else {
left = first_armor_centers.at(i);
continue;
}
}
if (first_armor_centers.at(i).y <= first_armor_centers.at(i - 1).y && first_armor_centers.at(i).y <= first_armor_centers.at(i + 1).y)
{
if (isTopVertexFound == -1) {
vertex += 1;
isTopVertexFound = 1;
top = first_armor_centers.at(i);
cout << "top vertex: " << top << endl;
continue;
}
else if (top.y < first_armor_centers.at(i).y)continue;
else {
top = first_armor_centers.at(i);
continue;
}
}
if (first_armor_centers.at(i).y >= first_armor_centers.at(i - 1).y && first_armor_centers.at(i).y >= first_armor_centers.at(i + 1).y)
{
if (isBottomVertexFound == -1) {
vertex += 1;
isBottomVertexFound = 1;
bottom = first_armor_centers.at(i);
cout << "bottom vertex: " << bottom << endl;
continue;
}
else if (bottom.y > first_armor_centers.at(i).y)continue;
else {
bottom = first_armor_centers.at(i);
continue;
}
}
}
/*for (int k = 0; k < first_armor_centers.size(); ++k) {
cout << k << " : " << first_armor_centers.at(k) << '\t';
float angle = static_cast<float>(180 / PI * atan2(-1 * (first_armor_centers.at(k).y - 298), (first_armor_centers.at(k).x - 298)));
cout << angle << endl;
}*/
}

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

@@ -8,6 +8,8 @@ using std::cout;
using std::endl; using std::endl;
using std::vector; using std::vector;
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于寻找图像内所有的大风车扇叶 // 此函数用于寻找图像内所有的大风车扇叶
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
@@ -17,52 +19,49 @@ int Energy::findFan(const cv::Mat src, int &last_fans_cnt) {
src_bin = src.clone(); src_bin = src.clone();
// threshold(src, src_bin, energy_part_param_.FAN_GRAY_THRESH, 255, THRESH_BINARY); // 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);//图像膨胀，防止图像断开并更方便寻找
StructingElementClose(src_bin,6,6);
// 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(fan_contour)) {
continue; continue;
} }
fans.emplace_back(fan_contour);
double cur_contour_area = contourArea(fan_contour); /* double cur_contour_area = contourArea(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;
//
// cout<<"cur_contour_area: "<<cur_contour_area<<'\t'<<"rect_area: "<<cur_size.area()<<'\t'<<"ratio: "<<cur_contour_area/cur_size.area()<<endl; cout<<"cur_contour_area: "<<cur_contour_area<<'\t'<<"rect_area: "<<cur_size.area()<<
'\t'<<"ratio: "<<cur_contour_area/cur_size.area()<<endl;
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;
// cout<<"fan area: "<<length<<'\t'<<width<<endl; cout<<"fan area: "<<length<<'\t'<<width<<endl;
// if(length>20&&width>20){ if(length>20&&width>20){
// cout<<cur_rect.center; cout<<cur_rect.center;
// fans.emplace_back(fan_contour);
// cout<<"fan area: "<<length<<'\t'<<width<<endl;
// }
//
fans.emplace_back(fan_contour); fans.emplace_back(fan_contour);
// cout<<"fan area: "<<length<<'\t'<<width<<endl; cout<<"fan area: "<<length<<'\t'<<width<<endl;
}
cout<<"fan area: "<<length<<'\t'<<width<<endl;*/
} }
// cout<<fans.size()<<endl;
if(fans.size() < last_fans_cnt){ if(fans.size() < last_fans_cnt){
last_fans_cnt = static_cast<int>(fans.size()); last_fans_cnt = static_cast<int>(fans.size());
return -1; return -1;//寻找到的扇叶比上一帧少，说明该帧有误，返回-1
} }
last_fans_cnt = static_cast<int>(fans.size()); last_fans_cnt = static_cast<int>(fans.size());
return static_cast<int>(fans.size()); return static_cast<int>(fans.size());
} }
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于寻找图像内所有的大风车装甲板模块 // 此函数用于寻找图像内所有的大风车装甲板模块
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
@@ -72,12 +71,12 @@ int Energy::findArmor(const cv::Mat src, int &last_armors_cnt) {
src_bin = src.clone(); src_bin = src.clone();
// threshold(src, src_bin, energy_part_param_.ARMOR_GRAY_THRESH, 255, THRESH_BINARY); // 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);//若读取三通道视频文件，需转换为单通道
} }
std::vector<vector<Point> > armor_contours; std::vector<vector<Point> > armor_contours;
std::vector<vector<Point> > armor_contours_external;//用总轮廓减去外轮廓，只保留内轮廓，除去流动条的影响。 std::vector<vector<Point> > armor_contours_external;//用总轮廓减去外轮廓，只保留内轮廓，除去流动条的影响。
StructingElementErodeDilate(src_bin); StructingElementErodeDilate(src_bin);//图像膨胀，防止图像断开并更方便寻找
// imshow("armor struct",src_bin); // imshow("armor struct",src_bin);
findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
@@ -97,36 +96,35 @@ int Energy::findArmor(const cv::Mat src, int &last_armors_cnt) {
} }
} }
} }
for (auto &armor_contour : armor_contours) { for (auto &armor_contour : armor_contours) {
if (!isValidArmorContour(armor_contour)) if (!isValidArmorContour(armor_contour))
{ {
continue; continue;
} }
armors.emplace_back(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);
// cout<<"armor area: "<<length<<'\t'<<width<<'\t'<<cur_rect.center<<endl;
// }
armors.emplace_back(armor_contour); armors.emplace_back(armor_contour);
cout<<"armor area: "<<length<<'\t'<<width<<'\t'<<cur_rect.center<<endl;
// cout<<"armor area: "<<length<<'\t'<<width<<endl; }
cout<<"armor area: "<<length<<'\t'<<width<<endl;*/
} }
// cout<<armors.size()<<endl;
if(armors.size() < last_armors_cnt){ if(armors.size() < last_armors_cnt){
last_armors_cnt = static_cast<int>(armors.size()); last_armors_cnt = static_cast<int>(armors.size());
return -1; return -1;//寻找到的装甲板比上一帧少，说明该帧有误，返回-1
} }
last_armors_cnt = static_cast<int>(armors.size()); last_armors_cnt = static_cast<int>(armors.size());
return static_cast<int>(armors.size()); return static_cast<int>(armors.size());
} }
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于寻找图像内大风车中心字母“R” // 此函数用于寻找图像内大风车中心字母“R”
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
@@ -139,10 +137,8 @@ int Energy::findCenterR(const cv::Mat src) {
cvtColor(src_bin, src_bin, CV_BGR2GRAY); cvtColor(src_bin, src_bin, CV_BGR2GRAY);
} }
std::vector<vector<Point> > center_R_contours; std::vector<vector<Point> > center_R_contours;
StructingElementErodeDilate(src_bin); StructingElementErodeDilate(src_bin);
// imshow("R struct",src_bin); // imshow("R struct",src_bin);
findContours(src_bin, center_R_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); findContours(src_bin, center_R_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
for (auto &center_R_contour : center_R_contours) { for (auto &center_R_contour : center_R_contours) {
@@ -150,24 +146,26 @@ int Energy::findCenterR(const cv::Mat src) {
{ {
continue; continue;
} }
centerRs.emplace_back(center_R_contour);
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);
// cout<<"center R area: "<<length<<'\t'<<width<<'\t'<<cur_rect.center<<endl;
// }
centerRs.emplace_back(center_R_contour); centerRs.emplace_back(center_R_contour);
cout<<"center R area: "<<length<<'\t'<<width<<'\t'<<cur_rect.center<<endl;
// cout<<"armor area: "<<length<<'\t'<<width<<endl;
} }
cout<<"armor area: "<<length<<'\t'<<width<<endl;*/
}
if(centerRs.size() == 0) return -1;//未找到合适的中心候选区，说明该帧有误，返回-1
return static_cast<int>(centerRs.size()); return static_cast<int>(centerRs.size());
} }
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断找到的矩形候选区是否为扇叶 // 此函数用于判断找到的矩形候选区是否为扇叶
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
@@ -179,31 +177,35 @@ bool Energy::isValidFanContour(const vector<cv::Point> fan_contour) {
//cout<<cur_contour_area<<" "<<energy_fan_param_.CONTOUR_AREA_MIN<<" "<<energy_fan_param_.CONTOUR_AREA_MAX<<endl; //cout<<cur_contour_area<<" "<<energy_fan_param_.CONTOUR_AREA_MIN<<" "<<energy_fan_param_.CONTOUR_AREA_MAX<<endl;
//cout<<"area fail."<<endl; //cout<<"area fail."<<endl;
return false; return false;
//选区面积大小不合适
} }
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 < energy_part_param_.FAN_CONTOUR_LENGTH_MIN || width < energy_part_param_.FAN_CONTOUR_WIDTH_MIN || if (length < energy_part_param_.FAN_CONTOUR_LENGTH_MIN || width < energy_part_param_.FAN_CONTOUR_WIDTH_MIN ||
length > energy_part_param_.FAN_CONTOUR_LENGTH_MAX || width > energy_part_param_.FAN_CONTOUR_WIDTH_MAX) length > energy_part_param_.FAN_CONTOUR_LENGTH_MAX || width > energy_part_param_.FAN_CONTOUR_WIDTH_MAX)
{ {
//cout<<"length width min fail."<<endl; //cout<<"length width fail."<<endl;
return false; return false;
//矩形边长不合适
} }
float length_width_ratio = length / width; float length_width_ratio = length / width;//计算矩形长宽比
if (length_width_ratio > energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX || if (length_width_ratio > energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX ||
length_width_ratio < energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN) length_width_ratio < energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN)
{ {
//cout<<"length width ratio fail."<<endl; //cout<<"length width ratio fail."<<endl;
return false; return false;
//长宽比不合适
} }
if (cur_contour_area / cur_size.area() < 0.6) return false; if (cur_contour_area / cur_size.area() < 0.6) return false;//轮廓对矩形的面积占有率不合适
return true; return true;
} }
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断找到的矩形候选区是否为装甲板 // 此函数用于判断找到的矩形候选区是否为装甲板
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
@@ -215,35 +217,36 @@ bool Energy::isValidArmorContour(const vector<cv::Point> armor_contour) {
//cout<<cur_contour_area<<" "<<energy_fan_param_.CONTOUR_AREA_MIN<<" "<<energy_fan_param_.CONTOUR_AREA_MAX<<endl; //cout<<cur_contour_area<<" "<<energy_fan_param_.CONTOUR_AREA_MIN<<" "<<energy_fan_param_.CONTOUR_AREA_MAX<<endl;
//cout<<"area fail."<<endl; //cout<<"area fail."<<endl;
return false; return false;
//选区面积大小不合适
} }
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 < energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN || width < energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN) if (length < energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN || width < energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN ||
length > energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX||width>energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX)
{ {
//cout<<"length width min fail."<<endl; //cout<<"length width fail."<<endl;
return false; return false;
//矩形边长不合适
} }
if (length > energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX||width>energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX) float length_width_ratio = length / width;//计算矩形长宽比
{
//cout<<"length width max fail."<<endl;
return false;
}
float length_width_ratio = length / width;
if (length_width_ratio > energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX || if (length_width_ratio > energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX ||
length_width_ratio < energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN) length_width_ratio < energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN)
{ {
//cout<<"length width ratio fail."<<endl; //cout<<"length width ratio fail."<<endl;
return false; return false;
//长宽比不合适
} }
if (cur_contour_area / cur_size.area() < 0.7) return false; if (cur_contour_area / cur_size.area() < 0.7) return false;//轮廓对矩形的面积占有率不合适
return true; return true;
} }
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断找到的矩形候选区是否为可能的风车中心R候选区 // 此函数用于判断找到的矩形候选区是否为可能的风车中心R候选区
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
@@ -255,31 +258,27 @@ bool Energy::isValidCenterRContour(const vector<cv::Point> center_R_contour) {
//cout<<cur_contour_area<<" "<<energy_fan_param_.CONTOUR_AREA_MIN<<" "<<energy_fan_param_.CONTOUR_AREA_MAX<<endl; //cout<<cur_contour_area<<" "<<energy_fan_param_.CONTOUR_AREA_MIN<<" "<<energy_fan_param_.CONTOUR_AREA_MAX<<endl;
//cout<<"area fail."<<endl; //cout<<"area fail."<<endl;
return false; return false;
//选区面积大小不合适
} }
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 < energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN || width < energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN) if (length < energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN || width < energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN
||length > energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX ||width>energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX)
{ {
//cout<<"length width min fail."<<endl; //cout<<"length width fail."<<endl;
return false; return false;
//矩形边长不合适
} }
float length_width_ratio = length / width;//计算矩形长宽比
if (length > energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX||width>energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX)
{
//cout<<"length width max fail."<<endl;
return false;
}
float length_width_ratio = length / width;
if (length_width_ratio > energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX || if (length_width_ratio > energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX ||
length_width_ratio < energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN) length_width_ratio < energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN)
{ {
//cout<<"length width ratio fail."<<endl; //cout<<"length width ratio fail."<<endl;
return false; return false;
//长宽比不合适
} }
if (cur_contour_area / cur_size.area() < 0.7) return false; if (cur_contour_area / cur_size.area() < 0.7) return false;//轮廓对矩形的面积占有率不合适
return true; return true;
} }

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

@@ -8,52 +8,46 @@ using std::cout;
using std::endl; using std::endl;
using std::vector; using std::vector;
void Energy::findTarget(std::vector<float>fanPosition, std::vector<float>armorPosition, float &target_armor) { void Energy::findTarget() {
if (fanPosition.size() >= armorPosition.size()) return; if (fan_polar_angle.size() >= armor_polar_angle.size()) return;//扇叶多于装甲板，识别错误
if (armorPosition.size()==0)return; if (armor_polar_angle.empty())return;//找不到扇叶，识别错误
if (fanPosition.size() == 0) { if (fan_polar_angle.empty()) {
target_armor = armorPosition.at(0); target_polar_angle = armor_polar_angle.at(0);//视野中没有扇叶，说明在击打第一个装甲板
for (const auto &armor : armors) for (const auto &armor : armors)
{ {
target_center = armor.rect.center; target_point = armor.rect.center;
// cout<<"target center: "<<target_center<<endl;
} }
return; return;
} }
sort(fanPosition.begin(), fanPosition.end()); sort(fan_polar_angle.begin(), fan_polar_angle.end());//对扇叶的极坐标角度进行排序
/*for (vector<float>::iterator it = fanPosition.begin(); it != fanPosition.end(); it++) { sort(armor_polar_angle.begin(), armor_polar_angle.end());//对装甲板的极坐标角度进行排序
cout << *it << endl;
}*/
sort(armorPosition.begin(), armorPosition.end());
/*for (vector<float>::iterator it = armorPosition.begin(); it != armorPosition.end(); it++) {
cout << *it << endl;
}*/
int i, j = 0; int i, j = 0;
for (i = 0; i < fanPosition.size(); ++i) { for (i = 0; i < fan_polar_angle.size(); ++i) {
if (armorPosition.at(i) - fanPosition.at(j) < energy_part_param_.TWIN_ANGEL_MAX && armorPosition.at(i) - fanPosition.at(j) > -1 * energy_part_param_.TWIN_ANGEL_MAX) { if (armor_polar_angle.at(i) - fan_polar_angle.at(j) < energy_part_param_.TWIN_ANGEL_MAX
&& armor_polar_angle.at(i) - fan_polar_angle.at(j) > -1 * energy_part_param_.TWIN_ANGEL_MAX) {
j++; j++;
continue; continue;//若某个扇叶的极坐标角度与第j个装甲板的极坐标角度接近，则两者匹配成功
} }
else { else {
target_armor = armorPosition.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 - cycle_center.y), (armor.rect.center.x - cycle_center.x))); float angle = static_cast<float>(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y),
if(target_armor==angle){ (armor.rect.center.x - circle_center_point.x)));
target_center = armor.rect.center; if(target_polar_angle == angle){
// cout<<"target center: "<<target_center<<endl; target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标
} }
} }
return; return;
} }
} }
target_armor = armorPosition.at(armorPosition.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 - cycle_center.y), (armor.rect.center.x - cycle_center.x))); float angle = static_cast<float>(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y),
if(target_armor == angle){ (armor.rect.center.x - circle_center_point.x)));
target_center = armor.rect.center; if(target_polar_angle == angle){
// cout<<"target center: "<<target_center<<endl; target_point = armor.rect.center;//根据已经确定的目标装甲板极坐标角度，获得该装甲板的中心坐标
} }
} }

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

@@ -10,6 +10,11 @@ using std::cout;
using std::endl; using std::endl;
using std::vector; using std::vector;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于操作手手动标定
// ---------------------------------------------------------------------------------------------------------------------
void Energy::changeMark() { void Energy::changeMark() {
if (mcuData.mark == 0 && last_mark == 1) { if (mcuData.mark == 0 && last_mark == 1) {
last_mark = mcuData.mark; last_mark = mcuData.mark;
@@ -32,41 +37,15 @@ void Energy::changeMark() {
} }
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于计算云台应当转到角度
// ---------------------------------------------------------------------------------------------------------------------
void Energy::gimbleRotation(){ void Energy::gimbleRotation(){
cv::Point2f real_hit_point; cv::Point2f real_predict_point;//计算在真实世界中的预测点位，进而计算云台的旋转角度
stretch(hit_point, real_hit_point); stretch(predict_point, real_predict_point);
yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance)); yaw_rotation = static_cast<float>(180 / PI *
pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance)); atan2((attack_distance * tan(origin_yaw * PI / 180) - real_predict_point.x), attack_distance));
pitch_rotation = static_cast<float>(180 / PI *
/*origin_yaw = mark_yaw; atan2((attack_distance*tan(origin_pitch*PI/180)-real_predict_point.y), attack_distance));
origin_pitch = mark_pitch;*/
// if(position_mode == 1){
// yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
// pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
// }
// else if(position_mode == 2){
// yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
// pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
// }
// else if(position_mode == 3){
// yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
// pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
// }
// else if(position_mode == 4){
// yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
// pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
// }
// else if(position_mode == 5){
// yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
// pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
// }
// else if(position_mode == 6){
// yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance * tan(origin_yaw * PI / 180) - real_hit_point.x), attack_distance));
// pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(origin_pitch*PI/180)-real_hit_point.y), attack_distance));
// }
// else{
// pitch_rotation = 5.5+static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance));
// }
} }

133
energy/src/energy/get/hit_point_get.cpp
View File

@@ -1,133 +0,0 @@
//
// Created by xixiliadorabarry on 1/24/19.
//
#include "energy/energy.h"
#include "energy/constant.h"
using namespace cv;
using std::cout;
using std::endl;
using std::vector;
void Energy::getHitPoint(){
int rad = 60;
if(energy_part_rotation==1) rotate(rad, radius, cycle_center, target_center, hit_point);
if(energy_part_rotation==-1) rotate(-rad, radius, cycle_center, target_center, hit_point);
// int hit_position = 1000;
// int limit_angle = 6;
// int angle_interval = 60;
//
// if(energy_part_rotation==1){
// if(target_armor>=0 && target_armor<=limit_angle){
// hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
// hit_position = -1*angle_interval;
// position_mode = 1;
// }
// else if(target_armor>=angle_interval && target_armor<angle_interval+limit_angle){
// hit_point.x = cycle_center.x + static_cast<int>(radius);
// hit_point.y = cycle_center.y;
// hit_position = 0;
// position_mode = 2;
// }
// else if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){
// hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
// hit_position = angle_interval;
// position_mode = 3;
// }
// else if(target_armor>=-180 && target_armor<-180+limit_angle){
// hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
// hit_position = 2*angle_interval;
// position_mode = 4;
// }
// else if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){
// hit_point.x = cycle_center.x - static_cast<int>(radius);
// hit_point.y = cycle_center.y;
// hit_position = 180;
// position_mode = 5;
// }
// else if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle) {
// hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
// hit_position = -2 * angle_interval;
// position_mode = 6;
// }
// else{
// position_mode = 0;
// return;
// }
// }
//
// if(energy_part_rotation==-1){
// if(target_armor>=0 && target_armor<=limit_angle){
// hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
// hit_position = 2*angle_interval;
// position_mode = 1;
// }
// else if(target_armor>=angle_interval && target_armor<angle_interval+limit_angle){
// hit_point.x = cycle_center.x - static_cast<int>(radius);
// hit_point.y = cycle_center.y;
// hit_position = 180;
// position_mode = 2;
// }
// else if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){
// hit_point.x = cycle_center.x - static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
// hit_position = -2*angle_interval;
// position_mode = 3;
// }
// else if(target_armor>=-180 && target_armor<-180+limit_angle){
// hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y + static_cast<int>(radius * sqrt(3) / 2);
// hit_position = -1*angle_interval;
// position_mode = 4;
// }
// else if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){
// hit_point.x = cycle_center.x + static_cast<int>(radius);
// hit_point.y = cycle_center.y;
// hit_position = 0;
// position_mode = 5;
// }
// else if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle){
// hit_point.x = cycle_center.x + static_cast<int>(radius / 2);
// hit_point.y = cycle_center.y - static_cast<int>(radius * sqrt(3) / 2);
// hit_position = angle_interval;
// position_mode = 6;
// }
// else{
// position_mode = 0;
// return;
// }
// }
//
// if(position_mode!=0 && position_mode!=last_position_mode){
// last_position_mode = position_mode;
// isSendTarget = true;
// //cout<<"hit position: "<<hit_position<<endl;
// return;
// }
// else if(position_mode == 0){
// isSendTarget = false;
// return;
// }
// else{
// last_position_mode = position_mode;
// isSendTarget = false;
// return;
// }
}
bool Energy::changeTarget(){
if(fabs(target_armor - last_target_armor) < 30||fabs(target_armor - last_target_armor) > 330){
last_target_armor = target_armor;
return false;
}
else{
last_target_armor = target_armor;
return true;
}
}

42
energy/src/energy/get/position_get.cpp
View File

@@ -8,42 +8,50 @@ using std::cout;
using std::endl; using std::endl;
using std::vector; using std::vector;
void Energy::getFanPosition(std::vector<float> &fanPosition, const std::vector<EnergyPart> &fans, cv::Point cycle_center, double radius) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于获得图像中所有扇叶的当前极坐标角度
// ---------------------------------------------------------------------------------------------------------------------
void Energy::getFanPosition() {
if (radius == 0)return; if (radius == 0)return;
for (const auto &fan : fans) for (const auto &fan : fans)
{ {
float angle = static_cast<float>(180 / PI * atan2(-1 * (fan.rect.center.y - cycle_center.y), (fan.rect.center.x - cycle_center.x))); float angle = static_cast<float>(180 / PI * atan2(-1 * (fan.rect.center.y - circle_center_point.y),
fanPosition.push_back(angle); (fan.rect.center.x - circle_center_point.x)));
fan_polar_angle.push_back(angle);
} }
// cout << "fanPosition.size() = " << fanPosition.size() << '\t' << endl; // cout << "fanPosition.size() = " << fanPosition.size() << '\t' << endl;
} }
void Energy::getArmorPosition(std::vector<float> &armorPosition, const std::vector<EnergyPart> &armors, cv::Point cycle_center, double radius) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于获得图像中所有装甲板的当前极坐标角度
// ---------------------------------------------------------------------------------------------------------------------
void Energy::getArmorPosition() {
if (radius == 0)return; if (radius == 0)return;
for (const auto &armor : armors) for (const auto &armor : armors)
{ {
float angle = static_cast<float>(180 / PI * atan2(-1 * (armor.rect.center.y - cycle_center.y), (armor.rect.center.x - cycle_center.x))); float angle = static_cast<float>(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y),
armorPosition.push_back(angle); (armor.rect.center.x - circle_center_point.x)));
armor_polar_angle.push_back(angle);
} }
// cout << "armorPosition.size() = " << armorPosition.size() << '\t' << endl; // cout << "armorPosition.size() = " << armorPosition.size() << '\t' << endl;
} }
void Energy::getFirstArmorCenters(vector<EnergyPart> &armors, std::vector<cv::Point> &first_armor_centers)
{
for (const auto &armor : armors) {
if (armors.size() < 2)first_armor_centers.push_back(armor.rect.center);
}
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于存储图像中所有装甲板的中心坐标，以便后续最小二乘法计算圆心和半径
// ---------------------------------------------------------------------------------------------------------------------
void Energy::getAllArmorCenters() void Energy::getAllArmorCenters()
{ {
for (const auto &armor : armors) { for (const auto &armor : armors) {
all_armor_centers.push_back(armor.rect.center); all_armor_centers.push_back(armor.rect.center);
} }
} }
void Energy::getPosition(cv::Point point, double &angle){
if (radius == 0)return;
angle = (180 / PI * atan2(-1 * (point.y - cycle_center.y), (point.x - cycle_center.x)));
}

82
energy/src/energy/param_init.cpp
View File

@@ -10,34 +10,22 @@ using std::endl;
using std::vector; using std::vector;
void Energy::initEnergy() { void Energy::initEnergy() {
isSendTarget = false;
isMark = false; isMark = false;
fans_cnt = 0; fans_cnt = 0;
armors_cnt = 0; armors_cnt = 0;
centerRs_cnt = 0; centerRs_cnt = 0;
cycle_center = Point(0, 0); circle_center_point = Point(0, 0);
target_center = Point(0, 0); target_point = Point(0, 0);
last_target_center = Point(0, 0); predict_point = Point(0, 0);
hit_point = Point(0, 0); target_polar_angle = -1000;
target_position = -1000; last_target_polar_angle = -1000;
last_target_position = -1000;
last_hit_position = 20000;
target_armor = -1000;
last_target_armor = -1000;
radius = 0; radius = 0;
energy_rotation_direction = CLOCKWISE;
// ally_color = ALLY_RED;
energy_part_rotation = CLOCKWISE;
attack_distance = ATTACK_DISTANCE; attack_distance = ATTACK_DISTANCE;
count = 1;
last_fans_cnt = 0; last_fans_cnt = 0;
last_armors_cnt = 0; last_armors_cnt = 0;
send_cnt = 0; send_cnt = 0;
//rectified_focal_length = 1000;
//theta = 0;
//phi = 0;
yaw_rotation = 0; yaw_rotation = 0;
pitch_rotation = 0; pitch_rotation = 0;
last_mark = 0; last_mark = 0;
@@ -48,30 +36,16 @@ void Energy::initEnergy() {
blue_origin_pitch = 15.688477; blue_origin_pitch = 15.688477;
target_cnt = 0; target_cnt = 0;
target_cnt_flag = true;
isLeftVertexFound = -1;
isTopVertexFound = -1;
isRightVertexFound = -1;
isBottomVertexFound = -1;
left = Point(640, 480);
right = Point(0, 0);
top = Point(640, 480);
bottom = Point(0, 0);
position_mode = 0;
last_position_mode = 0;
energy_rotation_init = false; energy_rotation_init = false;
predict_rad = 20;
fans.clear(); fans.clear();
armors.clear(); armors.clear();
centerRs.clear(); centerRs.clear();
fanPosition.clear();
armorPosition.clear(); fan_polar_angle.clear();
Armor_center.clear(); armor_polar_angle.clear();
first_armor_centers.clear();
all_armor_centers.clear(); all_armor_centers.clear();
clockwise_rotation_init_cnt = 0; clockwise_rotation_init_cnt = 0;
@@ -116,37 +90,27 @@ void Energy::initEnergyPartParam() {
energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1; energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1;
energy_part_param_.TWIN_ANGEL_MAX = 10; energy_part_param_.TWIN_ANGEL_MAX = 10;
lift_height_.LIFT_0 = 0;
lift_height_.LIFT_30 = 0;
lift_height_.LIFT_60 = 0;
lift_height_.LIFT_90 = 10;
lift_height_.LIFT_minus_30 = 0;
lift_height_.LIFT_minus_60 = 0;
lift_height_.LIFT_minus_90 = 0;
} }
void Energy::initRotation() { void Energy::initRotation() {
target_position = target_armor; if (target_polar_angle >= -180 && last_target_polar_angle >= -180
// cout << "target position: " << target_position << '\t' << "last target position: " << last_target_position << endl; && fabs(target_polar_angle - last_target_polar_angle) < 30) {
if (target_position >= -180 && last_target_position >= -180 && fabs(target_position - last_target_position) < 30) { //target_polar_angle和last_target_polar_angle的初值均为1000，大于-180表示刚开始几帧不要
if (target_position < last_target_position) clockwise_rotation_init_cnt++; //若两者比较接近，则说明没有切换目标，因此可以用于顺逆时针的判断
else if (target_position > last_target_position) anticlockwise_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++;
} }
//由于刚开始圆心判断不准，角度变化可能计算有误，因此需要在角度正向或逆向变化足够大时才可确定是否为顺逆时针
if (clockwise_rotation_init_cnt == 30) { if (clockwise_rotation_init_cnt == 30) {
energy_part_rotation = CLOCKWISE; energy_rotation_direction = CLOCKWISE;//顺时针变化30次，确定为顺时针
cout << "rotation: " << energy_part_rotation << 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_part_rotation = ANTICLOCKWISE; energy_rotation_direction = ANTICLOCKWISE;//逆时针变化30次，确定为顺时针
cout << "rotation: " << energy_part_rotation << endl; cout << "rotation: " << energy_rotation_direction << endl;
energy_rotation_init = false; energy_rotation_init = false;
} }
// else cout << clockwise_rotation_init_cnt <<'\t'<<anticlockwise_rotation_init_cnt<< endl; last_target_polar_angle = target_polar_angle;
last_target_position = target_position;
} }

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

@@ -18,10 +18,8 @@ int Energy::run(cv::Mat &src){
fans.clear(); fans.clear();
armors.clear(); armors.clear();
centerRs.clear(); centerRs.clear();
fanPosition.clear(); fan_polar_angle.clear();
armorPosition.clear(); armor_polar_angle.clear();
// gimble_zero_points.clear();
isSendTarget = false;
changeMark(); changeMark();
if (isMark)return 0; if (isMark)return 0;
@@ -31,18 +29,16 @@ int Energy::run(cv::Mat &src){
threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY); threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
// imshow("bin",src); // imshow("bin",src);
fans_cnt = findFan(src, last_fans_cnt); fans_cnt = findFan(src, last_fans_cnt);
// cout<<"fans_cnt: "<<fans_cnt<<endl; // cout<<"fans_cnt: "<<fans_cnt<<endl;
if(fans_cnt==-1) return 0;//滤去漏判的帧 if(fans_cnt==-1) return 0;//滤去漏判的帧
// if(fans_cnt>0)showFanContours("fan",src,fans); // if(fans_cnt>0)showFanContours("fan",src);
// fans_cnt=0; // fans_cnt=0;
armors_cnt = findArmor(src, last_armors_cnt); armors_cnt = findArmor(src, last_armors_cnt);
// cout<<"armors_cnt: "<<armors_cnt<<endl; // cout<<"armors_cnt: "<<armors_cnt<<endl;
if(armors_cnt==-1) return 0;//滤去漏判的帧 if(armors_cnt==-1) return 0;//滤去漏判的帧
// if(armors_cnt>0) showArmorContours("armor",src,armors); // if(armors_cnt>0) showArmorContours("armor",src);
if(armors_cnt>0||fans_cnt>0) showBothContours("Both",src); if(armors_cnt>0||fans_cnt>0) showBothContours("Both",src);
@@ -69,75 +65,32 @@ int Energy::run(cv::Mat &src){
fclose(fp_best); fclose(fp_best);
} }
} }
// cout<<"armors_cnt: "<<armors_cnt<<"fans_cnt: "<<fans_cnt<<endl; if(armors_cnt != fans_cnt+1)
// cout<<"armors_cnt: "<<armors_cnt<<"fans_cnt: "<<fans_cnt<<endl; {
// if(armors_cnt != fans_cnt+1) return 0;
// { }
// return 0;
// }
//cout<<"clock: "<<energy_part_rotation<<endl;
getAllArmorCenters(); getAllArmorCenters();
// cout<<"all_armor_centers.size(): "<<all_armor_centers.size()<<endl;
cycleLeastFit(); cycleLeastFit();
// cycle_center = cv::Point(335, 246);
// radius = 116.936;
// attack_distance = ATTACK_DISTANCE * 120/ radius;
// attack_distance = 674 + 1286 * 75 * (1/radius - 1/133.85);
// cout<<"radius"<<radius<<endl;
// cout << "attack distance: " << attack_distance << endl;
// attack_distance = 752;//单项赛
attack_distance = 718; attack_distance = 718;
getFanPosition(fanPosition, fans, cycle_center, radius); getFanPosition();
getArmorPosition(armorPosition, armors, cycle_center, radius); getArmorPosition();
findTarget(fanPosition, armorPosition, target_armor); findTarget();
// cout << "The target armor's position is " << target_armor << endl;
// cout<<"The target armor center is: "<<target_center<<endl;
// cout<<target_armor<<endl;
if (energy_rotation_init) { if (energy_rotation_init) {
initRotation(); initRotation();
return 0; return 0;
} }
getPredictPoint();
getHitPoint();
// hit_point = target_center;
// cout << "The hit point position is " << hit_point << endl;
// cout<<"send"<<endl;
// cout<<"position mode: "<<position_mode<<endl;
gimbleRotation(); gimbleRotation();
if(changeTarget())target_cnt++;
// if (!isSendTarget)return 0;
sendTargetByUart(yaw_rotation, pitch_rotation, target_cnt); sendTargetByUart(yaw_rotation, pitch_rotation, target_cnt);
// cout<<target_cnt<<endl;
// cout<<"yaw: "<<yaw_rotation<<'\t'<<"pitch: "<<pitch_rotation<<endl; // cout<<"yaw: "<<yaw_rotation<<'\t'<<"pitch: "<<pitch_rotation<<endl;
// cout<<"curr_yaw: "<<mcuData.curr_yaw<<'\t'<<"curr_pitch: "<<mcuData.curr_pitch<<endl; // cout<<"curr_yaw: "<<mcuData.curr_yaw<<'\t'<<"curr_pitch: "<<mcuData.curr_pitch<<endl;
// cout<<"send_cnt: "<<send_cnt<<endl; // cout<<"send_cnt: "<<send_cnt<<endl;
} }
//-----------------------------------------------------------------------------------
/*
//此处用于标定云台在摄像头视频中的零点
findGimbleZeroPoint(src,gimble_zero_points);
cout<<"gimble zero points: :"<<gimble_zero_points.size()<<endl;
showFanContours("zero",src,gimble_zero_points);
cycle_center = cv::Point(291,305);
if(gimble_zero_points.size()>0)hit_point = gimble_zero_points.at(0).rect.center;
*/

17
energy/src/energy/send_target_by_uart/send_target_by_uart.cpp
View File

@@ -7,22 +7,10 @@ using namespace std;
bool sendTarget(Serial& serial, float x, float y, float z) { bool 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;
// static time_t last_time = time(nullptr);
// static int fps;
uint8_t buff[8]; uint8_t buff[8];
// time_t t = time(nullptr);
// if (last_time != t) {
// last_time = t;
// std::cout << "fps:" << fps << ", (" << x << "," << y << "," << z << ")" << std::endl;
// fps = 0;
// }
// fps += 1;
x_tmp = static_cast<short>(x * (32768 - 1) / 100); x_tmp = static_cast<short>(x * (32768 - 1) / 100);
y_tmp = static_cast<short>(y * (32768 - 1) / 100); y_tmp = static_cast<short>(y * (32768 - 1) / 100);
z_tmp = static_cast<short>(z * (32768 - 1) / 1000); z_tmp = static_cast<short>(z * (32768 - 1) / 1000);
buff[0] = 's'; buff[0] = 's';
buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF); buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF); buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
@@ -31,14 +19,11 @@ bool sendTarget(Serial& serial, float x, float y, float z) {
buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF); buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF); buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
buff[7] = 'e'; buff[7] = 'e';
return serial.WriteData(buff, sizeof(buff)); return serial.WriteData(buff, sizeof(buff));
} }
void Energy::sendTargetByUart(float x, float y, float z) { void Energy::sendTargetByUart(float x, float y, float z) {
// if(!changeTarget()){ if(changeTarget())target_cnt++;
// return;
// }
sendTarget(serial, x, y, z); sendTarget(serial, x, y, z);
send_cnt+=1; send_cnt+=1;
// cout<<"send"<<endl; // cout<<"send"<<endl;

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

@@ -8,7 +8,10 @@ using std::cout;
using std::endl; using std::endl;
using std::vector; using std::vector;
void Energy::showFanContours(std::string windows_name, const cv::Mat &src, const std::vector<EnergyPart> &fans) { //----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中所有扇叶
// ---------------------------------------------------------------------------------------------------------------------
void Energy::showFanContours(std::string windows_name, const cv::Mat src) {
if (src.empty())return; if (src.empty())return;
static Mat image2show; static Mat image2show;
@@ -33,7 +36,10 @@ void Energy::showFanContours(std::string windows_name, const cv::Mat &src, const
imshow(windows_name, image2show); imshow(windows_name, image2show);
} }
void Energy::showArmorContours(std::string windows_name, const cv::Mat &src, const std::vector<EnergyPart> &armors) { //----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中所有装甲板
// ---------------------------------------------------------------------------------------------------------------------
void Energy::showArmorContours(std::string windows_name, const cv::Mat src) {
if (src.empty())return; if (src.empty())return;
static Mat image2show; static Mat image2show;
@@ -58,31 +64,6 @@ void Energy::showArmorContours(std::string windows_name, const cv::Mat &src, con
imshow(windows_name, image2show); imshow(windows_name, image2show);
} }
void Energy::showCenterRContours(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();
}
//cvtColor(image2show, image2show, COLOR_GRAY2RGB);
for (const auto &center_R : centerRs)
{
Point2f vertices[4]; //定义矩形的4个顶点
center_R.rect.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++)
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 255), 2);
//cout << armor.rect.center << '\t' << armor.rect.angle << '\t';
//cout << endl;
}
imshow(windows_name, image2show);
}
void Energy::showBothContours(std::string windows_name, const cv::Mat src) { void Energy::showBothContours(std::string windows_name, const cv::Mat src) {
if (src.empty())return; if (src.empty())return;
static Mat image2show; static Mat image2show;
@@ -121,4 +102,37 @@ void Energy::showBothContours(std::string windows_name, const cv::Mat src) {
imshow(windows_name, image2show); imshow(windows_name, image2show);
} }
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中所有可能的风车中心候选区R
// ---------------------------------------------------------------------------------------------------------------------
void Energy::showCenterRContours(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();
}
//cvtColor(image2show, image2show, COLOR_GRAY2RGB);
for (const auto &center_R : centerRs)
{
Point2f vertices[4]; //定义矩形的4个顶点
center_R.rect.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++)
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 255), 2);
//cout << armor.rect.center << '\t' << armor.rect.angle << '\t';
//cout << endl;
}
imshow(windows_name, image2show);
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中所有扇叶和装甲板
// ---------------------------------------------------------------------------------------------------------------------

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

@@ -20,69 +20,27 @@ void Energy::extract(cv::Mat &src){
imshow("extract", src); imshow("extract", src);
} }
void Energy::saveFourPoints(std::vector<cv::Point> &FourPoints, cv::Point point_1, cv::Point point_2, cv::Point point_3, cv::Point point_4) { void Energy::rotate() {
FourPoints.push_back(point_1);
FourPoints.push_back(point_2);
FourPoints.push_back(point_3);
FourPoints.push_back(point_4);
}
void Energy::savePoint2f(std::vector<cv::Point2f> &point_save, cv::Point point) {
point_save.push_back(static_cast<cv::Point2f>(point));
}
double Energy::pointDistance(cv::Point point_1, cv::Point point_2){
double distance = 0;
distance = sqrt(pow(static_cast<double>(point_1.x - point_2.x),2)
+ pow(static_cast<double>(point_1.y - point_2.y),2));
return distance;
}
void Energy::rotate(double rad, double radius, cv::Point center, cv::Point point_old, cv::Point &point_new) {
int x1, x2, y1, y2; int x1, x2, y1, y2;
// 为了减小强制转换的误差 // 为了减小强制转换的误差
x1 = center.x * 100; x1 = circle_center_point.x * 100;
x2 = point_old.x * 100; x2 = target_point.x * 100;
y1 = center.y * 100; y1 = circle_center_point.y * 100;
y2 = point_old.y * 100; y2 = target_point.y * 100;
point_new.x = static_cast<int>((x1 + (x2 - x1)*cos(-rad * d2r) - (y1 - y2)*sin(-rad * d2r))/100); predict_point.x = static_cast<int>((x1 + (x2 - x1)*cos(-predict_rad * d2r) - (y1 - y2)*sin(-predict_rad * d2r))/100);
point_new.y = static_cast<int>((y1 - (x2 - x1)*sin(-rad * d2r) - (y1 - y2)*cos(-rad * d2r))/100); predict_point.y = static_cast<int>((y1 - (x2 - x1)*sin(-predict_rad * d2r) - (y1 - y2)*cos(-predict_rad * d2r))/100);
} }
void Energy::stretch(cv::Point point_1, cv::Point2f &point_2){ void Energy::stretch(cv::Point point_1, cv::Point2f &point_2){
if(point_1==cycle_center){ if(point_1==circle_center_point){
cout<<"stretch wrong!"<<endl; // cout<<"stretch wrong!"<<endl;
return; return;
} }
double x_0 = point_1.x - cycle_center.x; double x_0 = point_1.x - circle_center_point.x;
double y_0 = point_1.y - cycle_center.y; double y_0 = point_1.y - circle_center_point.y;
double r_0 = sqrt(pow(x_0, 2)+ pow(y_0, 2)); 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.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); point_2.y = static_cast<float >( ARMOR_CENTER_TO_CYCLE_CENTER * y_0 / r_0);
} }
void Energy::cycle(cv::Point p1, cv::Point p2, cv::Point p3, cv::Point &center, double &radius){
double x1, y1, x2, y2, x3, y3;
double a, b, c, g, e, f;
x1 = p1.x;
y1 = p1.y;
x2 = p2.x;
y2 = p2.y;
x3 = p3.x;
y3 = p3.y;
//三点确定圆的方程：(2x2-2x1)X+(2y2-2y1)Y=x2²-x1²+y2²-y1²；(2x3-2x2)X+(2y3-2y2)Y=x3²-x2²+y3²-y2²
e = 2 * (x2 - x1);
f = 2 * (y2 - y1);
g = x2 * x2 - x1 * x1 + y2 * y2 - y1 * y1;
a = 2 * (x3 - x2);
b = 2 * (y3 - y2);
c = x3 * x3 - x2 * x2 + y3 * y3 - y2 * y2;
cycle_center.x = static_cast<int>((g*b - c * f) / (e*b - a * f));
cycle_center.y = static_cast<int>((a*g - c * e) / (a*f - b * e));
radius = sqrt((cycle_center.x - x1)*(cycle_center.x - x1) + (cycle_center.y - y1)*(cycle_center.y - y1));
}

4
main.cpp
View File

@@ -96,8 +96,6 @@ int main(int argc, char *argv[]) {
ArmorFinder armorFinder(mcuData.enemy_color, serial, PROJECT_DIR"/tools/para/", mcuData.use_classifier); ArmorFinder armorFinder(mcuData.enemy_color, serial, PROJECT_DIR"/tools/para/", mcuData.use_classifier);
Energy energy(serial, mcuData.enemy_color); Energy energy(serial, mcuData.enemy_color);
// energy.setAllyColor(ally_color);
energy.setRotation(CLOCKWISE);
bool ok = true; bool ok = true;
cout<<"start running"<<endl; cout<<"start running"<<endl;
@@ -169,7 +167,7 @@ int main(int argc, char *argv[]) {
} }
} }
// cout<<last_state<<endl; // cout<<last_state<<endl;
waitKey(3); waitKey(1);
}); });
} while (ok); } while (ok);