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

Merge remote-tracking branch 'origin/master'

Browse Source 
# Conflicts:
#	main.cpp
This commit is contained in:
xinyang
2019-07-06 20:22:06 +08:00
parent e9a0e9ad7b adc11e6d18
commit 1abe8dec17
34 changed files with 1062 additions and 1092 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -5,3 +5,4 @@ Mark
armor_box_photo
tools/TrainCNN/.idea
tools/TrainCNN/__pycache__
.DS_Store

5
CMakeLists.txt
View File

@@ -26,6 +26,8 @@ FIND_PACKAGE(Eigen3 REQUIRED)
FIND_PACKAGE(OpenCV 3 REQUIRED)
FIND_PACKAGE(Threads)
LINK_DIRECTORIES(${PROJECT_SOURCE_DIR}/others)
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/energy/include)
INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/armor/include)
@@ -42,6 +44,9 @@ IF (CMAKE_SYSTEM_NAME MATCHES "Linux")
ELSEIF (CMAKE_SYSTEM_NAME MATCHES "Windows")
MESSAGE(STATUS "current platform: Windows")
TARGET_LINK_LIBRARIES(${BIN_NAME} "${PROJECT_SOURCE_DIR}/others/MVCAMSDK_X64.lib")
ELSEIF (CMAKE_SYSTEM_NAME MATCHES "Darwin")
MESSAGE(STATUS "current platform: Mac")
TARGET_LINK_LIBRARIES(${BIN_NAME} "${PROJECT_SOURCE_DIR}/others/libmvsdk.dylib")
ELSE ()
MESSAGE(STATUS "Unsupport platform: ${CMAKE_SYSTEM_NAME}")
ENDIF()

18
energy/include/energy/constant.h
View File

@@ -17,29 +17,13 @@ const int SRC_HEIGHT = 240;
const double PI = 3.1415926;
const int CLOCKWISE = 1;
const int ANTICLOCKWISE = -1;
const float ATTACK_DISTANCE = 770;//cm
const double WHOLE_FAN = 80;//cm
const float ATTACK_DISTANCE = 718;//cm
const double ARMOR_CENTER_TO_CYCLE_CENTER = 75;//cm
//const double ARMOR_CENTER_TO_CYCLE_CENTER = 71;//cm
const int EXTRACT_POINT_X = 120;
const int EXTRACT_POINT_Y = 0;
const int EXTRACT_WIDTH = 400;
const int EXTRACT_HEIGHT = 300;
//以摄像头正方向位y轴
const int GM_L = 14;//云台摄像头z方向
const int GM_X = 15;//云台摄像头x方向
const int GM_H = 16;//云台摄像头y方向
//const double STRETCH = 231.0/640.0;//实际距离与图像伸缩比
const double STRETCH = 0.34;
const int ZERO_POINT_X = 281;
const int ZERO_POINT_Y = 188;
const double YAW_ORIGIN_RAD = PI/180*2.25;
const double PITCH_ORIGIN_RAD = PI/180*14.85;
const double LIFT_HEIGHT = 20;//云台抬升高度
#endif //CONSTANT_H

187
energy/include/energy/energy.h
View File

@@ -18,132 +18,107 @@
#include "additions/additions.h"
#include "options/options.h"
using std::vector;
class Energy {
public:
Energy(Serial &u, uint8_t &color);
~Energy();
int run(cv::Mat &src);
cv::Point2f uart_hit_point;
clock_t start;
Serial &serial;
// void setAllyColor(int color);
void setRotation(int rotation);
void setEnergyRotationInit();
void extract(cv::Mat &src);
void sendTargetByUart(float x, float y, float z);
Energy(Serial &u, uint8_t &color);//构造函数，参数为串口和敌方颜色
~Energy();//默认析构函数
int runBig(cv::Mat &gimble_src, cv::Mat &chassis_src);
int runBig(cv::Mat &gimble_src);
int runSmall(cv::Mat &gimble_src);
Serial &serial;//串口
void setEnergyRotationInit();//判断顺逆时针函数
void extract(cv::Mat &src);//框取图像中的一块区域进行处理
void sendTargetByUart(float x, float y, float z);//向主控板发送数据
private:
EnergyPartParam energy_part_param_;//能量机关的参数设置
bool isMark;//若操作手正在手动标定，则为true
bool centered=false;//云台是否对准中心
int fans_cnt;//图像中的扇叶个数
int armors_cnt;//图像中的装甲板个数
int centerRs_cnt;//图像中可能的风车中心字母R选区个数
int last_fans_cnt;//上一帧的扇叶个数
int last_armors_cnt;//上一帧的装甲板个数
int gimble_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
std::vector<EnergyPart> fans;//图像中所有扇叶
std::vector<EnergyPart> armors;//图像中所有装甲板
std::vector<EnergyPart> centerRs;//风车中心字母R的可能候选区
EnergyPartParam energy_part_param_;
LiftHeight lift_height_;
bool isSendTarget;
bool isMark;
int fans_cnt;
int armors_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;
cv::Point circle_center_point;//风车圆心坐标
cv::Point target_point;//目标装甲板中心坐标
cv::Point last_target_point;//上一帧目标装甲板中心坐标
cv::Point predict_point;//预测的击打点坐标
cv::Point former_point;//之前预测的圆心坐标
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;//通道分离中的三个图像通道
std::vector<EnergyPart> fans;
std::vector<EnergyPart> armors;
// std::vector<EnergyPart> gimble_zero_points;
void initEnergy();//能量机关初始化
void initEnergyPartParam();//能量机关参数初始化
void initRotation();//顺逆时针初始化
cv::Point cycle_center;
cv::Point target_center;
cv::Point last_target_center;
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;
int findFan(const cv::Mat src, int &last_fans_cnt);//寻找图中所有扇叶
int findArmor(const cv::Mat src, int &last_armors_cnt);//寻找图中所有装甲板
int findCenterR(const cv::Mat src);//寻找图中可能的风车中心字母R
void initEnergy();
void initEnergyPartParam();
void initRotation();
bool isValidFanContour(const vector<cv::Point> fan_contour);//扇叶矩形尺寸要求
bool isValidArmorContour(const vector<cv::Point> armor_contour);//装甲板矩形尺寸要求
bool isValidCenterRContour(const vector<cv::Point> center_R_contour);//风车中心选区尺寸要求
int findFan(const cv::Mat &src, vector<EnergyPart> &fans, int &last_fans_cnt);
int findArmor(const cv::Mat &src, vector<EnergyPart> &armors, int &last_armors_cnt);
int findGimbleZeroPoint(const cv::Mat &src, vector<EnergyPart> &gimble_zero_point);
void showFanContours(std::string windows_name, const cv::Mat src);//显示扇叶
void showArmorContours(std::string windows_name, const cv::Mat src);//显示装甲板
void showBothContours(std::string windows_name, const cv::Mat src);//显示扇叶和装甲板
void showCenterRContours(std::string windows_name, const cv::Mat src);//显示风车中心候选区R
void showFanContours(std::string windows_name, const cv::Mat &src, const std::vector<EnergyPart> &fans);
void showArmorContours(std::string windows_name, const cv::Mat &src, const std::vector<EnergyPart> &armors);
void showBothContours(std::string windows_name, const cv::Mat &src, const std::vector<EnergyPart> &fans,
const std::vector<EnergyPart> &armors);
void getFanPolarAngle();//获取扇叶极坐标角度
void getArmorPolarAngle();//获取装甲板极坐标角度
void getAllArmorCenters();//记录所有装甲板中心坐标
bool isValidFanContour(const vector<cv::Point> &fan_contour);
bool isValidArmorContour(const vector<cv::Point> &armor_contour);
void circleLeastFit();//利用所有记录的装甲板中心最小二乘法计算圆心和半径
void getFanPosition(std::vector<float> &fanPosition, const std::vector<EnergyPart> &fans, cv::Point cycle_center, double radius);
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 findTargetByPolar();//通过极坐标角度匹配获取目标装甲板的极坐标角度和装甲板中心坐标
void findTargetByIntersection();//通过面积重合度匹配获取目标装甲板的极坐标角度和装甲板中心坐标
void cycleQuickCalculate(std::vector<cv::Point> &first_armor_centers, cv::Point &cycle_center, double &radius);
void cycleDefaultCalculateConst(cv::Point &cycle_center, double &radius);
void cycleCalculate();
void cycleLeastFit();
void rotate();//获取预测点位
void stretch(cv::Point point_1, cv::Point2f &point_2);//将像素差转换为实际距离差
double pointDistance(cv::Point point_1, cv::Point point_2);//计算两点距离
void findTarget(const std::vector<float>fanPosition, const std::vector<float>armorPosition, float &target_armor);
void writeDownMark();//记录操作手标定的云台初始角度
void findWholeCycle(const std::vector<cv::Point>&first_armor_centers);
void getPredictPoint();//获取预测点位
void getAimPoint();//通过自瞄逻辑计算点位
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 savePoint2f(std::vector<cv::Point2f> &point_save, cv::Point point);
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 splitBayerBG(cv::Mat src, cv::Mat &blue, cv::Mat &red);//拜耳阵列分离
void imagePreprocess(cv::Mat &src);//图像通道分离
void getHitPoint();
bool changeTarget();
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);
void StructingElementClose(cv::Mat &src,int length, int width);//闭运算
void StructingElementErodeDilate(cv::Mat &src);//腐蚀和膨胀
};

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

@@ -10,6 +10,11 @@
using std::vector;
//----------------------------------------------------------------------------------------------------------------------
// 此结构体为能量机关模块的结构体，用于寻找矩形轮廓
// ---------------------------------------------------------------------------------------------------------------------
struct EnergyPart {
cv::RotatedRect rect;
float angle;
@@ -21,52 +26,50 @@ struct EnergyPart {
};
};
//----------------------------------------------------------------------------------------------------------------------
// 此结构体包括能量机关参数
// ---------------------------------------------------------------------------------------------------------------------
struct EnergyPartParam {
double RPM;
double HIT_TIME;
int GRAY_THRESH;//二值化阈值
int SPLIT_GRAY_THRESH;//通道分离二值化阈值
int FAN_GRAY_THRESH;//扇叶识别二值化阈值
int ARMOR_GRAY_THRESH;//装甲板识别二值化阈值
int GRAY_THRESH;
int SPLIT_GRAY_THRESH;
int FAN_GRAY_THRESH;
int ARMOR_GRAY_THRESH;
long FAN_CONTOUR_AREA_MAX;//扇叶面积最大值
long FAN_CONTOUR_AREA_MIN;//扇叶面积最小值
long FAN_CONTOUR_LENGTH_MIN;//扇叶长边长度最小值
long FAN_CONTOUR_LENGTH_MAX;//扇叶长边长度最大值
long FAN_CONTOUR_WIDTH_MIN;//扇叶宽边长度最小值
long FAN_CONTOUR_WIDTH_MAX;//扇叶宽边长度最大值
float FAN_CONTOUR_HW_RATIO_MAX;//扇叶长宽比最大值
float FAN_CONTOUR_HW_RATIO_MIN;//扇叶长宽比最小值
long FAN_CONTOUR_AREA_MAX;
long FAN_CONTOUR_AREA_MIN;
long FAN_CONTOUR_LENGTH_MIN;
long FAN_CONTOUR_LENGTH_MAX;
long FAN_CONTOUR_WIDTH_MIN;
long FAN_CONTOUR_WIDTH_MAX;
float FAN_CONTOUR_HW_RATIO_MAX;
float FAN_CONTOUR_HW_RATIO_MIN;
long ARMOR_CONTOUR_AREA_MAX;//装甲板面积最大值
long ARMOR_CONTOUR_AREA_MIN;//装甲板面积最小值
long ARMOR_CONTOUR_LENGTH_MIN;//装甲板长边长度最小值
long ARMOR_CONTOUR_WIDTH_MIN;//装甲板长边长度最大值
long ARMOR_CONTOUR_LENGTH_MAX;//装甲板宽边长度最小值
long ARMOR_CONTOUR_WIDTH_MAX;//装甲板宽边长度最大值
float ARMOR_CONTOUR_HW_RATIO_MAX;//装甲板长宽比最大值
float ARMOR_CONTOUR_HW_RATIO_MIN;//装甲板长宽比最小值
long ARMOR_CONTOUR_AREA_MAX;
long ARMOR_CONTOUR_AREA_MIN;
long ARMOR_CONTOUR_LENGTH_MIN;
long ARMOR_CONTOUR_WIDTH_MIN;
long ARMOR_CONTOUR_LENGTH_MAX;
long ARMOR_CONTOUR_WIDTH_MAX;
float ARMOR_CONTOUR_HW_RATIO_MAX;
float ARMOR_CONTOUR_HW_RATIO_MIN;
long CENTER_R_CONTOUR_AREA_MAX;//风车中心R面积最大值
long CENTER_R_CONTOUR_AREA_MIN;//风车中心R面积最小值
long CENTER_R_CONTOUR_LENGTH_MIN;//风车中心R长边长度最小值
long CENTER_R_CONTOUR_WIDTH_MIN;//风车中心R长边长度最大值
long CENTER_R_CONTOUR_LENGTH_MAX;//风车中心R宽边长度最小值
long CENTER_R_CONTOUR_WIDTH_MAX;//风车中心R宽边长度最大值
float CENTER_R_CONTOUR_HW_RATIO_MAX;//风车中心R长宽比最大值
float CENTER_R_CONTOUR_HW_RATIO_MIN;//风车中心R长宽比最小值
float TWIN_ANGEL_MAX;
float TWIN_ANGEL_MAX;//扇叶和装甲板匹配时极坐标角度差的最大值
long INTERSETION_CONTOUR_AREA_MIN;//扇叶与装甲板匹配时的最小重合面积
long TARGET_CHANGE_DISTANCE_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{
float yaw;
float pitch;
}GMAngle_t;
extern GMAngle_t aim;
#endif //PARAM_STRUCT_DEFINE_H

63
energy/src/energy/calculate/circle_calculate.cpp Normal file
View File

@@ -0,0 +1,63 @@
//
// 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_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_armor_centers.size());
for (int i = 0; i < N; i++)
{
double x = all_armor_centers.at(i).x;
double y = all_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;
}

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

@@ -1,106 +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::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()
{
cycle_center.x = 0;
cycle_center.y = 0;
radius = 0.0f;
if (all_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_armor_centers.size());
for (int i = 0; i < N; i++)
{
double x = all_armor_centers.at(i).x;
double y = all_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;
cycle_center.x = static_cast<int>(a / (-2));
cycle_center.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;
}

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

@@ -8,7 +8,12 @@ using std::cout;
using std::endl;
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* bayer_data[2];
for (int i = 0; i < src.rows; ++i) {
@@ -24,6 +29,12 @@ void Energy::splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red) {
}
}
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数对图像进行通道分离处理
// ---------------------------------------------------------------------------------------------------------------------
void Energy::imagePreprocess(cv::Mat &src) {
if(src.type() == CV_8UC1)
{

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

@@ -8,6 +8,10 @@ using std::cout;
using std::endl;
using std::vector;
//----------------------------------------------------------------------------------------------------------------------
// 此函数对图像进行闭运算操作
// ---------------------------------------------------------------------------------------------------------------------
void Energy::StructingElementClose(cv::Mat &src,int length, int width){
if (src.empty())return;
//threshold(src, src, energy_part_param_.CAMERA_GRAY_THRESH, 255, THRESH_BINARY);
@@ -15,10 +19,15 @@ void Energy::StructingElementClose(cv::Mat &src,int length, int width){
morphologyEx(src, src, MORPH_CLOSE, element);
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数对图像进行腐蚀与膨胀操作
// ---------------------------------------------------------------------------------------------------------------------
void Energy::StructingElementErodeDilate(cv::Mat &src) {
cv::Mat src_out, src_out_out;
Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(3, 3));
Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 1));
Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(3, 4));
Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(8, 6));
Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(4 , 4));
Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(3, 3));
@@ -33,9 +42,9 @@ void Energy::StructingElementErodeDilate(cv::Mat &src) {
dilate(src, src, element_dilate_2);
// imshow("dilate_2", src);
erode(src,src, element_erode_2);
// imshow("erode_2", src);
dilate(src, src, element_dilate_3);
// imshow("dilate_3", src);
erode(src,src, element_erode_2);
imshow("erode_2", src);
}

25
energy/src/energy/energy.cpp
View File

@@ -9,8 +9,10 @@ using std::cout;
using std::endl;
using std::vector;
extern uint8_t last_state;
//----------------------------------------------------------------------------------------------------------------------
// 此函数为能量机关构造函数，只要程序不重启就不会重新构造
// ---------------------------------------------------------------------------------------------------------------------
Energy::Energy(Serial &u, uint8_t &color):serial(u),ally_color(color),
src_blue(SRC_HEIGHT, SRC_WIDTH, CV_8UC1),
src_red(SRC_HEIGHT, SRC_WIDTH, CV_8UC1)
@@ -18,7 +20,7 @@ Energy::Energy(Serial &u, uint8_t &color):serial(u),ally_color(color),
initEnergy();
initEnergyPartParam();
save_new_mark = true;
save_new_mark = false;
if(ally_color == ALLY_RED){
origin_yaw = red_origin_yaw;
@@ -33,25 +35,24 @@ Energy::Energy(Serial &u, uint8_t &color):serial(u),ally_color(color),
}
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数为能量机关析构函数，设置为默认
// ---------------------------------------------------------------------------------------------------------------------
Energy::~Energy() = default;
//
//void Energy::setAllyColor(int color)
//{
// ally_color_ = color;
//}
void Energy::setRotation(int rotation){
energy_part_rotation = rotation;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数为能量机关再初始化函数，如果未重启程序但重新进入能量机关，则会进行初始化，但不会将save_new_mark置为false
// ---------------------------------------------------------------------------------------------------------------------
void Energy::setEnergyRotationInit() {
initEnergy();
initEnergyPartParam();
energy_rotation_init = true;
if(!save_new_mark){
if(save_new_mark){
FILE *fp = fopen(PROJECT_DIR"/Mark/mark.txt", "r");
if(fp){
fscanf(fp,"%f %f",&origin_yaw,&origin_pitch);

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;
}*/
}

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

@@ -8,51 +8,52 @@ using std::cout;
using std::endl;
using std::vector;
int Energy::findFan(const cv::Mat &src, vector<EnergyPart> &fans, int &last_fans_cnt) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于寻找图像内所有的大风车扇叶
// ---------------------------------------------------------------------------------------------------------------------
int Energy::findFan(const cv::Mat src, int &last_fans_cnt) {
if (src.empty())return 0;
static Mat src_bin;
src_bin = src.clone();
// threshold(src, src_bin, energy_part_param_.FAN_GRAY_THRESH, 255, THRESH_BINARY);
if(src.type() == CV_8UC3){
cvtColor(src_bin, src_bin, CV_BGR2GRAY);
cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
}
std::vector<vector<Point> > fan_contours;
StructingElementClose(src_bin,6,6);
StructingElementClose(src_bin,6,6);//图像膨胀，防止图像断开并更方便寻找
// imshow("fan struct",src_bin);
findContours(src_bin, fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
for (auto &fan_contour : fan_contours) {
if (!isValidFanContour(fan_contour)) {
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);
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 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){
// cout<<cur_rect.center;
// fans.emplace_back(fan_contour);
// cout<<"fan area: "<<length<<'\t'<<width<<endl;
// }
//
if(length>20&&width>20){
cout<<cur_rect.center;
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){
last_fans_cnt = static_cast<int>(fans.size());
return -1;
return -1;//寻找到的扇叶比上一帧少，说明该帧有误，返回-1
}
last_fans_cnt = static_cast<int>(fans.size());
return static_cast<int>(fans.size());
@@ -60,19 +61,22 @@ int Energy::findFan(const cv::Mat &src, vector<EnergyPart> &fans, int &last_fans
int Energy::findArmor(const cv::Mat &src, vector<EnergyPart> &armors, int &last_armors_cnt) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于寻找图像内所有的大风车装甲板模块
// ---------------------------------------------------------------------------------------------------------------------
int Energy::findArmor(const cv::Mat src, int &last_armors_cnt) {
if (src.empty())return 0;
static Mat src_bin;
src_bin = src.clone();
// threshold(src, src_bin, energy_part_param_.ARMOR_GRAY_THRESH, 255, THRESH_BINARY);
if(src.type() == CV_8UC3){
cvtColor(src_bin, src_bin, CV_BGR2GRAY);
cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道
}
std::vector<vector<Point> > armor_contours;
std::vector<vector<Point> > armor_contours_external;//用总轮廓减去外轮廓，只保留内轮廓，除去流动条的影响。
StructingElementErodeDilate(src_bin);
// StructingElementClose(src_bin,10,10);
StructingElementErodeDilate(src_bin);//图像膨胀，防止图像断开并更方便寻找
// imshow("armor struct",src_bin);
findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
@@ -92,70 +96,80 @@ int Energy::findArmor(const cv::Mat &src, vector<EnergyPart> &armors, int &last_
}
}
}
for (auto &armor_contour : armor_contours) {
if (!isValidArmorContour(armor_contour))
{
continue;
}
armors.emplace_back(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;
// 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;
// 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);
// cout<<"armor area: "<<length<<'\t'<<width<<endl;
}
// cout<<armors.size()<<endl;
if(armors.size() < last_armors_cnt){
last_armors_cnt = static_cast<int>(armors.size());
return -1;
return -1;//寻找到的装甲板比上一帧少，说明该帧有误，返回-1
}
last_armors_cnt = static_cast<int>(armors.size());
return static_cast<int>(armors.size());
}
int Energy::findGimbleZeroPoint(const cv::Mat &src, vector<EnergyPart> &gimble_zero_points) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于寻找图像内大风车中心字母“R”
// ---------------------------------------------------------------------------------------------------------------------
int Energy::findCenterR(const cv::Mat src) {
if (src.empty())return 0;
static Mat src_bin;
src_bin = src.clone();
// threshold(src, src_bin, energy_part_param_.FAN_GRAY_THRESH, 255, THRESH_BINARY);
// threshold(src, src_bin, energy_part_param_.ARMOR_GRAY_THRESH, 255, THRESH_BINARY);
if(src.type() == CV_8UC3){
cvtColor(src_bin, src_bin, CV_BGR2GRAY);
}
std::vector<vector<Point> > zero_point_contours;
std::vector<vector<Point> > center_R_contours;
StructingElementErodeDilate(src_bin);
// imshow("R struct",src_bin);
findContours(src_bin, center_R_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
findContours(src_bin, zero_point_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
for (auto &center_R_contour : center_R_contours) {
if (!isValidCenterRContour(center_R_contour))
{
continue;
}
centerRs.emplace_back(center_R_contour);
for (auto &zero_point_contour : zero_point_contours) {
double cur_contour_area = contourArea(zero_point_contour);
RotatedRect cur_rect = minAreaRect(zero_point_contour);
/*RotatedRect cur_rect = minAreaRect(center_R_contour);
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;
float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;
float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;
if(length<10&&width<10&&length>1&&width>1){
cout<<"zero point center: "<<cur_rect.center<<endl;
cout<<"zero point area: "<<length<<'\t'<<width<<endl;
gimble_zero_points.emplace_back(zero_point_contour);
if(length>10 && width>5){
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;*/
}
if(centerRs.size() == 0) return -1;//未找到合适的中心候选区，说明该帧有误，返回-1
return static_cast<int>(centerRs.size());
}
}
return static_cast<int>(fans.size());
}
bool Energy::isValidFanContour(const vector<cv::Point> &fan_contour) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断找到的矩形候选区是否为扇叶
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::isValidFanContour(const vector<cv::Point> fan_contour) {
double cur_contour_area = contourArea(fan_contour);
if (cur_contour_area > energy_part_param_.FAN_CONTOUR_AREA_MAX ||
cur_contour_area < energy_part_param_.FAN_CONTOUR_AREA_MIN)
@@ -163,66 +177,108 @@ 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<<"area fail."<<endl;
return false;
//选区面积大小不合适
}
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;
float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;//将矩形的长边设置为长
float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;//将矩形的短边设置为宽
if (length < 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)
{
//cout<<"length width min fail."<<endl;
//cout<<"length width fail."<<endl;
return false;
//矩形边长不合适
}
// float length_width_ratio = length / width;
// if (length_width_ratio > energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX ||
// length_width_ratio < energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN)
// {
// //cout<<"length width ratio fail."<<endl;
// return false;
// }
if (cur_contour_area / cur_size.area() < 0.6) return false;
float length_width_ratio = length / width;//计算矩形长宽比
if (length_width_ratio > energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX ||
length_width_ratio < energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN)
{
//cout<<"length width ratio fail."<<endl;
return false;
//长宽比不合适
}
if (cur_contour_area / cur_size.area() < 0.6) return false;//轮廓对矩形的面积占有率不合适
return true;
}
bool Energy::isValidArmorContour(const vector<cv::Point> &armor_contour) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断找到的矩形候选区是否为装甲板
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::isValidArmorContour(const vector<cv::Point> armor_contour) {
double cur_contour_area = contourArea(armor_contour);
// if (cur_contour_area > energy_part_param_.ARMOR_CONTOUR_AREA_MAX ||
// cur_contour_area < energy_part_param_.ARMOR_CONTOUR_AREA_MIN)
// {
// //cout<<cur_contour_area<<" "<<energy_fan_param_.CONTOUR_AREA_MIN<<" "<<energy_fan_param_.CONTOUR_AREA_MAX<<endl;
// //cout<<"area fail."<<endl;
// return false;
// }
if (cur_contour_area > energy_part_param_.ARMOR_CONTOUR_AREA_MAX ||
cur_contour_area < energy_part_param_.ARMOR_CONTOUR_AREA_MIN)
{
//cout<<cur_contour_area<<" "<<energy_fan_param_.CONTOUR_AREA_MIN<<" "<<energy_fan_param_.CONTOUR_AREA_MAX<<endl;
//cout<<"area fail."<<endl;
return false;
//选区面积大小不合适
}
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;
if (length < energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN || width < energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN)
float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;//将矩形的长边设置为长
float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;//将矩形的短边设置为宽
if (length < 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;
//矩形边长不合适
}
if (length > energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX||width>energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX)
{
//cout<<"length width max fail."<<endl;
return false;
}
float length_width_ratio = length / width;
float length_width_ratio = length / width;//计算矩形长宽比
if (length_width_ratio > energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX ||
length_width_ratio < energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN)
{
//cout<<"length width ratio fail."<<endl;
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;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于判断找到的矩形候选区是否为可能的风车中心R候选区
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::isValidCenterRContour(const vector<cv::Point> center_R_contour) {
double cur_contour_area = contourArea(center_R_contour);
if (cur_contour_area > energy_part_param_.ARMOR_CONTOUR_AREA_MAX ||
cur_contour_area < energy_part_param_.ARMOR_CONTOUR_AREA_MIN)
{
//cout<<cur_contour_area<<" "<<energy_fan_param_.CONTOUR_AREA_MIN<<" "<<energy_fan_param_.CONTOUR_AREA_MAX<<endl;
//cout<<"area fail."<<endl;
return false;
//选区面积大小不合适
}
RotatedRect cur_rect = minAreaRect(center_R_contour);
Size2f cur_size = cur_rect.size;
float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width;//将矩形的长边设置为长
float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width;//将矩形的短边设置为宽
if (length < 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 fail."<<endl;
return false;
//矩形边长不合适
}
float length_width_ratio = length / width;//计算矩形长宽比
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)
{
//cout<<"length width ratio fail."<<endl;
return false;
//长宽比不合适
}
if (cur_contour_area / cur_size.area() < 0.7) return false;//轮廓对矩形的面积占有率不合适
return true;
}

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

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

23
energy/src/energy/get/aim_point_get.cpp Normal file
View File

@@ -0,0 +1,23 @@
//
// Created by sun on 19-7-6.
//
#include "energy/energy.h"
#include "energy/constant.h"
using namespace std;
using namespace cv;
#define FOCUS_PIXAL_5MM (917)
#define FOCUS_PIXAL FOCUS_PIXAL_5MM
//----------------------------------------------------------------------------------------------------------------------
// 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击
// ---------------------------------------------------------------------------------------------------------------------
void Energy::getAimPoint(){
double dx = target_point.x - 320;
double dy = target_point.y - 240;
yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;
pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;
}

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

@@ -10,6 +10,11 @@ using std::cout;
using std::endl;
using std::vector;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于操作手手动标定
// ---------------------------------------------------------------------------------------------------------------------
void Energy::changeMark() {
if (mcuData.mark == 0 && last_mark == 1) {
last_mark = mcuData.mark;
@@ -32,41 +37,15 @@ void Energy::changeMark() {
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于计算云台应当转到的角度
// ---------------------------------------------------------------------------------------------------------------------
void Energy::gimbleRotation(){
cv::Point2f real_hit_point;
stretch(hit_point, real_hit_point);
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));
/*origin_yaw = mark_yaw;
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));
// }
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));
}

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

57
energy/src/energy/get/polar_angle_get.cpp Normal file
View File

@@ -0,0 +1,57 @@
//
// 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::getFanPolarAngle() {
if (radius == 0)return;
for (const auto &fan : fans)
{
float angle = static_cast<float>(180 / PI * atan2(-1 * (fan.rect.center.y - circle_center_point.y),
(fan.rect.center.x - circle_center_point.x)));
fan_polar_angle.push_back(angle);
}
// cout << "fanPosition.size() = " << fanPosition.size() << '\t' << endl;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于获得图像中所有装甲板的当前极坐标角度
// ---------------------------------------------------------------------------------------------------------------------
void Energy::getArmorPolarAngle() {
if (radius == 0)return;
for (const auto &armor : armors)
{
float angle = static_cast<float>(180 / PI * atan2(-1 * (armor.rect.center.y - circle_center_point.y),
(armor.rect.center.x - circle_center_point.x)));
armor_polar_angle.push_back(angle);
}
// cout << "armorPosition.size() = " << armorPosition.size() << '\t' << endl;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于存储图像中所有装甲板的中心坐标，以便后续最小二乘法计算圆心和半径
// ---------------------------------------------------------------------------------------------------------------------
void Energy::getAllArmorCenters()
{
for (const auto &armor : armors) {
all_armor_centers.push_back(armor.rect.center);
}
}

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

@@ -1,49 +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::getFanPosition(std::vector<float> &fanPosition, const std::vector<EnergyPart> &fans, cv::Point cycle_center, double radius) {
if (radius == 0)return;
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)));
fanPosition.push_back(angle);
}
// 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) {
if (radius == 0)return;
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)));
armorPosition.push_back(angle);
}
// 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()
{
for (const auto &armor : armors) {
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)));
}

37
energy/src/energy/get/predict_point_get.cpp Normal file
View File

@@ -0,0 +1,37 @@
//
// 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::getPredictPoint(){
if(energy_rotation_direction==1) rotate();
if(energy_rotation_direction==-1) rotate();
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于操作手手动标定
// ---------------------------------------------------------------------------------------------------------------------
bool Energy::changeTarget(){
if(pointDistance(target_point,last_target_point) < energy_part_param_.TARGET_CHANGE_DISTANCE_MAX){
last_target_point = target_point;
return false;
}
else{
last_target_point= target_point;
return true;
}
}

31
energy/src/energy/mark/mark.cpp Normal file
View File

@@ -0,0 +1,31 @@
//
// Created by sun on 19-7-5.
//
#include "energy/energy.h"
using namespace std;
using namespace cv;
void Energy::writeDownMark() {
if(armors_cnt>=4 && fans_cnt>=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);
save_new_mark = true;
}
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(armors_cnt==5){
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);
}
}
}

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

@@ -9,34 +9,31 @@ using std::cout;
using std::endl;
using std::vector;
void Energy::initEnergy() {
isSendTarget = false;
isMark = false;
//----------------------------------------------------------------------------------------------------------------------
// 此函数对能量机关成员变量进行初始化
// ---------------------------------------------------------------------------------------------------------------------
void Energy::initEnergy() {
isMark = false;
centered=false;
fans_cnt = 0;
armors_cnt = 0;
cycle_center = Point(0, 0);
target_center = Point(0, 0);
last_target_center = Point(0, 0);
hit_point = Point(0, 0);
target_position = -1000;
last_target_position = -1000;
last_hit_position = 20000;
target_armor = -1000;
last_target_armor = -1000;
centerRs_cnt = 0;
gimble_cnt = 0;
circle_center_point = Point(0, 0);
target_point = Point(0, 0);
last_target_point = Point(0, 0);
predict_point = Point(0, 0);
former_point = Point(0,0);
target_polar_angle = -1000;
last_target_polar_angle = -1000;
radius = 0;
// ally_color = ALLY_RED;
energy_part_rotation = CLOCKWISE;
energy_rotation_direction = ANTICLOCKWISE;
attack_distance = ATTACK_DISTANCE;
count = 1;
last_fans_cnt = 0;
last_armors_cnt = 0;
send_cnt = 0;
//rectified_focal_length = 1000;
//theta = 0;
//phi = 0;
yaw_rotation = 0;
pitch_rotation = 0;
last_mark = 0;
@@ -47,41 +44,29 @@ void Energy::initEnergy() {
blue_origin_pitch = 15.688477;
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;
predict_rad = 20;
fans.clear();
armors.clear();
fanPosition.clear();
armorPosition.clear();
Armor_center.clear();
first_armor_centers.clear();
centerRs.clear();
fan_polar_angle.clear();
armor_polar_angle.clear();
all_armor_centers.clear();
clockwise_rotation_init_cnt = 0;
anticlockwise_rotation_init_cnt = 0;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数对能量机关参数进行初始化
// ---------------------------------------------------------------------------------------------------------------------
void Energy::initEnergyPartParam() {
energy_part_param_.RPM = 10;
energy_part_param_.HIT_TIME = 1.14;
energy_part_param_.GRAY_THRESH = 240;
energy_part_param_.GRAY_THRESH = 235;
energy_part_param_.SPLIT_GRAY_THRESH = 60;
energy_part_param_.FAN_GRAY_THRESH = 75;
energy_part_param_.ARMOR_GRAY_THRESH = 80;
@@ -97,45 +82,52 @@ void Energy::initEnergyPartParam() {
energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 100000;
energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0;
energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 30;
energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 15;
energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 50;
energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 45;
energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 15;
energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 5;
energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 30;
energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 20;
energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;
energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 100000;
energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 0;
energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 10;
energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 10;
energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 30;
energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 30;
energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 3;
energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1;
energy_part_param_.TWIN_ANGEL_MAX = 10;
energy_part_param_.INTERSETION_CONTOUR_AREA_MIN = 60;
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;
energy_part_param_.TARGET_CHANGE_DISTANCE_MAX = 20;
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数对能量机关旋转方向进行初始化
// ---------------------------------------------------------------------------------------------------------------------
void Energy::initRotation() {
target_position = target_armor;
// cout << "target position: " << target_position << '\t' << "last target position: " << last_target_position << endl;
if (target_position >= -180 && last_target_position >= -180 && fabs(target_position - last_target_position) < 30) {
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 >= -180 && last_target_polar_angle >= -180
&& fabs(target_polar_angle - last_target_polar_angle) < 30) {
//target_polar_angle和last_target_polar_angle的初值均为1000，大于-180表示刚开始几帧不要
//若两者比较接近，则说明没有切换目标，因此可以用于顺逆时针的判断
if (target_polar_angle < last_target_polar_angle) clockwise_rotation_init_cnt++;
else if (target_polar_angle > last_target_polar_angle) anticlockwise_rotation_init_cnt++;
}
//由于刚开始圆心判断不准，角度变化可能计算有误，因此需要在角度正向或逆向变化足够大时才可确定是否为顺逆时针
if (clockwise_rotation_init_cnt == 30) {
energy_part_rotation = CLOCKWISE;
cout << "rotation: " << energy_part_rotation << endl;
energy_rotation_direction = CLOCKWISE;//顺时针变化30次，确定为顺时针
cout << "rotation: " << energy_rotation_direction << endl;
energy_rotation_init = false;
}
else if (anticlockwise_rotation_init_cnt == 30) {
energy_part_rotation = ANTICLOCKWISE;
cout << "rotation: " << energy_part_rotation << endl;
energy_rotation_direction = ANTICLOCKWISE;//逆时针变化30次，确定为顺时针
cout << "rotation: " << energy_rotation_direction << endl;
energy_rotation_init = false;
}
// else cout << clockwise_rotation_init_cnt <<'\t'<<anticlockwise_rotation_init_cnt<< endl;
last_target_position = target_position;
last_target_polar_angle = target_polar_angle;
}

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

@@ -9,134 +9,148 @@ using std::cout;
using std::endl;
using std::vector;
//extern float curr_yaw, curr_pitch, mark_yaw, mark_pitch;
//extern int mark;
int Energy::run(cv::Mat &src){
// imshow("src",src);
fans.clear();
//----------------------------------------------------------------------------------------------------------------------
// 此函数为大能量机关模式主控制流函数，且步兵需要同时拥有云台摄像头和底盘摄像头
// ---------------------------------------------------------------------------------------------------------------------
int Energy::runBig(cv::Mat &gimble_src, cv::Mat &chassis_src){
if(chassis_src.empty())
runBig(gimble_src);//仅拥有云台摄像头则调用单摄像头的run函数
else if(!centered) {
armors.clear();
fanPosition.clear();
armorPosition.clear();
// gimble_zero_points.clear();
isSendTarget = false;
armor_polar_angle.clear();
changeMark();
if (isMark)return 0;
// cout<<"yaw"<<origin_yaw<<endl;
// if(all_armor_centers.size()>200)all_armor_centers.clear();
// if(first_armor_centers.size()>200)first_armor_centers.clear();
// cout<<"first_armor_centers.size(): "<<first_armor_centers.size()<<endl;
threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
imshow("yun",gimble_src);
// imagePreprocess(src);
// imshow("img_preprocess",src);
armors_cnt = findArmor(gimble_src, last_armors_cnt);
if(armors_cnt!=1) return 0;//滤去漏判的帧
threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
// imshow("bin",src);
fans_cnt = findFan(src, fans, last_fans_cnt);
// cout<<"fans_cnt: "<<fans_cnt<<endl;
if(fans_cnt==-1) return 0;//滤去漏判的帧
// if(fans_cnt>0)showFanContours("fan",src,fans);
// fans_cnt=0;
armors_cnt = findArmor(src, armors, last_armors_cnt);
// cout<<"armors_cnt: "<<armors_cnt<<endl;
if(armors_cnt==-1) return 0;//滤去漏判的帧
// if(armors_cnt>0) showArmorContours("armor",src,armors);
if(armors_cnt>0||fans_cnt>0) showBothContours("Both",src, fans, armors);
if(armors_cnt>=4 && fans_cnt>=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);
save_new_mark = false;
}
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(armors_cnt==5){
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);
}
}
// cout<<"armors_cnt: "<<armors_cnt<<"fans_cnt: "<<fans_cnt<<endl;
// cout<<"armors_cnt: "<<armors_cnt<<"fans_cnt: "<<fans_cnt<<endl;
// if(armors_cnt != fans_cnt+1)
// {
// return 0;
// }
//cout<<"clock: "<<energy_part_rotation<<endl;
getAllArmorCenters();
// cout<<"all_armor_centers.size(): "<<all_armor_centers.size()<<endl;
cycleLeastFit();
circleLeastFit();
// 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;
getFanPosition(fanPosition, fans, cycle_center, radius);
getArmorPosition(armorPosition, armors, cycle_center, radius);
findTarget(fanPosition, armorPosition, target_armor);
// 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) {
initRotation();
return 0;
}
getHitPoint();
// hit_point = target_center;
// cout << "The hit point position is " << hit_point << endl;
if(++gimble_cnt%8==0){
former_point=circle_center_point;
//gimble_cnt=0;
}
if(former_point==predict_point&&gimble_cnt%8==7&&predict_point!=Point(0,0)) {
centered=true;
cout<<"gimble focused!"<<endl;
cout<<"number of framse: "<<gimble_cnt<<endl;
}
predict_point=circle_center_point;
// cout<<gimble_cnt<<endl;
// cout<<"center:("<<predict_point.x<<','<<predict_point.y<<")\n";
gimbleRotation();
// cout<<"send"<<endl;
// cout<<"position mode: "<<position_mode<<endl;
gimbleRotation();
if(changeTarget())target_cnt++;
// if (!isSendTarget)return 0;
sendTargetByUart(yaw_rotation, pitch_rotation, target_cnt);
return 0;
}
// if(centered)
// destroyAllWindows();
return 0;
}
// cout<<target_cnt<<endl;
//----------------------------------------------------------------------------------------------------------------------
// 此函数为大能量机关模式主控制流函数，且步兵仅拥有云台摄像头
// ---------------------------------------------------------------------------------------------------------------------
int Energy::runBig(cv::Mat &gimble_src){
// imshow("src",src);
fans.clear();
armors.clear();
centerRs.clear();
fan_polar_angle.clear();
armor_polar_angle.clear();
changeMark();
if (isMark)return 0;
// imagePreprocess(src);
// imshow("img_preprocess",src);
threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
// imshow("bin",src);
fans_cnt = findFan(gimble_src, last_fans_cnt);
// cout<<"fans_cnt: "<<fans_cnt<<endl;
if(fans_cnt==-1) return 0;//滤去漏判的帧
// if(fans_cnt>0)showFanContours("fan",src);
// fans_cnt=0;
armors_cnt = findArmor(gimble_src, last_armors_cnt);
// cout<<"armors_cnt: "<<armors_cnt<<endl;
if(armors_cnt==-1) return 0;//滤去漏判的帧
// if(armors_cnt>0) showArmorContours("armor",src);
if(armors_cnt != fans_cnt+1) return 0;
centerRs_cnt = findCenterR(gimble_src);
// if(centerRs_cnt>0)showCenterRContours("R", gimble_src);
writeDownMark();
getAllArmorCenters();
circleLeastFit();
// attack_distance = ATTACK_DISTANCE;
getFanPolarAngle();
getArmorPolarAngle();
findTargetByPolar();
// findTargetByIntersection();
if(armors_cnt>0||fans_cnt>0) showBothContours("Both", gimble_src);
if (energy_rotation_init) {
initRotation();
return 0;
}
getPredictPoint();
gimbleRotation();
sendTargetByUart(yaw_rotation, pitch_rotation, target_cnt);
// cout<<"yaw: "<<yaw_rotation<<'\t'<<"pitch: "<<pitch_rotation<<endl;
// cout<<"curr_yaw: "<<mcuData.curr_yaw<<'\t'<<"curr_pitch: "<<mcuData.curr_pitch<<endl;
// cout<<"send_cnt: "<<send_cnt<<endl;
return 0;
}
//-----------------------------------------------------------------------------------
/*
//此处用于标定云台在摄像头视频中的零点
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;
*/
//----------------------------------------------------------------------------------------------------------------------
// 此函数为小能量机关模式主控制流函数，击打小符只需要拥有云台摄像头
// ---------------------------------------------------------------------------------------------------------------------
int Energy::runSmall(cv::Mat &gimble_src){
imshow("gimble src", gimble_src);
if(gimble_src.type()== CV_8UC3)cvtColor(gimble_src, gimble_src, COLOR_BGR2GRAY);
fans.clear();
armors.clear();
threshold(gimble_src, gimble_src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
imshow("bin",gimble_src);
fans_cnt = findFan(gimble_src, last_fans_cnt);
armors_cnt = findArmor(gimble_src, last_armors_cnt);
if(fans_cnt==-1 || armors_cnt==-1 || armors_cnt != fans_cnt+1) return 0;
findTargetByIntersection();
if(armors_cnt>0||fans_cnt>0) showBothContours("Both", gimble_src);
getAimPoint();
sendTargetByUart(yaw_rotation, pitch_rotation, target_cnt);
}

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

@@ -5,24 +5,17 @@
#include <iostream>
using namespace std;
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于向主控板发送数据
// ---------------------------------------------------------------------------------------------------------------------
bool sendTarget(Serial& serial, float x, float y, float z) {
static short x_tmp, y_tmp, z_tmp;
// static time_t last_time = time(nullptr);
// static int fps;
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);
y_tmp = static_cast<short>(y * (32768 - 1) / 100);
z_tmp = static_cast<short>(z * (32768 - 1) / 1000);
buff[0] = 's';
buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
@@ -31,14 +24,16 @@ bool sendTarget(Serial& serial, float x, float y, float z) {
buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
buff[7] = 'e';
return serial.WriteData(buff, sizeof(buff));
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于操作手数据发送
// ---------------------------------------------------------------------------------------------------------------------
void Energy::sendTargetByUart(float x, float y, float z) {
// if(!changeTarget()){
// return;
// }
if(changeTarget())target_cnt++;
sendTarget(serial, x, y, z);
send_cnt+=1;
// cout<<"send"<<endl;

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

@@ -8,7 +8,12 @@ using std::cout;
using std::endl;
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;
static Mat image2show;
@@ -20,20 +25,22 @@ void Energy::showFanContours(std::string windows_name, const cv::Mat &src, const
{
image2show = src.clone();
}
//cvtColor(image2show, image2show, COLOR_GRAY2RGB);
for (const auto &fan : fans)
{
Point2f vertices[4]; //定义矩形的4个顶点
fan.rect.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++)
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 2);
//cout << fan.rect.center << '\t' << fan.rect.angle << '\t';
//cout << endl;
}
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;
static Mat image2show;
@@ -45,21 +52,22 @@ void Energy::showArmorContours(std::string windows_name, const cv::Mat &src, con
{
image2show = src.clone();
}
//cvtColor(image2show, image2show, COLOR_GRAY2RGB);
for (const auto &armor : armors)
{
Point2f vertices[4]; //定义矩形的4个顶点
armor.rect.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++)
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 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, const std::vector<EnergyPart> &fans,
const std::vector<EnergyPart> &armors) {
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中所有扇叶和装甲板,并框出待击打装甲板
// ---------------------------------------------------------------------------------------------------------------------
void Energy::showBothContours(std::string windows_name, const cv::Mat src) {
if (src.empty())return;
static Mat image2show;
if(src.type() == CV_8UC1) // 黑白图像
@@ -70,25 +78,23 @@ void Energy::showBothContours(std::string windows_name, const cv::Mat &src, cons
{
image2show = src.clone();
}
//cvtColor(image2show, image2show, COLOR_GRAY2RGB);
for (const auto &fan : fans)
{
Point2f vertices[4]; //定义矩形的4个顶点
fan.rect.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++)
line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 0, 0), 4);
// cout << "fan" << fan.rect.size.height <<'\t'<< fan.rect.size.width << '\t' << '\t';
// cout << endl;
}
for (const auto &armor : armors)
{
Point2f vertices[4]; //定义矩形的4个顶点
armor.rect.points(vertices); //计算矩形的4个顶点
for (int i = 0; i < 4; i++)
for (int i = 0; i < 4; i++){
if(pointDistance(static_cast<cv::Point2f>(armor.rect.center),target_point) < 5)
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);
// cout << "armor center: "<< armor.rect.center << '\t'<< "armor angle: "<< armor.rect.angle;
// cout << endl;
}
cv::Point2f point = armor.rect.center;
cv::circle(image2show, point, 2, cv::Scalar(0, 0, 255));//在图像中画出特征点，2是圆的半径
@@ -98,3 +104,38 @@ void Energy::showBothContours(std::string windows_name, const cv::Mat &src, cons
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中所有可能的风车中心候选区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);
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于显示图像中所有扇叶和装甲板
// ---------------------------------------------------------------------------------------------------------------------

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

@@ -13,6 +13,11 @@ using std::cout;
using std::endl;
using std::vector;
//----------------------------------------------------------------------------------------------------------------------
// 此函数选取图像中的一部分进行处理
// ---------------------------------------------------------------------------------------------------------------------
void Energy::extract(cv::Mat &src){
cv::Rect rect(EXTRACT_POINT_X, EXTRACT_POINT_Y, EXTRACT_WIDTH, EXTRACT_HEIGHT);
src = src(rect).clone();
@@ -20,16 +25,45 @@ void Energy::extract(cv::Mat &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) {
FourPoints.push_back(point_1);
FourPoints.push_back(point_2);
FourPoints.push_back(point_3);
FourPoints.push_back(point_4);
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于计算预测的击打点坐标
// ---------------------------------------------------------------------------------------------------------------------
void Energy::rotate() {
int x1, x2, y1, y2;
// 为了减小强制转换的误差
x1 = circle_center_point.x * 100;
x2 = target_point.x * 100;
y1 = circle_center_point.y * 100;
y2 = target_point.y * 100;
predict_point.x = static_cast<int>((x1 + (x2 - x1)*cos(-predict_rad * d2r) - (y1 - y2)*sin(-predict_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::savePoint2f(std::vector<cv::Point2f> &point_save, cv::Point point) {
point_save.push_back(static_cast<cv::Point2f>(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);
}
//----------------------------------------------------------------------------------------------------------------------
// 此函数用于计算两点距离
// ---------------------------------------------------------------------------------------------------------------------
double Energy::pointDistance(cv::Point point_1, cv::Point point_2){
double distance = 0;
@@ -38,51 +72,4 @@ double Energy::pointDistance(cv::Point point_1, cv::Point point_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;
// 为了减小强制转换的误差
x1 = center.x * 100;
x2 = point_old.x * 100;
y1 = center.y * 100;
y2 = point_old.y * 100;
point_new.x = static_cast<int>((x1 + (x2 - x1)*cos(-rad * d2r) - (y1 - y2)*sin(-rad * d2r))/100);
point_new.y = static_cast<int>((y1 - (x2 - x1)*sin(-rad * d2r) - (y1 - y2)*cos(-rad * d2r))/100);
}
void Energy::stretch(cv::Point point_1, cv::Point2f &point_2){
if(point_1==cycle_center){
cout<<"stretch wrong!"<<endl;
return;
}
double x_0 = point_1.x - cycle_center.x;
double y_0 = point_1.y - cycle_center.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);
}
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));
}

157
main.cpp
View File

@@ -20,9 +20,6 @@
#include <log.h>
#define ENERGY_STATE 'e'
#define ARMOR_STATE 'a'
using namespace cv;
using namespace std;
@@ -32,13 +29,20 @@ mcu_data mcuData = {
ARMOR_STATE,
0,
1,
ENEMY_BLUE,
ENEMY_RED,
};
WrapperHead *video_gimble = nullptr;
WrapperHead *video_chassis = nullptr;
Serial serial(115200);
uint8_t last_state = mcuData.state;
ArmorFinder armorFinder(mcuData.enemy_color, serial, PROJECT_DIR"/tools/para/", mcuData.use_classifier);
Energy energy(serial, mcuData.enemy_color);
int main(int argc, char *argv[]) {
process_options(argc, argv);
Serial serial(115200);
uint8_t last_state = mcuData.state;
thread receive(uartReceive, &serial);
int from_camera = 1;
@@ -48,128 +52,79 @@ int main(int argc, char *argv[]) {
}
while (true) {
VideoWriter armor_video_writer, energy_video_writer;
if (save_video) {
initVideoWriter(armor_video_writer, PROJECT_DIR"/armor_video/");
initVideoWriter(energy_video_writer, PROJECT_DIR"/energy_video/");
}
WrapperHead *video_armor=nullptr;
WrapperHead *video_energy=nullptr;
if (from_camera) {
video_armor = new CameraWrapper(0/*, "armor"*/);
video_energy = new CameraWrapper(1, "energy");
video_gimble = new CameraWrapper(0/*, "armor"*/);
video_chassis = new CameraWrapper(1/*, "energy"*/);
} else {
video_armor = new VideoWrapper("/home/sjturm/Desktop/valid_video/armor/65.avi");
video_energy = new VideoWrapper("/home/sjturm/Desktop/valid_video/energy/121.avi");
video_gimble = new VideoWrapper("/Users/leo/Desktop/videos/179.avi");
video_chassis = new VideoWrapper("/Users/leo/Desktop/videos/179.avi");
}
if (video_armor->init()) {
LOGM("video_armor source initialization successfully.");
if (video_gimble->init()) {
LOGM("video_gimble source initialization successfully.");
} else {
LOGW("video_armor source unavailable!");
delete video_armor;
video_armor = nullptr;
LOGW("video_gimble source unavailable!");
delete video_gimble;
video_gimble = nullptr;
}
if (video_energy->init()) {
LOGM("video_energy source initialization successfully.");
if (video_chassis->init()) {
LOGM("video_chassis source initialization successfully.");
} else {
LOGW("video_energy source unavailable!");
delete video_energy;
video_energy = nullptr;
LOGW("video_chassis source unavailable!");
delete video_chassis;
video_chassis = nullptr;
}
Mat energy_src, armor_src;
Mat gimble_src, chassis_src;
for (int i = 0; i < 10; i++) {
if (video_armor) {
video_armor->read(armor_src);
if (video_gimble) {
video_gimble->read(gimble_src);
}
if (video_energy) {
video_energy->read(energy_src);
if (video_chassis) {
video_chassis->read(chassis_src);
}
}
ArmorFinder armorFinder(mcuData.enemy_color, serial, PROJECT_DIR"/tools/para/", mcuData.use_classifier);
Energy energy(serial, mcuData.enemy_color);
// energy.setAllyColor(ally_color);
energy.setRotation(CLOCKWISE);
bool ok = true;
cout<<"start running"<<endl;
do {
CNT_TIME("Total", {
if (mcuData.state == ENERGY_STATE) {
if (last_state == ARMOR_STATE) {
if (mcuData.state == BIG_ENERGY_STATE) {//大符模式
ok = checkReconnect(video_gimble->read(gimble_src), video_chassis->read(chassis_src));//检查有几个摄像头
if (save_video) saveVideos(gimble_src, chassis_src);//保存视频
if (show_origin) showOrigin(gimble_src, chassis_src);//显示原始图像
// if (from_camera == 0) {
// cv::resize(chassis_src, chassis_src, cv::Size(640, 480), 2);
// imshow("resize", chassis_src);
// energy.extract(chassis_src);
// }
if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大符模式，即刚往完成切换，则需要初始化
energy.setEnergyRotationInit();
cout << "set" << endl;
}
energy.runBig(gimble_src, chassis_src);//击打大符
last_state = mcuData.state;//更新上一帧状态
}
else if (mcuData.state != BIG_ENERGY_STATE) {//自瞄或小符模式
last_state = mcuData.state;
if (video_energy) {
ok = video_energy->read(energy_src);
if (!ok) {
delete video_energy;
video_energy = nullptr;
}
if(save_video){
Mat energy_save = energy_src.clone();
cvtColor(energy_save,energy_save,COLOR_GRAY2BGR);
energy_video_writer.write(energy_save);
// cout<<energy_src.type()<<endl;
// LOGM(STR_CTR(WORD_GREEN,"Save ENERGY!"));
}
if (show_origin) {
imshow("energy src", energy_src);
}
// if (from_camera == 0) {
// cv::resize(energy_src, energy_src, cv::Size(640, 480), 2);
// imshow("resize", energy_src);
// energy.extract(energy_src);
// }
energy.run(energy_src);
// waitKey(1);
} else {
video_energy = new CameraWrapper(1, "energy");
if(!video_energy->init()){
delete video_energy;
video_energy = nullptr;
}
}
} else if (mcuData.state == ARMOR_STATE) {
last_state = mcuData.state;
if (video_armor) {
ok = video_armor->read(armor_src);
if (!ok) {
delete video_armor;
video_armor = nullptr;
}
if(save_video){
armor_video_writer.write(armor_src);
// cout<<armor_src.type()<<endl;
// LOGM(STR_CTR(WORD_GREEN,"Save ARMOR!"));
}
// flip(armor_src, armor_src, 0);
if (show_origin) {
imshow("armor src", armor_src);
}
ok = checkReconnect(video_gimble->read(gimble_src));
if (save_video) saveVideos(gimble_src);
if (show_origin) showOrigin(gimble_src);
if (mcuData.state == ARMOR_STATE){
CNT_TIME("Armor Time", {
armorFinder.run(armor_src);
armorFinder.run(gimble_src);
});
} else {
video_armor = new CameraWrapper(0, "armor");
if(!video_armor->init()){
delete video_armor;
video_armor = nullptr;
}
else if(mcuData.state == SMALL_ENERGY_STATE){
energy.runSmall(gimble_src);
}
}
}
// cout<<last_state<<endl;
waitKey(1);
cv::waitKey(1);
});
} while (ok);
delete video_armor;
delete video_energy;
delete video_gimble;
video_gimble = nullptr;
delete video_chassis;
video_chassis = nullptr;
cout << "Program fails. Restarting" << endl;
}
return 0;

12
others/include/additions/additions.h
View File

@@ -13,6 +13,10 @@
#define ENEMY_BLUE 0
#define ENEMY_RED 1
#define BIG_ENERGY_STATE 'b'
#define SMALL_ENERGY_STATE 's'
#define ARMOR_STATE 'a'
struct mcu_data{
float curr_yaw;
float curr_pitch;
@@ -25,7 +29,11 @@ struct mcu_data{
extern mcu_data mcuData;
void uartReceive(Serial *pSerial);
void initVideoWriter(cv::VideoWriter& video, const std::string &filename_prefix);
void lastVideo(std::string &video_name, const std::string &filename_prefix);
bool checkReconnect(bool is_gimble_connect, bool is_chassis_connect);
bool checkReconnect(bool is_gimble_connect);
void saveVideos(const cv::Mat &gimble_src, const cv::Mat &chassis_src);
void saveVideos(const cv::Mat &gimble_src);
void showOrigin(const cv::Mat &gimble_src, const cv::Mat &chassis_src);
void showOrigin(const cv::Mat &gimble_src);
#endif /* _ADDITIONS_H_ */

2
others/include/camera/camera_wrapper.h
View File

@@ -16,7 +16,7 @@
#include "camera/wrapper_head.h"
#ifdef Windows
#include "camera/CameraApi.h"
#elif defined(Linux)
#elif defined(Linux) || defined(Darwin)
#include "camera/camera_api.h"
#endif

1
others/include/camera/wrapper_head.h
View File

@@ -15,7 +15,6 @@ public:
virtual ~WrapperHead() = default;;
virtual bool init() = 0;
virtual bool read(cv::Mat &src) = 0;
};

2
others/include/serial/serial.h
View File

@@ -29,7 +29,7 @@ private:
DWORD dwCommEvents;
};
#elif defined(Linux)
#elif defined(Linux) || defined(Darwin)
#include <errno.h>
#include <fcntl.h>

BIN
others/libmvsdk.dylib Normal file
View File

Binary file not shown.

104
others/src/additions/additions.cpp
View File

@@ -4,14 +4,30 @@
#include <cstring>
#include <fstream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <additions/additions.h>
#include <camera/camera_wrapper.h>
#include <log.h>
#include <opencv2/videoio/videoio_c.h>
#include <iostream>
#include <energy/energy.h>
#include <armor_finder/armor_finder.h>
#define RECEIVE_LOG_LEVEL LOG_MSG
using namespace std;
using namespace cv;
extern WrapperHead *video_gimble;
extern WrapperHead *video_chassis;
extern Serial serial;
extern uint8_t last_state;
extern ArmorFinder armorFinder;
extern Energy energy;
void uartReceive(Serial* pSerial) {
char buffer[20];
@@ -38,7 +54,8 @@ void uartReceive(Serial* pSerial) {
}
}
void initVideoWriter(cv::VideoWriter& video, const std::string &filename_prefix){
cv::VideoWriter initVideoWriter(const std::string &filename_prefix){
cv::VideoWriter video;
std::ifstream in(filename_prefix + "cnt.txt");
int cnt = 0;
if (in.is_open())
@@ -54,16 +71,85 @@ void initVideoWriter(cv::VideoWriter& video, const std::string &filename_prefix)
out.close();
}
video.open(file_name, CV_FOURCC('P', 'I', 'M', '1'), 90, cv::Size(640, 480), true);
return video;
}
void lastVideo(std::string &video_name, const std::string &filename_prefix){
std::ifstream in(filename_prefix + "cnt.txt");
int cnt = 0;
if (in.is_open())
{
in >> cnt;
in.close();
bool checkReconnect(bool is_gimble_connect, bool is_chassis_connect){
if(!is_gimble_connect){
video_gimble = new CameraWrapper(0, "armor");
if(!(is_gimble_connect = video_gimble->init())){
delete video_gimble;
video_gimble = nullptr;
}
if(cnt > 1) std::string video_name = filename_prefix + std::to_string(cnt) + ".avi";
}
if(!is_chassis_connect){
video_chassis = new CameraWrapper(1, "energy");
if(!(is_chassis_connect = video_chassis->init())){
delete video_chassis;
video_chassis = nullptr;
}
}
return is_gimble_connect && is_chassis_connect;
}
bool checkReconnect(bool is_gimble_connect){
if(!is_gimble_connect){
video_gimble = new CameraWrapper(0, "armor");
if(!(is_gimble_connect = video_gimble->init())){
delete video_gimble;
video_gimble = nullptr;
}
}
return is_gimble_connect;
}
auto gimble_video_writer = initVideoWriter(PROJECT_DIR"/gimble_video/");
auto chassis_video_writer = initVideoWriter(PROJECT_DIR"/chassis_video/");
void saveVideos(const cv::Mat &gimble_src, const cv::Mat &chassis_src){
if(!gimble_src.empty() && !chassis_src.empty()){
gimble_video_writer.write(gimble_src);
Mat chassis_save = chassis_src.clone();
cvtColor(chassis_save,chassis_save,COLOR_GRAY2BGR);
chassis_video_writer.write(chassis_save);
}
else if(!gimble_src.empty() && chassis_src.empty()){
gimble_video_writer.write(gimble_src);
}
else if(gimble_src.empty() && !chassis_src.empty()){
Mat chassis_save = chassis_src.clone();
cvtColor(chassis_save,chassis_save,COLOR_GRAY2BGR);
chassis_video_writer.write(chassis_save);
}
else return;
}
void saveVideos(const cv::Mat &gimble_src){
if(!gimble_src.empty()){
gimble_video_writer.write(gimble_src);
}
else return;
}
void showOrigin(const cv::Mat &gimble_src, const cv::Mat &chassis_src){
if(!gimble_src.empty() && !chassis_src.empty()){
imshow("gimble", gimble_src);
imshow("chassis", chassis_src);
}
else if(!gimble_src.empty() && chassis_src.empty()){
imshow("gimble", gimble_src);
}
else if(gimble_src.empty() && !chassis_src.empty()){
imshow("chassis", chassis_src);
}
else return;
cv::waitKey(1);
}
void showOrigin(const cv::Mat &gimble_src){
if(!gimble_src.empty()){
imshow("gimble", gimble_src);
}
else return;
cv::waitKey(1);
}

2
others/src/camera/camera_wrapper.cpp
View File

@@ -136,6 +136,7 @@ bool CameraWrapper::readRaw(cv::Mat &src) {
return true;
} else {
src = cv::Mat();
return false;
}
}
@@ -155,6 +156,7 @@ bool CameraWrapper::readProcessed(cv::Mat &src) {
CameraReleaseImageBuffer(h_camera, pby_buffer);
return true;
} else {
src = cv::Mat();
return false;
}
}

2
others/src/serial/serial.cpp
View File

@@ -174,7 +174,7 @@ bool Serial::ReadData(unsigned char *buffer, unsigned int length) {
return bResult;
}
#elif defined(Linux)
#elif defined(Linux) || defined(Darwin)
#include <string.h>