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

反陀螺v1.0完成。

Browse Source
This commit is contained in:
xinyang
2019-07-18 11:54:36 +08:00
parent d9bfdbb2eb
commit 6a0020ce81
11 changed files with 164 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -2,7 +2,7 @@
PROJECT(SJTU-RM-CV)
SET(CMAKE_CXX_STANDARD 11)
SET(CMAKE_BUILD_TYPE RELEASE)
SET(CMAKE_BUILD_TYPE DEBUG)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DPATH=\"\\\"${PROJECT_SOURCE_DIR}\\\"\"")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D${CMAKE_SYSTEM_NAME}")

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

@@ -6,6 +6,7 @@
#define _ARMOR_FINDER_H_
#include <map>
#include <sys/time.h>
#include <constants.h>
#include <opencv2/core.hpp>
#include <opencv2/tracking.hpp>
@@ -22,6 +23,21 @@
#define DISTANCE_HEIGHT_5MM (113.0) // 单位: m*pixel
#define DISTANCE_HEIGHT DISTANCE_HEIGHT_5MM
#define B1 1
#define B2 2
#define B3 3
#define B4 4
#define B5 5
#define B7 6
#define B8 7
#define R1 8
#define R2 9
#define R3 10
#define R4 11
#define R5 12
#define R7 13
#define R8 14
extern std::map<int, string> id2name; //装甲板id到名称的map
extern std::map<string, int> name2id; //装甲板名称到id的map
@@ -45,6 +61,10 @@ typedef std::vector<LightBlob> LightBlobs;
/******************* 装甲板类定义　**********************/
class ArmorBox{
public:
typedef enum{
FRONT, SIDE, UNKNOWN
} BoxOrientation;
cv::Rect2d rect;
LightBlobs light_blobs;
uint8_t box_color;
@@ -55,6 +75,8 @@ public:
double blobsDistance() const;
double lengthRatio() const;
double lengthDistanceRatio() const;
double getDistance() const;
BoxOrientation getOrientation() const;
};
typedef std::vector<ArmorBox> ArmorBoxes;
@@ -72,6 +94,10 @@ private:
SEARCHING_STATE, TRACKING_STATE, STANDBY_STATE
} State; // 自瞄状态枚举定义
typedef enum{
NORMAL, ANTI_TOP
} AntiTopState;
const uint8_t &enemy_color; // 敌方颜色，引用外部变量，自动变化
State state; // 自瞄状态对象实例
ArmorBox armor_box; // 当前目标装甲板
@@ -81,6 +107,10 @@ private:
int tracking_cnt; // 记录追踪帧数，用于定时退出追踪
Serial &serial; // 串口对象，引用外部变量，用于和能量机关共享同一个变量
const uint8_t &use_classifier; // 标记是否启用CNN分类器，引用外部变量，自动变化
ArmorBox::BoxOrientation last_orient; // 上一帧目标装甲板方向，用于反陀螺
timeval last_switch_time; // 上一次发生装甲板方向切换的时间
int anti_top_cnt; // 满足条件的装甲板方向切换持续次数，用于反陀螺
AntiTopState anti_top_state; // 当前是否识别到陀螺
bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs);
bool findArmorBox(const cv::Mat &src, ArmorBox &box);
@@ -89,7 +119,7 @@ private:
bool stateTrackingTarget(cv::Mat &src); // tracking state主函数
bool stateStandBy(); // stand by state主函数（已弃用）
void anti_top(); // 反小陀螺
void antiTop(); // 反小陀螺
public:
void run(cv::Mat &src); // 自瞄主函数

95
armor/src/armor_finder/anti_top/anti_top.cpp
View File

@@ -4,49 +4,56 @@
#include <armor_finder/armor_finder.h>
#define B1 1
#define B2 2
#define B3 3
#define B4 4
#define B5 5
#define B7 6
#define B8 7
#define R1 8
#define R2 9
#define R3 10
#define R4 11
#define R5 12
#define R7 13
#define R8 14
void ArmorFinder::anti_top() {
// switch (armor_box.id) {
// case R1:
// case R7:
// case B1:
// case B7:
// if (armor_box.rect != cv::Rect2d()) {
// sendBoxPosition();
// }
// case R2:
// case R3:
// case R4:
// case R5:
// case B2:
// case B3:
// case B4:
// case B5:
// if (armor_box.rect != cv::Rect2d()) {
// sendBoxPosition();
// }
// default:
// if (armor_box.rect != cv::Rect2d()) {
// sendBoxPosition();
// }
// }
if (armor_box.rect != cv::Rect2d()) {
sendBoxPosition();
}
static double getTimeIntervalms(const timeval& now, const timeval &last){
return (now.tv_sec-last.tv_sec)*1000.0 + (now.tv_usec-last.tv_usec)/1000.0;
}
void ArmorFinder::antiTop() {
if(anti_top_state == ANTI_TOP){
cout << "anti top" << endl;
}else if(anti_top_state == NORMAL){
cout << "Normal" << endl;
}
ArmorBox::BoxOrientation orientation = armor_box.getOrientation();
if(orientation == ArmorBox::UNKNOWN){
if(anti_top_state == NORMAL){
sendBoxPosition();
return;
}else{
return;
}
}
if(orientation!=last_orient){
timeval curr_time;
gettimeofday(&curr_time, nullptr);
auto interval = getTimeIntervalms(curr_time, last_switch_time);
cout << interval << endl;
if(50 < interval && interval < 700){
anti_top_cnt++;
}else{
anti_top_cnt = 0;
}
last_switch_time = curr_time;
}else{
timeval curr_time;
gettimeofday(&curr_time, nullptr);
if(getTimeIntervalms(curr_time, last_switch_time) > 700) {
anti_top_state = NORMAL;
}
}
if(anti_top_cnt > 4){
anti_top_state = ANTI_TOP;
}
if(anti_top_state == ANTI_TOP){
if(orientation == ArmorBox::FRONT){
sendBoxPosition();
}
}else if(anti_top_state == NORMAL){
sendBoxPosition();
}
last_orient = orientation;
}

41
armor/src/armor_finder/armor_box/armor_box.cpp
View File

@@ -36,3 +36,44 @@ double ArmorBox::lengthDistanceRatio() const {
return 100;
}
}
double ArmorBox::getDistance() const{
if(light_blobs.size() == 2 ){
return DISTANCE_HEIGHT / 2 / max(light_blobs[0].length, light_blobs[1].length);
} else {
return DISTANCE_HEIGHT / rect.height;
}
}
ArmorBox::BoxOrientation ArmorBox::getOrientation() const{
// cout << lengthDistanceRatio() << endl;
if(light_blobs.size() != 2){
return UNKNOWN;
}
switch (id) {
case R1:
case R7:
case B1:
case B7:
if(lengthDistanceRatio() < 0.3){
return FRONT;
}else{
return SIDE;
}
case R2:
case R3:
case R4:
case R5:
case B2:
case B3:
case B4:
case B5:
if (lengthDistanceRatio() < 0.43) {
return FRONT;
}else{
return SIDE;
}
default:
return UNKNOWN;
}
}

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

@@ -78,7 +78,7 @@ void ArmorFinder::run(cv::Mat &src) {
stateStandBy();
}
anti_top();
antiTop();
if (show_armor_box) { // 根据条件显示当前目标装甲板
showArmorBox("box", src, armor_box);

11
armor/src/armor_finder/find/find_armor_box.cpp
View File

@@ -72,20 +72,11 @@ static bool boxAngleJudge(const LightBlob &light_blob_i, const LightBlob &light_
}
static bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_blob_j, uint8_t enemy_color) {
// cout << (light_blob_i.blob_color==BLOB_RED) << endl;
// cout << (light_blob_j.blob_color==BLOB_RED) << endl;
// cout << lengthRatioJudge(light_blob_i, light_blob_j) << endl;
// cout << lengthJudge(light_blob_i, light_blob_j) << endl;
//// cout << heightJudge(light_blob_i, light_blob_j) << endl;
// cout << angelJudge(light_blob_i, light_blob_j) << endl;
// cout << boxAngleJudge(light_blob_i, light_blob_j) << endl;
// cout << CuoWeiDuJudge(light_blob_i, light_blob_j) << endl;
// cout << "=============" << endl;
return light_blob_i.blob_color == enemy_color &&
light_blob_j.blob_color == enemy_color &&
lengthRatioJudge(light_blob_i, light_blob_j) &&
lengthJudge(light_blob_i, light_blob_j) &&
// heightJudge(light_blob_i, light_blob_j) &&
// 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);

2
armor/src/armor_finder/find/find_light_blobs.cpp
View File

@@ -121,7 +121,7 @@ bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
color_channel = channels[0]; /* 根据目标颜色进行通道提取 */
else if (enemy_color == ENEMY_RED) /* */
color_channel = channels[2]; /************************/
cv::threshold(color_channel, src_bin, 160, 255, CV_THRESH_BINARY); // 二值化对应通道
cv::threshold(color_channel, src_bin, 170, 255, CV_THRESH_BINARY); // 二值化对应通道
imagePreProcess(src_bin); // 开闭运算
if(src_bin.size() == cv::Size(640, 480))

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

@@ -95,14 +95,41 @@ void showArmorBox(std::string windows_name, const cv::Mat &src, const ArmorBox &
// if(box.light_blobs.size() == 2)
// fprintf(fp, "%lf %lf %lf\n", box.light_blobs[0].length, box.light_blobs[1].length, box.blobsDistance())
// cout << box.lengthDistanceRatio() << endl;
if(box.lengthDistanceRatio() < 0.4 && box.lengthRatio() > 0.9){
switch (box.id) {
case R1:
case R7:
case B1:
case B7:
if(box.lengthDistanceRatio() < 0.3){
rectangle(image2show, box.rect, Scalar(0, 255, 0), 3);
}else{
rectangle(image2show, box.rect, Scalar(0, 255, 0), 1);
};
}
case R2:
case R3:
case R4:
case R5:
case B2:
case B3:
case B4:
case B5:
if (box.lengthDistanceRatio() < 0.42) {
rectangle(image2show, box.rect, Scalar(0, 255, 0), 3);
}else{
rectangle(image2show, box.rect, Scalar(0, 255, 0), 1);
}
break;
default:
rectangle(image2show, box.rect, Scalar(0, 255, 0), 1);
}
// if((id2name.at(box.id)=="B1"||id2name.at(box.id)=="B7") && box.lengthDistanceRatio() < 0.3/* && box.lengthRatio() > 0.9*/){
// rectangle(image2show, box.rect, Scalar(0, 255, 0), 3);
// }else{
// rectangle(image2show, box.rect, Scalar(0, 255, 0), 1);
// };
char dist[5];
// sprintf(dist, "%.1f", distance);
sprintf(dist, "%.1f", box.getDistance());
if (box.id == -1)
putText(image2show, id2name[box.id]+" "+dist, Point(box.rect.x + 2, box.rect.y + 2), cv::FONT_HERSHEY_TRIPLEX, 1,
Scalar(0, 255, 0));

2
main.cpp
View File

@@ -30,7 +30,7 @@ using namespace std;
mcu_data mcuData = { // 单片机端回传结构体
0, // 当前云台yaw角
0, // 当前云台pitch角
BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符
ARMOR_STATE, // 当前状态，自瞄-大符-小符
0, // 云台角度标记位
1, // 是否启用数字识别
ENEMY_RED, // 敌方颜色

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

@@ -48,6 +48,7 @@ public:
bool readRaw(cv::Mat& src);
bool readProcessed(cv::Mat& src);
bool changeBrightness(int brightness);
// bool once
};

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

@@ -78,10 +78,12 @@ bool CameraWrapper::init() {
CameraSetAeState(h_camera, false);
CameraSetExposureTime(h_camera, CAMERA_EXPOSURE*1000);
CameraSetAnalogGain(h_camera, ARMOR_CAMERA_GAIN);
CameraSetWbMode(h_camera, false);
if(mode == 0){
CameraSetGain(h_camera, CAMERA_BLUE_GAIN, CAMERA_GREEN_GAIN, CAMERA_RED_GAIN);
CameraSetLutMode(h_camera, LUTMODE_PRESET);
}
CameraSetOnceWB(h_camera);
#endif
double t;
CameraGetExposureTime(h_camera, &t);