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

整理代码结构。

Browse Source
This commit is contained in:
xinyang
2019-07-08 20:22:48 +08:00
parent cc508cfec5
commit ab0ddede0c
8 changed files with 199 additions and 291 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -8,6 +8,18 @@
#include <opencv2/highgui.hpp>
#include <armor_finder/armor_finder.h>
std::map<int, string> id2name = {
{-1, "OO"},{ 0, "NO"},
{ 1, "B1"},{ 2, "B2"},{ 3, "B3"},{ 4, "B4"},{ 5, "B5"},{ 6, "B7"},{ 7, "B8"},
{ 8, "R1"},{ 9, "R2"},{10, "R3"},{11, "R4"},{12, "R5"},{13, "R7"},{14, "R8"},
};
std::map<string, int> name2id = {
{"OO", -1},{"NO", 0},
{"B1", 1},{"B2", 2},{"B3", 3},{"B4", 4},{"B5", 5},{"B7", 6},{"B8", 7},
{"R1", 8},{"R2", 9},{"R3", 10},{"R4", 11},{"R5", 12},{"R7", 13},{"R8", 14},
};
ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, string paras_folder, const uint8_t &use) :
serial(u),
enemy_color(color),
@@ -23,14 +35,13 @@ void ArmorFinder::run(cv::Mat &src) {
static int tracking_cnt = 0;
cv::Mat src_use;
src_use = src.clone();
cv::cvtColor(src_use, src_gray, CV_RGB2GRAY);
if(show_armor_box){
showArmorBox("box", src, armor_box, boxid);
cv::waitKey(1);
}
stateSearchingTarget(src_use);
return;
// stateSearchingTarget(src_use);
// return;
switch (state){
case SEARCHING_STATE:
if(stateSearchingTarget(src_use)){

306
armor/src/armor_finder/state_machine/searching_state/searching_state.cpp
View File

@@ -4,43 +4,42 @@
#include <armor_finder/armor_finder.h>
#include <opencv2/highgui.hpp>
#include "image_process/image_process.h"
#include <image_process/image_process.h>
#include <log.h>
#include <show_images/show_images.h>
#include <options/options.h>
typedef std::vector<LightBlob> LightBlobs;
static double lw_rate(const cv::RotatedRect &rect){
return rect.size.height>rect.size.width ?
rect.size.height / rect.size.width :
rect.size.width / rect.size.height ;
static double lw_rate(const cv::RotatedRect &rect) {
return rect.size.height > rect.size.width ?
rect.size.height / rect.size.width :
rect.size.width / rect.size.height;
}
bool rectangleContainPoint(cv::RotatedRect rectangle, cv::Point2f point)
{
bool rectangleContainPoint(cv::RotatedRect rectangle, cv::Point2f point) {
//转化为轮廓
cv::Point2f corners[4];
rectangle.points(corners);
cv::Point2f *lastItemPointer = (corners+sizeof corners/sizeof corners[0]);
vector<cv::Point2f> contour(corners,lastItemPointer);
cv::Point2f *lastItemPointer = (corners + sizeof corners / sizeof corners[0]);
vector<cv::Point2f> contour(corners, lastItemPointer);
//判断
double indicator = pointPolygonTest(contour,point,true);
double indicator = pointPolygonTest(contour, point, true);
return indicator >= 0;
}
/// Todo: 下面的函数可以有性能优化，暂时未做。
static double nonZeroRateOfRotateRect(const cv::Mat &bin, const cv::RotatedRect &rotrect){
static double nonZeroRateOfRotateRect(const cv::Mat &bin, const cv::RotatedRect &rotrect) {
auto rect = rotrect.boundingRect();
if(rect.x < 0 || rect.y < 0 || rect.x+rect.width > bin.cols || rect.y+rect.height > bin.rows){
if (rect.x < 0 || rect.y < 0 || rect.x + rect.width > bin.cols || rect.y + rect.height > bin.rows) {
return 0;
}
auto roi=bin(rect);
int cnt=0;
for(int r=0; r<roi.rows; r++){
for(int c=0; c<roi.cols; c++){
if(rectangleContainPoint(rotrect, cv::Point(c+rect.x, r+rect.y))){
if(roi.at<uint8_t>(r, c)){
auto roi = bin(rect);
int cnt = 0;
for (int r = 0; r < roi.rows; r++) {
for (int c = 0; c < roi.cols; c++) {
if (rectangleContainPoint(rotrect, cv::Point(c + rect.x, r + rect.y))) {
if (roi.at<uint8_t>(r, c)) {
cnt++;
}
}
@@ -49,57 +48,44 @@ static double nonZeroRateOfRotateRect(const cv::Mat &bin, const cv::RotatedRect
return double(cnt) / rotrect.size.area();
}
int linePointX(const cv::Point2f &p1, const cv::Point2f &p2, int y){
return (p2.x-p1.x)/(p2.y-p1.y)*(y-p1.y)+p1.x;
int linePointX(const cv::Point2f &p1, const cv::Point2f &p2, int y) {
return (p2.x - p1.x) / (p2.y - p1.y) * (y - p1.y) + p1.x;
}
///Todo: 性能优化后的函数。(暂时还有点问题)
static double nonZeroRateOfRotateRect_opt(const cv::Mat &bin, const cv::RotatedRect &rotrect){
int cnt=0;
static double nonZeroRateOfRotateRect_opt(const cv::Mat &bin, const cv::RotatedRect &rotrect) {
int cnt = 0;
cv::Point2f corners[4];
rotrect.points(corners);
sort(corners, &corners[4], [](cv::Point2f p1, cv::Point2f p2){
sort(corners, &corners[4], [](cv::Point2f p1, cv::Point2f p2) {
return p1.y < p2.y;
});
// for(int r=corners[0].y; r<corners[3].y; r++){
// int val[]={
// linePointX(corners[0],corners[1], r),
// linePointX(corners[0],corners[2], r),
// linePointX(corners[1],corners[3], r),
// linePointX(corners[2],corners[3], r),
// };
// for(int c=val[1]; c<val[2]; c++){
// if(bin.at<uint8_t >(c, r)){
// cnt++;
// }
// }
// }
for(int r=corners[0].y; r<corners[1].y; r++){
auto start = min(linePointX(corners[0],corners[1], r), linePointX(corners[0],corners[2], r))-1;
auto end = max(linePointX(corners[0],corners[1], r), linePointX(corners[0],corners[2], r))+1;
if(start<0 || end>640) return 0;
for(int c=start; c<end; c++){
if(bin.at<uint8_t >(c, r)){
for (int r = corners[0].y; r < corners[1].y; r++) {
auto start = min(linePointX(corners[0], corners[1], r), linePointX(corners[0], corners[2], r)) - 1;
auto end = max(linePointX(corners[0], corners[1], r), linePointX(corners[0], corners[2], r)) + 1;
if (start < 0 || end > 640) return 0;
for (int c = start; c < end; c++) {
if (bin.at<uint8_t>(c, r)) {
cnt++;
}
}
}
for(int r=corners[1].y; r<corners[2].y; r++){
auto start = min(linePointX(corners[0],corners[2], r), linePointX(corners[1],corners[3], r))-1;
auto end = max(linePointX(corners[0],corners[2], r), linePointX(corners[1],corners[3], r))+1;
if(start<0 || end>640) return 0;
for(int c=start; c<end; c++){
if(bin.at<uint8_t >(r, c)){
for (int r = corners[1].y; r < corners[2].y; r++) {
auto start = min(linePointX(corners[0], corners[2], r), linePointX(corners[1], corners[3], r)) - 1;
auto end = max(linePointX(corners[0], corners[2], r), linePointX(corners[1], corners[3], r)) + 1;
if (start < 0 || end > 640) return 0;
for (int c = start; c < end; c++) {
if (bin.at<uint8_t>(r, c)) {
cnt++;
}
}
}
for(int r=corners[2].y; r<corners[3].y; r++){
auto start = min(linePointX(corners[1],corners[3], r), linePointX(corners[2],corners[3], r))-1;
auto end = max(linePointX(corners[1],corners[3], r), linePointX(corners[2],corners[3], r))+1;
if(start<0 || end>640) return 0;
for(int c=start; c<end; c++){
if(bin.at<uint8_t >(c, r)){
for (int r = corners[2].y; r < corners[3].y; r++) {
auto start = min(linePointX(corners[1], corners[3], r), linePointX(corners[2], corners[3], r)) - 1;
auto end = max(linePointX(corners[1], corners[3], r), linePointX(corners[2], corners[3], r)) + 1;
if (start < 0 || end > 640) return 0;
for (int c = start; c < end; c++) {
if (bin.at<uint8_t>(c, r)) {
cnt++;
}
}
@@ -107,26 +93,19 @@ static double nonZeroRateOfRotateRect_opt(const cv::Mat &bin, const cv::RotatedR
return double(cnt) / rotrect.size.area();
}
static bool isValidLightBlob(const cv::Mat &bin, const cv::RotatedRect &rect){
static bool isValidLightBlob(const cv::Mat &bin, const cv::RotatedRect &rect) {
return (lw_rate(rect) > 1.8) &&
// (rect.size.width*rect.size.height < 3000) &&
(rect.size.width*rect.size.height > 1) &&
// (nonZeroRateOfRotateRect_opt(bin, rect) > 0.8);
// (rect.size.width*rect.size.height < 3000) &&
(rect.size.width * rect.size.height > 1) &&
// (nonZeroRateOfRotateRect_opt(bin, rect) > 0.8);
(nonZeroRateOfRotateRect(bin, rect) > 0.8);
}
static void pipelineLightBlobPreprocess(cv::Mat &src) {
src -= 150;
src *= 3.5;
src -= 150;
src *= 3.5;
}
static bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
static cv::Mat src_gray;
if(src.type() == CV_8UC3){
if (src.type() == CV_8UC3) {
cvtColor(src, src_gray, CV_BGR2GRAY);
}else if(src.type() == CV_8UC1){
} else if (src.type() == CV_8UC1) {
src_gray = src.clone();
}
std::vector<std::vector<cv::Point> > light_contours;
@@ -134,7 +113,7 @@ static bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
for (auto &light_contour : light_contours) {
cv::RotatedRect rect = cv::minAreaRect(light_contour);
if(isValidLightBlob(src_gray, rect)){
if (isValidLightBlob(src_gray, rect)) {
light_blobs.emplace_back(rect);
}
}
@@ -142,15 +121,16 @@ static bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
}
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 :
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;
return abs(angle_i-angle_j)<10;
return abs(angle_i - angle_j) < 10;
}
bool heightJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
cv::Point2f centers = light_blob_i.rect.center - light_blob_j.rect.center;
return abs(centers.y)<30;
return abs(centers.y) < 30;
}
bool lengthJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
@@ -166,30 +146,31 @@ bool lengthRatioJudge(const LightBlob &light_blob_i, const LightBlob &light_blob
}
/* 判断两个灯条的错位度，不知道英文是什么！！！ */
bool CuoWeiDuJudge(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:
bool CuoWeiDuJudge(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 :
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;
float angle = (angle_i+angle_j)/2.0/180.0*3.14159265459;
if(abs(angle_i-angle_j)>90){
angle += 3.14159265459/2;
float angle = (angle_i + angle_j) / 2.0 / 180.0 * 3.14159265459;
if (abs(angle_i - angle_j) > 90) {
angle += 3.14159265459 / 2;
}
Vector2f orientation(cos(angle), sin(angle));
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);
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);
return abs(orientation.dot(p2p)) < 20;
}
bool boxAngleJudge(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:
bool boxAngleJudge(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 :
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;
float angle = (angle_i+angle_j)/2.0;
if(abs(angle_i-angle_j)>90){
float angle = (angle_i + angle_j) / 2.0;
if (abs(angle_i - angle_j) > 90) {
angle += 90.0;
}
return (-120.0<angle && angle<-60.0) || (60.0<angle && angle<120.0);
return (-120.0 < angle && angle < -60.0) || (60.0 < angle && angle < 120.0);
}
bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_blob_j, uint8_t enemy_color) {
@@ -197,17 +178,17 @@ bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_blob_j,
light_blob_j.BlobColor == 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);
}
double centerDistance(const cv::Rect2d &box){
double dx = box.x-box.width/2 - 320;
double dy = box.y-box.height/2 - 240;
return dx*dx + dy*dy;
double centerDistance(const cv::Rect2d &box) {
double dx = box.x - box.width / 2 - 320;
double dy = box.y - box.height / 2 - 240;
return dx * dx + dy * dy;
}
static bool findArmorBoxes(LightBlobs &light_blobs, std::vector<cv::Rect2d> &armor_boxes, uint8_t enemy_color) {
@@ -221,40 +202,26 @@ static bool findArmorBoxes(LightBlobs &light_blobs, std::vector<cv::Rect2d> &arm
double min_x, min_y, max_x, max_y;
min_x = fmin(rect_left.x, rect_right.x) - 4;
max_x = fmax(rect_left.x + rect_left.width, rect_right.x + rect_right.width) + 4;
min_y = fmin(rect_left.y, rect_right.y) - 0.5*(rect_left.height+rect_right.height)/2.0;
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;
min_y = fmin(rect_left.y, rect_right.y) - 0.5 * (rect_left.height + rect_right.height) / 2.0;
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 > 640 || min_y < 0 || max_y > 480) {
continue;
}
armor_boxes.emplace_back(cv::Rect2d(min_x, min_y, max_x - min_x, max_y - min_y));
}
}
if(armor_boxes.empty()){
if (armor_boxes.empty()) {
return false;
}
sort(armor_boxes.begin(), armor_boxes.end(), [](cv::Rect2d box1, cv::Rect2d box2)->bool{
sort(armor_boxes.begin(), armor_boxes.end(), [](cv::Rect2d box1, cv::Rect2d box2) -> bool {
return centerDistance(box1) < centerDistance(box2);
});
return true;
}
bool judge_light_color(std::vector<LightBlob> &light, std::vector<LightBlob> &color, std::vector<LightBlob> &result) {
for (auto &i:color) {
for (auto &j:light) {
cv::Rect2d a = i.rect.boundingRect2f();
cv::Rect2d b = j.rect.boundingRect2f();
cv::Rect2d ab = a & b;
if (ab.area() / fmin(a.area(), b.area()) >= 0.2) {
result.emplace_back(j);
break;
}
}
}
return !result.empty();
}
void get_blob_color(const cv::Mat &src, std::vector<LightBlob> &blobs) {
for(auto &blob : blobs){
for (auto &blob : blobs) {
auto region = blob.rect.boundingRect();
region.x -= fmax(2, region.width * 0.1);
region.y -= fmax(2, region.height * 0.05);
@@ -263,122 +230,113 @@ void get_blob_color(const cv::Mat &src, std::vector<LightBlob> &blobs) {
region &= cv::Rect(0, 0, 640, 480);
cv::Mat roi = src(region);
long long int red_cnt = 0, blue_cnt = 0;
for(int row=0; row<roi.rows; row++){
for(int col=0; col<roi.cols; col++){
for (int row = 0; row < roi.rows; row++) {
for (int col = 0; col < roi.cols; col++) {
red_cnt += roi.at<cv::Vec3b>(row, col)[2];
blue_cnt += roi.at<cv::Vec3b>(row, col)[0];
}
}
if(red_cnt > blue_cnt){
if (red_cnt > blue_cnt) {
blob.BlobColor = BLOB_RED;
}else{
} else {
blob.BlobColor = BLOB_BLUE;
}
}
}
int prior_blue[] = {6, 0, 2, 3, 4, 5, 1, 13, 7, 9, 10, 11, 12, 8};
int prior_red[]= {13, 7, 9, 10, 11, 12, 8, 6, 0, 2, 3, 4, 5, 1};
static string prior_blue[] = {
"B8", "B1", "B3", "B4", "B5", "B7", "B2",
"R8", "R1", "R3", "R4", "R5", "R7", "R2",
};
static string prior_red[] = {
"B8", "B1", "B3", "B4", "B5", "B7", "B2",
"R8", "R1", "R3", "R4", "R5", "R7", "R2",
};
bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
cv::Mat split, src_bin/*, edge*/;
LightBlobs light_blobs, light_blobs_, light_blobs_real;
cv::Mat split, src_bin, color;
LightBlobs light_blobs;
std::vector<cv::Rect2d> armor_boxes, boxes_number[14];
armor_box = cv::Rect2d(0,0,0,0);
std::vector<cv::Mat> channels;
cv::cvtColor(src, src_gray, CV_BGR2GRAY);
// cv::Canny(src_gray, edge, 100, 150);
// src_gray -= edge;
// cv::imshow("minus", src_gray);
// pipelineLightBlobPreprocess(src_gray);
cv::threshold(src_gray, src_bin, 170, 255, CV_THRESH_BINARY);
armor_box = cv::Rect2d(0, 0, 0, 0);
cv::split(src, channels);
if (enemy_color == ENEMY_BLUE)
color = channels[0];
else if (enemy_color == ENEMY_RED)
color = channels[2];
cv::threshold(color, src_bin, 170, 255, CV_THRESH_BINARY);
imagePreProcess(src_bin);
// imshow("gray bin", src_bin);
if(!findLightBlobs(src_bin, light_blobs)){
if (!findLightBlobs(src_bin, light_blobs)) {
return false;
}
if(show_light_blobs){
if (show_light_blobs) {
showContours("blobs_gray", src_bin, light_blobs);
cv::waitKey(1);
}
// imageColorSplit(src, split, enemy_color);
//// imshow("split123",split);
// imagePreProcess(split);
//// imshow("split",split);
// cv::threshold(split, src_bin, 170, 255, CV_THRESH_BINARY);
// if(!findLightBlobs(src_bin, light_blobs_)){
// return false;
// }
// if(show_light_blobs){
// showContours("blobs_split", src_bin, light_blobs_);
// cv::waitKey(1);
// }
//
// if(!judge_light_color(light_blobs, light_blobs_, light_blobs_real)){
// return false;
// }
light_blobs_real = light_blobs;
get_blob_color(src, light_blobs_real);
if(show_light_blobs){
showContours("light_blobs", src, light_blobs_real);
// showCuoWeiDu(src, light_blobs_real);
get_blob_color(src, light_blobs);
if (show_light_blobs) {
showContours("light_blobs", src, light_blobs);
cv::waitKey(1);
}
if(!findArmorBoxes(light_blobs_real, armor_boxes, enemy_color)){
if (!findArmorBoxes(light_blobs, armor_boxes, enemy_color)) {
return false;
}
if(show_armor_boxes){
if (show_armor_boxes) {
showArmorBoxVector("boxes", src, armor_boxes);
cv::waitKey(1);
}
if(classifier && use_classifier){
for(auto box : armor_boxes){
if (classifier && use_classifier) {
for (auto box : armor_boxes) {
cv::Mat roi = src(box).clone();
cv::resize(roi, roi, cv::Size(48, 36));
int c = classifier(roi);
if(c){
boxes_number[c-1].emplace_back(box);
}
boxes_number[c].emplace_back(box);
}
if(enemy_color == ENEMY_BLUE) {
for(auto id : prior_blue){
if(!boxes_number[id].empty()){
armor_box = boxes_number[id][0];
boxid = id;
if (enemy_color == ENEMY_BLUE) {
for (auto name : prior_blue) {
if (!boxes_number[name2id[name]].empty()) {
armor_box = boxes_number[name2id[name]][0];
boxid = name2id[name];
break;
}
}
}else if(enemy_color == ENEMY_RED) {
for(auto id : prior_red){
if(!boxes_number[id].empty()){
armor_box = boxes_number[id][0];
boxid = id;
} else if (enemy_color == ENEMY_RED) {
for (auto name : prior_red) {
if (!boxes_number[name2id[name]].empty()) {
armor_box = boxes_number[name2id[name]][0];
boxid = name2id[name];
break;
}
}
}else{
} else {
LOGE("enemy_color ERROR!");
}
if(armor_box == cv::Rect2d(0,0,0,0)){
if (armor_box == cv::Rect2d(0, 0, 0, 0)) {
return false;
}
if(show_armor_boxes){
if (show_armor_boxes) {
showArmorBoxClass("class", src, boxes_number);
for(int i=0; i<sizeof(boxes_number)/ sizeof(boxes_number[0]); i++){
for(auto &box : boxes_number[i]){
}
if (save_labelled_boxes) {
for (int i = 0; i < sizeof(boxes_number) / sizeof(boxes_number[0]); i++) {
for (auto &box : boxes_number[i]) {
char filename[100];
sprintf(filename, PROJECT_DIR"/armor_box_photo/%s_%d.jpg", id2name[i].data(), time(nullptr)+clock());
sprintf(filename, PROJECT_DIR"/armor_box_photo/%s_%d.jpg", id2name[i].data(),
time(nullptr) + clock());
cv::imwrite(filename, src(box));
}
}
}
}else{
} else {
armor_box = armor_boxes[0];
boxid = -1;
}
if(split.size() == cv::Size(320, 240)){
if (split.size() == cv::Size(320, 240)) {
armor_box.x *= 2;
armor_box.y *= 2;
armor_box.width *= 2;

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

@@ -3,31 +3,11 @@
using namespace cv;
std::map<int, string> id2name = {
{-1, "NO"},
{ 0, "B1"},
{ 1, "B2"},
{ 2, "B3"},
{ 3, "B4"},
{ 4, "B5"},
{ 5, "B7"},
{ 6, "B8"},
{ 7, "R1"},
{ 8, "R2"},
{ 9, "R3"},
{10, "R4"},
{11, "R5"},
{12, "R7"},
{13, "R8"},
};
void showArmorBoxVector(std::string windows_name, const cv::Mat &src, const std::vector<cv::Rect2d> &armor_box) {
static Mat image2show;
if (src.type() == CV_8UC1) // 黑白图像
{
if (src.type() == CV_8UC1) {// 黑白图像
cvtColor(src, image2show, COLOR_GRAY2RGB);
} else if(src.type() == CV_8UC3) //RGB 彩色
{
} else if (src.type() == CV_8UC3) { //RGB 彩色
image2show = src.clone();
}
@@ -37,26 +17,27 @@ void showArmorBoxVector(std::string windows_name, const cv::Mat &src, const std:
imshow(windows_name, image2show);
}
void showArmorBoxClass(std::string window_names, const cv::Mat &src, vector<cv::Rect2d> boxes[10]){
void showArmorBoxClass(std::string window_names, const cv::Mat &src, vector<cv::Rect2d> boxes[10]) {
static Mat image2show;
if (src.type() == CV_8UC1) // 黑白图像
{
if (src.type() == CV_8UC1) { // 黑白图像
cvtColor(src, image2show, COLOR_GRAY2RGB);
} else if(src.type() == CV_8UC3) //RGB 彩色
{
} else if (src.type() == CV_8UC3) { //RGB 彩色
image2show = src.clone();
}
for(int i=0; i<14; i++){
if(!boxes[i].empty()){
for(auto box : boxes[i]){
for (int i = 0; i < 14; i++) {
if (!boxes[i].empty()) {
for (auto box : boxes[i]) {
rectangle(image2show, box, Scalar(0, 255, 0), 1);
if(i == -1)
putText(image2show, id2name[i], Point(box.x+2, box.y+2), cv::FONT_HERSHEY_TRIPLEX, 1, Scalar(0,255,0));
else if(0<=i && i<7)
putText(image2show, id2name[i], Point(box.x+2, box.y+2), cv::FONT_HERSHEY_TRIPLEX, 1, Scalar(255,0,0));
else if(7<=i && i<14)
putText(image2show, id2name[i], Point(box.x+2, box.y+2), cv::FONT_HERSHEY_TRIPLEX, 1, Scalar(0,0,255));
else
if (i == -1)
putText(image2show, id2name[i], Point(box.x + 2, box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(0, 255, 0));
else if (1 <= i && i < 8)
putText(image2show, id2name[i], Point(box.x + 2, box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(255, 0, 0));
else if (8 <= i && i < 15)
putText(image2show, id2name[i], Point(box.x + 2, box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(0, 0, 255));
else if (i != 0)
LOGE_INFO("Invalid box id:%d!", i);
}
}
@@ -66,21 +47,22 @@ void showArmorBoxClass(std::string window_names, const cv::Mat &src, vector<cv::
void showArmorBox(std::string windows_name, const cv::Mat &src, cv::Rect2d armor_box, int boxid) {
static Mat image2show;
if (src.type() == CV_8UC1) // 黑白图像
{
if (src.type() == CV_8UC1) { // 黑白图像
cvtColor(src, image2show, COLOR_GRAY2RGB);
} else if(src.type() == CV_8UC3) //RGB 彩色
{
} else if (src.type() == CV_8UC3) { //RGB 彩色
image2show = src.clone();
}
rectangle(image2show, armor_box, Scalar(0, 255, 0), 1);
if(boxid == -1)
putText(image2show, id2name[boxid], Point(armor_box.x+2, armor_box.y+2), cv::FONT_HERSHEY_TRIPLEX, 1, Scalar(0,255,0));
else if(0<=boxid && boxid<7)
putText(image2show, id2name[boxid], Point(armor_box.x+2, armor_box.y+2), cv::FONT_HERSHEY_TRIPLEX, 1, Scalar(255,0,0));
else if(7<=boxid && boxid<14)
putText(image2show, id2name[boxid], Point(armor_box.x+2, armor_box.y+2), cv::FONT_HERSHEY_TRIPLEX, 1, Scalar(0,0,255));
else
if (boxid == -1)
putText(image2show, id2name[boxid], Point(armor_box.x + 2, armor_box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(0, 255, 0));
else if (1 <= boxid && boxid < 8)
putText(image2show, id2name[boxid], Point(armor_box.x + 2, armor_box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(255, 0, 0));
else if (8 <= boxid && boxid < 15)
putText(image2show, id2name[boxid], Point(armor_box.x + 2, armor_box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(0, 0, 255));
else if (boxid != 0)
LOGE_INFO("Invalid box id:%d!", boxid);
imshow(windows_name, image2show);
}
@@ -88,60 +70,25 @@ void showArmorBox(std::string windows_name, const cv::Mat &src, cv::Rect2d armor
void showContours(std::string windows_name, const cv::Mat &src, const std::vector<LightBlob> &light_blobs) {
static Mat image2show;
if(src.type() == CV_8UC1) // 黑白图像
{
if (src.type() == CV_8UC1) { // 黑白图像
cvtColor(src, image2show, COLOR_GRAY2RGB);
}
else if(src.type() == CV_8UC3) //RGB 彩色
{
} else if (src.type() == CV_8UC3) { //RGB 彩色
image2show = src.clone();
}
for(const auto &light_blob:light_blobs)
{
for (const auto &light_blob:light_blobs) {
Scalar color;
if(light_blob.BlobColor == BLOB_RED)
color = Scalar(0,0,255);
else if(light_blob.BlobColor == BLOB_BLUE)
color = Scalar(255,0,0);
if (light_blob.BlobColor == BLOB_RED)
color = Scalar(0, 0, 255);
else if (light_blob.BlobColor == BLOB_BLUE)
color = Scalar(255, 0, 0);
else
color = Scalar(0,255,0);
color = Scalar(0, 255, 0);
cv::Point2f vertices[4];
light_blob.rect.points(vertices);
for (int j = 0; j < 4; j++){
for (int j = 0; j < 4; j++) {
cv::line(image2show, vertices[j], vertices[(j + 1) % 4], color, 2);
}
}
imshow(windows_name, image2show);
}
void drawCuoWeiDu(cv::Mat &src, 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:
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:
light_blob_j.rect.angle - 90;
float angle = (angle_i+angle_j)/2.0/180.0*3.14159265459;
if(abs(angle_i-angle_j)>90){
angle += 3.14159265459/2;
}
Point2f orientation(cos(angle), sin(angle));
Vector2f p2p(light_blob_j.rect.center.x-light_blob_i.rect.center.x, light_blob_i.rect.center.y-light_blob_j.rect.center.y);
cv::line(
src,
(light_blob_j.rect.center+light_blob_i.rect.center)/2.0,
(light_blob_j.rect.center+light_blob_i.rect.center)/2.0 + 100*orientation,
Scalar(0,255,0),
2
);
}
void showCuoWeiDu(const cv::Mat &src, const std::vector<LightBlob> &light_blobs){
Mat image2show = src.clone();
for (int i = 0; i < light_blobs.size() - 1; ++i) {
for (int j = i + 1; j < light_blobs.size(); ++j) {
drawCuoWeiDu(image2show, light_blobs[i], light_blobs[j]);
}
}
imshow("CuoWeiDu", image2show);
}