Commit edd7f67f authored by Wagner José Kramer Vieira's avatar Wagner José Kramer Vieira
Browse files

Big update - GUI

parent 0c5a98e1
#-------------------------------------------------
#
# Project created by QtCreator 2019-10-10T21:51:08
#
#-------------------------------------------------
QT += core gui
greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
TARGET = VSSS
TEMPLATE = app
# The following define makes your compiler emit warnings if you use
# any feature of Qt which has been marked as deprecated (the exact warnings
# depend on your compiler). Please consult the documentation of the
# deprecated API in order to know how to port your code away from it.
DEFINES += QT_DEPRECATED_WARNINGS
# You can also make your code fail to compile if you use deprecated APIs.
# In order to do so, uncomment the following line.
# You can also select to disable deprecated APIs only up to a certain version of Qt.
#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0
CONFIG += c++17
CONFIG += link_pkgconfig
PKGCONFIG += opencv4
SOURCES += \
main.cpp \
mainwindow.cpp \
backend.cpp \
global.cpp
LIBS += -lstdc++fs
HEADERS += \
mainwindow.h \
global.h \
backend.h
FORMS += \
mainwindow.ui
# Default rules for deployment.
qnx: target.path = /tmp/$${TARGET}/bin
else: unix:!android: target.path = /opt/$${TARGET}/bin
!isEmpty(target.path): INSTALLS += target
This diff is collapsed.
#include <opencv2/opencv.hpp>
#include <thread>
#include <mutex>
#include <chrono>
#include <future>
#include <QTimer>
#include "global.h"
#include <QCloseEvent>
#include "backend.h"
#include "ui_mainwindow.h"
#include <experimental/filesystem>
#include <math.h>
using namespace std;
using namespace cv;
using namespace Global;
namespace fs = std::experimental::filesystem;
std::chrono::high_resolution_clock::time_point lastFrame;
QTimer timer;
int counter = 0;
void countFrames(){
fps = frames;
frames = 0;
QTimer::singleShot(1000, &countFrames);
};
double angleBetweenTwoPoints(Point2f p1, Point2f p2) {
if(p1.y > p2.y){
double x = p2.x - p1.x;
double y = p2.y - p1.y;
double angleRadian = atan2(x, y);
return abs(angleRadian * 180 / 3.14159265);
}else{
double x = p1.x - p2.x;
double y = p1.y - p2.y;
double angleRadian = atan2(x, y);
return abs((angleRadian * 180) / 3.14159265);
}
}
void startCapture(){
cap = VideoCapture("/home/kramer/campo.mp4");
//cap.set(CAP_PROP_FRAME_WIDTH, 640);
//cap.set(CAP_PROP_FRAME_HEIGHT, 480);
cap.set(CAP_PROP_FPS, 30);
countFrames();
timer.setInterval(33);
timer.callOnTimeout(&getFrame);
timer.start();
qApp->processEvents();
}
void getFrame(){
if(!capActive){
cap.release();
cap.~VideoCapture();
timer.stop();
return;
}
if (counter == cap.get(CAP_PROP_FRAME_COUNT))
{
cap.set(CAP_PROP_POS_FRAMES, 0);
counter = 0;
}
lastFrame = std::chrono::high_resolution_clock::now();
cap >> frame;
counter++;
frames++;
qApp->processEvents();
frameMod();
}
void frameMod(){
warpPerspective(frame, correctedFrame, Global::homography, correctedFrame.size());
cvtColor(correctedFrame, frameHSV, COLOR_BGR2HSV);
thread enemy(&findEnemies) ;
thread ball(&findBall);
thread ally(&detectAlly);
enemy.join();
ball.join();
ally.join();
qApp->processEvents();
estrategia();
//cout << std::chrono::duration_cast<std::chrono::milliseconds>
// (std::chrono::high_resolution_clock::now() - lastFrame).count() << endl;
}
void calibration() {
std::string path = Global::calibrationFramesPath;
vector<string> files;
for (const auto &entry : fs::directory_iterator(path))
files.push_back(entry.path());
int numCornersHor = 9;
int numCornersVer = 4;
int numSquares = numCornersHor * numCornersVer;
Size board_sz = Size(numCornersHor, numCornersVer);
vector<Point2f> corners;
for (size_t i = 0; i < files.size(); i++) {
Mat image;
image = imread(files[i]);
Mat gray_image;
vector<Point3f> obj;
for (int j = 0; j < numSquares; j++)
obj.push_back(Point3f(j / numCornersHor, j % numCornersHor, 0.0f));
cvtColor(image, gray_image, COLOR_BGR2GRAY);
bool found = findChessboardCorners(image, board_sz, corners,
CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FILTER_QUADS);
if (found) {
cornerSubPix(gray_image, corners, Size(11, 11), Size(-1, -1),
TermCriteria(TermCriteria::EPS | TermCriteria::MAX_ITER, 30, 0.1));
drawChessboardCorners(gray_image, board_sz, corners, found);
object_points.push_back(obj);
image_points.push_back(corners);
} else {
cout << "ERROR" << files[i] << endl;
}
}
Mat image;
image = imread(files[0]);
intrinsic.ptr<float>(0)[0] = 1;
intrinsic.ptr<float>(1)[1] = 1;
calibrateCamera(object_points, image_points, image.size(), intrinsic, distCoeffs, rvecs, tvecs);
}
void findBall() {
Mat frame_HSV = frameHSV;
inRange(frame_HSV, Scalar(Global::ballLH, Global::ballLS, Global::ballLV),
Scalar(Global::ballUH, Global::ballUS, Global::ballUV), thresholdBall);
vector<vector<Point> > contours;
vector<Vec4i> hierarchy;
findContours(thresholdBall, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0));
qApp->processEvents();
std::sort(contours.begin(), contours.end(), []( std::vector<cv::Point> contour1, std::vector<cv::Point> contour2 ) {
double i = fabs( contourArea(cv::Mat(contour1)) );
double j = fabs( contourArea(cv::Mat(contour2)) );
return ( i < j );
});
if(contours.size() > 0){
Point2f center;
float radius;
minEnclosingCircle(contours[contours.size()-1], center, radius);
if(radius > 10){
ballPosition = center;
return;
}
}
ballPosition = Point2f(-NAN, -NAN);
}
void detectAlly() {
Mat kernel = getStructuringElement(MORPH_RECT, Size(3, 3));
Mat frame_HSV = frameHSV;
inRange(frame_HSV, Scalar(Global::allyLH, Global::allyLS, Global::allyLV),
Scalar(Global::allyUH, Global::allyUS, Global::allyUV), thresholdAllyC);
vector<vector<Point> > contours;
vector<Vec4i> hierarchy;
findContours(thresholdAllyC, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0));
qApp->processEvents();
lastAllyFormats.clear();
std::sort(contours.begin(), contours.end(), []( std::vector<cv::Point> contour1, std::vector<cv::Point> contour2 ) {
double i = fabs( contourArea(cv::Mat(contour1)) );
double j = fabs( contourArea(cv::Mat(contour2)) );
return ( i < j );
});
bool rob3Found = false;
bool rob4Found = false;
bool rob5Found = false;
for (const auto &contour : contours) {
if(contourArea(contour) > 100){
vector<Point2f> points;
approxPolyDP(contour, points, 0.05 * arcLength(contour, true), true);
if (points.size() == 3) {
if(!rob3Found){
posRobot1 = Point2f((points[0].x + points[1].x + points[2].x) / 3,
(points[0].y + points[1].y + points[2].y) / 3);
lastAllyFormats.emplace_back(points);
rob3Found = true;
}
}
if (points.size() == 4) {
if(!rob4Found){
posRobot2 = Point2f((points[0].x + points[1].x + points[2].x + points[3].x) / 4,
(points[0].y + points[1].y + points[2].y + points[3].y) / 4);
lastAllyFormats.emplace_back(points);
rob4Found = true;
}
}
if (points.size() == 5) {
if(!rob5Found){
posRobot3 = Point2f((points[0].x + points[1].x + points[2].x + points[3].x + points[4].x) / 5,
(points[0].y + points[1].y + points[2].y + points[3].y + points[4].y) / 5);
lastAllyFormats.emplace_back(points);
}
}
}
}
contours.clear();
hierarchy.clear();
inRange(frame_HSV, Scalar(Global::allyFLH, Global::allyFLS, Global::allyFLV),
Scalar(Global::allyFUH, Global::allyFUS, Global::allyFUV), thresholdAllyF);
findContours(thresholdAllyF, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0));
qApp->processEvents();
vector<vector<Point2f>> triangles;
lastTriangles.clear();
for (const auto &contour : contours) {
if(contourArea(contour) > 100){
vector<Point2f> points;
approxPolyDP(contour, points, arcLength(contour, true) * 0.05, true);
if (points.size() == 3) {
triangles.emplace_back(points);
lastTriangles.emplace_back(points);
}
}
}
if (triangles.size() == 6)
for (int j = 0; j < 3; j++) {
vector<Point2f> mediaT0;
mediaT0.emplace_back(
Point2f((triangles[0][0].x + triangles[0][1].x) / 2, (triangles[0][0].y + triangles[0][1].y) / 2));
mediaT0.emplace_back(
Point2f((triangles[0][1].x + triangles[0][2].x) / 2, (triangles[0][1].y + triangles[0][2].y) / 2));
mediaT0.emplace_back(
Point2f((triangles[0][2].x + triangles[0][0].x) / 2, (triangles[0][2].y + triangles[0][0].y) / 2));
for (std::size_t k = 1; k < triangles.size(); k++) {
vector<Point2f> mediaTi;
mediaTi.emplace_back(Point2f((triangles[k][0].x + triangles[k][1].x) / 2,
(triangles[k][0].y + triangles[k][1].y) / 2));
mediaTi.emplace_back(Point2f((triangles[k][1].x + triangles[k][2].x) / 2,
(triangles[k][1].y + triangles[k][2].y) / 2));
mediaTi.emplace_back(Point2f((triangles[k][2].x + triangles[k][0].x) / 2,
(triangles[k][2].y + triangles[k][0].y) / 2));
vector<double> distance;
for (size_t x = 0; x < 3; x++) {
for (size_t y = 0; y < 3; y++) {
distance.emplace_back(norm(mediaT0[x] - mediaTi[y]));
}
}
int pos = max_element(distance.begin(), distance.end()) - distance.begin();
size_t x = floor(pos/3);
size_t y = pos%3;
if (norm(triangles[0][(x+2)%3] - triangles[k][(y+2)%3]) < DISTANCE && std::abs(angleBetweenTwoPoints(triangles[0][x], triangles[0][(x+1)%3]) - angleBetweenTwoPoints(triangles[k][y], triangles[k][(y+1)%3])) < 10) {
Point2f media = (triangles[0][0] + triangles[0][1] + triangles[0][2]
+ triangles[k][0] + triangles[k][1] + triangles[k][2]) / 6;
double dist1 = norm(posRobot1 - media);
double dist2 = norm(posRobot2 - media);
double dist3 = norm(posRobot3 - media);
if ((dist1 < dist2) && (dist1 < dist3)) {
frenteRobot1 = (triangles[0][(x+2)%3] + triangles[k][(y+2)%3]) / 2;
if (norm(posRobot1 - triangles[0][x]) > norm(posRobot1 - triangles[0][(x+1)%3])) {
contornosRobot1.clear();
contornosRobot1.emplace_back(triangles[0][x]);
if (norm(posRobot1 - triangles[k][y]) > norm(posRobot1 - triangles[k][(y+1)%3])) {
contornosRobot1.emplace_back(triangles[k][y]);
contornosRobot1.emplace_back(((triangles[0][x] - posRobot1) * -1) + posRobot1);
contornosRobot1.emplace_back(((triangles[k][y] - posRobot1) * -1) + posRobot1);
} else {
contornosRobot1.emplace_back(triangles[k][(y+1)%3]);
contornosRobot1.emplace_back(((triangles[0][x] - posRobot1) * -1) + posRobot1);
contornosRobot1.emplace_back(((triangles[k][(y+1)%3] - posRobot1) * -1) + posRobot1);
}
} else {
contornosRobot1.clear();
contornosRobot1.emplace_back(triangles[0][(x+1)%3]);
if (norm(posRobot1 - triangles[k][y]) > norm(posRobot1 - triangles[k][(y+1)%3])) {
contornosRobot1.emplace_back(triangles[k][y]);
contornosRobot1.emplace_back(((triangles[0][(x+1)%3] - posRobot1) * -1) + posRobot1);
contornosRobot1.emplace_back(((triangles[k][y] - posRobot1) * -1) + posRobot1);
} else {
contornosRobot1.emplace_back(triangles[k][(y+1)%3]);
contornosRobot1.emplace_back(((triangles[0][(x+1)%3] - posRobot1) * -1) + posRobot1);
contornosRobot1.emplace_back(((triangles[k][(y+1)%3] - posRobot1) * -1) + posRobot1);
}
}
} else if (dist2 < dist1 && dist2 < dist3) {
frenteRobot2 = (triangles[0][(x+2)%3] + triangles[k][(y+2)%3]) / 2;
if (norm(posRobot2 - triangles[0][x]) > norm(posRobot2 - triangles[0][(x+1)%3])) {
contornosRobot2.clear();
contornosRobot2.emplace_back(triangles[0][x]);
if (norm(posRobot2 - triangles[k][y]) > norm(posRobot2 - triangles[k][(y+1)%3])) {
contornosRobot2.emplace_back(triangles[k][y]);
contornosRobot2.emplace_back(((triangles[0][x] - posRobot2) * -1) + posRobot2);
contornosRobot2.emplace_back(((triangles[k][y] - posRobot2) * -1) + posRobot2);
} else {
contornosRobot2.emplace_back(triangles[k][(y+1)%3]);
contornosRobot2.emplace_back(((triangles[0][x] - posRobot2) * -1) + posRobot2);
contornosRobot2.emplace_back(((triangles[k][(y+1)%3] - posRobot2) * -1) + posRobot2);
}
} else {
contornosRobot2.clear();
contornosRobot2.emplace_back(triangles[0][(x+1)%3]);
if (norm(posRobot2 - triangles[k][y]) > norm(posRobot2 - triangles[k][(y+1)%3])) {
contornosRobot2.emplace_back(triangles[k][y]);
contornosRobot2.emplace_back(((triangles[0][(x+1)%3] - posRobot2) * -1) + posRobot2);
contornosRobot2.emplace_back(((triangles[k][y] - posRobot2) * -1) + posRobot2);
} else {
contornosRobot2.emplace_back(triangles[k][(y+1)%3]);
contornosRobot2.emplace_back(((triangles[0][(x+1)%3] - posRobot2) * -1) + posRobot2);
contornosRobot2.emplace_back(((triangles[k][(y+1)%3] - posRobot2) * -1) + posRobot2);
}
}
} else {
frenteRobot3 = (triangles[0][(x+2)%3] + triangles[k][(y+2)%3]) / 2;
if (norm(posRobot3 - triangles[0][x]) > norm(posRobot3 - triangles[0][(x+1)%3])) {
contornosRobot3.clear();
contornosRobot3.emplace_back(triangles[0][x]);
if (norm(posRobot3 - triangles[k][y]) > norm(posRobot3 - triangles[k][(y+1)%3])) {
contornosRobot3.emplace_back(triangles[k][y]);
contornosRobot3.emplace_back(((triangles[0][x] - posRobot3) * -1) + posRobot3);
contornosRobot3.emplace_back(((triangles[k][y] - posRobot3) * -1) + posRobot3);
} else {
contornosRobot3.emplace_back(triangles[k][(y+1)%3]);
contornosRobot3.emplace_back(((triangles[0][x] - posRobot3) * -1) + posRobot3);
contornosRobot3.emplace_back(((triangles[k][(y+1)%3] - posRobot3) * -1) + posRobot3);
}
} else {
contornosRobot3.clear();
contornosRobot3.emplace_back(triangles[0][(x+1)%3]);
if (norm(posRobot3 - triangles[k][y]) > norm(posRobot3 - triangles[k][(y+1)%3])) {
contornosRobot3.emplace_back(triangles[k][y]);
contornosRobot3.emplace_back(((triangles[0][(x+1)%3] - posRobot3) * -1) + posRobot3);
contornosRobot3.emplace_back(((triangles[k][y] - posRobot3) * -1) + posRobot3);
} else {
contornosRobot3.emplace_back(triangles[k][(y+1)%3]);
contornosRobot3.emplace_back(((triangles[0][(x+1)%3] - posRobot3) * -1) + posRobot3);
contornosRobot3.emplace_back(((triangles[k][(y+1)%3] - posRobot3) * -1) + posRobot3);
}
}
}
triangles.erase(triangles.begin() + 0);
triangles.erase(triangles.begin() + k - 1);
}
}
}
}
void findEnemies() {
Mat frame_HSV = frameHSV;
inRange(frame_HSV, Scalar(Global::enemyLH, Global::enemyLS, Global::enemyLV),
Scalar(Global::enemyUH, Global::enemyUS, Global::enemyUV), thresholdEnemy);
vector<vector<Point> > contours;
vector<Vec4i> hierarchy;
Mat kernel = getStructuringElement(MORPH_ELLIPSE, Size(5, 5));
erode(thresholdBall, thresholdBall, kernel, Point(1, 1), 1);
findContours(thresholdEnemy, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0));
vector<Moments> mu;
for (size_t i = 0; i < contours.size(); i++) {
if(contourArea(contours[i]) > 30 )
mu.emplace_back(moments(contours[i]));
}
vector<Point2f> mc;
for (size_t i = 0; i < mu.size(); i++) {
if ((mu[i].m10 / mu[i].m00) > 0)
mc.push_back(Point2f(mu[i].m10 / mu[i].m00, mu[i].m01 / mu[i].m00));
}
if (mc.size() > 3) {
vector<Point2f> ret;
for (size_t i = 0; i < 3; i++) {
vector<Point2f> vectorBackup;
Point2f ponto(mc[0].x, mc[0].y);
for (size_t j = 1; j < mc.size(); j++) {
if (norm(ponto - mc[j]) <= distanceTolerance) {
ponto = Point2f((int) ((ponto.x + mc[j].x) / 2), (int) ((ponto.y + mc[j].y) / 2));
} else {
vectorBackup.push_back(mc[j]);
}
}
mc.clear();
for (size_t k = 0; k < vectorBackup.size(); k++)
mc.push_back(vectorBackup[k]);
ret.push_back(ponto);
}
enemies = ret;;
}else if (mc.size() > 0){
enemies = mc;
}else{
mc.push_back(Point2f(-NAN, -NAN));
enemies = mc;
}
}
void estrategia(){
};
#ifndef BACKEND_HPP
#define BACKEND_HPP
#include <chrono>
#include <opencv2/opencv.hpp>
extern std::chrono::high_resolution_clock::time_point lastFrame;
double angleBetweenTwoPoints(cv::Point2f p1, cv::Point2f p2);
void startCapture();
void calibration();
void gerenciador();
void getFrame();
void frameMod();
void findBall();
void detectAlly();
void findEnemies();
void estrategia();
#endif // BACKEND_HPP
File added
#include <opencv2/opencv.hpp>
#include "global.h"
#include <mutex>
using namespace std;
using namespace cv;
namespace Global {
int defaultImageHeight;
vector<vector<Point2f>> lastTriangles;
vector<vector<Point2f>> lastAllyFormats;
bool showAllyThreshold = true;
bool showAllyFThreshold = true;
Mutex mtx;
int cameraStatus = 0;
VideoCapture cap;
bool capActive = false;
Mat frame;
Mat correctedFrame;
Mat frameHSV;
Mat thresholdAllyF;
Mat thresholdAllyC;
Mat thresholdBall;
Mat thresholdEnemy;
int frames = 0;
int fps = 0;
bool frameModReady = true;
bool enemyDetectionReady = true;
bool ballDetectionReady = true;
bool allyDetectionReady = true;
bool strategyCompilerReady = true;
int actualTab; ///< Index atual do tab widget
/**
* @defgroup Global
* @brief Constantes e variáveis globais do software
* @addtogroup Global
* @{
* @defgroup Constantes
* @brief Constantes do software
* @addtogroup Constantes
* @{
*/
const double proporcao = 7.5; ///< Proporção do tamanho do campo para píxels, equivale a 1 cm
const int comprimento = 150; ///< Largura do campo em cm
const int altura = 130; ///< Altura do campo em cm
const cv::Point2f marcas[] = {
cv::Point2f(25, 37.5),
cv::Point2f(65, 37.5),
cv::Point2f(105, 37.5),
cv::Point2f(25, 112.5),
cv::Point2f(65, 112.5),
cv::Point2f(105, 112.5)
}; ///< Posição das marcações do campo
const int alturaGol = 40; ///< Altura do gol
const int alturaAG = 70; ///< Altura da área do gol
const int larguraAG = 15; ///< Largura da área do gol
/**
*@}
* @defgroup Bola
* @brief Variáveis utilizadas no reconhecimento da bola
* @addtogroup Bola
* @{
*/
int ballUH = 20; ///< Hue máximo
int ballLH = 9; ///< Hue mínimo
int ballUS = 255; ///< Saturation máximo
int ballLS = 120; ///< Saturation mínimo
int ballUV = 255; ///< Value máximo
int ballLV = 120; ///< Value mínimo
cv::Point2f ballPosition(0, 0);
///@}
/**
* @defgroup Oponentes
* @brief Variáveis usadas no reconhecimento dos oponentes
* @addtogroup Oponentes
* @{
*/
int enemyUH = 60; ///< Hue máximo
int enemyLH = 25; ///< Hue mínimo
int enemyUS = 255; ///< Saturation máximo
int enemyLS = 120; ///< Saturation mínimo
int enemyUV = 255; ///< Value máximo
int enemyLV = 120; ///< Value mínimo
double distanceTolerance = 15; ///< Tolerância de distancia entre pontos para considerar como um robô
vector<cv::Point2f> enemies;
///@}
/**
* @defgroup Aliados
* @brief Vaiáveis de localização dos nossos robôs
* @addtogroup Aliados
* @{
*/
cv::Point2f posRobot1(0,0);
cv::Point2f posRobot2(0,0);
cv::Point2f posRobot3(0,0);
cv::Point2f frenteRobot1(0,0);
cv::Point2f frenteRobot2(0,0);
cv::Point2f frenteRobot3(0,0);
std::vector<cv::Point2f> contornosRobot1;
std::vector<cv::Point2f> contornosRobot2;
std::vector<cv::Point2f> contornosRobot3;
int allyUH = 150; ///< Hue máximo
int allyLH = 100; ///< Hue mínimo
int allyUS = 255; ///< Saturation máximo
int allyLS = 120; ///< Saturation mínimo
int allyUV = 255; ///< Value máximo
int allyLV = 120; ///< Value mínimo
int allyFUH = 180; ///< Hue máximo