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

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

@@ -5,12 +5,16 @@
#ifndef _ARMOR_FINDER_H_ #ifndef _ARMOR_FINDER_H_
#define _ARMOR_FINDER_H_ #define _ARMOR_FINDER_H_
#include <map>
#include <opencv2/core.hpp> #include <opencv2/core.hpp>
#include <opencv2/tracking.hpp> #include <opencv2/tracking.hpp>
#include <serial/serial.h> #include <serial/serial.h>
#include <armor_finder/classifier/classifier.h> #include <armor_finder/classifier/classifier.h>
#include "additions/additions.h" #include "additions/additions.h"
extern std::map<int, string> id2name;
extern std::map<string, int> name2id;
class ArmorFinder{ class ArmorFinder{
public: public:
ArmorFinder(uint8_t &color, Serial &u, string paras_folder, const uint8_t &use); ArmorFinder(uint8_t &color, Serial &u, string paras_folder, const uint8_t &use);
@@ -28,7 +32,6 @@ private:
cv::Rect2d armor_box; cv::Rect2d armor_box;
int boxid; int boxid;
cv::Ptr<cv::Tracker> tracker; cv::Ptr<cv::Tracker> tracker;
cv::Mat src_gray;
Classifier classifier; Classifier classifier;

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

@@ -9,13 +9,10 @@
#include <opencv2/highgui.hpp> #include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp> #include <opencv2/imgproc.hpp>
#include <armor_finder/armor_finder.h> #include <armor_finder/armor_finder.h>
#include <map>
extern std::map<int, string> id2name;
void showArmorBoxVector(std::string windows_name, const cv::Mat &src, const std::vector<cv::Rect2d> &armor_box); 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 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 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 showArmorBoxClass(std::string window_names, const cv::Mat &src, vector<cv::Rect2d> boxes[10]);
void showCuoWeiDu(const cv::Mat &src, const std::vector<LightBlob> &light_blobs);
#endif /* _SHOW_IMAGES_H_ */ #endif /* _SHOW_IMAGES_H_ */

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

@@ -8,6 +8,18 @@
#include <opencv2/highgui.hpp> #include <opencv2/highgui.hpp>
#include <armor_finder/armor_finder.h> #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) : ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, string paras_folder, const uint8_t &use) :
serial(u), serial(u),
enemy_color(color), enemy_color(color),
@@ -23,14 +35,13 @@ void ArmorFinder::run(cv::Mat &src) {
static int tracking_cnt = 0; static int tracking_cnt = 0;
cv::Mat src_use; cv::Mat src_use;
src_use = src.clone(); src_use = src.clone();
cv::cvtColor(src_use, src_gray, CV_RGB2GRAY);
if(show_armor_box){ if(show_armor_box){
showArmorBox("box", src, armor_box, boxid); showArmorBox("box", src, armor_box, boxid);
cv::waitKey(1); cv::waitKey(1);
} }
stateSearchingTarget(src_use); // stateSearchingTarget(src_use);
return; // return;
switch (state){ switch (state){
case SEARCHING_STATE: case SEARCHING_STATE:
if(stateSearchingTarget(src_use)){ 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 <armor_finder/armor_finder.h>
#include <opencv2/highgui.hpp> #include <opencv2/highgui.hpp>
#include "image_process/image_process.h" #include <image_process/image_process.h>
#include <log.h> #include <log.h>
#include <show_images/show_images.h> #include <show_images/show_images.h>
#include <options/options.h> #include <options/options.h>
typedef std::vector<LightBlob> LightBlobs; typedef std::vector<LightBlob> LightBlobs;
static double lw_rate(const cv::RotatedRect &rect){ static double lw_rate(const cv::RotatedRect &rect) {
return rect.size.height>rect.size.width ? return rect.size.height > rect.size.width ?
rect.size.height / rect.size.width : rect.size.height / rect.size.width :
rect.size.width / rect.size.height ; 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]; cv::Point2f corners[4];
rectangle.points(corners); rectangle.points(corners);
cv::Point2f *lastItemPointer = (corners+sizeof corners/sizeof corners[0]); cv::Point2f *lastItemPointer = (corners + sizeof corners / sizeof corners[0]);
vector<cv::Point2f> contour(corners,lastItemPointer); vector<cv::Point2f> contour(corners, lastItemPointer);
//判断 //判断
double indicator = pointPolygonTest(contour,point,true); double indicator = pointPolygonTest(contour, point, true);
return indicator >= 0; return indicator >= 0;
} }
/// Todo: 下面的函数可以有性能优化，暂时未做。 /// 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(); 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; return 0;
} }
auto roi=bin(rect); auto roi = bin(rect);
int cnt=0; int cnt = 0;
for(int r=0; r<roi.rows; r++){ for (int r = 0; r < roi.rows; r++) {
for(int c=0; c<roi.cols; c++){ for (int c = 0; c < roi.cols; c++) {
if(rectangleContainPoint(rotrect, cv::Point(c+rect.x, r+rect.y))){ if (rectangleContainPoint(rotrect, cv::Point(c + rect.x, r + rect.y))) {
if(roi.at<uint8_t>(r, c)){ if (roi.at<uint8_t>(r, c)) {
cnt++; cnt++;
} }
} }
@@ -49,57 +48,44 @@ static double nonZeroRateOfRotateRect(const cv::Mat &bin, const cv::RotatedRect
return double(cnt) / rotrect.size.area(); return double(cnt) / rotrect.size.area();
} }
int linePointX(const cv::Point2f &p1, const cv::Point2f &p2, int y){ 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; return (p2.x - p1.x) / (p2.y - p1.y) * (y - p1.y) + p1.x;
} }
///Todo: 性能优化后的函数。(暂时还有点问题) ///Todo: 性能优化后的函数。(暂时还有点问题)
static double nonZeroRateOfRotateRect_opt(const cv::Mat &bin, const cv::RotatedRect &rotrect){ static double nonZeroRateOfRotateRect_opt(const cv::Mat &bin, const cv::RotatedRect &rotrect) {
int cnt=0; int cnt = 0;
cv::Point2f corners[4]; cv::Point2f corners[4];
rotrect.points(corners); 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; return p1.y < p2.y;
}); });
// for(int r=corners[0].y; r<corners[3].y; r++){ for (int r = corners[0].y; r < corners[1].y; r++) {
// int val[]={ auto start = min(linePointX(corners[0], corners[1], r), linePointX(corners[0], corners[2], r)) - 1;
// linePointX(corners[0],corners[1], r), auto end = max(linePointX(corners[0], corners[1], r), linePointX(corners[0], corners[2], r)) + 1;
// linePointX(corners[0],corners[2], r), if (start < 0 || end > 640) return 0;
// linePointX(corners[1],corners[3], r), for (int c = start; c < end; c++) {
// linePointX(corners[2],corners[3], r), if (bin.at<uint8_t>(c, 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)){
cnt++; cnt++;
} }
} }
} }
for(int r=corners[1].y; r<corners[2].y; r++){ 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 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; auto end = max(linePointX(corners[0], corners[2], r), linePointX(corners[1], corners[3], r)) + 1;
if(start<0 || end>640) return 0; if (start < 0 || end > 640) return 0;
for(int c=start; c<end; c++){ for (int c = start; c < end; c++) {
if(bin.at<uint8_t >(r, c)){ if (bin.at<uint8_t>(r, c)) {
cnt++; cnt++;
} }
} }
} }
for(int r=corners[2].y; r<corners[3].y; 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 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; auto end = max(linePointX(corners[1], corners[3], r), linePointX(corners[2], corners[3], r)) + 1;
if(start<0 || end>640) return 0; if (start < 0 || end > 640) return 0;
for(int c=start; c<end; c++){ for (int c = start; c < end; c++) {
if(bin.at<uint8_t >(c, r)){ if (bin.at<uint8_t>(c, r)) {
cnt++; cnt++;
} }
} }
@@ -107,26 +93,19 @@ static double nonZeroRateOfRotateRect_opt(const cv::Mat &bin, const cv::RotatedR
return double(cnt) / rotrect.size.area(); 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) && return (lw_rate(rect) > 1.8) &&
// (rect.size.width*rect.size.height < 3000) && // (rect.size.width*rect.size.height < 3000) &&
(rect.size.width*rect.size.height > 1) && (rect.size.width * rect.size.height > 1) &&
// (nonZeroRateOfRotateRect_opt(bin, rect) > 0.8); // (nonZeroRateOfRotateRect_opt(bin, rect) > 0.8);
(nonZeroRateOfRotateRect(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 bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
static cv::Mat src_gray; static cv::Mat src_gray;
if(src.type() == CV_8UC3){ if (src.type() == CV_8UC3) {
cvtColor(src, src_gray, CV_BGR2GRAY); cvtColor(src, src_gray, CV_BGR2GRAY);
}else if(src.type() == CV_8UC1){ } else if (src.type() == CV_8UC1) {
src_gray = src.clone(); src_gray = src.clone();
} }
std::vector<std::vector<cv::Point> > light_contours; 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) { for (auto &light_contour : light_contours) {
cv::RotatedRect rect = cv::minAreaRect(light_contour); cv::RotatedRect rect = cv::minAreaRect(light_contour);
if(isValidLightBlob(src_gray, rect)){ if (isValidLightBlob(src_gray, rect)) {
light_blobs.emplace_back(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) { 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; 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; 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) { 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; 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) { 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){ 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: 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; 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; light_blob_j.rect.angle - 90;
float angle = (angle_i+angle_j)/2.0/180.0*3.14159265459; float angle = (angle_i + angle_j) / 2.0 / 180.0 * 3.14159265459;
if(abs(angle_i-angle_j)>90){ if (abs(angle_i - angle_j) > 90) {
angle += 3.14159265459/2; angle += 3.14159265459 / 2;
} }
Vector2f orientation(cos(angle), sin(angle)); 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; return abs(orientation.dot(p2p)) < 20;
} }
bool boxAngleJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j){ 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: 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; 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; light_blob_j.rect.angle - 90;
float angle = (angle_i+angle_j)/2.0; float angle = (angle_i + angle_j) / 2.0;
if(abs(angle_i-angle_j)>90){ if (abs(angle_i - angle_j) > 90) {
angle += 90.0; 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) { 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 && light_blob_j.BlobColor == 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);
} }
double centerDistance(const cv::Rect2d &box){ double centerDistance(const cv::Rect2d &box) {
double dx = box.x-box.width/2 - 320; double dx = box.x - box.width / 2 - 320;
double dy = box.y-box.height/2 - 240; double dy = box.y - box.height / 2 - 240;
return dx*dx + dy*dy; return dx * dx + dy * dy;
} }
static bool findArmorBoxes(LightBlobs &light_blobs, std::vector<cv::Rect2d> &armor_boxes, uint8_t enemy_color) { 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; double min_x, min_y, max_x, max_y;
min_x = fmin(rect_left.x, rect_right.x) - 4; 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; 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; 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; 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) { if (min_x < 0 || max_x > 640 || min_y < 0 || max_y > 480) {
continue; continue;
} }
armor_boxes.emplace_back(cv::Rect2d(min_x, min_y, max_x - min_x, max_y - min_y)); 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; 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 centerDistance(box1) < centerDistance(box2);
}); });
return true; 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) { 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(); auto region = blob.rect.boundingRect();
region.x -= fmax(2, region.width * 0.1); region.x -= fmax(2, region.width * 0.1);
region.y -= fmax(2, region.height * 0.05); 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); region &= cv::Rect(0, 0, 640, 480);
cv::Mat roi = src(region); cv::Mat roi = src(region);
long long int red_cnt = 0, blue_cnt = 0; long long int red_cnt = 0, blue_cnt = 0;
for(int row=0; row<roi.rows; row++){ for (int row = 0; row < roi.rows; row++) {
for(int col=0; col<roi.cols; col++){ for (int col = 0; col < roi.cols; col++) {
red_cnt += roi.at<cv::Vec3b>(row, col)[2]; red_cnt += roi.at<cv::Vec3b>(row, col)[2];
blue_cnt += roi.at<cv::Vec3b>(row, col)[0]; blue_cnt += roi.at<cv::Vec3b>(row, col)[0];
} }
} }
if(red_cnt > blue_cnt){ if (red_cnt > blue_cnt) {
blob.BlobColor = BLOB_RED; blob.BlobColor = BLOB_RED;
}else{ } else {
blob.BlobColor = BLOB_BLUE; blob.BlobColor = BLOB_BLUE;
} }
} }
} }
int prior_blue[] = {6, 0, 2, 3, 4, 5, 1, 13, 7, 9, 10, 11, 12, 8}; static string prior_blue[] = {
int prior_red[]= {13, 7, 9, 10, 11, 12, 8, 6, 0, 2, 3, 4, 5, 1}; "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) { bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
cv::Mat split, src_bin/*, edge*/; cv::Mat split, src_bin, color;
LightBlobs light_blobs, light_blobs_, light_blobs_real; LightBlobs light_blobs;
std::vector<cv::Rect2d> armor_boxes, boxes_number[14]; 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); armor_box = cv::Rect2d(0, 0, 0, 0);
// cv::Canny(src_gray, edge, 100, 150);
// src_gray -= edge; cv::split(src, channels);
// cv::imshow("minus", src_gray); if (enemy_color == ENEMY_BLUE)
// pipelineLightBlobPreprocess(src_gray); color = channels[0];
cv::threshold(src_gray, src_bin, 170, 255, CV_THRESH_BINARY); else if (enemy_color == ENEMY_RED)
color = channels[2];
cv::threshold(color, src_bin, 170, 255, CV_THRESH_BINARY);
imagePreProcess(src_bin); imagePreProcess(src_bin);
// imshow("gray bin", src_bin); if (!findLightBlobs(src_bin, light_blobs)) {
if(!findLightBlobs(src_bin, light_blobs)){
return false; return false;
} }
if(show_light_blobs){ if (show_light_blobs) {
showContours("blobs_gray", src_bin, light_blobs); showContours("blobs_gray", src_bin, light_blobs);
cv::waitKey(1); cv::waitKey(1);
} }
// imageColorSplit(src, split, enemy_color); get_blob_color(src, light_blobs);
//// imshow("split123",split); if (show_light_blobs) {
// imagePreProcess(split); showContours("light_blobs", src, light_blobs);
//// 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);
cv::waitKey(1); cv::waitKey(1);
} }
if(!findArmorBoxes(light_blobs_real, armor_boxes, enemy_color)){ if (!findArmorBoxes(light_blobs, armor_boxes, enemy_color)) {
return false; return false;
} }
if(show_armor_boxes){ if (show_armor_boxes) {
showArmorBoxVector("boxes", src, armor_boxes); showArmorBoxVector("boxes", src, armor_boxes);
cv::waitKey(1); cv::waitKey(1);
} }
if(classifier && use_classifier){ if (classifier && use_classifier) {
for(auto box : armor_boxes){ for (auto box : armor_boxes) {
cv::Mat roi = src(box).clone(); cv::Mat roi = src(box).clone();
cv::resize(roi, roi, cv::Size(48, 36)); cv::resize(roi, roi, cv::Size(48, 36));
int c = classifier(roi); int c = classifier(roi);
if(c){ boxes_number[c].emplace_back(box);
boxes_number[c-1].emplace_back(box);
}
} }
if(enemy_color == ENEMY_BLUE) { if (enemy_color == ENEMY_BLUE) {
for(auto id : prior_blue){ for (auto name : prior_blue) {
if(!boxes_number[id].empty()){ if (!boxes_number[name2id[name]].empty()) {
armor_box = boxes_number[id][0]; armor_box = boxes_number[name2id[name]][0];
boxid = id; boxid = name2id[name];
break; break;
} }
} }
}else if(enemy_color == ENEMY_RED) { } else if (enemy_color == ENEMY_RED) {
for(auto id : prior_red){ for (auto name : prior_red) {
if(!boxes_number[id].empty()){ if (!boxes_number[name2id[name]].empty()) {
armor_box = boxes_number[id][0]; armor_box = boxes_number[name2id[name]][0];
boxid = id; boxid = name2id[name];
break; break;
} }
} }
}else{ } else {
LOGE("enemy_color ERROR!"); LOGE("enemy_color ERROR!");
} }
if(armor_box == cv::Rect2d(0,0,0,0)){ if (armor_box == cv::Rect2d(0, 0, 0, 0)) {
return false; return false;
} }
if(show_armor_boxes){ if (show_armor_boxes) {
showArmorBoxClass("class", src, boxes_number); 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]; 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)); cv::imwrite(filename, src(box));
} }
} }
} }
}else{ } else {
armor_box = armor_boxes[0]; armor_box = armor_boxes[0];
boxid = -1; boxid = -1;
} }
if(split.size() == cv::Size(320, 240)){ if (split.size() == cv::Size(320, 240)) {
armor_box.x *= 2; armor_box.x *= 2;
armor_box.y *= 2; armor_box.y *= 2;
armor_box.width *= 2; armor_box.width *= 2;

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

@@ -3,31 +3,11 @@
using namespace cv; 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) { void showArmorBoxVector(std::string windows_name, const cv::Mat &src, const std::vector<cv::Rect2d> &armor_box) {
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();
} }
@@ -37,26 +17,27 @@ void showArmorBoxVector(std::string windows_name, const cv::Mat &src, const std:
imshow(windows_name, image2show); 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; 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=0; i<14; i++){ for (int i = 0; i < 14; i++) {
if(!boxes[i].empty()){ if (!boxes[i].empty()) {
for(auto box : boxes[i]){ for (auto box : boxes[i]) {
rectangle(image2show, box, Scalar(0, 255, 0), 1); rectangle(image2show, box, Scalar(0, 255, 0), 1);
if(i == -1) if (i == -1)
putText(image2show, id2name[i], Point(box.x+2, box.y+2), cv::FONT_HERSHEY_TRIPLEX, 1, Scalar(0,255,0)); putText(image2show, id2name[i], Point(box.x + 2, box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
else if(0<=i && i<7) Scalar(0, 255, 0));
putText(image2show, id2name[i], Point(box.x+2, box.y+2), cv::FONT_HERSHEY_TRIPLEX, 1, Scalar(255,0,0)); else if (1 <= i && i < 8)
else if(7<=i && i<14) putText(image2show, id2name[i], Point(box.x + 2, box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
putText(image2show, id2name[i], Point(box.x+2, box.y+2), cv::FONT_HERSHEY_TRIPLEX, 1, Scalar(0,0,255)); Scalar(255, 0, 0));
else 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); 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) { void showArmorBox(std::string windows_name, const cv::Mat &src, cv::Rect2d armor_box, int boxid) {
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();
} }
rectangle(image2show, armor_box, Scalar(0, 255, 0), 1); rectangle(image2show, armor_box, Scalar(0, 255, 0), 1);
if(boxid == -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)); putText(image2show, id2name[boxid], Point(armor_box.x + 2, armor_box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
else if(0<=boxid && boxid<7) Scalar(0, 255, 0));
putText(image2show, id2name[boxid], Point(armor_box.x+2, armor_box.y+2), cv::FONT_HERSHEY_TRIPLEX, 1, Scalar(255,0,0)); else if (1 <= boxid && boxid < 8)
else if(7<=boxid && boxid<14) putText(image2show, id2name[boxid], Point(armor_box.x + 2, armor_box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
putText(image2show, id2name[boxid], Point(armor_box.x+2, armor_box.y+2), cv::FONT_HERSHEY_TRIPLEX, 1, Scalar(0,0,255)); Scalar(255, 0, 0));
else 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); LOGE_INFO("Invalid box id:%d!", boxid);
imshow(windows_name, image2show); 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) { void showContours(std::string windows_name, const cv::Mat &src, const std::vector<LightBlob> &light_blobs) {
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(const auto &light_blob:light_blobs) for (const auto &light_blob:light_blobs) {
{
Scalar color; Scalar color;
if(light_blob.BlobColor == BLOB_RED) if (light_blob.BlobColor == BLOB_RED)
color = Scalar(0,0,255); color = Scalar(0, 0, 255);
else if(light_blob.BlobColor == BLOB_BLUE) else if (light_blob.BlobColor == BLOB_BLUE)
color = Scalar(255,0,0); color = Scalar(255, 0, 0);
else else
color = Scalar(0,255,0); color = Scalar(0, 255, 0);
cv::Point2f vertices[4]; cv::Point2f vertices[4];
light_blob.rect.points(vertices); 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); cv::line(image2show, vertices[j], vertices[(j + 1) % 4], color, 2);
} }
} }
imshow(windows_name, image2show); 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);
}

10
main.cpp
View File

@@ -84,7 +84,7 @@ int main(int argc, char *argv[]) {
} }
} }
bool ok = true; bool ok = true;
cout<<"start running"<<endl; cout << "start running" << endl;
do { do {
CNT_TIME("Total", { CNT_TIME("Total", {
if (mcuData.state == BIG_ENERGY_STATE) {//大符模式 if (mcuData.state == BIG_ENERGY_STATE) {//大符模式
@@ -102,18 +102,16 @@ int main(int argc, char *argv[]) {
} }
energy.runBig(gimble_src, chassis_src);//击打大符 energy.runBig(gimble_src, chassis_src);//击打大符
last_state = mcuData.state;//更新上一帧状态 last_state = mcuData.state;//更新上一帧状态
} } else if (mcuData.state != BIG_ENERGY_STATE) {//自瞄或小符模式
else if (mcuData.state != BIG_ENERGY_STATE) {//自瞄或小符模式
last_state = mcuData.state; last_state = mcuData.state;
ok = checkReconnect(video_gimble->read(gimble_src)); ok = checkReconnect(video_gimble->read(gimble_src));
if (save_video) saveVideos(gimble_src); if (save_video) saveVideos(gimble_src);
if (show_origin) showOrigin(gimble_src); if (show_origin) showOrigin(gimble_src);
if (mcuData.state == ARMOR_STATE){ if (mcuData.state == ARMOR_STATE) {
CNT_TIME("Armor Time", { CNT_TIME("Armor Time", {
armorFinder.run(gimble_src); armorFinder.run(gimble_src);
}); });
} } else if (mcuData.state == SMALL_ENERGY_STATE) {
else if(mcuData.state == SMALL_ENERGY_STATE){
// energy.runSmall(gimble_src); // energy.runSmall(gimble_src);
energy.runBig(gimble_src); energy.runBig(gimble_src);
} }

3
others/include/options/options.h
View File

@@ -11,11 +11,10 @@ extern bool show_armor_box;
extern bool show_armor_boxes; extern bool show_armor_boxes;
extern bool show_light_blobs; extern bool show_light_blobs;
extern bool show_origin; extern bool show_origin;
extern bool save_labelled;
extern bool run_with_camera; extern bool run_with_camera;
extern bool save_video; extern bool save_video;
extern bool collect_data;
extern bool wait_uart; extern bool wait_uart;
extern bool save_labelled_boxes;
void process_options(int argc, char *argv[]); void process_options(int argc, char *argv[]);

15
others/src/options/options.cpp
View File

@@ -10,11 +10,10 @@ bool show_armor_box = false;
bool show_armor_boxes = false; bool show_armor_boxes = false;
bool show_light_blobs = false; bool show_light_blobs = false;
bool show_origin = false; bool show_origin = false;
bool save_labelled = false;
bool run_with_camera = false; bool run_with_camera = false;
bool save_video = false; bool save_video = false;
bool collect_data = false;
bool wait_uart = false; bool wait_uart = false;
bool save_labelled_boxes = false;
void process_options(int argc, char *argv[]){ void process_options(int argc, char *argv[]){
if(argc >= 2){ if(argc >= 2){
@@ -24,10 +23,9 @@ void process_options(int argc, char *argv[]){
LOGM("--show-armor-boxes: show the candidate aim boxes."); LOGM("--show-armor-boxes: show the candidate aim boxes.");
LOGM("--show-light-blobs: show the candidate light blobs."); LOGM("--show-light-blobs: show the candidate light blobs.");
LOGM("--show-origin: show the origin image."); LOGM("--show-origin: show the origin image.");
LOGM("--save-label: save the image when box found.");
LOGM("--run-with-camera: start the program with camera directly without asking."); LOGM("--run-with-camera: start the program with camera directly without asking.");
LOGM("--save-video: save the video."); LOGM("--save-video: save the video.");
LOGM("--collect-data: collect data sent from mcu."); LOGM("--save-labelled-boxes: save labelled armor boxes.");
}else if(strcmp(argv[i], "--show-armor-box") == 0){ }else if(strcmp(argv[i], "--show-armor-box") == 0){
show_armor_box = true; show_armor_box = true;
LOGM("Enable show armor box"); LOGM("Enable show armor box");
@@ -49,21 +47,18 @@ void process_options(int argc, char *argv[]){
LOGM("Enable show light blobs"); LOGM("Enable show light blobs");
show_origin = true; show_origin = true;
LOGM("Enable show origin"); LOGM("Enable show origin");
}else if(strcmp(argv[i], "--save-labeled") == 0){
save_labelled = true;
LOGM("Enable save labeled");
}else if(strcmp(argv[i], "--run-with-camera") == 0){ }else if(strcmp(argv[i], "--run-with-camera") == 0){
run_with_camera = true; run_with_camera = true;
LOGM("Run with camera!"); LOGM("Run with camera!");
}else if(strcmp(argv[i], "--save-video") == 0){ }else if(strcmp(argv[i], "--save-video") == 0){
save_video = true; save_video = true;
LOGM("Save video!"); LOGM("Save video!");
}else if(strcmp(argv[i], "--collect-data") == 0){
collect_data = true;
LOGM("Enable data collection!");
}else if(strcmp(argv[i], "--wait-uart") == 0){ }else if(strcmp(argv[i], "--wait-uart") == 0){
wait_uart = true; wait_uart = true;
LOGM("Wait uart until available!"); LOGM("Wait uart until available!");
}else if(strcmp(argv[i], "--save-labelled-boxes") == 0){
save_labelled_boxes = true;
LOGM("labelled armor boxes will be saved!");
}else{ }else{
LOGW("Unknown option: %s. Use --help to see options.", argv[i]); LOGW("Unknown option: %s. Use --help to see options.", argv[i]);
} }