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

Merge remote-tracking branch 'origin/master'

Browse Source 
# Conflicts:
#	armor/src/armor_finder/anti_top/anti_top.cpp
#	energy/src/energy/find/energy_finder.cpp
This commit is contained in:
xinyang
2019-08-05 01:16:30 +08:00
parent cbadab7f83 125c8d36c9
commit 63e326f883
34 changed files with 18041 additions and 18043 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -79,11 +79,11 @@ public:
explicit ArmorBox(const cv::Rect &pos=cv::Rect2d(), const LightBlobs &blobs=LightBlobs(), uint8_t color=0, int i=0);
cv::Point2f getCenter() const;
double getBlobsDistance() const;
double lengthDistanceRatio() const;
double getBoxDistance() const;
BoxOrientation getOrientation() const;
// double
bool operator<(const ArmorBox &box) const;
};
@@ -107,24 +107,19 @@ private:
NORMAL, ANTI_TOP
} AntiTopState;
typedef enum{
INCREASE, DECREASE, NOCHANGE
} BoxRatioChangeType;
systime frame_time; // 当前帧对应时间;
const uint8_t &enemy_color; // 敌方颜色，引用外部变量，自动变化
State state; // 自瞄状态对象实例
ArmorBox armor_box; // 当前目标装甲板
ArmorBox target_box, last_box; // 目标装甲板
int anti_switch_cnt; // 防止乱切目标计数器
cv::Ptr<cv::Tracker> tracker; // tracker对象实例
Classifier classifier; // CNN分类器对象实例，用于数字识别
int contour_area; // 装甲区域亮点个数，用于数字识别未启用时判断是否跟丢（已弃用）
int contour_area; // 装甲区域亮点个数，用于数字识别未启用时判断是否跟丢（已弃用）
int tracking_cnt; // 记录追踪帧数，用于定时退出追踪
Serial &serial; // 串口对象，引用外部变量，用于和能量机关共享同一个变量
const uint8_t &use_classifier; // 标记是否启用CNN分类器，引用外部变量，自动变化
RoundQueue<double, 4> top_periodms;
RoundQueue<double, 5> box_ratioes;
RoundQueue<double, 4> top_periodms; // 陀螺周期循环队列
systime last_front_time; // 上一次发生装甲板方向切换的时间
BoxRatioChangeType last_ratio_type;
int anti_top_cnt; // 满足条件的装甲板方向切换持续次数，用于反陀螺
AntiTopState anti_top_state; // 当前是否识别到陀螺
@@ -135,12 +130,11 @@ private:
bool stateTrackingTarget(cv::Mat &src); // tracking state主函数
bool stateStandBy(); // stand by state主函数（已弃用）
void antiTop(); // 反小陀螺
BoxRatioChangeType getRatioChangeType(RoundQueue<double, 5> &vec);
void antiTop(); // 反小陀螺
bool sendBoxPosition(uint16_t shoot); // 和主控板通讯
public:
void run(cv::Mat &src); // 自瞄主函数
bool sendBoxPosition(uint16_t shoot); // 和主控板通讯
};
#endif /* _ARMOR_FINDER_H_ */

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

@@ -6,13 +6,6 @@
#include <additions.h>
#include <log.h>
static double boxDistance(const cv::Rect2d &a, const cv::Rect2d &b) {
cv::Point2d centerA(a.x + a.width / 2, a.y + a.height / 2);
cv::Point2d centerB(b.x + b.width / 2, b.y + b.height / 2);
auto dist = centerA - centerB;
return sqrt(dist.x * dist.x + dist.y * dist.y);
}
template<int length>
static double mean(RoundQueue<double, length> &vec) {
double sum = 0;
@@ -22,75 +15,16 @@ static double mean(RoundQueue<double, length> &vec) {
return sum / length;
}
ArmorFinder::BoxRatioChangeType ArmorFinder::getRatioChangeType(RoundQueue<double, 5> &vec) {
auto d = (vec[0] - vec[1] + vec[3] + vec[4]);
if (d > 0.15) {
return INCREASE;
} else if (d < -0.15) {
return DECREASE;
} else {
return NOCHANGE;
}
}
/*
void ArmorFinder::antiTop() {
if (target_box.rect == cv::Rect2d()) return;
uint16_t shoot_delay = 0;
auto interval = getTimeIntervalms(frame_time, last_front_time);
box_ratioes.push(target_box.rect.width / target_box.rect.height);
auto change_type = getRatioChangeType(box_ratioes);
auto orientation = target_box.getOrientation();
if (interval > 700) {
anti_top_cnt = 0;
if (anti_top_state == ANTI_TOP) {
anti_top_state = NORMAL;
LOGM(STR_CTR(WORD_YELLOW, "switch to normal"));
}
}
if (change_type == INCREASE && last_ratio_type != change_type) {
last_front_time = frame_time;
if (150 < interval && interval < 700) {
if (anti_top_state == ANTI_TOP) {
top_periodms.push(interval);
LOGM(STR_CTR(WORD_LIGHT_GREEN, "top period: %.1lf ms"), interval);
systime curr_time;
getsystime(curr_time);
auto calculate_time = getTimeIntervalms(curr_time, frame_time);
shoot_delay = mean(top_periodms) - calculate_time;
} else if (anti_top_state == NORMAL) {
if (++anti_top_cnt > 4) {
anti_top_state = ANTI_TOP;
LOGM(STR_CTR(WORD_CYAN, "switch to anti-top"));
}
}
}
}
if (change_type != NOCHANGE) {
last_ratio_type = change_type;
}
if (anti_top_state == ANTI_TOP) {
if (orientation == ArmorBox::FRONT) {
sendBoxPosition(shoot_delay);
}
} else if (anti_top_state == NORMAL) {
sendBoxPosition(shoot_delay);
}
}
*/
void ArmorFinder::antiTop() {
if (target_box.rect == cv::Rect2d()) return;
uint16_t shoot_delay = 0;
auto interval = getTimeIntervalms(frame_time, last_front_time);
if (anti_top_state == ANTI_TOP && interval > 500) {
if (anti_top_state == ANTI_TOP && interval > 700) {
anti_top_state = NORMAL;
LOGM(STR_CTR(WORD_YELLOW, "switch to normal"));
}
if (last_box.rect != cv::Rect2d() &&
getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.0) {
LOGM("switch! %lf", getPointLength(last_box.getCenter() - target_box.getCenter()) / last_box.rect.height);
if (150 < interval && interval < 500) {
if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.5) {
if (150 < interval && interval < 700) {
if (anti_top_state == ANTI_TOP) {
top_periodms.push(interval);
LOGM(STR_CTR(WORD_LIGHT_GREEN, "top period: %.1lf ms"), interval);
@@ -110,8 +44,8 @@ void ArmorFinder::antiTop() {
}
if (anti_top_state == NORMAL) {
sendBoxPosition(0);
} else if (interval < top_periodms[-1] * 0.10) {
sendBoxPosition(0);
} else if (interval < top_periodms[-1] * 0.1){
sendBoxPosition(shoot_delay);
}
last_box = target_box;
}

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

@@ -8,6 +8,15 @@
ArmorBox::ArmorBox(const cv::Rect &pos, const LightBlobs &blobs, uint8_t color, int i) :
rect(pos), light_blobs(blobs), box_color(color), id(i) {};
cv::Point2f ArmorBox::getCenter() const {
return cv::Point2f(
rect.x + rect.width / 2,
rect.y + rect.height / 2
);
}
double ArmorBox::getBlobsDistance() const {
if (light_blobs.size() == 2) {
auto &x = light_blobs[0].rect.center;
@@ -78,10 +87,10 @@ bool ArmorBox::operator<(const ArmorBox &box) const {
return prior_red[id2name[id]] < prior_red[id2name[box.id]];
}
} else {
auto d1 = (rect.x-IMAGE_CENTER_X)*(rect.x-IMAGE_CENTER_X)
+ (rect.y-IMAGE_CENTER_Y)*(rect.y-IMAGE_CENTER_Y);
auto d2 = (box.rect.x-IMAGE_CENTER_X)*(box.rect.x-IMAGE_CENTER_X)
+ (box.rect.y-IMAGE_CENTER_Y)*(box.rect.y-IMAGE_CENTER_Y);
auto d1 = (rect.x - IMAGE_CENTER_X) * (rect.x - IMAGE_CENTER_X)
+ (rect.y - IMAGE_CENTER_Y) * (rect.y - IMAGE_CENTER_Y);
auto d2 = (box.rect.x - IMAGE_CENTER_X) * (box.rect.x - IMAGE_CENTER_X)
+ (box.rect.y - IMAGE_CENTER_Y) * (box.rect.y - IMAGE_CENTER_Y);
return d1 < d2;
}
}

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

@@ -50,6 +50,9 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder,
serial(u),
enemy_color(color),
state(STANDBY_STATE),
anti_top_cnt(0),
anti_switch_cnt(0),
anti_top_state(NORMAL),
classifier(paras_folder),
contour_area(0),
use_classifier(use),
@@ -63,15 +66,15 @@ void ArmorFinder::run(cv::Mat &src) {
switch (state) {
case SEARCHING_STATE:
if (stateSearchingTarget(src)) {
if ((armor_box.rect & cv::Rect2d(0, 0, 640, 480)) == armor_box.rect) { // 判断装甲板区域是否脱离图像区域
if ((target_box.rect & cv::Rect2d(0, 0, 640, 480)) == target_box.rect) { // 判断装甲板区域是否脱离图像区域
if (!classifier || !use_classifier) { /* 如果分类器不可用或者不使用分类器 */
cv::Mat roi = src(armor_box.rect).clone(), roi_gray; /* 就使用装甲区域亮点数判断是否跟丢 */
cv::Mat roi = src(target_box.rect).clone(), roi_gray; /* 就使用装甲区域亮点数判断是否跟丢 */
cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
contour_area = cv::countNonZero(roi_gray);
}
tracker = TrackerToUse::create(); // 成功搜寻到装甲板，创建tracker对象
tracker->init(src, armor_box.rect);
tracker->init(src, target_box.rect);
state = TRACKING_STATE;
tracking_cnt = 0;
LOGM(STR_CTR(WORD_LIGHT_CYAN, "into track"));
@@ -92,7 +95,7 @@ end:
antiTop();
if (show_armor_box) { // 根据条件显示当前目标装甲板
showArmorBox("box", src, armor_box);
showArmorBox("box", src, target_box);
cv::waitKey(1);
}
}

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

@@ -147,9 +147,18 @@ bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box) {
armor_box.id = c;
}
}, armor_boxes.size());
sort(armor_boxes.begin(), armor_boxes.end());
if(armor_boxes[0].id != 0){
box = armor_boxes[0];
sort(armor_boxes.begin(), armor_boxes.end(), [&](const ArmorBox &a, const ArmorBox &b){
if(last_box.rect != cv::Rect2d()){
return getPointLength(a.getCenter()-last_box.getCenter()) <
getPointLength(b.getCenter()-last_box.getCenter());
}else{
return a < b;
}
});
for(auto &one_box : armor_boxes){
if(one_box.id != 0){
box = one_box;
}
}
if (save_labelled_boxes) {
for (const auto &one_box : armor_boxes) {

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

@@ -8,10 +8,31 @@
#include <log.h>
bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
if(findArmorBox(src, armor_box)){
return true;
}else{
armor_box = ArmorBox();
if (findArmorBox(src, target_box)) {
if (last_box.rect != cv::Rect2d() &&
(getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 2.0) &&
anti_switch_cnt++ < 3) {
target_box = ArmorBox();
LOGM("anti-switch!");
return false;
} else {
anti_switch_cnt = 0;
return true;
}
} else {
target_box = ArmorBox();
anti_switch_cnt++;
return false;
}
}
/*
bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
if (findArmorBox(src, target_box)) {
return true;
} else {
target_box = ArmorBox();
return false;
}
}
*/

6
armor/src/armor_finder/send_target/send_target.cpp
View File

@@ -42,11 +42,11 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
}
bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
if (armor_box.rect == cv::Rect2d()) return false;
if (target_box.rect == cv::Rect2d()) return false;
if (shoot_delay) {
LOGM(STR_CTR(WORD_BLUE, "shoot after %dms"), shoot_delay);
LOGM(STR_CTR(WORD_BLUE, "next box %dms"), shoot_delay);
}
auto rect = armor_box.rect;
auto rect = target_box.rect;
double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X;
double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y;
double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;

56
armor/src/armor_finder/tracking_state/tracking_state.cpp
View File

@@ -7,13 +7,13 @@
#include <show_images/show_images.h>
bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
auto pos = armor_box.rect;
auto pos = target_box.rect;
if(!tracker->update(src, pos)){
armor_box = ArmorBox();
target_box = ArmorBox();
return false;
}
if((pos & cv::Rect2d(0, 0, 640, 480)) != pos){
armor_box = ArmorBox();
target_box = ArmorBox();
return false;
}
@@ -32,35 +32,37 @@ bool ArmorFinder::stateTrackingTarget(cv::Mat &src) {
ArmorBox box;
if(findArmorBox(roi, box)) {
armor_box = box;
armor_box.rect.x += bigger_rect.x;
armor_box.rect.y += bigger_rect.y;
for(auto &blob : armor_box.light_blobs){
target_box = box;
target_box.rect.x += bigger_rect.x;
target_box.rect.y += bigger_rect.y;
for(auto &blob : target_box.light_blobs){
blob.rect.center.x += bigger_rect.x;
blob.rect.center.y += bigger_rect.y;
}
tracker = TrackerToUse::create();
tracker->init(src, armor_box.rect);
tracker->init(src, target_box.rect);
}else{
roi = src(pos).clone();
if(classifier){
cv::resize(roi, roi, cv::Size(48, 36));
if(classifier(roi) == 0){
armor_box = ArmorBox();
return false;
}
}else{
cv::Mat roi_gray;
cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
contour_area = cv::countNonZero(roi_gray);
if(abs(cv::countNonZero(roi_gray) - contour_area) > contour_area * 0.3){
armor_box = ArmorBox();
return false;
}
}
armor_box.rect = pos;
armor_box.light_blobs.clear();
// roi = src(pos).clone();
// if(classifier){
// cv::resize(roi, roi, cv::Size(48, 36));
// if(classifier(roi) == 0){
// target_box = ArmorBox();
// return false;
// }
// }else{
// cv::Mat roi_gray;
// cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
// cv::threshold(roi_gray, roi_gray, 180, 255, cv::THRESH_BINARY);
// contour_area = cv::countNonZero(roi_gray);
// if(abs(cv::countNonZero(roi_gray) - contour_area) > contour_area * 0.3){
// target_box = ArmorBox();
// return false;
// }
// }
// target_box.rect = pos;
// target_box.light_blobs.clear();
target_box = ArmorBox();
return false;
}
return true;
}