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-16 16:31:58 +08:00
parent 298c4c9114
commit 73230d91f1
24 changed files with 18873 additions and 18675 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -13,8 +13,11 @@
#include <armor_finder/classifier/classifier.h>
#include <additions/additions.h>
#define BLOB_RED ENEMY_RED
#define BLOB_BLUE ENEMY_BLUE
#define BLOB_RED ENEMY_RED
#define BLOB_BLUE ENEMY_BLUE
#define BOX_RED ENEMY_RED
#define BOX_BLUE ENEMY_BLUE
#define DISTANCE_HEIGHT_5MM (113.0) // 单位: m*pixel
#define DISTANCE_HEIGHT DISTANCE_HEIGHT_5MM
@@ -22,21 +25,53 @@
extern std::map<int, string> id2name; //装甲板id到名称的map
extern std::map<string, int> name2id; //装甲板名称到id的map
/******************* 灯条类定义 ***********************/
class LightBlob {
public:
cv::RotatedRect rect; //灯条位置
double length; //灯条长度
uint8_t BlobColor; //灯条颜色
uint8_t blob_color; //灯条颜色
LightBlob(cv::RotatedRect &r) : rect(r) {
LightBlob(cv::RotatedRect &r, uint8_t color) : rect(r), blob_color(color) {
length = max(rect.size.height, rect.size.width);
};
LightBlob() = default;
};
typedef std::vector<LightBlob> LightBlobs;
/******************* 装甲板类定义　**********************/
class ArmorBox{
public:
cv::Rect2d rect;
LightBlobs light_blobs;
uint8_t box_color;
int id;
explicit ArmorBox(const cv::Rect &pos=cv::Rect2d(), const LightBlobs &blobs=LightBlobs(), uint8_t color=0, int i=0);
double blobsDistance() 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));
}
}
double lengthDistanceRatio() const {
if(light_blobs.size() == 2){
return (light_blobs[0].length+light_blobs[1].length)/2.0
/ blobsDistance();
}else{
return 100;
}
}
};
typedef std::vector<ArmorBox> ArmorBoxes;
/********************* 自瞄类定义 **********************/
class ArmorFinder{
public:
@@ -50,11 +85,9 @@ private:
SEARCHING_STATE, TRACKING_STATE, STANDBY_STATE
} State; // 自瞄状态枚举定义
cv::Mat src_raw; // 当前原图
const uint8_t &enemy_color; // 敌方颜色，引用外部变量，自动变化
State state; // 自瞄状态对象实例
cv::Rect2d armor_box; // 当前目标位置
int boxid; // 当前目标id
ArmorBox armor_box; // 当前目标装甲板
cv::Ptr<cv::Tracker> tracker; // tracker对象实例
Classifier classifier; // CNN分类器对象实例，用于数字识别
int contour_area; // 装甲区域亮点个数，用于数字识别未启用时判断是否跟丢（已弃用）
@@ -62,15 +95,15 @@ private:
Serial &serial; // 串口对象，引用外部变量，用于和能量机关共享同一个变量
const uint8_t &use_classifier; // 标记是否启用CNN分类器，引用外部变量，自动变化
bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs);
bool findArmorBox(const cv::Mat &src, ArmorBox &box);
bool stateSearchingTarget(cv::Mat &src); // searching state主函数
bool stateTrackingTarget(cv::Mat &src); // tracking state主函数
bool stateStandBy(); // stand by state主函数（已弃用）
bool findLightBlobs(const cv::Mat &src_bin, // 在二值图上寻找灯条
LightBlobs &light_blobs);
bool findArmorBoxes(const LightBlobs &light_blobs, // 根据灯条匹配装甲板候选区
std::vector<cv::Rect2d> &armor_boxes);
void anti_top(); // 反小陀螺
public:
void run(cv::Mat &src); // 自瞄主函数
bool sendBoxPosition(); // 和主控板通讯

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

@@ -9,9 +9,10 @@
#include <armor_finder/armor_finder.h>
//
void showArmorBoxVector(std::string windows_name, const cv::Mat &src, const std::vector<cv::Rect2d> &armor_box);
void showArmorBox(std::string windows_name, const cv::Mat &src, cv::Rect2d armor_box, int boxid);
void showContours(std::string windows_name, const cv::Mat &src, const std::vector<LightBlob> &light_blobs);
void showArmorBoxClass(std::string window_names, const cv::Mat &src, vector<cv::Rect2d> boxes[10]);
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 showTrackSearchingPos(std::string window_names, const cv::Mat &src, const cv::Rect2d pos);
#endif /* _SHOW_IMAGES_H_ */

52
armor/src/armor_finder/anti_top/anti_top.cpp Normal file
View File

@@ -0,0 +1,52 @@
//
// Created by xinyang on 19-7-15.
//
#include <armor_finder/armor_finder.h>
#define B1 1
#define B2 2
#define B3 3
#define B4 4
#define B5 5
#define B7 6
#define B8 7
#define R1 8
#define R2 9
#define R3 10
#define R4 11
#define R5 12
#define R7 13
#define R8 14
void ArmorFinder::anti_top() {
// switch (armor_box.id) {
// case R1:
// case R7:
// case B1:
// case B7:
// if (armor_box.rect != cv::Rect2d()) {
// sendBoxPosition();
// }
// case R2:
// case R3:
// case R4:
// case R5:
// case B2:
// case B3:
// case B4:
// case B5:
// if (armor_box.rect != cv::Rect2d()) {
// sendBoxPosition();
// }
// default:
// if (armor_box.rect != cv::Rect2d()) {
// sendBoxPosition();
// }
// }
if (armor_box.rect != cv::Rect2d()) {
sendBoxPosition();
}
}

212
armor/src/armor_finder/armor_box/armor_box.cpp Normal file
View File

@@ -0,0 +1,212 @@
//
// Created by xinyang on 19-7-13.
//
#include <armor_finder/armor_finder.h>
#include <show_images/show_images.h>
#include <options/options.h>
#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",
};
ArmorBox::ArmorBox(const cv::Rect &pos, const LightBlobs &blobs, uint8_t color, int i) :
rect(pos), light_blobs(blobs), box_color(color), id(i){};
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 :
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;
return abs(angle_i - angle_j) < 10;
}
static 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;
}
static bool lengthJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
double side_length;
cv::Point2f centers = light_blob_i.rect.center - light_blob_j.rect.center;
side_length = sqrt(centers.ddot(centers));
return (side_length / light_blob_i.length < 6 && side_length / light_blob_i.length > 0.5);
}
static bool lengthRatioJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
return (light_blob_i.length / light_blob_j.length < 2
&& light_blob_i.length / light_blob_j.length > 0.5);
}
/* 判断两个灯条的错位度，不知道英文是什么！！！ */
static 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 :
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;
}
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);
return abs(orientation.dot(p2p)) < 20;
}
static 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 :
light_blob_j.rect.angle - 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);
}
static bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_blob_j, uint8_t enemy_color) {
// cout << (light_blob_i.blob_color==BLOB_RED) << endl;
// cout << (light_blob_j.blob_color==BLOB_RED) << endl;
// cout << lengthRatioJudge(light_blob_i, light_blob_j) << endl;
// cout << lengthJudge(light_blob_i, light_blob_j) << endl;
//// cout << heightJudge(light_blob_i, light_blob_j) << endl;
// cout << angelJudge(light_blob_i, light_blob_j) << endl;
// cout << boxAngleJudge(light_blob_i, light_blob_j) << endl;
// cout << CuoWeiDuJudge(light_blob_i, light_blob_j) << endl;
// cout << "=============" << endl;
return light_blob_i.blob_color == enemy_color &&
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) &&
angelJudge(light_blob_i, light_blob_j) &&
boxAngleJudge(light_blob_i, light_blob_j) &&
CuoWeiDuJudge(light_blob_i, light_blob_j);
}
static 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;
}
bool matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBoxes &armor_boxes, uint8_t color) {
armor_boxes.clear();
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();
cv::Rect2d rect_right = light_blobs.at(static_cast<unsigned long>(j)).rect.boundingRect();
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;
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)};
armor_boxes.emplace_back(
cv::Rect2d(min_x, min_y, max_x - min_x, max_y - min_y),
pair_blobs,
color
);
}
}
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;
}
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;
if (!findLightBlobs(src, light_blobs)) {
return false;
}
if (show_light_blobs && src.size()==cv::Size(640, 480)) {
showLightBlobs("light_blobs", src, light_blobs);
cv::waitKey(1);
}
if (!matchArmorBoxes(src, light_blobs, armor_boxes, enemy_color)) {
// cout << "Box fail!" << endl;
return false;
}
if (show_armor_boxes && src.size()==cv::Size(640, 480)) {
showArmorBoxes("boxes", src, armor_boxes);
cv::waitKey(1);
}
if (classifier && use_classifier) {
for (auto &armor_box : armor_boxes) {
cv::Mat roi = src(armor_box.rect).clone();
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!");
}
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));
}
}
}
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);
}
} else {
box = armor_boxes[0];
}
return true;
}

136
armor/src/armor_finder/armor_box/find_light_blobs.cpp Normal file
View File

@@ -0,0 +1,136 @@
//
// Created by xinyang on 19-7-10.
//
#include <armor_finder/armor_finder.h>
#include <opencv2/highgui.hpp>
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 areaRatio(const std::vector<cv::Point> &contour, const cv::RotatedRect &rect){
return cv::contourArea(contour) / rect.size.area();
}
static bool isValidLightBlob(const std::vector<cv::Point> &contour, const cv::RotatedRect &rect) {
return (lw_rate(rect) > 1.5) &&
// (rect.size.area() < 3000) &&
(rect.size.area() > 5) &&
(areaRatio(contour, rect) > 0.7);
}
// 此函数可以有性能优化.
static uint8_t get_blob_color(const cv::Mat &src, const cv::RotatedRect &blobPos) {
auto region = blobPos.boundingRect();
region.x -= fmax(2, region.width * 0.1);
region.y -= fmax(2, region.height * 0.05);
region.width += 2 * fmax(2, region.width * 0.1);
region.height += 2 * fmax(2, region.height * 0.05);
region &= cv::Rect(0, 0, src.cols, src.rows);
cv::Mat roi = src(region);
int red_cnt = 0, blue_cnt = 0;
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) {
return BLOB_RED;
} else {
return BLOB_BLUE;
}
}
static float 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;
}
// 性能优化后的函数
static double get_blob_color_opt(const cv::Mat &src, cv::RotatedRect blobPos) {
int blue_cnt=0, red_cnt=0;
blobPos.size.height *= 1.05;
blobPos.size.width *= 1.1;
cv::Point2f corners[4];
blobPos.points(corners);
sort(corners, &corners[4], [](cv::Point2f p1, cv::Point2f p2) {
return p1.y < p2.y;
});
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++) {
red_cnt += src.at<cv::Vec3b>(r, c)[2];
blue_cnt += src.at<cv::Vec3b>(r, c)[0];
}
}
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++) {
red_cnt += src.at<cv::Vec3b>(r, c)[2];
blue_cnt += src.at<cv::Vec3b>(r, c)[0];
}
}
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++) {
red_cnt += src.at<cv::Vec3b>(r, c)[2];
blue_cnt += src.at<cv::Vec3b>(r, c)[0];
}
}
if (red_cnt > blue_cnt) {
return BLOB_RED;
} else {
return BLOB_BLUE;
}
}
static void imagePreProcess(cv::Mat &src) {
static cv::Mat kernel_erode = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5));
erode(src, src, kernel_erode);
static cv::Mat kernel_dilate = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5));
dilate(src, src, kernel_dilate);
static cv::Mat kernel_dilate2 = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5));
dilate(src, src, kernel_dilate2);
static cv::Mat kernel_erode2 = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5));
erode(src, src, kernel_erode2);
// float alpha = 1.5;
// int beta = 0;
// src.convertTo(src, -1, alpha, beta);
}
bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
cv::Mat src_bin, color_channel;
std::vector<cv::Mat> channels; // 通道拆分
cv::split(src, channels); /************************/
if (enemy_color == ENEMY_BLUE) /* */
color_channel = channels[0]; /* 根据目标颜色进行通道提取 */
else if (enemy_color == ENEMY_RED) /* */
color_channel = channels[2]; /************************/
cv::threshold(color_channel, src_bin, 160, 255, CV_THRESH_BINARY); // 二值化对应通道
imagePreProcess(src_bin); // 开闭运算
std::vector<std::vector<cv::Point> > light_contours;
cv::findContours(src_bin, light_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
for (auto &light_contour : light_contours) {
cv::RotatedRect rect = cv::minAreaRect(light_contour);
if (isValidLightBlob(light_contour, rect)) {
light_blobs.emplace_back(rect, get_blob_color(src, rect));
}
}
return light_blobs.size() >= 2;
}

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

@@ -42,32 +42,25 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder,
classifier(paras_folder),
contour_area(0),
use_classifier(use),
boxid(-1),
tracking_cnt(0) {
}
void ArmorFinder::run(cv::Mat &src) {
src_raw = src;
cv::Mat src_use = src.clone(); // 实际参与计算的图像对象
if (show_armor_box) { // 根据条件显示当前目标装甲板
showArmorBox("box", src, armor_box, boxid);
cv::waitKey(1);
}
// stateSearchingTarget(src_use); // for debug
// return;
switch (state) {
case SEARCHING_STATE:
if (stateSearchingTarget(src_use)) {
if ((armor_box & cv::Rect2d(0, 0, 640, 480)) == armor_box) { // 判断装甲板区域是否脱离图像区域
if (stateSearchingTarget(src)) {
// cout << armor_box.rect << endl;
if ((armor_box.rect & cv::Rect2d(0, 0, 640, 480)) == armor_box.rect) { // 判断装甲板区域是否脱离图像区域
if (!classifier || !use_classifier) { /* 如果分类器不可用或者不使用分类器 */
cv::Mat roi = src_use.clone()(armor_box), roi_gray; /* 就使用装甲区域亮点数判断是否跟丢 */
cv::Mat roi = src(armor_box.rect).clone(), roi_gray; /* 就使用装甲区域亮点数判断是否跟丢 */
cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
contour_area = cv::countNonZero(roi_gray);
}
tracker = TrackerToUse::create(); // 成功搜寻到装甲板，创建tracker对象
tracker->init(src_use, armor_box);
tracker->init(src, armor_box.rect);
state = TRACKING_STATE;
tracking_cnt = 0;
LOGM(STR_CTR(WORD_LIGHT_CYAN, "into track"));
@@ -75,7 +68,7 @@ void ArmorFinder::run(cv::Mat &src) {
}
break;
case TRACKING_STATE:
if (!stateTrackingTarget(src_use) || ++tracking_cnt > 100) { // 最多追踪100帧图像
if (!stateTrackingTarget(src) || ++tracking_cnt > 100) { // 最多追踪100帧图像
state = SEARCHING_STATE;
LOGM(STR_CTR(WORD_LIGHT_YELLOW, "into search!"));
}
@@ -84,5 +77,12 @@ void ArmorFinder::run(cv::Mat &src) {
default:
stateStandBy();
}
anti_top();
if (show_armor_box) { // 根据条件显示当前目标装甲板
showArmorBox("box", src, armor_box);
cv::waitKey(1);
}
}

105
armor/src/armor_finder/searching_state/find_armor_boxes.cpp
View File

@@ -1,105 +0,0 @@
//
// Created by xinyang on 19-7-10.
//
#include <armor_finder/armor_finder.h>
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 :
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;
return abs(angle_i - angle_j) < 10;
}
static 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;
}
static bool lengthJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
double side_length;
cv::Point2f centers = light_blob_i.rect.center - light_blob_j.rect.center;
side_length = sqrt(centers.ddot(centers));
return (side_length / light_blob_i.length < 6 && side_length / light_blob_i.length > 0.5);
}
static bool lengthRatioJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
return (light_blob_i.length / light_blob_j.length < 2
&& light_blob_i.length / light_blob_j.length > 0.5);
}
/* 判断两个灯条的错位度，不知道英文是什么！！！ */
static 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 :
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;
}
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);
return abs(orientation.dot(p2p)) < 20;
}
static 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 :
light_blob_j.rect.angle - 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);
}
static bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_blob_j, uint8_t enemy_color) {
return light_blob_i.BlobColor == enemy_color &&
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) &&
angelJudge(light_blob_i, light_blob_j) &&
boxAngleJudge(light_blob_i, light_blob_j) &&
CuoWeiDuJudge(light_blob_i, light_blob_j);
}
static 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;
}
bool ArmorFinder::findArmorBoxes(const LightBlobs &light_blobs, std::vector<cv::Rect2d> &armor_boxes) {
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), enemy_color)) {
continue;
}
cv::Rect2d rect_left = light_blobs.at(static_cast<unsigned long>(i)).rect.boundingRect();
cv::Rect2d rect_right = light_blobs.at(static_cast<unsigned long>(j)).rect.boundingRect();
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;
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()) {
return false;
}
sort(armor_boxes.begin(), armor_boxes.end(), [](cv::Rect2d box1, cv::Rect2d box2) -> bool {
return centerDistance(box1) < centerDistance(box2);
});
return true;
}

133
armor/src/armor_finder/searching_state/find_light_blobs.cpp
View File

@@ -1,133 +0,0 @@
//
// Created by xinyang on 19-7-10.
//
#include <armor_finder/armor_finder.h>
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) {
//转化为轮廓
cv::Point2f corners[4];
rectangle.points(corners);
cv::Point2f *lastItemPointer = (corners + sizeof corners / sizeof corners[0]);
vector<cv::Point2f> contour(corners, lastItemPointer);
//判断
double indicator = pointPolygonTest(contour, point, true);
return indicator >= 0;
}
// 下面的函数可以有性能优化，暂时未做。
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) {
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)) {
cnt++;
}
}
}
}
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;
}
// 性能优化后的函数
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) {
return p1.y < p2.y;
});
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>(r, c)) {
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)) {
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>(r, c)) {
cnt++;
}
}
}
return double(cnt) / rotrect.size.area();
}
static bool isValidLightBlob(const cv::Mat &bin, const cv::RotatedRect &rect) {
return (lw_rate(rect) > 1.5) &&
// (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);
}
void get_blob_color(const cv::Mat &src, std::vector<LightBlob> &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);
region.width += 2 * fmax(2, region.width * 0.1);
region.height += 2 * fmax(2, region.height * 0.05);
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++) {
red_cnt += roi.at<cv::Vec3b>(row, col)[2];
blue_cnt += roi.at<cv::Vec3b>(row, col)[0];
}
}
if (red_cnt > blue_cnt) {
blob.BlobColor = BLOB_RED;
} else {
blob.BlobColor = BLOB_BLUE;
}
}
}
bool ArmorFinder::findLightBlobs(const cv::Mat &src_bin, LightBlobs &light_blobs) {
std::vector<std::vector<cv::Point> > light_contours;
cv::findContours(src_bin, light_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
for (auto &light_contour : light_contours) {
cv::RotatedRect rect = cv::minAreaRect(light_contour);
if (isValidLightBlob(src_bin, rect)) {
light_blobs.emplace_back(rect);
}
}
get_blob_color(src_raw, light_blobs);
return light_blobs.size() >= 2;
}

111
armor/src/armor_finder/searching_state/searching_state.cpp
View File

@@ -3,118 +3,15 @@
//
#include <armor_finder/armor_finder.h>
#include <opencv2/highgui.hpp>
#include <show_images/show_images.h>
#include <options/options.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",
};
void imagePreProcess(cv::Mat &src) {
static cv::Mat kernel_erode = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5));
erode(src, src, kernel_erode);
static cv::Mat kernel_dilate = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5));
dilate(src, src, kernel_dilate);
static cv::Mat kernel_dilate2 = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5));
dilate(src, src, kernel_dilate2);
static cv::Mat kernel_erode2 = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 5));
erode(src, src, kernel_erode2);
float alpha = 1.5;
int beta = 0;
src.convertTo(src, -1, alpha, beta);
}
bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
std::vector<cv::Mat> channels; // 通道拆分
cv::Mat src_bin, color; // 二值图和颜色通道图
LightBlobs light_blobs; // 存储所有可能的灯条
std::vector<cv::Rect2d> armor_boxes; // 装甲板候选区
std::vector<cv::Rect2d> boxes_number[15]; // 装甲板候选区放置在对应id位置
armor_box = cv::Rect2d(0, 0, 0, 0); // 重置目标装甲板位置
boxid = -1; // 重置目标装甲板id
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); // 开闭运算
if (!findLightBlobs(src_bin, light_blobs)) {
if(findArmorBox(src, armor_box)){
return true;
}else{
armor_box = ArmorBox();
return false;
}
if (show_light_blobs) {
showContours("light_blobs", src, light_blobs);
cv::waitKey(1);
}
if (!findArmorBoxes(light_blobs, armor_boxes)) {
return false;
}
if (show_armor_boxes) {
showArmorBoxVector("boxes", src, armor_boxes);
cv::waitKey(1);
}
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);
boxes_number[c].emplace_back(box);
}
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 name : prior_red) {
if (!boxes_number[name2id[name]].empty()) {
armor_box = boxes_number[name2id[name]][0];
boxid = name2id[name];
break;
}
}
} else {
LOGE_INFO("enemy_color ERROR!");
}
if (armor_box == cv::Rect2d(0, 0, 0, 0)) {
return false;
}
if (show_armor_boxes) {
showArmorBoxClass("class", src, boxes_number);
}
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());
cv::imwrite(filename, src(box));
}
}
}
} else {
armor_box = armor_boxes[0];
boxid = -1;
}
return sendBoxPosition();
}

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

@@ -35,7 +35,7 @@ static bool sendTarget(Serial &serial, double x, double y, double z) {
}
bool ArmorFinder::sendBoxPosition() {
auto rect = armor_box;
auto rect = armor_box.rect;
double dx = rect.x + rect.width / 2 - 320;
double dy = rect.y + rect.height / 2 - 240 - 20;
double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;

74
armor/src/armor_finder/tracking_state/tracking_state.cpp
View File

@@ -2,30 +2,76 @@
// Created by xinyang on 19-3-27.
//
#include <log.h>
#include <options/options.h>
#include <armor_finder/armor_finder.h>
#include <show_images/show_images.h>
bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
if(!tracker->update(src, armor_box)){
auto pos = armor_box.rect;
if(!tracker->update(src, pos)){
armor_box = ArmorBox();
return false;
}
if((armor_box & cv::Rect2d(0, 0, 640, 480)) != armor_box){
if((pos & cv::Rect2d(0, 0, 640, 480)) != pos){
armor_box = ArmorBox();
return false;
}
cv::Mat roi = src.clone()(armor_box);
if(classifier){
cv::resize(roi, roi, cv::Size(48, 36));
if(classifier(roi) == 0){
return false;
cv::Rect2d bigger_rect;
bigger_rect.x = pos.x - pos.width / 2.0;
bigger_rect.y = pos.y - pos.height / 2.0;
bigger_rect.height = pos.height * 2;
bigger_rect.width = pos.width * 2;
bigger_rect &= cv::Rect2d(0, 0, 640, 480);
if(show_armor_box)
showTrackSearchingPos("track", src, bigger_rect);
cv::Mat roi = src(bigger_rect).clone();
ArmorBox box;
if(findArmorBox(roi, box)) {
armor_box = box;
armor_box.rect.x += bigger_rect.x;
armor_box.rect.y += bigger_rect.y;
for(auto &blob : armor_box.light_blobs){
blob.rect.center.x += bigger_rect.x;
blob.rect.center.y += bigger_rect.y;
}
}else{
cv::Mat roi_gray;
cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
contour_area = cv::countNonZero(roi_gray);
if(abs(cv::countNonZero(roi_gray) - contour_area) > contour_area * 0.3){
return false;
roi = src(pos).clone();
if(classifier){
cv::resize(roi, roi, cv::Size(48, 36));
if(classifier(roi) == 0){
armor_box = ArmorBox();
return false;
}
}else{
cv::Mat roi_gray;
cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
contour_area = cv::countNonZero(roi_gray);
if(abs(cv::countNonZero(roi_gray) - contour_area) > contour_area * 0.3){
armor_box = ArmorBox();
return false;
}
}
armor_box.rect = pos;
armor_box.light_blobs.clear();
}
return sendBoxPosition();
// armor_box.x -= armor_box.width / 4.0;
// armor_box.y -= armor_box.height / 4.0;
// armor_box.height *= 1.5;
// armor_box.width *= 1.5;
// roi = src(armor_box);
// if(findSearchingTarget(roi)){
//
// }
sendBoxPosition();
return true;
}

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

@@ -1,13 +1,30 @@
#include <show_images/show_images.h>
#include <opencv2/highgui.hpp>
#include <options/options.h>
#include <log.h>
using namespace cv;
void drawLightBlobs(cv::Mat &src, const LightBlobs &blobs){
for (const auto &blob:blobs) {
Scalar color(0,255,0);
if (blob.blob_color == BLOB_RED)
color = Scalar(0, 0, 255);
else if (blob.blob_color == BLOB_BLUE)
color = Scalar(255, 0, 0);
cv::Point2f vertices[4];
blob.rect.points(vertices);
for (int j = 0; j < 4; j++) {
cv::line(src, vertices[j], vertices[(j + 1) % 4], color, 2);
}
}
}
/**************************
* 显示多个装甲板区域 *
**************************/
void showArmorBoxVector(std::string windows_name, const cv::Mat &src, const std::vector<cv::Rect2d> &armor_box) {
void showArmorBoxes(std::string windows_name, const cv::Mat &src, const ArmorBoxes &armor_boxes) {
static Mat image2show;
if (src.type() == CV_8UC1) {// 黑白图像
cvtColor(src, image2show, COLOR_GRAY2RGB);
@@ -15,8 +32,15 @@ void showArmorBoxVector(std::string windows_name, const cv::Mat &src, const std:
image2show = src.clone();
}
for (auto &box:armor_box) {
rectangle(image2show, box, Scalar(0, 255, 0), 1);
for (auto &box:armor_boxes) {
if(box.box_color == BOX_BLUE) {
rectangle(image2show, box.rect, Scalar(0, 255, 0), 1);
drawLightBlobs(image2show, box.light_blobs);
}else if(box.box_color == BOX_RED){
rectangle(image2show, box.rect, Scalar(0, 255, 0), 1);
drawLightBlobs(image2show, box.light_blobs);
}
}
imshow(windows_name, image2show);
}
@@ -24,25 +48,26 @@ void showArmorBoxVector(std::string windows_name, const cv::Mat &src, const std:
/**************************
* 显示多个装甲板区域及其类别 *
**************************/
void showArmorBoxClass(std::string window_names, const cv::Mat &src, vector<cv::Rect2d> boxes[10]) {
void showArmorBoxesClass(std::string window_names, const cv::Mat &src, const ArmorBoxes boxes[15]) {
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 = 0; i < 14; i++) {
for (int i = 1; i < 15; i++) {
if (!boxes[i].empty()) {
for (auto box : boxes[i]) {
rectangle(image2show, box, Scalar(0, 255, 0), 1);
rectangle(image2show, box.rect, Scalar(0, 255, 0), 1);
drawLightBlobs(image2show, box.light_blobs);
if (i == -1)
putText(image2show, id2name[i], Point(box.x + 2, box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 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.x + 2, box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
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.x + 2, box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
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);
@@ -55,34 +80,47 @@ 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) {
void showArmorBox(std::string windows_name, const cv::Mat &src, const ArmorBox &box) {
static Mat image2show;
if(box.rect == cv::Rect2d()){
imshow(windows_name, src);
}
if (src.type() == CV_8UC1) { // 黑白图像
cvtColor(src, image2show, COLOR_GRAY2RGB);
} else if (src.type() == CV_8UC3) { //RGB 彩色
image2show = src.clone();
}
rectangle(image2show, armor_box, Scalar(0, 255, 0), 1);
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())
cout << box.rect.width/box.rect.height << endl;
if(box.rect.width/box.rect.height > 1.6){
rectangle(image2show, box.rect, Scalar(0, 255, 0), 3);
}else{
rectangle(image2show, box.rect, Scalar(0, 255, 0), 1);
};
char dist[5];
// sprintf(dist, "%.1f", distance);
if (boxid == -1)
putText(image2show, id2name[boxid]+" "+dist, Point(armor_box.x + 2, armor_box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 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,
Scalar(0, 255, 0));
else if (1 <= boxid && boxid < 8)
putText(image2show, id2name[boxid]+" "+dist, Point(armor_box.x + 2, armor_box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
else if (1 <= box.id && box.id < 8)
putText(image2show, id2name[box.id]+" "+dist, Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(255, 0, 0));
else if (8 <= boxid && boxid < 15)
putText(image2show, id2name[boxid]+" "+dist, Point(armor_box.x + 2, armor_box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
else if (8 <= box.id && box.id < 15)
putText(image2show, id2name[box.id]+" "+dist, Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(0, 0, 255));
else if (boxid != 0)
LOGE_INFO("Invalid box id:%d!", boxid);
else if (box.id != 0)
LOGE_INFO("Invalid box id:%d!", box.id);
imshow(windows_name, image2show);
}
/**************************
* 显示多个灯条区域 *
**************************/
void showContours(std::string windows_name, const cv::Mat &src, const std::vector<LightBlob> &light_blobs) {
void showLightBlobs(std::string windows_name, const cv::Mat &src, const LightBlobs &light_blobs) {
static Mat image2show;
if (src.type() == CV_8UC1) { // 黑白图像
@@ -92,13 +130,11 @@ void showContours(std::string windows_name, const cv::Mat &src, const std::vecto
}
for (const auto &light_blob:light_blobs) {
Scalar color;
if (light_blob.BlobColor == BLOB_RED)
Scalar color(0, 255, 0);
if (light_blob.blob_color == BLOB_RED)
color = Scalar(0, 0, 255);
else if (light_blob.BlobColor == BLOB_BLUE)
else if (light_blob.blob_color == BLOB_BLUE)
color = Scalar(255, 0, 0);
else
color = Scalar(0, 255, 0);
cv::Point2f vertices[4];
light_blob.rect.points(vertices);
for (int j = 0; j < 4; j++) {
@@ -107,3 +143,15 @@ void showContours(std::string windows_name, const cv::Mat &src, const std::vecto
}
imshow(windows_name, image2show);
}
void showTrackSearchingPos(std::string window_names, const cv::Mat &src, const cv::Rect2d pos){
static Mat image2show;
if (src.type() == CV_8UC1) { // 黑白图像
cvtColor(src, image2show, COLOR_GRAY2RGB);
} else if (src.type() == CV_8UC3) { //RGB 彩色
image2show = src.clone();
}
rectangle(image2show, pos, Scalar(0, 255, 0), 1);
imshow(window_names, image2show);
}