VSAG/amadeus_26_fb
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

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;