整理代码结构。
This commit is contained in:
xinyang
210
armor/src/armor_finder/find/find_armor_box.cpp
Normal file
210
armor/src/armor_finder/find/find_armor_box.cpp
Normal file
@@ -0,0 +1,210 @@
|
||||
//
|
||||
// Created by xinyang on 19-7-18.
|
||||
//
|
||||
|
||||
#include <armor_finder/armor_finder.h>
|
||||
#include <show_images/show_images.h>
|
||||
#include <options/options.h>
|
||||
#include <opencv2/highgui.hpp>
|
||||
#include <log.h>
|
||||
|
||||
static string prior_blue[] = {
|
||||
"B8", "B1", "B3", "B4", "B5", "B7", "B2",
|
||||
"R8", "R1", "R3", "R4", "R5", "R7", "R2",
|
||||
};
|
||||
|
||||
static string prior_red[] = {
|
||||
"R8", "R1", "R3", "R4", "R5", "R7", "R2",
|
||||
"B8", "B1", "B3", "B4", "B5", "B7", "B2",
|
||||
};
|
||||
|
||||
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) {
|
||||
// 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) &&
|
||||
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 matchArmorBoxes(const cv::Mat &src, const LightBlobs &light_blobs, ArmorBoxes &armor_boxes, uint8_t color) {
|
||||
armor_boxes.clear();
|
||||
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), color)) {
|
||||
// cout << "match fail" << endl;
|
||||
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 > src.cols || min_y < 0 || max_y > src.rows) {
|
||||
// cout << "out of range" << endl;
|
||||
continue;
|
||||
}
|
||||
LightBlobs pair_blobs = {light_blobs.at(i), light_blobs.at(j)};
|
||||
armor_boxes.emplace_back(
|
||||
cv::Rect2d(min_x, min_y, max_x - min_x, max_y - min_y),
|
||||
pair_blobs,
|
||||
color
|
||||
);
|
||||
}
|
||||
}
|
||||
if (armor_boxes.empty()) {
|
||||
return false;
|
||||
}
|
||||
sort(armor_boxes.begin(), armor_boxes.end(), [](ArmorBox box1, ArmorBox box2) -> bool {
|
||||
return centerDistance(box1.rect) < centerDistance(box2.rect);
|
||||
});
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box){
|
||||
LightBlobs light_blobs; // 存储所有可能的灯条
|
||||
ArmorBoxes armor_boxes; // 装甲板候选区
|
||||
ArmorBoxes boxes_number[15]; // 装甲板候选区放置在对应id位置
|
||||
|
||||
box.rect = cv::Rect2d(0,0,0,0);
|
||||
box.id = -1;
|
||||
|
||||
if (!findLightBlobs(src, light_blobs)) {
|
||||
return false;
|
||||
}
|
||||
if (show_light_blobs && src.size()==cv::Size(640, 480)) {
|
||||
showLightBlobs("light_blobs", src, light_blobs);
|
||||
cv::waitKey(1);
|
||||
}
|
||||
|
||||
if (!matchArmorBoxes(src, light_blobs, armor_boxes, enemy_color)) {
|
||||
// cout << "Box fail!" << endl;
|
||||
return false;
|
||||
}
|
||||
if (show_armor_boxes && src.size()==cv::Size(640, 480)) {
|
||||
showArmorBoxes("boxes", src, armor_boxes);
|
||||
cv::waitKey(1);
|
||||
}
|
||||
|
||||
if (classifier && use_classifier) {
|
||||
for (auto &armor_box : armor_boxes) {
|
||||
cv::Mat roi = src(armor_box.rect).clone();
|
||||
cv::resize(roi, roi, cv::Size(48, 36));
|
||||
int c = classifier(roi);
|
||||
armor_box.id = c;
|
||||
boxes_number[c].emplace_back(armor_box);
|
||||
}
|
||||
if (enemy_color == ENEMY_BLUE) {
|
||||
for (auto &name : prior_blue) {
|
||||
if (!boxes_number[name2id[name]].empty()) {
|
||||
box = boxes_number[name2id[name]][0];
|
||||
break;
|
||||
}
|
||||
}
|
||||
} else if (enemy_color == ENEMY_RED) {
|
||||
for (auto &name : prior_red) {
|
||||
if (!boxes_number[name2id[name]].empty()) {
|
||||
box = boxes_number[name2id[name]][0];
|
||||
break;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
LOGE_INFO("enemy_color ERROR!");
|
||||
}
|
||||
if (save_labelled_boxes) {
|
||||
for (int i = 0; i < sizeof(boxes_number) / sizeof(boxes_number[0]); i++) {
|
||||
for (auto &armor_box : boxes_number[i]) {
|
||||
char filename[100];
|
||||
sprintf(filename, PROJECT_DIR"/armor_box_photo/%s_%d.jpg", id2name[i].data(),
|
||||
time(nullptr) + clock());
|
||||
cv::imwrite(filename, src(armor_box.rect));
|
||||
}
|
||||
}
|
||||
}
|
||||
if (box.rect == cv::Rect2d(0, 0, 0, 0)) {
|
||||
return false;
|
||||
}
|
||||
if (show_armor_boxes && src.size()==cv::Size(640, 480)) {
|
||||
showArmorBoxesClass("class", src, boxes_number);
|
||||
}
|
||||
} else {
|
||||
box = armor_boxes[0];
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
139
armor/src/armor_finder/find/find_light_blobs.cpp
Normal file
139
armor/src/armor_finder/find/find_light_blobs.cpp
Normal file
@@ -0,0 +1,139 @@
|
||||
//
|
||||
// Created by xinyang on 19-7-10.
|
||||
//
|
||||
|
||||
#include <armor_finder/armor_finder.h>
|
||||
#include <opencv2/highgui.hpp>
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
static double areaRatio(const std::vector<cv::Point> &contour, const cv::RotatedRect &rect){
|
||||
return cv::contourArea(contour) / rect.size.area();
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
// 此函数可以有性能优化.
|
||||
static uint8_t get_blob_color(const cv::Mat &src, const cv::RotatedRect &blobPos) {
|
||||
auto region = blobPos.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, src.cols, src.rows);
|
||||
cv::Mat roi = src(region);
|
||||
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) {
|
||||
return BLOB_RED;
|
||||
} else {
|
||||
return BLOB_BLUE;
|
||||
}
|
||||
}
|
||||
|
||||
static float 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 get_blob_color_opt(const cv::Mat &src, cv::RotatedRect blobPos) {
|
||||
int blue_cnt=0, red_cnt=0;
|
||||
blobPos.size.height *= 1.05;
|
||||
blobPos.size.width *= 1.1;
|
||||
cv::Point2f corners[4];
|
||||
blobPos.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++) {
|
||||
red_cnt += src.at<cv::Vec3b>(r, c)[2];
|
||||
blue_cnt += src.at<cv::Vec3b>(r, c)[0];
|
||||
}
|
||||
}
|
||||
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++) {
|
||||
red_cnt += src.at<cv::Vec3b>(r, c)[2];
|
||||
blue_cnt += src.at<cv::Vec3b>(r, c)[0];
|
||||
}
|
||||
}
|
||||
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++) {
|
||||
red_cnt += src.at<cv::Vec3b>(r, c)[2];
|
||||
blue_cnt += src.at<cv::Vec3b>(r, c)[0];
|
||||
}
|
||||
}
|
||||
if (red_cnt > blue_cnt) {
|
||||
return BLOB_RED;
|
||||
} else {
|
||||
return BLOB_BLUE;
|
||||
}
|
||||
}
|
||||
|
||||
static 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::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
|
||||
cv::Mat src_bin, color_channel;
|
||||
std::vector<cv::Mat> channels; // 通道拆分
|
||||
|
||||
cv::split(src, channels); /************************/
|
||||
if (enemy_color == ENEMY_BLUE) /* */
|
||||
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); // 二值化对应通道
|
||||
imagePreProcess(src_bin); // 开闭运算
|
||||
|
||||
if(src_bin.size() == cv::Size(640, 480))
|
||||
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::RotatedRect rect = cv::minAreaRect(light_contour);
|
||||
if (isValidLightBlob(light_contour, rect)) {
|
||||
light_blobs.emplace_back(rect, get_blob_color(src, rect));
|
||||
}
|
||||
}
|
||||
return light_blobs.size() >= 2;
|
||||
}
|
||||
Reference in New Issue
Block a user