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

反陀螺v1.2,以及识别优化。

Browse Source
This commit is contained in:
xinyang
2019-07-21 17:25:09 +08:00
parent 7a5a9c4c3c
commit 8684662536
8 changed files with 83 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
armor/include/armor_finder/armor_finder.h
View File

@@ -20,7 +20,7 @@
#define BOX_RED ENEMY_RED #define BOX_RED ENEMY_RED
#define BOX_BLUE ENEMY_BLUE #define BOX_BLUE ENEMY_BLUE
#define DISTANCE_HEIGHT_5MM (113.0) // 单位: m*pixel #define DISTANCE_HEIGHT_5MM (107.0) // 单位: m*pixel
#define DISTANCE_HEIGHT DISTANCE_HEIGHT_5MM #define DISTANCE_HEIGHT DISTANCE_HEIGHT_5MM
#define B1 1 #define B1 1
@@ -123,7 +123,7 @@ private:
public: public:
void run(cv::Mat &src); // 自瞄主函数 void run(cv::Mat &src); // 自瞄主函数
bool sendBoxPosition(uint8_t shoot, bool isTrack); // 和主控板通讯 bool sendBoxPosition(uint16_t shoot); // 和主控板通讯
}; };
#endif /* _ARMOR_FINDER_H_ */ #endif /* _ARMOR_FINDER_H_ */

63
armor/src/armor_finder/anti_top/anti_top.cpp
View File

@@ -12,19 +12,16 @@ static double getTimeIntervalms(const timeval& now, const timeval &last){
void ArmorFinder::antiTop() { void ArmorFinder::antiTop() {
static double top_periodms = 0; static double top_periodms = 0;
static double last_top_periodms = 0; static double last_top_periodms = 0;
uint16_t shoot_delay = 0;
timeval curr_time; timeval curr_time;
uint8_t shoot = 0; // if(anti_top_state == ANTI_TOP){
/*if(anti_top_state == ANTI_TOP){ // cout << "anti top" << endl;
cout << "anti top" << endl; // }else if(anti_top_state == NORMAL){
}else if(anti_top_state == NORMAL){ // cout << "Normal" << endl;
cout << "Normal" << endl; // }
}*/
gettimeofday(&curr_time, nullptr); gettimeofday(&curr_time, nullptr);
auto interval = getTimeIntervalms(curr_time, last_front_time); auto interval = getTimeIntervalms(curr_time, last_front_time);
LOGM("interval:%.1lf", interval); if(interval > 700){
if(anti_top_state == ANTI_TOP && (top_periodms+last_top_periodms)/2.0-120<interval&&interval<(top_periodms+last_top_periodms)/2.0-80){
shoot = 1;
}else if(interval > 700){
anti_top_state = NORMAL; anti_top_state = NORMAL;
anti_top_cnt = 0; anti_top_cnt = 0;
} }
@@ -32,50 +29,38 @@ void ArmorFinder::antiTop() {
ArmorBox::BoxOrientation orientation = armor_box.getOrientation(); ArmorBox::BoxOrientation orientation = armor_box.getOrientation();
if(orientation == ArmorBox::UNKNOWN){ if(orientation == ArmorBox::UNKNOWN){
if(anti_top_state == NORMAL){ if(anti_top_state == NORMAL){
sendBoxPosition(shoot, true); sendBoxPosition(shoot_delay);
return;
}else{
sendBoxPosition(shoot, false);
return;
} }
return;
} }
if(orientation != last_orient){ if(orientation!=last_orient && orientation==ArmorBox::FRONT){
if(interval > 700){ last_front_time = curr_time;
anti_top_cnt = 0; if(150<interval && interval<700){
anti_top_state = NORMAL; if(anti_top_state == ANTI_TOP){
if(orientation == ArmorBox::FRONT){ last_top_periodms = top_periodms;
last_front_time = curr_time; top_periodms = interval;
} LOGM(STR_CTR(WORD_LIGHT_GREEN, "top period: %.1lf ms"), top_periodms);
}else if(interval > 150){ shoot_delay = (last_top_periodms+top_periodms)/2.0-110;
if(orientation == ArmorBox::FRONT){ last_orient = orientation;
anti_top_cnt++; }else if(anti_top_state == NORMAL){
if(anti_top_state == ANTI_TOP){ // LOGM("interval:%.1lf", interval);
last_top_periodms = top_periodms; if(++anti_top_cnt > 4){
top_periodms = interval; anti_top_state = ANTI_TOP;
LOGM(STR_CTR(WORD_LIGHT_GREEN, "top period: %.1lf ms"), top_periodms);
} }
last_front_time = curr_time;
} }
} }
last_orient = orientation;
}
if(anti_top_cnt > 4){
anti_top_state = ANTI_TOP;
} }
if(anti_top_state == ANTI_TOP){ if(anti_top_state == ANTI_TOP){
if(orientation == ArmorBox::FRONT){ if(orientation == ArmorBox::FRONT){
sendBoxPosition(shoot, true); sendBoxPosition(shoot_delay);
}else if(shoot){
sendBoxPosition(shoot, false);
} }
}else if(anti_top_state == NORMAL){ }else if(anti_top_state == NORMAL){
sendBoxPosition(shoot, true); sendBoxPosition(shoot_delay);
} }
last_orient = orientation; last_orient = orientation;
} }

4
armor/src/armor_finder/armor_box/armor_box.cpp
View File

@@ -57,7 +57,7 @@ ArmorBox::BoxOrientation ArmorBox::getOrientation() const{
case B1: case B1:
case B7: case B7:
case B8: case B8:
if(lengthDistanceRatio() < 0.30){ if(lengthDistanceRatio() < 0.24){
return FRONT; return FRONT;
}else{ }else{
return SIDE; return SIDE;
@@ -70,7 +70,7 @@ ArmorBox::BoxOrientation ArmorBox::getOrientation() const{
case B3: case B3:
case B4: case B4:
case B5: case B5:
if (lengthDistanceRatio() < 0.46) { if (lengthDistanceRatio() < 0.48) {
return FRONT; return FRONT;
}else{ }else{
return SIDE; return SIDE;

6
armor/src/armor_finder/armor_finder.cpp
View File

@@ -46,8 +46,8 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder,
} }
void ArmorFinder::run(cv::Mat &src) { void ArmorFinder::run(cv::Mat &src) {
// stateSearchingTarget(src_use); // for debug // stateSearchingTarget(src); // for debug
// return; // goto end;
switch (state) { switch (state) {
case SEARCHING_STATE: case SEARCHING_STATE:
if (stateSearchingTarget(src)) { if (stateSearchingTarget(src)) {
@@ -77,7 +77,7 @@ void ArmorFinder::run(cv::Mat &src) {
default: default:
stateStandBy(); stateStandBy();
} }
end:
antiTop(); antiTop();
if (show_armor_box) { // 根据条件显示当前目标装甲板 if (show_armor_box) { // 根据条件显示当前目标装甲板

10
armor/src/armor_finder/find/find_armor_box.cpp
View File

@@ -23,7 +23,7 @@ static bool angelJudge(const LightBlob &light_blob_i, const LightBlob &light_blo
light_blob_i.rect.angle - 90; light_blob_i.rect.angle - 90;
float angle_j = light_blob_j.rect.size.width > light_blob_j.rect.size.height ? light_blob_j.rect.angle : float angle_j = light_blob_j.rect.size.width > light_blob_j.rect.size.height ? light_blob_j.rect.angle :
light_blob_j.rect.angle - 90; light_blob_j.rect.angle - 90;
return abs(angle_i - angle_j) < 10; return abs(angle_i - angle_j) < 20;
} }
static bool heightJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { static bool heightJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
@@ -35,12 +35,12 @@ static bool lengthJudge(const LightBlob &light_blob_i, const LightBlob &light_bl
double side_length; double side_length;
cv::Point2f centers = light_blob_i.rect.center - light_blob_j.rect.center; cv::Point2f centers = light_blob_i.rect.center - light_blob_j.rect.center;
side_length = sqrt(centers.ddot(centers)); side_length = sqrt(centers.ddot(centers));
return (side_length / light_blob_i.length < 6 && side_length / light_blob_i.length > 0.5); return (side_length / light_blob_i.length < 8 && side_length / light_blob_i.length > 0.5);
} }
static bool lengthRatioJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { static bool lengthRatioJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
return (light_blob_i.length / light_blob_j.length < 2 return (light_blob_i.length / light_blob_j.length < 2.5
&& light_blob_i.length / light_blob_j.length > 0.5); && light_blob_i.length / light_blob_j.length > 0.4);
} }
/* 判断两个灯条的错位度，不知道英文是什么！！！ */ /* 判断两个灯条的错位度，不知道英文是什么！！！ */
@@ -56,7 +56,7 @@ static bool CuoWeiDuJudge(const LightBlob &light_blob_i, const LightBlob &light_
Vector2f orientation(cos(angle), sin(angle)); Vector2f orientation(cos(angle), sin(angle));
Vector2f p2p(light_blob_j.rect.center.x - light_blob_i.rect.center.x, Vector2f p2p(light_blob_j.rect.center.x - light_blob_i.rect.center.x,
light_blob_j.rect.center.y - light_blob_i.rect.center.y); light_blob_j.rect.center.y - light_blob_i.rect.center.y);
return abs(orientation.dot(p2p)) < 20; return abs(orientation.dot(p2p)) < 25;
} }
static bool boxAngleJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { static bool boxAngleJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {

41
armor/src/armor_finder/find/find_light_blobs.cpp
View File

@@ -20,8 +20,8 @@ static double areaRatio(const std::vector<cv::Point> &contour, const cv::Rotated
static bool isValidLightBlob(const std::vector<cv::Point> &contour, const cv::RotatedRect &rect) { static bool isValidLightBlob(const std::vector<cv::Point> &contour, const cv::RotatedRect &rect) {
return (1.5 < lw_rate(rect) && lw_rate(rect) < 10) && return (1.5 < lw_rate(rect) && lw_rate(rect) < 10) &&
// (rect.size.area() < 3000) && // (rect.size.area() < 3000) &&
((rect.size.area() < 50 && areaRatio(contour, rect) > 0.5) || ((rect.size.area() < 50 && areaRatio(contour, rect) > 0.4) ||
(rect.size.area() >= 50 && areaRatio(contour, rect) > 0.7)); (rect.size.area() >= 50 && areaRatio(contour, rect) > 0.6));
} }
// 此函数可以有性能优化. // 此函数可以有性能优化.
@@ -114,23 +114,40 @@ static void imagePreProcess(cv::Mat &src) {
} }
bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) { bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
cv::Mat src_bin, color_channel; cv::Mat color_channel;
cv::Mat src_bin_light, src_bin_dim;
std::vector<cv::Mat> channels; // 通道拆分 std::vector<cv::Mat> channels; // 通道拆分
cv::split(src, channels); /************************/ cv::split(src, channels); /************************/
if (enemy_color == ENEMY_BLUE) /* */ if (enemy_color == ENEMY_BLUE){ /* */
color_channel = channels[0]; /* 根据目标颜色进行通道提取 */ color_channel = channels[0]; /* 根据目标颜色进行通道提取 */
else if (enemy_color == ENEMY_RED) /* */ }else if (enemy_color == ENEMY_RED){ /* */
color_channel = channels[2]; /************************/ color_channel = channels[2]; /************************/
cv::threshold(color_channel, src_bin, 140, 255, CV_THRESH_BINARY); // 二值化对应通道 }
imagePreProcess(src_bin); // 开闭运算
if(src_bin.size() == cv::Size(640, 480) && show_light_blobs)
imshow("bin", src_bin);
std::vector<std::vector<cv::Point> > light_contours;
cv::findContours(src_bin, light_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); cv::threshold(color_channel, src_bin_light, 200, 255, CV_THRESH_BINARY); // 二值化对应通道
for (auto &light_contour : light_contours) { imagePreProcess(src_bin_light); // 开闭运算
cv::threshold(color_channel, src_bin_dim, 160, 255, CV_THRESH_BINARY); // 二值化对应通道
imagePreProcess(src_bin_dim); // 开闭运算
if(src_bin_light.size() == cv::Size(640, 480) && show_light_blobs) {
imshow("bin_light", src_bin_light);
imshow("bin_dim", src_bin_dim);
}
std::vector<std::vector<cv::Point> > light_contours_light, light_contours_dim;
cv::findContours(src_bin_light, light_contours_light, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
cv::findContours(src_bin_dim, light_contours_dim, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
for (auto &light_contour : light_contours_light) {
cv::RotatedRect rect = cv::minAreaRect(light_contour);
if (isValidLightBlob(light_contour, rect)) {
light_blobs.emplace_back(rect, get_blob_color(src, rect));
}
}
for (auto &light_contour : light_contours_dim) {
cv::RotatedRect rect = cv::minAreaRect(light_contour); cv::RotatedRect rect = cv::minAreaRect(light_contour);
if (isValidLightBlob(light_contour, rect)) { if (isValidLightBlob(light_contour, rect)) {
light_blobs.emplace_back(rect, get_blob_color(src, rect)); light_blobs.emplace_back(rect, get_blob_color(src, rect));

29
armor/src/armor_finder/send_target/send_target.cpp
View File

@@ -5,11 +5,11 @@
#include <armor_finder/armor_finder.h> #include <armor_finder/armor_finder.h>
#include<log.h> #include<log.h>
static bool sendTarget(Serial &serial, double x, double y, double z, uint8_t shoot) { static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t shoot_delay) {
static short x_tmp, y_tmp, z_tmp; static short x_tmp, y_tmp, z_tmp;
static time_t last_time = time(nullptr); static time_t last_time = time(nullptr);
static int fps; static int fps;
uint8_t buff[9]; uint8_t buff[10];
time_t t = time(nullptr); time_t t = time(nullptr);
if (last_time != t) { if (last_time != t) {
@@ -30,14 +30,19 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint8_t sho
buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF); buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF); buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF); buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
buff[7] = shoot; buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
buff[8] = 'e'; buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
buff[9] = 'e';
// if(buff[7]<<8 | buff[8])
// cout << (buff[7]<<8 | buff[8]) << endl;
return serial.WriteData(buff, sizeof(buff)); return serial.WriteData(buff, sizeof(buff));
} }
bool ArmorFinder::sendBoxPosition(uint8_t shoot, bool isTrack) { bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
if(armor_box.rect == cv::Rect2d()) return false; if(armor_box.rect == cv::Rect2d()) return false;
if(shoot_delay){
LOGM(STR_CTR(WORD_BLUE, "shoot after %dms"), shoot_delay);
}
auto rect = armor_box.rect; auto rect = armor_box.rect;
double dx = rect.x + rect.width / 2 - 320; double dx = rect.x + rect.width / 2 - 320;
double dy = rect.y + rect.height / 2 - 240 - 20; double dy = rect.y + rect.height / 2 - 240 - 20;
@@ -45,15 +50,5 @@ bool ArmorFinder::sendBoxPosition(uint8_t shoot, bool isTrack) {
double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI; double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
double dist = DISTANCE_HEIGHT / rect.height; double dist = DISTANCE_HEIGHT / rect.height;
// cout << yaw << endl; // cout << yaw << endl;
if(isTrack){ return sendTarget(serial, yaw, -pitch, dist, shoot_delay);
if(shoot){
LOGM(STR_CTR(WORD_RED,"Shoot!!!"));
}
return sendTarget(serial, yaw, -pitch, dist, shoot);
LOGM(STR_CTR(WORD_RED,"Shoot!!!"));
return sendTarget(serial, 0, -0, dist, 1);
}else if(shoot){
LOGM(STR_CTR(WORD_RED,"Shoot!!!"));
return sendTarget(serial, 0, 0, 4, 1);
}
} }

38
armor/src/show_images/show_images.cpp
View File

@@ -95,38 +95,12 @@ void showArmorBox(std::string windows_name, const cv::Mat &src, const ArmorBox &
// if(box.light_blobs.size() == 2) // if(box.light_blobs.size() == 2)
// fprintf(fp, "%lf %lf %lf\n", box.light_blobs[0].length, box.light_blobs[1].length, box.blobsDistance()) // fprintf(fp, "%lf %lf %lf\n", box.light_blobs[0].length, box.light_blobs[1].length, box.blobsDistance())
// cout << box.lengthDistanceRatio() << endl; // cout << box.lengthDistanceRatio() << endl;
switch (box.id) {
case R1: if(box.getOrientation() == ArmorBox::FRONT){
case R7: rectangle(image2show, box.rect, Scalar(0, 255, 0), 3);
case B1: }else{
case B7: rectangle(image2show, box.rect, Scalar(0, 255, 0), 1);
if(box.lengthDistanceRatio() < 0.3){ };
rectangle(image2show, box.rect, Scalar(0, 255, 0), 3);
}else{
rectangle(image2show, box.rect, Scalar(0, 255, 0), 1);
}
case R2:
case R3:
case R4:
case R5:
case B2:
case B3:
case B4:
case B5:
if (box.lengthDistanceRatio() < 0.42) {
rectangle(image2show, box.rect, Scalar(0, 255, 0), 3);
}else{
rectangle(image2show, box.rect, Scalar(0, 255, 0), 1);
}
break;
default:
rectangle(image2show, box.rect, Scalar(0, 255, 0), 1);
}
// if((id2name.at(box.id)=="B1"||id2name.at(box.id)=="B7") && box.lengthDistanceRatio() < 0.3/* && box.lengthRatio() > 0.9*/){
// rectangle(image2show, box.rect, Scalar(0, 255, 0), 3);
// }else{
// rectangle(image2show, box.rect, Scalar(0, 255, 0), 1);
// };
char dist[5]; char dist[5];
sprintf(dist, "%.1f", box.getDistance()); sprintf(dist, "%.1f", box.getDistance());