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

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

@@ -9,13 +9,10 @@
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#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 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 showCuoWeiDu(const cv::Mat &src, const std::vector<LightBlob> &light_blobs);
#endif /* _SHOW_IMAGES_H_ */

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);
}

10
main.cpp
View File

@@ -84,7 +84,7 @@ int main(int argc, char *argv[]) {
}
}
bool ok = true;
cout<<"start running"<<endl;
cout << "start running" << endl;
do {
CNT_TIME("Total", {
if (mcuData.state == BIG_ENERGY_STATE) {//大符模式
@@ -102,18 +102,16 @@ int main(int argc, char *argv[]) {
}
energy.runBig(gimble_src, chassis_src);//击打大符
last_state = mcuData.state;//更新上一帧状态
}
else if (mcuData.state != BIG_ENERGY_STATE) {//自瞄或小符模式
} else if (mcuData.state != BIG_ENERGY_STATE) {//自瞄或小符模式
last_state = mcuData.state;
ok = checkReconnect(video_gimble->read(gimble_src));
if (save_video) saveVideos(gimble_src);
if (show_origin) showOrigin(gimble_src);
if (mcuData.state == ARMOR_STATE){
if (mcuData.state == ARMOR_STATE) {
CNT_TIME("Armor Time", {
armorFinder.run(gimble_src);
});
}
else if(mcuData.state == SMALL_ENERGY_STATE){
} else if (mcuData.state == SMALL_ENERGY_STATE) {
// energy.runSmall(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_light_blobs;
extern bool show_origin;
extern bool save_labelled;
extern bool run_with_camera;
extern bool save_video;
extern bool collect_data;
extern bool wait_uart;
extern bool save_labelled_boxes;
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_light_blobs = false;
bool show_origin = false;
bool save_labelled = false;
bool run_with_camera = false;
bool save_video = false;
bool collect_data = false;
bool wait_uart = false;
bool save_labelled_boxes = false;
void process_options(int argc, char *argv[]){
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-light-blobs: show the candidate light blobs.");
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("--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){
show_armor_box = true;
LOGM("Enable show armor box");
@@ -49,21 +47,18 @@ void process_options(int argc, char *argv[]){
LOGM("Enable show light blobs");
show_origin = true;
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){
run_with_camera = true;
LOGM("Run with camera!");
}else if(strcmp(argv[i], "--save-video") == 0){
save_video = true;
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){
wait_uart = true;
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{
LOGW("Unknown option: %s. Use --help to see options.", argv[i]);
}