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

Merge branch 'master' of https://github.com/lloi7/RM_auto-aim

Browse Source
This commit is contained in:
sun
2019-07-10 21:37:42 +08:00
parent 2e1538902a 4582e997f1
commit 9e8ebe7a32
12 changed files with 324 additions and 367 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -27,7 +27,7 @@ INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/armor/include)
INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/others/include)
FILE(GLOB_RECURSE sourcefiles "others/src/*.cpp" "energy/src/*cpp" "armor/src/*.cpp")
ADD_EXECUTABLE(${BIN_NAME} main.cpp ${sourcefiles} others/include/constants.h)
ADD_EXECUTABLE(${BIN_NAME} main.cpp ${sourcefiles} others/include/constants.h armor/src/armor_finder/searching_state/find_light_blobs.cpp armor/src/armor_finder/searching_state/find_armor_boxes.cpp)
TARGET_LINK_LIBRARIES(${BIN_NAME} ${CMAKE_THREAD_LIBS_INIT})
TARGET_LINK_LIBRARIES(${BIN_NAME} ${OpenCV_LIBS})

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

@@ -16,10 +16,27 @@
#define BLOB_RED ENEMY_RED
#define BLOB_BLUE ENEMY_BLUE
#define DISTANCE_HEIGHT_5MM (113.0) // 单位: m*pixel
#define DISTANCE_HEIGHT DISTANCE_HEIGHT_5MM
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; //灯条颜色
LightBlob(cv::RotatedRect &r) : rect(r) {
length = max(rect.size.height, rect.size.width);
};
};
typedef std::vector<LightBlob> LightBlobs;
/********************* 自瞄类定义 **********************/
class ArmorFinder{
public:
@@ -33,6 +50,7 @@ private:
SEARCHING_STATE, TRACKING_STATE, STANDBY_STATE
} State; // 自瞄状态枚举定义
cv::Mat src_raw; // 当前原图
const uint8_t &enemy_color; // 敌方颜色，引用外部变量，自动变化
State state; // 自瞄状态对象实例
cv::Rect2d armor_box; // 当前目标位置
@@ -48,21 +66,14 @@ private:
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);
public:
void run(cv::Mat &src); // 自瞄主函数
bool sendBoxPosition(); // 和主控板通讯
};
/******************* 灯条类定义 ***********************/
class LightBlob {
public:
cv::RotatedRect rect; //灯条位置
double length; //灯条长度
uint8_t BlobColor; //灯条颜色
LightBlob(cv::RotatedRect &r) : rect(r) {
length = max(rect.size.height, rect.size.width);
};
};
#endif /* _ARMOR_FINDER_H_ */

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

@@ -15,13 +15,13 @@
/* */
/*===========================================================================*/
//#define LOG_LEVEL LOG_NONE
#define LOG_LEVEL LOG_NONE
#include <log.h>
#include <options/options.h>
#include <show_images/show_images.h>
#include <opencv2/highgui.hpp>
#include <armor_finder/armor_finder.h>
#include <sys/time.h>
std::map<int, string> id2name = { //装甲板id到名称的map
{-1, "OO"},{ 0, "NO"},
@@ -46,9 +46,8 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder,
tracking_cnt(0) {
}
extern int box_distance;
void ArmorFinder::run(cv::Mat &src) {
src_raw = src;
cv::Mat src_use = src.clone(); // 实际参与计算的图像对象
if (show_armor_box) { // 根据条件显示当前目标装甲板
@@ -71,14 +70,14 @@ void ArmorFinder::run(cv::Mat &src) {
tracker->init(src_use, armor_box);
state = TRACKING_STATE;
tracking_cnt = 0;
// LOGM(STR_CTR(WORD_LIGHT_CYAN, "into track"));
LOGM(STR_CTR(WORD_LIGHT_CYAN, "into track"));
}
}
break;
case TRACKING_STATE:
if (!stateTrackingTarget(src_use) || ++tracking_cnt > 100) { // 最多追踪100帧图像
state = SEARCHING_STATE;
// LOGM(STR_CTR(WORD_LIGHT_YELLOW, "into search!"));
LOGM(STR_CTR(WORD_LIGHT_YELLOW, "into search!"));
}
break;
case STANDBY_STATE:
@@ -117,9 +116,6 @@ bool sendTarget(Serial &serial, double x, double y, double z) {
return serial.WriteData(buff, sizeof(buff));
}
#define DISTANCE_HEIGHT_5MM (113.0) // 单位: m*pixel
#define DISTANCE_HEIGHT DISTANCE_HEIGHT_5MM
bool ArmorFinder::sendBoxPosition() {
auto rect = armor_box;
double dx = rect.x + rect.width / 2 - 320;

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

@@ -0,0 +1,105 @@
//
// 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 Normal file
View File

@@ -0,0 +1,133 @@
//
// 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;
}
/// Todo: 下面的函数可以有性能优化，暂时未做。
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;
}
///Todo: 性能优化后的函数。(暂时还有点问题)
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>(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)) {
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)) {
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;
}

65
armor/src/armor_finder/searching_state/image_process.cpp
View File

@@ -1,65 +0,0 @@
//
// Created by xinyang on 19-3-27.
//
#include "image_process.h"
static void splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red) {
uchar* data;
uchar* bayer_data[2];
for (int i = 0; i < src.rows; ++i) {
data = src.ptr<uchar>(i);
bayer_data[0] = blue.ptr<uchar>(i / 2);
for (int j = 0; j < blue.cols; ++j, data += 2) {
bayer_data[0][j] = *data;
}
data = src.ptr<uchar>(++i) + 1;
bayer_data[1] = red.ptr<uchar>(i / 2);
for (int j = 0; j < red.cols; ++j, data += 2) {
bayer_data[1][j] = *data;
}
}
}
void imageColorSplit(cv::Mat &src_input, cv::Mat &split, uint8_t color) {
cv::Mat blue(240, 320, CV_8UC1), red(240, 320, CV_8UC1);
if(src_input.type() == CV_8UC1){
splitBayerBG(src_input, blue, red);
if(color == ENEMY_RED){
split = red - blue;
}else if(color == ENEMY_BLUE){
split = blue - red;
}
}else if(src_input.type() == CV_8UC3){
std::vector<cv::Mat> channels;
cv::split(src_input, channels);
resize(channels.at(0), blue, cv::Size(640, 480));
resize(channels.at(2), red, cv::Size(640, 480));
if(color == ENEMY_RED){
split = red;// - blue;
}else if(color == ENEMY_BLUE){
split = blue;// - red;
}
}
}
void imagePreProcess(cv::Mat &src) {
// cv::medianBlur(src, src, 5);
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);
}

14
armor/src/armor_finder/searching_state/image_process.h
View File

@@ -1,14 +0,0 @@
//
// Created by xinyang on 19-3-27.
//
#ifndef _IMAGE_PROCESS_H_
#define _IMAGE_PROCESS_H_
#include <opencv2/core.hpp>
#include <armor_finder/armor_finder.h>
void imageColorSplit(cv::Mat &src_input, cv::Mat &split, uint8_t color);
void imagePreProcess(cv::Mat &src);
#endif /* _IMAGE_PROCESS_H_ */

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

@@ -7,242 +7,7 @@
#include <show_images/show_images.h>
#include <options/options.h>
#include <log.h>
#include "image_process.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;
}
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;
}
/// Todo: 下面的函数可以有性能优化，暂时未做。
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;
}
///Todo: 性能优化后的函数。(暂时还有点问题)
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>(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)) {
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)) {
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);
}
static bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
static cv::Mat src_gray;
if (src.type() == CV_8UC3) {
cvtColor(src, src_gray, CV_BGR2GRAY);
} else if (src.type() == CV_8UC1) {
src_gray = src.clone();
}
std::vector<std::vector<cv::Point> > light_contours;
cv::findContours(src_gray, 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_gray, rect)) {
light_blobs.emplace_back(rect);
}
}
return light_blobs.size() >= 2;
}
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;
}
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;
}
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);
}
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);
}
/* 判断两个灯条的错位度，不知道英文是什么！！！ */
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;
}
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);
}
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);
}
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) {
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;
}
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;
}
}
}
static string prior_blue[] = {
"B8", "B1", "B3", "B4", "B5", "B7", "B2",
@@ -250,10 +15,28 @@ static string prior_blue[] = {
};
static string prior_red[] = {
"B8", "B1", "B3", "B4", "B5", "B7", "B2",
"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; // 二值图和颜色通道图
@@ -274,24 +57,19 @@ bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
if (!findLightBlobs(src_bin, light_blobs)) {
return false;
}
if (show_light_blobs) {
showContours("blobs_gray", src_bin, light_blobs);
cv::waitKey(1);
}
get_blob_color(src, light_blobs);
if (show_light_blobs) {
showContours("light_blobs", src, light_blobs);
cv::waitKey(1);
}
if (!findArmorBoxes(light_blobs, armor_boxes, enemy_color)) {
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();
@@ -316,7 +94,7 @@ bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
}
}
} else {
LOGE("enemy_color ERROR!");
LOGE_INFO("enemy_color ERROR!");
}
if (armor_box == cv::Rect2d(0, 0, 0, 0)) {
return false;

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

@@ -53,7 +53,7 @@ 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;
@@ -63,14 +63,16 @@ void showArmorBox(std::string windows_name, const cv::Mat &src, cv::Rect2d armor
image2show = src.clone();
}
rectangle(image2show, armor_box, Scalar(0, 255, 0), 1);
char dist[5];
// sprintf(dist, "%.1f", distance);
if (boxid == -1)
putText(image2show, id2name[boxid], Point(armor_box.x + 2, armor_box.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
putText(image2show, id2name[boxid]+" "+dist, 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,
putText(image2show, id2name[boxid]+" "+dist, 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,
putText(image2show, id2name[boxid]+" "+dist, 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);

29
main.cpp
View File

@@ -27,7 +27,7 @@ using namespace std;
mcu_data mcuData = { // 单片机端回传结构体
0, // 当前云台yaw角
0, // 当前云台pitch角
BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符
ARMOR_STATE, // 当前状态，自瞄-大符-小符
0, // 云台角度标记位
1, // 是否启用数字识别
ENEMY_RED, // 敌方颜色
@@ -94,28 +94,37 @@ int main(int argc, char *argv[]) {
do {
CNT_TIME("Total", {
if (mcuData.state == BIG_ENERGY_STATE) {//大符模式
if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大符模式，即刚往完成切换，则需要初始化
energy.setEnergyRotationInit();
((CameraWrapper*)video_gimble)->changeBrightness(20);
cout << "set" << endl;
}
ok = checkReconnect(video_gimble->read(gimble_src), video_chassis->read(chassis_src));//检查有几个摄像头
if (save_video) saveVideos(gimble_src, chassis_src);//保存视频
if (show_origin) showOrigin(gimble_src, chassis_src);//显示原始图像
if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大符模式，即刚往完成切换，则需要初始化
energy.setEnergyRotationInit();
cout << "set" << endl;
}
energy.runBig(gimble_src, chassis_src);//击打大符
// energy.runBig(gimble_src);
last_state = mcuData.state;//更新上一帧状态
} else if (mcuData.state != BIG_ENERGY_STATE) {//自瞄或小符模式
} else if (mcuData.state == SMALL_ENERGY_STATE) {// 小符模式
if(last_state == ARMOR_STATE){
((CameraWrapper*)video_gimble)->changeBrightness(20);
}
last_state = mcuData.state;
ok = checkReconnect(video_gimble->read(gimble_src));
if (save_video) saveVideos(gimble_src);
if (show_origin) showOrigin(gimble_src);
energy.runSmall(gimble_src);
} else { // 自瞄模式
if(last_state != ARMOR_STATE){
((CameraWrapper*)video_gimble)->changeBrightness(40);
}
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) {
CNT_TIME("Armor Time", {
armorFinder.run(gimble_src);
});
} else if (mcuData.state == SMALL_ENERGY_STATE) {
energy.runSmall(gimble_src);
}
}
// cv::waitKey(0);
});

3
others/include/camera/camera_wrapper.h
View File

@@ -47,8 +47,7 @@ public:
bool read(cv::Mat& src) final;
bool readRaw(cv::Mat& src);
bool readProcessed(cv::Mat& src);
bool changeBrightness(int brightness);
};

3
others/src/camera/camera_wrapper.cpp
View File

@@ -109,6 +109,9 @@ bool CameraWrapper::init() {
return true;
}
bool CameraWrapper::changeBrightness(int brightness) {
CameraSetAnalogGain(h_camera, brightness);
}
bool CameraWrapper::read(cv::Mat& src) {
if(mode==0)return readProcessed(src);