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

auto-aim

Browse Source
This commit is contained in:
xinyang
2019-04-14 17:12:43 +08:00
commit 42c5434dc8
47 changed files with 6423 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
energy/include/energy/constant.h Normal file
View File

@@ -0,0 +1,43 @@
//
// Created by xixiliadorabarry on 1/24/19.
//
#ifndef CONSTANT_H
#define CONSTANT_H
#define d2r (CV_PI / 180.0)
const int ALLY_BLUE = 123;
const int ALLY_RED = 456;
const int SRC_WIDTH_CAMERA = 640;
const int SRC_HEIGHT_CAMERA = 480;
const int SRC_WIDTH = 320;
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 double ARMOR_CENTER_TO_CYCLE_CENTER = 75;//cm
const double ARMOR_CENTER_TO_CYCLE_CENTER = 71;//cm
const int EXTRACT_POINT_X = 200;
const int EXTRACT_POINT_Y = 20;
const int EXTRACT_WIDTH = 240;
const int EXTRACT_HEIGHT = 180;
//以摄像头正方向位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 = 231.0/640.0;
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

129
energy/include/energy/energy.h Normal file
View File

@@ -0,0 +1,129 @@
//
// Created by xixiliadorabarry on 1/24/19.
//
#ifndef ENERGY_H
#define ENERGY_H
#include <iostream>
#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <stdio.h>
#include <time.h>
#include <sys/timeb.h>
#include "energy/constant.h"
#include "energy/param_struct_define.h"
#include "uart/uart.h"
using std::vector;
class Energy {
public:
Energy(Uart &u);
~Energy();
int run(cv::Mat &src);
cv::Point2f uart_hit_point;
clock_t start;
Uart &uart;
void setAllyColor(int color);
void setRotation(int rotation);
void extract(cv::Mat &src);
void sendTargetByUart(float x, float y, float z);
private:
EnergyPartParam energy_part_param_;
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;
float target_armor;
int 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;
int isLeftVertexFound, isTopVertexFound, isRightVertexFound, isBottomVertexFound;
std::vector<EnergyPart> fans;
std::vector<EnergyPart> armors;
std::vector<EnergyPart> gimble_zero_points;
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;
void initEnergyPartParam();
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, 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);
bool isValidFanContour(const vector<cv::Point> &fan_contour);
bool isValidArmorContour(const vector<cv::Point> &armor_contour);
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 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 findTarget(const std::vector<float>fanPosition, const std::vector<float>armorPosition, float &target_armor);
void findWholeCycle(const std::vector<cv::Point>&first_armor_centers);
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 getHitPoint();
bool changeTarget();
void gimbleRotation();
void splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red);
void imagePreprocess(cv::Mat &src);
void StructingElementClose(cv::Mat &src);
void StructingElementErodeDilate(cv::Mat &src);
};
#endif //ENERGY_H

60
energy/include/energy/param_struct_define.h Normal file
View File

@@ -0,0 +1,60 @@
//
// Created by xixiliadorabarry on 1/24/19.
//
#ifndef PARAM_STRUCT_DEFINE_H
#define PARAM_STRUCT_DEFINE_H
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <vector>
using std::vector;
struct EnergyPart {
cv::RotatedRect rect;
float angle;
vector<cv::Point> contour;
explicit EnergyPart(vector<cv::Point> &c) : contour(c) {
rect = cv::minAreaRect(c);
angle = cv::minAreaRect(c).angle;
};
};
struct EnergyPartParam {
double RPM;
double HIT_TIME;
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_WIDTH_MIN;
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;
float TWIN_ANGEL_MAX;
};
typedef struct GMAngle_t{
float yaw;
float pitch;
}GMAngle_t;
extern GMAngle_t aim;
#endif //PARAM_STRUCT_DEFINE_H

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

@@ -0,0 +1,106 @@
//
// 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;
}

59
energy/src/energy/calibrate/split.cpp Normal file
View File

@@ -0,0 +1,59 @@
//
// 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::splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red) {
uchar* data;
uchar* bayer_data[2];
for (int i = 0; i < src.rows; ++i) {
data = src.ptr<uchar>(i);
bayer_data[0] = blue.ptr<uchar>(i / 2);
for (int j = 0; j < blue.cols; ++j, data += 2) {
bayer_data[0][j] = *data;
}
data = src.ptr<uchar>(++i) + 1;
bayer_data[1] = red.ptr<uchar>(i / 2);
for (int j = 0; j < red.cols; ++j, data += 2) {
bayer_data[1][j] = *data;
}
}
}
void Energy::imagePreprocess(cv::Mat &src) {
if(src.type() == CV_8UC1)
{
splitBayerBG(src, src_blue, src_red);
if(ally_color_ == ALLY_RED)
{
src = src_red - src_blue;
}else if(ally_color_ == ALLY_BLUE){
src = src_blue - src_red;
}
}
else if(src.type() == CV_8UC3)
{
std::vector<Mat> channels;
split(src, channels);
resize(channels.at(0), src_blue, Size(SRC_WIDTH, SRC_HEIGHT));
resize(channels.at(1), src_green, Size(SRC_WIDTH, SRC_HEIGHT));
resize(channels.at(2), src_red, Size(SRC_WIDTH, SRC_HEIGHT));
if(ally_color_ == ALLY_RED)
{
src = src_red-src_blue;
//src=src_red;
}else if(ally_color_ == ALLY_BLUE){
src = src_blue-src_red;
//src=src_blue;
}
}
cv::resize(src, src, cv::Size(640, 480), 2);
threshold(src, src, energy_part_param_.SPLIT_GRAY_THRESH, 255, THRESH_BINARY);
}

26
energy/src/energy/calibrate/structing.cpp Normal file
View File

@@ -0,0 +1,26 @@
//
// 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::StructingElementClose(cv::Mat &src){
if (src.empty())return;
//threshold(src, src, energy_part_param_.CAMERA_GRAY_THRESH, 255, THRESH_BINARY);
Mat element = getStructuringElement(MORPH_RECT, Size(4, 4));
morphologyEx(src, src, MORPH_CLOSE, element);
}
void Energy::StructingElementErodeDilate(cv::Mat &src) {
cv::Mat src_out, src_out_out;
Mat element_erode = getStructuringElement(MORPH_RECT, Size(4, 4));
Mat element_dilate = getStructuringElement(MORPH_RECT, Size(20, 20));
erode(src,src_out, element_erode);
imshow("erode", src_out);
dilate(src_out, src_out_out, element_dilate);
imshow("dilate", src_out_out);
}

64
energy/src/energy/energy.cpp Normal file
View File

@@ -0,0 +1,64 @@
//
// Created by xixiliadorabarry on 1/24/19.
//
#include "energy/energy.h"
using namespace cv;
using std::cout;
using std::endl;
using std::vector;
Energy::Energy(Uart &u):uart(u),
src_blue(SRC_HEIGHT, SRC_WIDTH, CV_8UC1),
src_red(SRC_HEIGHT, SRC_WIDTH, CV_8UC1)
{
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 = -1;
last_target_position = -1;
target_armor = -1;
radius = 0;
ally_color_ = ALLY_RED;
energy_part_rotation = CLOCKWISE;
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;
isLeftVertexFound = -1;
isTopVertexFound = -1;
isRightVertexFound = -1;
isBottomVertexFound = -1;
left = Point(640, 480);
right = Point(0, 0);
top = Point(640, 480);
bottom = Point(0, 0);
initEnergyPartParam();
}
Energy::~Energy() = default;
void Energy::setAllyColor(int color)
{
ally_color_ = color;
}
void Energy::setRotation(int rotation){
energy_part_rotation = rotation;
}

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

@@ -0,0 +1,89 @@
//
// 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;
}*/
}

222
energy/src/energy/find/energy_finder.cpp Normal file
View File

@@ -0,0 +1,222 @@
//
// Created by xixiliadorabarry on 1/24/19.
//
#include "energy/energy.h"
using namespace cv;
using std::cout;
using std::endl;
using std::vector;
int Energy::findFan(const cv::Mat &src, vector<EnergyPart> &fans, 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);
}
std::vector<vector<Point> > fan_contours;
StructingElementClose(src_bin);
// 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;
}
// 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;
// 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>5&&width>5){
// cout<<cur_rect.center;
// fans.emplace_back(fan_contour);
// cout<<"fan area: "<<length<<'\t'<<width<<endl;
// }
fans.emplace_back(fan_contour);
// cout<<"fan area: "<<length<<'\t'<<width<<endl;
}
if(fans.size() < last_fans_cnt){
last_fans_cnt = static_cast<int>(fans.size());
return -1;
}
last_fans_cnt = static_cast<int>(fans.size());
return static_cast<int>(fans.size());
}
int Energy::findArmor(const cv::Mat &src, vector<EnergyPart> &armors, 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);
}
std::vector<vector<Point> > armor_contours;
std::vector<vector<Point> > armor_contours_external;//用总轮廓减去外轮廓，只保留内轮廓，除去流动条的影响。
StructingElementClose(src_bin);
// imshow("armor struct",src_bin);
findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
for (int i = 0; i < armor_contours_external.size(); i++)//去除外轮廓
{
unsigned long external_contour_size = armor_contours_external[i].size();
for (int j = 0; j < armor_contours.size(); j++)
{
unsigned long all_size = armor_contours[j].size();
if (external_contour_size == all_size)
{
swap(armor_contours[j], armor_contours[armor_contours.size() - 1]);
armor_contours.pop_back();
break;
}
}
}
for (auto &armor_contour : armor_contours) {
if (!isValidArmorContour(armor_contour))
{
continue;
}
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>10){
// armors.emplace_back(armor_contour);
// cout<<"armor area: "<<length<<'\t'<<width<<endl;
// }
armors.emplace_back(armor_contour);
cout<<"armor area: "<<length<<'\t'<<width<<endl;
}
if(armors.size() < last_armors_cnt){
last_armors_cnt = static_cast<int>(armors.size());
return -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) {
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);
}
std::vector<vector<Point> > zero_point_contours;
findContours(src_bin, zero_point_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
for (auto &zero_point_contour : zero_point_contours) {
double cur_contour_area = contourArea(zero_point_contour);
RotatedRect cur_rect = minAreaRect(zero_point_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);
}
}
return static_cast<int>(fans.size());
}
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)
{
//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;
if (length < energy_part_param_.FAN_CONTOUR_LENGTH_MIN || width < energy_part_param_.FAN_CONTOUR_WIDTH_MIN)
{
//cout<<"length width min 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;
return true;
}
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;
// }
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)
{
//cout<<"length width min 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;
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;
return true;
}

61
energy/src/energy/find/target_finder.cpp Normal file
View File

@@ -0,0 +1,61 @@
//
// 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::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);
for (const auto &armor : armors)
{
target_center = armor.rect.center;
// cout<<"target center: "<<target_center<<endl;
}
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) {
j++;
continue;
}
else {
target_armor = armorPosition.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;
}
}
return;
}
}
target_armor = armorPosition.at(armorPosition.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;
}
}
}

26
energy/src/energy/get/gimble_rotation_get.cpp Normal file
View File

@@ -0,0 +1,26 @@
//
// Created by xixiliadorabarry on 19-3-23.
//
#include "energy/energy.h"
#include <cmath>
using namespace cv;
using std::cout;
using std::endl;
using std::vector;
void Energy::gimbleRotation(){
//该方法用于标定激光零点的情况，对操作手友好，但建立在云台稳定情况下
// yaw_rotation = static_cast<float>(180 / PI * atan2(-1*STRETCH*(hit_point.x-ZERO_POINT_X), ATTACK_DISTANCE));
// pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(PITCH_ORIGIN_RAD)-STRETCH*(hit_point.y-ZERO_POINT_Y)), ATTACK_DISTANCE));
//该方法用于操作手自己完成对心工作的情况，对操作手要求高
cv::Point2f real_hit_point;
stretch(hit_point, real_hit_point);
// yaw_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(YAW_ORIGIN_RAD)-real_hit_point.x), ATTACK_DISTANCE));
// pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(PITCH_ORIGIN_RAD)-real_hit_point.y), ATTACK_DISTANCE));
yaw_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(aim.yaw)-real_hit_point.x), ATTACK_DISTANCE));
pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(aim.pitch)-real_hit_point.y), ATTACK_DISTANCE));
}

31
energy/src/energy/get/hit_point_get.cpp Normal file
View File

@@ -0,0 +1,31 @@
//
// 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(){
// if(!changeTarget()){
// //last_target_center = target_center;
// //last_target_position = target_position;
// return;
// }
// else {
double rad = static_cast<double>(energy_part_rotation * energy_part_param_.RPM
* energy_part_param_.HIT_TIME * 360 / 60);
rotate(rad, radius, cycle_center, target_center, hit_point);
//last_target_center = target_center;
//last_target_position = target_position;
// }
}
bool Energy::changeTarget(){
return !(pointDistance(target_center, last_target_center) < 25 || fabs(target_position - last_target_position) < 30
||fabs(target_position - last_target_position) > 330);
}

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

@@ -0,0 +1,49 @@
//
// 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)));
}

42
energy/src/energy/param_init.cpp Normal file
View File

@@ -0,0 +1,42 @@
//
// 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::initEnergyPartParam() {
energy_part_param_.RPM = 10;
energy_part_param_.HIT_TIME = 1.0;
energy_part_param_.GRAY_THRESH = 240;
energy_part_param_.SPLIT_GRAY_THRESH = 80;
energy_part_param_.FAN_GRAY_THRESH = 75;
energy_part_param_.ARMOR_GRAY_THRESH = 80;
energy_part_param_.FAN_CONTOUR_AREA_MAX = 17000;
energy_part_param_.FAN_CONTOUR_AREA_MIN = 7000;
energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 150;
energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 50;
energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 4;
energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1;
energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 100000;
energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0;
energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 50;
energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 25;
energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 80;
energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 50;
energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3;
energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1;
energy_part_param_.TWIN_ANGEL_MAX = 10;
}

76
energy/src/energy/run.cpp Normal file
View File

@@ -0,0 +1,76 @@
//
// Created by xixiliadorabarry on 3/5/19.
//
#include "energy/energy.h"
using namespace cv;
using std::cout;
using std::endl;
using std::vector;
int Energy::run(cv::Mat &src){
fans.clear();
armors.clear();
fanPosition.clear();
armorPosition.clear();
gimble_zero_points.clear();
// 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;
// imagePreprocess(src);
// imshow("img_preprocess",src);
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 != fans_cnt+1) return 0;
//此处用于标定云台在摄像头视频中的零点
// findGimbleZeroPoint(src,gimble_zero_points);
// cout<<"gimble zero points: :"<<gimble_zero_points.size()<<endl;
// showFanContours("zero",src,gimble_zero_points);
getAllArmorCenters();
cout<<"all_armor_centers.size(): "<<all_armor_centers.size()<<endl;
cycleLeastFit();
// cycle_center = cv::Point(248,247);
// radius = 208.439;
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;
getHitPoint();
cout << "The hit point position is " << hit_point << endl;
// hit_point = cycle_center;
gimbleRotation();
sendTargetByUart(yaw_rotation, pitch_rotation, attack_distance);
cout<<"yaw: "<<yaw_rotation<<'\t'<<"pitch: "<<pitch_rotation<<endl;
// cout<<"send_cnt: "<<send_cnt<<endl;
}

15
energy/src/energy/send_target_by_uart/send_target_by_uart.cpp Normal file
View File

@@ -0,0 +1,15 @@
//
// Created by xixiliadorabarry on 1/24/19.
//
#include "energy/energy.h"
void Energy::sendTargetByUart(float x, float y, float z) {
// if(!changeTarget()){
// return;
// }
uart.sendTarget(x, y, z);
send_cnt+=1;
}

96
energy/src/energy/show/show.cpp Normal file
View File

@@ -0,0 +1,96 @@
//
// 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::showFanContours(std::string windows_name, const cv::Mat &src, const std::vector<EnergyPart> &fans) {
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 &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) {
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 &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) {
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 &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++)
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;
}
imshow(windows_name, image2show);
}

88
energy/src/energy/tool/tool.cpp Normal file
View File

@@ -0,0 +1,88 @@
//
// Created by xixiliadorabarry on 1/24/19.
//
#include "energy/energy.h"
#include <iostream>
#include <algorithm>
#include <cmath>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
using namespace cv;
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();
cv::resize(src, src, cv::Size(640, 480), 2);
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::savePoint2f(std::vector<cv::Point2f> &point_save, cv::Point point) {
point_save.push_back(static_cast<cv::Point2f>(point));
}
double Energy::pointDistance(cv::Point point_1, cv::Point point_2){
double distance = 0;
distance = sqrt(pow(static_cast<double>(point_1.x - point_2.x),2)
+ pow(static_cast<double>(point_1.y - point_2.y),2));
return distance;
}
void Energy::rotate(double rad, double radius, cv::Point center, cv::Point point_old, cv::Point &point_new) {
int x1, x2, y1, y2;
// 为了减小强制转换的误差
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));
}