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_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 DISTANCE_HEIGHT_5MM
@@ -40,6 +43,9 @@
extern std::map<int, string> id2name; //装甲板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 {
@@ -72,11 +78,13 @@ public:
explicit ArmorBox(const cv::Rect &pos=cv::Rect2d(), const LightBlobs &blobs=LightBlobs(), uint8_t color=0, int i=0);
double blobsDistance() const;
double lengthRatio() const;
double getBlobsDistance() const;
double lengthDistanceRatio() const;
double getDistance() const;
double getBoxDistance() const;
BoxOrientation getOrientation() const;
// double
bool operator<(const ArmorBox &box) const;
};
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 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 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);
#endif /* _SHOW_IMAGES_H_ */

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

@@ -6,48 +6,38 @@
#include <log.h>
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{
if(light_blobs.size() == 2){
double ArmorBox::getBlobsDistance() const {
if (light_blobs.size() == 2) {
auto &x = light_blobs[0].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));
}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 sqrt((x.x - y.x) * (x.x - y.x) + (x.y - y.y) * (x.y - y.y));
} else {
return 0;
}
}
double ArmorBox::lengthDistanceRatio() const {
if(light_blobs.size() == 2){
if (light_blobs.size() == 2) {
return max(light_blobs[0].length, light_blobs[1].length)
/ blobsDistance();
}else{
/ getBlobsDistance();
} else {
return 100;
}
}
double ArmorBox::getDistance() const{
if(light_blobs.size() == 2 ){
double ArmorBox::getBoxDistance() const {
if (light_blobs.size() == 2) {
return DISTANCE_HEIGHT / 2 / max(light_blobs[0].length, light_blobs[1].length);
} else {
return DISTANCE_HEIGHT / rect.height;
}
}
ArmorBox::BoxOrientation ArmorBox::getOrientation() const{
ArmorBox::BoxOrientation ArmorBox::getOrientation() const {
// cout << lengthDistanceRatio() << endl;
if(light_blobs.size() != 2){
if (light_blobs.size() != 2) {
return UNKNOWN;
}
switch (id) {
@@ -57,9 +47,9 @@ ArmorBox::BoxOrientation ArmorBox::getOrientation() const{
case B1:
case B7:
case B8:
if(lengthDistanceRatio() < 0.24){
if (lengthDistanceRatio() < 0.24) {
return FRONT;
}else{
} else {
return SIDE;
}
case R2:
@@ -72,10 +62,27 @@ ArmorBox::BoxOrientation ArmorBox::getOrientation() const{
case B5:
if (lengthDistanceRatio() < 0.48) {
return FRONT;
}else{
} else {
return SIDE;
}
default:
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},
};
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) :
serial(u),
enemy_color(color),

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

@@ -8,15 +8,7 @@
#include <opencv2/highgui.hpp>
#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) {
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 &&
lengthRatioJudge(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) &&
boxAngleJudge(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 j = i + 1; j < light_blobs.size(); ++j) {
if (!isCoupleLight(light_blobs.at(i), light_blobs.at(j), color)) {
// cout << "match fail" << endl;
continue;
}
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) +
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) {
// cout << "out of range" << endl;
continue;
}
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 false;
}
sort(armor_boxes.begin(), armor_boxes.end(), [](ArmorBox box1, ArmorBox box2) -> bool {
return centerDistance(box1.rect) < centerDistance(box2.rect);
});
return true;
return !armor_boxes.empty();
}
bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box){
bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box) {
LightBlobs light_blobs; // 存储所有可能的灯条
ArmorBoxes armor_boxes; // 装甲板候选区
ArmorBoxes boxes_number[15]; // 装甲板候选区放置在对应id位置
box.rect = cv::Rect2d(0,0,0,0);
box.id = -1;
box.rect = cv::Rect2d(0, 0, 0, 0);
box.id = -1;
if (!findLightBlobs(src, light_blobs)) {
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);
cv::waitKey(1);
}
@@ -146,7 +129,7 @@ bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box){
// cout << "Box fail!" << endl;
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);
cv::waitKey(1);
}
@@ -157,40 +140,24 @@ bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box){
cv::resize(roi, roi, cv::Size(48, 36));
int c = classifier(roi);
armor_box.id = c;
boxes_number[c].emplace_back(armor_box);
}
if (enemy_color == ENEMY_BLUE) {
for (auto &name : prior_blue) {
if (!boxes_number[name2id[name]].empty()) {
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!");
sort(armor_boxes.begin(), armor_boxes.end());
if(armor_boxes[0].id != 0){
box = armor_boxes[0];
}
if (save_labelled_boxes) {
for (int i = 0; i < sizeof(boxes_number) / sizeof(boxes_number[0]); i++) {
for (auto &armor_box : boxes_number[i]) {
char filename[100];
sprintf(filename, PROJECT_DIR"/armor_box_photo/%s_%d.jpg", id2name[i].data(),
time(nullptr) + clock());
cv::imwrite(filename, src(armor_box.rect));
}
for (const auto &one_box : armor_boxes) {
char filename[100];
sprintf(filename, PROJECT_DIR"/armor_box_photo/%s_%d.jpg", id2name[one_box.id].data(),
time(nullptr) + clock());
cv::imwrite(filename, src(armor_box.rect));
}
}
if (box.rect == cv::Rect2d(0, 0, 0, 0)) {
return false;
}
if (show_armor_boxes && src.size()==cv::Size(640, 480)) {
showArmorBoxesClass("class", src, boxes_number);
if (show_armor_boxes && src.size() == cv::Size(640, 480)) {
showArmorBoxesClass("class", src, armor_boxes);
}
} else {
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);
}
auto rect = armor_box.rect;
double dx = rect.x + rect.width / 2 - 320;
double dy = rect.y + rect.height / 2 - 240 - 20;
double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X;
double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y;
double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
double dist = DISTANCE_HEIGHT / rect.height;

39
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;
if (src.type() == CV_8UC1) { // 黑白图像
cvtColor(src, image2show, COLOR_GRAY2RGB);
} else if (src.type() == CV_8UC3) { //RGB 彩色
image2show = src.clone();
}
for (int i = 1; i < 15; i++) {
if (!boxes[i].empty()) {
for (auto box : boxes[i]) {
rectangle(image2show, box.rect, Scalar(0, 255, 0), 1);
drawLightBlobs(image2show, box.light_blobs);
if (i == -1)
putText(image2show, id2name[i], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(0, 255, 0));
else if (1 <= i && i < 8)
putText(image2show, id2name[i], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(255, 0, 0));
else if (8 <= i && i < 15)
putText(image2show, id2name[i], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(0, 0, 255));
else if (i != 0)
LOGE_INFO("Invalid box id:%d!", i);
}
for (const auto &box : boxes) {
if(box.id != 0) {
rectangle(image2show, box.rect, Scalar(0, 255, 0), 1);
drawLightBlobs(image2show, box.light_blobs);
if (box.id == -1)
putText(image2show, id2name[box.id], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(0, 255, 0));
else if (1 <= box.id && box.id < 8)
putText(image2show, id2name[box.id], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(255, 0, 0));
else if (8 <= box.id && box.id < 15)
putText(image2show, id2name[box.id], Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(0, 0, 255));
else if (box.id != 0)
LOGE_INFO("Invalid box id:%d!", box.id);
}
}
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);
// static FILE *fp = fopen(PROJECT_DIR"/ratio.txt", "w");
// 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;
if(box.getOrientation() == ArmorBox::FRONT){
@@ -103,7 +102,7 @@ void showArmorBox(std::string windows_name, const cv::Mat &src, const ArmorBox &
};
char dist[5];
sprintf(dist, "%.1f", box.getDistance());
sprintf(dist, "%.1f", box.getBoxDistance());
if (box.id == -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));

2
main.cpp
View File

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