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)){

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

@@ -4,7 +4,7 @@
#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>
@@ -17,8 +17,7 @@ static double lw_rate(const cv::RotatedRect &rect){
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);
@@ -61,19 +60,6 @@ static double nonZeroRateOfRotateRect_opt(const cv::Mat &bin, const cv::RotatedR
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;
@@ -115,13 +101,6 @@ static bool isValidLightBlob(const cv::Mat &bin, const cv::RotatedRect &rect){
(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) {
@@ -148,6 +127,7 @@ bool angelJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
light_blob_j.rect.angle - 90;
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;
@@ -176,7 +156,8 @@ bool CuoWeiDuJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j)
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;
}
@@ -222,7 +203,8 @@ static bool findArmorBoxes(LightBlobs &light_blobs, std::vector<cv::Rect2d> &arm
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;
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;
}
@@ -238,21 +220,6 @@ static bool findArmorBoxes(LightBlobs &light_blobs, std::vector<cv::Rect2d> &arm
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) {
auto region = blob.rect.boundingRect();
@@ -277,23 +244,31 @@ void get_blob_color(const cv::Mat &src, std::vector<LightBlob> &blobs) {
}
}
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];
std::vector<cv::Mat> channels;
armor_box = cv::Rect2d(0, 0, 0, 0);
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);
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)) {
return false;
}
@@ -302,31 +277,13 @@ bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
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);
get_blob_color(src, light_blobs);
if (show_light_blobs) {
showContours("light_blobs", src, light_blobs_real);
// showCuoWeiDu(src, light_blobs_real);
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) {
@@ -338,23 +295,21 @@ bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
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;
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;
for (auto name : prior_red) {
if (!boxes_number[name2id[name]].empty()) {
armor_box = boxes_number[name2id[name]][0];
boxid = name2id[name];
break;
}
}
@@ -366,10 +321,13 @@ bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
}
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());
sprintf(filename, PROJECT_DIR"/armor_box_photo/%s_%d.jpg", id2name[i].data(),
time(nullptr) + clock());
cv::imwrite(filename, src(box));
}
}

107
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();
}
@@ -39,11 +19,9 @@ 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]) {
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++) {
@@ -51,12 +29,15 @@ void showArmorBoxClass(std::string window_names, const cv::Mat &src, vector<cv::
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
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
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,17 +70,13 @@ 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);
@@ -114,34 +92,3 @@ void showContours(std::string windows_name, const cv::Mat &src, const std::vecto
}
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);
}

6
main.cpp
View File

@@ -102,8 +102,7 @@ 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);
@@ -112,8 +111,7 @@ int main(int argc, char *argv[]) {
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]);
}