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

update priority.

Browse Source
This commit is contained in:
wanpiqiu123
2019-07-22 11:53:40 +08:00
parent 68a642e6a6
commit 4075352a70
8 changed files with 95 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -20,6 +20,9 @@
#define BOX_RED ENEMY_RED #define BOX_RED ENEMY_RED
#define BOX_BLUE ENEMY_BLUE #define BOX_BLUE ENEMY_BLUE
#define IMAGE_CENTER_X (320)
#define IMAGE_CENTER_Y (240-20)
#define DISTANCE_HEIGHT_5MM (107.0) // 单位: m*pixel #define DISTANCE_HEIGHT_5MM (107.0) // 单位: m*pixel
#define DISTANCE_HEIGHT DISTANCE_HEIGHT_5MM #define DISTANCE_HEIGHT DISTANCE_HEIGHT_5MM
@@ -40,6 +43,9 @@
extern std::map<int, string> id2name; //装甲板id到名称的map extern std::map<int, string> id2name; //装甲板id到名称的map
extern std::map<string, int> name2id; //装甲板名称到id的map extern std::map<string, int> name2id; //装甲板名称到id的map
extern std::map<string, int> prior_blue;
extern std::map<string, int> prior_red;
/******************* 灯条类定义 ***********************/ /******************* 灯条类定义 ***********************/
class LightBlob { class LightBlob {
@@ -72,11 +78,13 @@ public:
explicit ArmorBox(const cv::Rect &pos=cv::Rect2d(), const LightBlobs &blobs=LightBlobs(), uint8_t color=0, int i=0); explicit ArmorBox(const cv::Rect &pos=cv::Rect2d(), const LightBlobs &blobs=LightBlobs(), uint8_t color=0, int i=0);
double blobsDistance() const; double getBlobsDistance() const;
double lengthRatio() const;
double lengthDistanceRatio() const; double lengthDistanceRatio() const;
double getDistance() const; double getBoxDistance() const;
BoxOrientation getOrientation() const; BoxOrientation getOrientation() const;
// double
bool operator<(const ArmorBox &box) const;
}; };
typedef std::vector<ArmorBox> ArmorBoxes; typedef std::vector<ArmorBox> ArmorBoxes;

2
armor/include/show_images/show_images.h
View File

@@ -12,7 +12,7 @@
void showArmorBoxes(std::string windows_name, const cv::Mat &src, const ArmorBoxes &armor_boxes); void showArmorBoxes(std::string windows_name, const cv::Mat &src, const ArmorBoxes &armor_boxes);
void showArmorBox(std::string windows_name, const cv::Mat &src, const ArmorBox &armor_box); void showArmorBox(std::string windows_name, const cv::Mat &src, const ArmorBox &armor_box);
void showLightBlobs(std::string windows_name, const cv::Mat &src, const LightBlobs &light_blobs); void showLightBlobs(std::string windows_name, const cv::Mat &src, const LightBlobs &light_blobs);
void showArmorBoxesClass(std::string window_names, const cv::Mat &src, const ArmorBoxes boxes[15]); void showArmorBoxesClass(std::string window_names, const cv::Mat &src, const ArmorBoxes &boxes);
void showTrackSearchingPos(std::string window_names, const cv::Mat &src, const cv::Rect2d pos); void showTrackSearchingPos(std::string window_names, const cv::Mat &src, const cv::Rect2d pos);
#endif /* _SHOW_IMAGES_H_ */ #endif /* _SHOW_IMAGES_H_ */

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

@@ -6,48 +6,38 @@
#include <log.h> #include <log.h>
ArmorBox::ArmorBox(const cv::Rect &pos, const LightBlobs &blobs, uint8_t color, int i) : ArmorBox::ArmorBox(const cv::Rect &pos, const LightBlobs &blobs, uint8_t color, int i) :
rect(pos), light_blobs(blobs), box_color(color), id(i){}; rect(pos), light_blobs(blobs), box_color(color), id(i) {};
double ArmorBox::blobsDistance() const{ double ArmorBox::getBlobsDistance() const {
if(light_blobs.size() == 2){ if (light_blobs.size() == 2) {
auto &x = light_blobs[0].rect.center; auto &x = light_blobs[0].rect.center;
auto &y = light_blobs[1].rect.center; auto &y = light_blobs[1].rect.center;
return sqrt((x.x-y.x)*(x.x-y.x) + (x.y-y.y)*(x.y-y.y)); return sqrt((x.x - y.x) * (x.x - y.x) + (x.y - y.y) * (x.y - y.y));
}else{ } else {
return 0;
}
}
double ArmorBox::lengthRatio() const{
if(light_blobs.size() == 2){
return light_blobs[0].length / light_blobs[1].length < 1 ?
(light_blobs[0].length / light_blobs[1].length) :
(light_blobs[1].length / light_blobs[0].length);
}else{
return 0; return 0;
} }
} }
double ArmorBox::lengthDistanceRatio() const { double ArmorBox::lengthDistanceRatio() const {
if(light_blobs.size() == 2){ if (light_blobs.size() == 2) {
return max(light_blobs[0].length, light_blobs[1].length) return max(light_blobs[0].length, light_blobs[1].length)
/ blobsDistance(); / getBlobsDistance();
}else{ } else {
return 100; return 100;
} }
} }
double ArmorBox::getDistance() const{ double ArmorBox::getBoxDistance() const {
if(light_blobs.size() == 2 ){ if (light_blobs.size() == 2) {
return DISTANCE_HEIGHT / 2 / max(light_blobs[0].length, light_blobs[1].length); return DISTANCE_HEIGHT / 2 / max(light_blobs[0].length, light_blobs[1].length);
} else { } else {
return DISTANCE_HEIGHT / rect.height; return DISTANCE_HEIGHT / rect.height;
} }
} }
ArmorBox::BoxOrientation ArmorBox::getOrientation() const{ ArmorBox::BoxOrientation ArmorBox::getOrientation() const {
// cout << lengthDistanceRatio() << endl; // cout << lengthDistanceRatio() << endl;
if(light_blobs.size() != 2){ if (light_blobs.size() != 2) {
return UNKNOWN; return UNKNOWN;
} }
switch (id) { switch (id) {
@@ -57,9 +47,9 @@ ArmorBox::BoxOrientation ArmorBox::getOrientation() const{
case B1: case B1:
case B7: case B7:
case B8: case B8:
if(lengthDistanceRatio() < 0.24){ if (lengthDistanceRatio() < 0.24) {
return FRONT; return FRONT;
}else{ } else {
return SIDE; return SIDE;
} }
case R2: case R2:
@@ -72,10 +62,27 @@ ArmorBox::BoxOrientation ArmorBox::getOrientation() const{
case B5: case B5:
if (lengthDistanceRatio() < 0.48) { if (lengthDistanceRatio() < 0.48) {
return FRONT; return FRONT;
}else{ } else {
return SIDE; return SIDE;
} }
default: default:
return UNKNOWN; return UNKNOWN;
} }
} }
bool ArmorBox::operator<(const ArmorBox &box) const {
if (id != box.id) {
if (box_color == BOX_BLUE) {
return prior_blue[id2name[id]] < prior_blue[id2name[box.id]];
} else {
return prior_red[id2name[id]] < prior_red[id2name[box.id]];
}
} else {
auto d1 = (rect.x-IMAGE_CENTER_X)*(rect.x-IMAGE_CENTER_X)
+ (rect.y-IMAGE_CENTER_Y)*(rect.y-IMAGE_CENTER_Y);
auto d2 = (box.rect.x-IMAGE_CENTER_X)*(box.rect.x-IMAGE_CENTER_X)
+ (box.rect.y-IMAGE_CENTER_Y)*(box.rect.y-IMAGE_CENTER_Y);
return d1 < d2;
}
}

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

@@ -35,6 +35,18 @@ std::map<string, int> name2id = { //装甲板名
{"R1", 8},{"R2", 9},{"R3", 10},{"R4", 11},{"R5", 12},{"R7", 13},{"R8", 14}, {"R1", 8},{"R2", 9},{"R3", 10},{"R4", 11},{"R5", 12},{"R7", 13},{"R8", 14},
}; };
std::map<string, int> prior_blue = {
{"B8", 0}, {"B1", 1}, {"B3", 2}, {"B4", 2}, {"B5", 2}, {"B7", 3}, {"B2", 4},
{"R8", 5}, {"R1", 6}, {"R3", 7}, {"R4", 7}, {"R5", 7}, {"R7", 8}, {"R2", 9},
{"NO", 10},
};
std::map<string, int> prior_red = {
{"R8", 0}, {"R1", 1}, {"R3", 2}, {"R4", 2}, {"R5", 2}, {"R7", 3}, {"R2", 4},
{"B8", 5}, {"B1", 6}, {"B3", 7}, {"B4", 7}, {"B5", 7}, {"B7", 8}, {"B2", 9},
{"NO", 10},
};
ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder, const uint8_t &use) : ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder, const uint8_t &use) :
serial(u), serial(u),
enemy_color(color), enemy_color(color),

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

@@ -8,15 +8,7 @@
#include <opencv2/highgui.hpp> #include <opencv2/highgui.hpp>
#include <log.h> #include <log.h>
static string prior_blue[] = {
"B8", "B1", "B3", "B4", "B5", "B7", "B2",
"R8", "R1", "R3", "R4", "R5", "R7", "R2",
};
static string prior_red[] = {
"R8", "R1", "R3", "R4", "R5", "R7", "R2",
"B8", "B1", "B3", "B4", "B5", "B7", "B2",
};
static bool angelJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) { static bool angelJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
float angle_i = light_blob_i.rect.size.width > light_blob_i.rect.size.height ? light_blob_i.rect.angle : float angle_i = light_blob_i.rect.size.width > light_blob_i.rect.size.height ? light_blob_i.rect.angle :
@@ -76,7 +68,7 @@ static bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_
light_blob_j.blob_color == enemy_color && light_blob_j.blob_color == enemy_color &&
lengthRatioJudge(light_blob_i, light_blob_j) && lengthRatioJudge(light_blob_i, light_blob_j) &&
lengthJudge(light_blob_i, light_blob_j) && lengthJudge(light_blob_i, light_blob_j) &&
// heightJudge(light_blob_i, light_blob_j) && // heightJudge(light_blob_i, light_blob_j) &&
angelJudge(light_blob_i, light_blob_j) && angelJudge(light_blob_i, light_blob_j) &&
boxAngleJudge(light_blob_i, light_blob_j) && boxAngleJudge(light_blob_i, light_blob_j) &&
CuoWeiDuJudge(light_blob_i, light_blob_j); CuoWeiDuJudge(light_blob_i, light_blob_j);
@@ -94,7 +86,6 @@ bool matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBox
for (int i = 0; i < light_blobs.size() - 1; ++i) { for (int i = 0; i < light_blobs.size() - 1; ++i) {
for (int j = i + 1; j < light_blobs.size(); ++j) { for (int j = i + 1; j < light_blobs.size(); ++j) {
if (!isCoupleLight(light_blobs.at(i), light_blobs.at(j), color)) { if (!isCoupleLight(light_blobs.at(i), light_blobs.at(j), color)) {
// cout << "match fail" << endl;
continue; continue;
} }
cv::Rect2d rect_left = light_blobs.at(static_cast<unsigned long>(i)).rect.boundingRect(); cv::Rect2d rect_left = light_blobs.at(static_cast<unsigned long>(i)).rect.boundingRect();
@@ -106,7 +97,6 @@ bool matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBox
max_y = fmax(rect_left.y + rect_left.height, rect_right.y + rect_right.height) + max_y = fmax(rect_left.y + rect_left.height, rect_right.y + rect_right.height) +
0.5 * (rect_left.height + rect_right.height) / 2.0; 0.5 * (rect_left.height + rect_right.height) / 2.0;
if (min_x < 0 || max_x > src.cols || min_y < 0 || max_y > src.rows) { if (min_x < 0 || max_x > src.cols || min_y < 0 || max_y > src.rows) {
// cout << "out of range" << endl;
continue; continue;
} }
LightBlobs pair_blobs = {light_blobs.at(i), light_blobs.at(j)}; LightBlobs pair_blobs = {light_blobs.at(i), light_blobs.at(j)};
@@ -117,27 +107,20 @@ bool matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBox
); );
} }
} }
if (armor_boxes.empty()) { return !armor_boxes.empty();
return false;
}
sort(armor_boxes.begin(), armor_boxes.end(), [](ArmorBox box1, ArmorBox box2) -> bool {
return centerDistance(box1.rect) < centerDistance(box2.rect);
});
return true;
} }
bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box){ bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box) {
LightBlobs light_blobs; // 存储所有可能的灯条 LightBlobs light_blobs; // 存储所有可能的灯条
ArmorBoxes armor_boxes; // 装甲板候选区 ArmorBoxes armor_boxes; // 装甲板候选区
ArmorBoxes boxes_number[15]; // 装甲板候选区放置在对应id位置
box.rect = cv::Rect2d(0,0,0,0); box.rect = cv::Rect2d(0, 0, 0, 0);
box.id = -1; box.id = -1;
if (!findLightBlobs(src, light_blobs)) { if (!findLightBlobs(src, light_blobs)) {
return false; return false;
} }
if (show_light_blobs && src.size()==cv::Size(640, 480)) { if (show_light_blobs && src.size() == cv::Size(640, 480)) {
showLightBlobs("light_blobs", src, light_blobs); showLightBlobs("light_blobs", src, light_blobs);
cv::waitKey(1); cv::waitKey(1);
} }
@@ -146,7 +129,7 @@ bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box){
// cout << "Box fail!" << endl; // cout << "Box fail!" << endl;
return false; return false;
} }
if (show_armor_boxes && src.size()==cv::Size(640, 480)) { if (show_armor_boxes && src.size() == cv::Size(640, 480)) {
showArmorBoxes("boxes", src, armor_boxes); showArmorBoxes("boxes", src, armor_boxes);
cv::waitKey(1); cv::waitKey(1);
} }
@@ -157,40 +140,24 @@ bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box){
cv::resize(roi, roi, cv::Size(48, 36)); cv::resize(roi, roi, cv::Size(48, 36));
int c = classifier(roi); int c = classifier(roi);
armor_box.id = c; armor_box.id = c;
boxes_number[c].emplace_back(armor_box);
} }
if (enemy_color == ENEMY_BLUE) { sort(armor_boxes.begin(), armor_boxes.end());
for (auto &name : prior_blue) { if(armor_boxes[0].id != 0){
if (!boxes_number[name2id[name]].empty()) { box = armor_boxes[0];
box = boxes_number[name2id[name]][0];
break;
}
}
} else if (enemy_color == ENEMY_RED) {
for (auto &name : prior_red) {
if (!boxes_number[name2id[name]].empty()) {
box = boxes_number[name2id[name]][0];
break;
}
}
} else {
LOGE_INFO("enemy_color ERROR!");
} }
if (save_labelled_boxes) { if (save_labelled_boxes) {
for (int i = 0; i < sizeof(boxes_number) / sizeof(boxes_number[0]); i++) { for (const auto &one_box : armor_boxes) {
for (auto &armor_box : boxes_number[i]) {
char filename[100]; char filename[100];
sprintf(filename, PROJECT_DIR"/armor_box_photo/%s_%d.jpg", id2name[i].data(), sprintf(filename, PROJECT_DIR"/armor_box_photo/%s_%d.jpg", id2name[one_box.id].data(),
time(nullptr) + clock()); time(nullptr) + clock());
cv::imwrite(filename, src(armor_box.rect)); cv::imwrite(filename, src(armor_box.rect));
} }
} }
}
if (box.rect == cv::Rect2d(0, 0, 0, 0)) { if (box.rect == cv::Rect2d(0, 0, 0, 0)) {
return false; return false;
} }
if (show_armor_boxes && src.size()==cv::Size(640, 480)) { if (show_armor_boxes && src.size() == cv::Size(640, 480)) {
showArmorBoxesClass("class", src, boxes_number); showArmorBoxesClass("class", src, armor_boxes);
} }
} else { } else {
box = armor_boxes[0]; box = armor_boxes[0];

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

@@ -47,8 +47,8 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
LOGM(STR_CTR(WORD_BLUE, "shoot after %dms"), 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 - IMAGE_CENTER_X;
double dy = rect.y + rect.height / 2 - 240 - 20; double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y;
double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI; double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
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;

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

@@ -48,33 +48,32 @@ void showArmorBoxes(std::string windows_name, const cv::Mat &src, const ArmorBox
/************************** /**************************
* 显示多个装甲板区域及其类别 * * 显示多个装甲板区域及其类别 *
**************************/ **************************/
void showArmorBoxesClass(std::string window_names, const cv::Mat &src, const ArmorBoxes boxes[15]) { void showArmorBoxesClass(std::string window_names, const cv::Mat &src, const ArmorBoxes &boxes) {
static Mat image2show; static Mat image2show;
if (src.type() == CV_8UC1) { // 黑白图像 if (src.type() == CV_8UC1) { // 黑白图像
cvtColor(src, image2show, COLOR_GRAY2RGB); cvtColor(src, image2show, COLOR_GRAY2RGB);
} else if (src.type() == CV_8UC3) { //RGB 彩色 } else if (src.type() == CV_8UC3) { //RGB 彩色
image2show = src.clone(); image2show = src.clone();
} }
for (int i = 1; i < 15; i++) { for (const auto &box : boxes) {
if (!boxes[i].empty()) { if(box.id != 0) {
for (auto box : boxes[i]) {
rectangle(image2show, box.rect, Scalar(0, 255, 0), 1); rectangle(image2show, box.rect, Scalar(0, 255, 0), 1);
drawLightBlobs(image2show, box.light_blobs); drawLightBlobs(image2show, box.light_blobs);
if (i == -1) if (box.id == -1)
putText(image2show, id2name[i], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, putText(image2show, id2name[box.id], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(0, 255, 0)); Scalar(0, 255, 0));
else if (1 <= i && i < 8) else if (1 <= box.id && box.id < 8)
putText(image2show, id2name[i], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, putText(image2show, id2name[box.id], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(255, 0, 0)); Scalar(255, 0, 0));
else if (8 <= i && i < 15) else if (8 <= box.id && box.id < 15)
putText(image2show, id2name[i], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, putText(image2show, id2name[box.id], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(0, 0, 255)); Scalar(0, 0, 255));
else if (i != 0) else if (box.id != 0)
LOGE_INFO("Invalid box id:%d!", i); LOGE_INFO("Invalid box id:%d!", box.id);
}
} }
} }
imshow(window_names, image2show); imshow(window_names, image2show);
} }
/************************** /**************************
@@ -93,7 +92,7 @@ void showArmorBox(std::string windows_name, const cv::Mat &src, const ArmorBox &
drawLightBlobs(image2show, box.light_blobs); drawLightBlobs(image2show, box.light_blobs);
// static FILE *fp = fopen(PROJECT_DIR"/ratio.txt", "w"); // static FILE *fp = fopen(PROJECT_DIR"/ratio.txt", "w");
// 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.getBlobsDistance())
// cout << box.lengthDistanceRatio() << endl; // cout << box.lengthDistanceRatio() << endl;
if(box.getOrientation() == ArmorBox::FRONT){ if(box.getOrientation() == ArmorBox::FRONT){
@@ -103,7 +102,7 @@ void showArmorBox(std::string windows_name, const cv::Mat &src, const ArmorBox &
}; };
char dist[5]; char dist[5];
sprintf(dist, "%.1f", box.getDistance()); sprintf(dist, "%.1f", box.getBoxDistance());
if (box.id == -1) if (box.id == -1)
putText(image2show, id2name[box.id]+" "+dist, Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1, putText(image2show, id2name[box.id]+" "+dist, Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(0, 255, 0)); Scalar(0, 255, 0));

2
main.cpp
View File

@@ -30,7 +30,7 @@ using namespace std;
mcu_data mcuData = { // 单片机端回传结构体 mcu_data mcuData = { // 单片机端回传结构体
0, // 当前云台yaw角 0, // 当前云台yaw角
0, // 当前云台pitch角 0, // 当前云台pitch角
BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符 ARMOR_STATE, // 当前状态，自瞄-大符-小符
0, // 云台角度标记位 0, // 云台角度标记位
1, // 是否启用数字识别 1, // 是否启用数字识别
ENEMY_RED, // 敌方颜色 ENEMY_RED, // 敌方颜色