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

开机启动脚本现在会尝试从github上更新当前分支的最新代码并编译运行。

Browse Source
This commit is contained in:
xinyang
2019-07-02 20:55:20 +08:00
parent 17fe564f98
commit cb6cf4b2fc
6 changed files with 102 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -28,31 +28,90 @@ bool rectangleContainPoint(cv::RotatedRect rectangle, cv::Point2f point)
double indicator = pointPolygonTest(contour,point,true);
return indicator >= 0;
}
/// Todo: 下面的函数可以有性能优化，暂时未做。
static double nonZeroRateOfRotateRect(const cv::Mat &bin, const cv::RotatedRect &rorect){
auto rect = rorect.boundingRect();
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){
// cout << "break" << endl;
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(rorect, cv::Point(c+rect.x, r+rect.y))){
if(rectangleContainPoint(rotrect, cv::Point(c+rect.x, r+rect.y))){
if(roi.at<uint8_t>(r, c)){
cnt++;
}
}
}
}
return double(cnt) / rorect.size.area();
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[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;
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) &&
return (lw_rate(rect) > 1.8) &&
// (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);
}
@@ -70,7 +129,7 @@ static bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
}else if(src.type() == CV_8UC1){
src_gray = src.clone();
}
LightBlobs all;
std::vector<std::vector<cv::Point> > light_contours;
cv::findContours(src_gray, light_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
@@ -79,12 +138,13 @@ static bool findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
if(isValidLightBlob(src_gray, rect)){
light_blobs.emplace_back(rect);
}
all.emplace_back(rect);
}
showContours("all", src, all);
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 :
@@ -93,7 +153,6 @@ bool angelJudge(const LightBlob &light_blob_i, const LightBlob &light_blob_j) {
}
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;
}
@@ -101,38 +160,18 @@ 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));
// std::cout << "side:" << side_length << " length:" << light_blob_i.length << std::endl;
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) {
// std::cout << "i:" << light_blob_i.length << " j:" << light_blob_j.length << std::endl;
return (light_blob_i.length / light_blob_j.length < 2
&& light_blob_i.length / light_blob_j.length > 0.5);
}
bool isCoupleLight(const LightBlob &light_blob_i, const LightBlob &light_blob_j, uint8_t enemy_color) {
if(light_blob_i.BlobColor != enemy_color || light_blob_j.BlobColor != enemy_color){
return false;
}
if(!lengthRatioJudge(light_blob_i, light_blob_j)){
// std::cout << "lengthRatioJudge" << std::endl;
return false;
}
if(!lengthJudge(light_blob_i, light_blob_j)){
// std::cout << "lengthJudge" << std::endl;
return false;
}
if(!heightJudge(light_blob_i, light_blob_j)){
// std::cout << "heightJudge" << std::endl;
return false;
}
if(!angelJudge(light_blob_i, light_blob_j)){
// std::cout << "angelJudge" << std::endl;
return false;
}
return true;
return lengthRatioJudge(light_blob_i, light_blob_j) &&
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);
@@ -156,8 +195,8 @@ static bool findArmorBoxes(LightBlobs &light_blobs, std::vector<cv::Rect2d> &arm
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) - 4;
max_y = fmax(rect_left.y + rect_left.height, rect_right.y + rect_right.height) + 4;
min_y = fmin(rect_left.y, rect_right.y) - 0.3*(rect_left.height+rect_right.height)/2.0;
max_y = fmax(rect_left.y + rect_left.height, rect_right.y + rect_right.height) + 0.3*(rect_left.height+rect_right.height)/2.0;
if (min_x < 0 || max_x > 640 || min_y < 0 || max_y > 480) {
continue;
}
@@ -216,14 +255,18 @@ int prior_red[] = {0, 2, 3, 4, 1, 5, 7, 8, 9, 6};
int prior_blue[]= {5, 7, 8, 9, 6, 0, 2, 3, 4, 1};
bool ArmorFinder::stateSearchingTarget(cv::Mat &src) {
cv::Mat split, src_bin;
cv::Mat split, src_bin/*, edge*/;
LightBlobs light_blobs, light_blobs_, light_blobs_real;
std::vector<cv::Rect2d> armor_boxes, boxes_number[10];
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);
imagePreProcess(src_bin);
// imshow("gray bin", src_bin);
if(!findLightBlobs(src_bin, light_blobs)){
return false;