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

反陀螺v1.2,以及识别优化。

Browse Source
This commit is contained in:
xinyang
2019-07-21 17:25:09 +08:00
parent 7a5a9c4c3c
commit 8684662536
8 changed files with 83 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -20,7 +20,7 @@
#define BOX_RED ENEMY_RED
#define BOX_BLUE ENEMY_BLUE
#define DISTANCE_HEIGHT_5MM (113.0) // 单位: m*pixel
#define DISTANCE_HEIGHT_5MM (107.0) // 单位: m*pixel
#define DISTANCE_HEIGHT DISTANCE_HEIGHT_5MM
#define B1 1
@@ -123,7 +123,7 @@ private:
public:
void run(cv::Mat &src); // 自瞄主函数
bool sendBoxPosition(uint8_t shoot, bool isTrack); // 和主控板通讯
bool sendBoxPosition(uint16_t shoot); // 和主控板通讯
};
#endif /* _ARMOR_FINDER_H_ */

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

@@ -12,19 +12,16 @@ static double getTimeIntervalms(const timeval& now, const timeval &last){
void ArmorFinder::antiTop() {
static double top_periodms = 0;
static double last_top_periodms = 0;
uint16_t shoot_delay = 0;
timeval curr_time;
uint8_t shoot = 0;
/*if(anti_top_state == ANTI_TOP){
cout << "anti top" << endl;
}else if(anti_top_state == NORMAL){
cout << "Normal" << endl;
}*/
// if(anti_top_state == ANTI_TOP){
// cout << "anti top" << endl;
// }else if(anti_top_state == NORMAL){
// cout << "Normal" << endl;
// }
gettimeofday(&curr_time, nullptr);
auto interval = getTimeIntervalms(curr_time, last_front_time);
LOGM("interval:%.1lf", interval);
if(anti_top_state == ANTI_TOP && (top_periodms+last_top_periodms)/2.0-120<interval&&interval<(top_periodms+last_top_periodms)/2.0-80){
shoot = 1;
}else if(interval > 700){
if(interval > 700){
anti_top_state = NORMAL;
anti_top_cnt = 0;
}
@@ -32,50 +29,38 @@ void ArmorFinder::antiTop() {
ArmorBox::BoxOrientation orientation = armor_box.getOrientation();
if(orientation == ArmorBox::UNKNOWN){
if(anti_top_state == NORMAL){
sendBoxPosition(shoot, true);
return;
}else{
sendBoxPosition(shoot, false);
return;
sendBoxPosition(shoot_delay);
}
return;
}
if(orientation != last_orient){
if(interval > 700){
anti_top_cnt = 0;
anti_top_state = NORMAL;
if(orientation == ArmorBox::FRONT){
last_front_time = curr_time;
}
}else if(interval > 150){
if(orientation == ArmorBox::FRONT){
anti_top_cnt++;
if(anti_top_state == ANTI_TOP){
last_top_periodms = top_periodms;
top_periodms = interval;
LOGM(STR_CTR(WORD_LIGHT_GREEN, "top period: %.1lf ms"), top_periodms);
if(orientation!=last_orient && orientation==ArmorBox::FRONT){
last_front_time = curr_time;
if(150<interval && interval<700){
if(anti_top_state == ANTI_TOP){
last_top_periodms = top_periodms;
top_periodms = interval;
LOGM(STR_CTR(WORD_LIGHT_GREEN, "top period: %.1lf ms"), top_periodms);
shoot_delay = (last_top_periodms+top_periodms)/2.0-110;
last_orient = orientation;
}else if(anti_top_state == NORMAL){
// LOGM("interval:%.1lf", interval);
if(++anti_top_cnt > 4){
anti_top_state = ANTI_TOP;
}
last_front_time = curr_time;
}
}
last_orient = orientation;
}
if(anti_top_cnt > 4){
anti_top_state = ANTI_TOP;
}
if(anti_top_state == ANTI_TOP){
if(orientation == ArmorBox::FRONT){
sendBoxPosition(shoot, true);
}else if(shoot){
sendBoxPosition(shoot, false);
sendBoxPosition(shoot_delay);
}
}else if(anti_top_state == NORMAL){
sendBoxPosition(shoot, true);
sendBoxPosition(shoot_delay);
}
last_orient = orientation;
}

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

@@ -57,7 +57,7 @@ ArmorBox::BoxOrientation ArmorBox::getOrientation() const{
case B1:
case B7:
case B8:
if(lengthDistanceRatio() < 0.30){
if(lengthDistanceRatio() < 0.24){
return FRONT;
}else{
return SIDE;
@@ -70,7 +70,7 @@ ArmorBox::BoxOrientation ArmorBox::getOrientation() const{
case B3:
case B4:
case B5:
if (lengthDistanceRatio() < 0.46) {
if (lengthDistanceRatio() < 0.48) {
return FRONT;
}else{
return SIDE;

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

@@ -46,8 +46,8 @@ ArmorFinder::ArmorFinder(uint8_t &color, Serial &u, const string &paras_folder,
}
void ArmorFinder::run(cv::Mat &src) {
// stateSearchingTarget(src_use); // for debug
// return;
// stateSearchingTarget(src); // for debug
// goto end;
switch (state) {
case SEARCHING_STATE:
if (stateSearchingTarget(src)) {
@@ -77,7 +77,7 @@ void ArmorFinder::run(cv::Mat &src) {
default:
stateStandBy();
}
end:
antiTop();
if (show_armor_box) { // 根据条件显示当前目标装甲板

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

@@ -23,7 +23,7 @@ static bool angelJudge(const LightBlob &light_blob_i, const LightBlob &light_blo
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;
return abs(angle_i - angle_j) < 20;
}
static bool heightJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
@@ -35,12 +35,12 @@ static bool lengthJudge(const LightBlob &light_blob_i, const LightBlob &light_bl
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);
return (side_length / light_blob_i.length < 8 && 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);
return (light_blob_i.length / light_blob_j.length < 2.5
&& light_blob_i.length / light_blob_j.length > 0.4);
}
/* 判断两个灯条的错位度，不知道英文是什么！！！ */
@@ -56,7 +56,7 @@ static bool CuoWeiDuJudge(const LightBlob &light_blob_i, const LightBlob &light_
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;
return abs(orientation.dot(p2p)) < 25;
}
static bool boxAngleJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {

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

@@ -20,8 +20,8 @@ static double areaRatio(const std::vector<cv::Point> &contour, const cv::Rotated
static bool isValidLightBlob(const std::vector<cv::Point> &contour, const cv::RotatedRect &rect) {
return (1.5 < lw_rate(rect) && lw_rate(rect) < 10) &&
// (rect.size.area() < 3000) &&
((rect.size.area() < 50 && areaRatio(contour, rect) > 0.5) ||
(rect.size.area() >= 50 && areaRatio(contour, rect) > 0.7));
((rect.size.area() < 50 && areaRatio(contour, rect) > 0.4) ||
(rect.size.area() >= 50 && areaRatio(contour, rect) > 0.6));
}
// 此函数可以有性能优化.
@@ -114,23 +114,40 @@ static void imagePreProcess(cv::Mat &src) {
}
bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
cv::Mat src_bin, color_channel;
cv::Mat color_channel;
cv::Mat src_bin_light, src_bin_dim;
std::vector<cv::Mat> channels; // 通道拆分
cv::split(src, channels); /************************/
if (enemy_color == ENEMY_BLUE) /* */
if (enemy_color == ENEMY_BLUE){ /* */
color_channel = channels[0]; /* 根据目标颜色进行通道提取 */
else if (enemy_color == ENEMY_RED) /* */
}else if (enemy_color == ENEMY_RED){ /* */
color_channel = channels[2]; /************************/
cv::threshold(color_channel, src_bin, 140, 255, CV_THRESH_BINARY); // 二值化对应通道
imagePreProcess(src_bin); // 开闭运算
}
if(src_bin.size() == cv::Size(640, 480) && show_light_blobs)
imshow("bin", src_bin);
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::threshold(color_channel, src_bin_light, 200, 255, CV_THRESH_BINARY); // 二值化对应通道
imagePreProcess(src_bin_light); // 开闭运算
cv::threshold(color_channel, src_bin_dim, 160, 255, CV_THRESH_BINARY); // 二值化对应通道
imagePreProcess(src_bin_dim); // 开闭运算
if(src_bin_light.size() == cv::Size(640, 480) && show_light_blobs) {
imshow("bin_light", src_bin_light);
imshow("bin_dim", src_bin_dim);
}
std::vector<std::vector<cv::Point> > light_contours_light, light_contours_dim;
cv::findContours(src_bin_light, light_contours_light, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
cv::findContours(src_bin_dim, light_contours_dim, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
for (auto &light_contour : light_contours_light) {
cv::RotatedRect rect = cv::minAreaRect(light_contour);
if (isValidLightBlob(light_contour, rect)) {
light_blobs.emplace_back(rect, get_blob_color(src, rect));
}
}
for (auto &light_contour : light_contours_dim) {
cv::RotatedRect rect = cv::minAreaRect(light_contour);
if (isValidLightBlob(light_contour, rect)) {
light_blobs.emplace_back(rect, get_blob_color(src, rect));

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

@@ -5,11 +5,11 @@
#include <armor_finder/armor_finder.h>
#include<log.h>
static bool sendTarget(Serial &serial, double x, double y, double z, uint8_t shoot) {
static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t shoot_delay) {
static short x_tmp, y_tmp, z_tmp;
static time_t last_time = time(nullptr);
static int fps;
uint8_t buff[9];
uint8_t buff[10];
time_t t = time(nullptr);
if (last_time != t) {
@@ -30,14 +30,19 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint8_t sho
buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
buff[7] = shoot;
buff[8] = 'e';
buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
buff[9] = 'e';
// if(buff[7]<<8 | buff[8])
// cout << (buff[7]<<8 | buff[8]) << endl;
return serial.WriteData(buff, sizeof(buff));
}
bool ArmorFinder::sendBoxPosition(uint8_t shoot, bool isTrack) {
bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
if(armor_box.rect == cv::Rect2d()) return false;
if(shoot_delay){
LOGM(STR_CTR(WORD_BLUE, "shoot after %dms"), shoot_delay);
}
auto rect = armor_box.rect;
double dx = rect.x + rect.width / 2 - 320;
double dy = rect.y + rect.height / 2 - 240 - 20;
@@ -45,15 +50,5 @@ bool ArmorFinder::sendBoxPosition(uint8_t shoot, bool isTrack) {
double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
double dist = DISTANCE_HEIGHT / rect.height;
// cout << yaw << endl;
if(isTrack){
if(shoot){
LOGM(STR_CTR(WORD_RED,"Shoot!!!"));
}
return sendTarget(serial, yaw, -pitch, dist, shoot);
LOGM(STR_CTR(WORD_RED,"Shoot!!!"));
return sendTarget(serial, 0, -0, dist, 1);
}else if(shoot){
LOGM(STR_CTR(WORD_RED,"Shoot!!!"));
return sendTarget(serial, 0, 0, 4, 1);
}
return sendTarget(serial, yaw, -pitch, dist, shoot_delay);
}

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

@@ -95,38 +95,12 @@ 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;
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);
// };
if(box.getOrientation() == ArmorBox::FRONT){
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", box.getDistance());