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

update 9-10

parent 57e242de
set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17")
cmake_minimum_required(VERSION 3.10)
cmake_minimum_required(VERSION 3.15)
project( VSSS )
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )
......
File added
No preview for this file type
......@@ -40,12 +40,12 @@ namespace Global{
* @addtogroup Bola
* @{
*/
int ballUH = 28; ///< Hue máximo
int ballLH = 0; ///< Hue mínimo
int ballUS = 231; ///< Saturation máximo
int ballLS = 127; ///< Saturation mínimo
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 = 205; ///< Value mínimo
int ballLV = 120; ///< Value mínimo
///@}
/**
......@@ -54,15 +54,50 @@ namespace Global{
* @addtogroup Oponentes
* @{
*/
int enemyUH = 0; ///< Hue máximo
int enemyLH = 0; ///< Hue mínimo
int enemyUS = 0; ///< Saturation máximo
int enemyLS = 0; ///< Saturation mínimo
int enemyHV = 0; ///< Value máximo
int enemyLV = 0; ///< Value mínimo
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 = 25; ///< Tolerância de distancia entre pontos para considerar como um robô
///@}
/**
* @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
int allyFLH = 0; ///< Hue mínimo
int allyFUS = 255; ///< Saturation máximo
int allyFLS = 120; ///< Saturation mínimo
int allyFUV = 255; ///< Value máximo
int allyFLV = 120; ///< Value mínimo
int DISTANCE = 18;
/**
* @}
* @defgroup Calibração
* @brief Variáveis usadas na calibração da câmera
* @addtogroup Calibração
......
......@@ -15,10 +15,17 @@ using namespace cv;
namespace fs = std::experimental::filesystem;
void calibration(){
double angleBetweenTwoPoints(Point2f p1, Point2f p2) {
double x = p1.x - p2.x;
double y = p1.y - p2.y;
int angleRadian = atan2(x, y);
return (angleRadian * 180) / 3.14159265;
}
void calibration() {
std::string path = Global::calibrationFramesPath;
vector<string> files;
for (const auto & entry : fs::directory_iterator(path))
for (const auto &entry : fs::directory_iterator(path))
files.push_back(entry.path());
int numCornersHor = 9;
......@@ -27,7 +34,7 @@ void calibration(){
Size board_sz = Size(numCornersHor, numCornersVer);
vector<Point2f> corners;
for(int i = 0; i < files.size(); i++) {
for (int i = 0; i < files.size(); i++) {
Mat image;
image = imread(files[i]);
Mat gray_image;
......@@ -44,7 +51,7 @@ void calibration(){
drawChessboardCorners(gray_image, board_sz, corners, found);
object_points.push_back(obj);
image_points.push_back(corners);
}else{
} else {
cout << "ERROR" << files[i] << endl;
}
}
......@@ -56,51 +63,253 @@ void calibration(){
calibrateCamera(object_points, image_points, image.size(), intrinsic, distCoeffs, rvecs, tvecs);
}
Point2f findBall(Mat frame_HSV){
Point2f findBall(Mat frame_HSV) {
Mat frame_threshold;
inRange(frame_HSV, Scalar(Global::ballLH, Global::ballLS, Global::ballLV), Scalar(Global::ballUH, Global::ballUS, Global::ballUV), frame_threshold);
inRange(frame_HSV, Scalar(Global::ballLH, Global::ballLS, Global::ballLV),
Scalar(Global::ballUH, Global::ballUS, Global::ballUV), frame_threshold);
vector<vector<Point> > contours;
vector<Vec4i> hierarchy;
findContours( frame_threshold, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0) );
findContours(frame_threshold, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0));
vector<Moments> mu(contours.size());
for( int i = 0; i<contours.size(); i++ )
{ mu[i] = moments( contours[i], false ); }
for (int i = 0; i < contours.size(); i++) { mu[i] = moments(contours[i], false); }
vector<Point2f> mc(contours.size());
for( int i = 0; i<contours.size(); i++)
{ mc[i] = Point2f( mu[i].m10/mu[i].m00 , mu[i].m01/mu[i].m00 ); }
if(mc.size() > 0) return mc[0];
imshow("BALLTHRESHOLD", frame_threshold);
if (mc.size() > 0) return mc[0];
else return Point2d(-NAN, -NAN);
}
vector<Point2f> findEnemies(Mat frame_HSV){
vector<Point2f> findEnemies(Mat frame_HSV) {
Mat frame_threshold;
inRange(frame_HSV, Scalar(Global::ballLH, Global::ballLS, Global::ballLV), Scalar(Global::ballUH, Global::ballUS, Global::ballUV), frame_threshold);
inRange(frame_HSV, Scalar(Global::enemyLH, Global::enemyLS, Global::enemyLV),
Scalar(Global::enemyUH, Global::enemyUS, Global::enemyUV), frame_threshold);
namedWindow("ENEMYTHRESHOLD");
imshow("ENEMYTHRESHOLD", frame_threshold);
vector<vector<Point> > contours;
vector<Vec4i> hierarchy;
findContours( frame_threshold, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0) );
findContours(frame_threshold, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0));
vector<Moments> mu(contours.size());
for( int i = 0; i<contours.size(); i++ )
{ mu[i] = moments( contours[i], false ); }
for (int i = 0; i < contours.size(); i++) { mu[i] = moments(contours[i], false); }
vector<Point2f> mc(contours.size());
for( int i = 0; i<contours.size(); i++)
{ mc[i] = Point2f( mu[i].m10/mu[i].m00 , mu[i].m01/mu[i].m00 ); }
if(mc.size() > 0) return mc;
vector<Point2f> mc;
for (int i = 0; i < contours.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 (int i = 0; i < 3; i++) {
vector<Point2f> vectorBackup;
Point2f ponto(mc[0].x, mc[0].y);
for (int 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 (int k = 0; k < vectorBackup.size(); k++)
mc.push_back(vectorBackup[k]);
ret.push_back(ponto);
}
return ret;
}
if (mc.size() > 0) return mc;
mc.push_back(Point2f(-NAN, -NAN));
return mc;
}
void windowClicked(int event, int x, int y, int, void*){
void detectAlly(Mat frame_HSV) {
Mat frame_threshold;
inRange(frame_HSV, Scalar(Global::allyLH, Global::allyLS, Global::allyLV),
Scalar(Global::allyUH, Global::allyUS, Global::allyUV), frame_threshold);
vector<vector<Point> > contours;
vector<Vec4i> hierarchy;
findContours(frame_threshold, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0));
namedWindow("ALLYTHRESHOLD");
imshow("ALLYTHRESHOLD", frame_threshold);
for (const auto &contour : contours) {
vector<Point2f> points;
approxPolyDP(contour, points, 0.05 * arcLength(contour, true), true);
if (points.size() == 3) {
posRobot1 = Point2f((points[0].x + points[1].x + points[2].x) / 3,
(points[0].y + points[1].y + points[2].y) / 3);
}
if (points.size() == 4) {
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);
}
if (points.size() == 5) {
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);
}
}
inRange(frame_HSV, Scalar(Global::allyFLH, Global::allyFLS, Global::allyFLV),
Scalar(Global::allyFUH, Global::allyFUS, Global::allyFUV), frame_threshold);
contours.clear();
hierarchy.clear();
findContours(frame_threshold, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0));
namedWindow("ALLYFTHRESHOLD");
imshow("ALLYFTHRESHOLD", frame_threshold);
inRange(frame_HSV, Scalar(Global::allyFLH, Global::allyFLS, Global::allyFLV),
Scalar(Global::allyFUH, Global::allyFUS, Global::allyFUV), frame_threshold);
findContours(frame_threshold, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0));
vector<vector<Point2f>> triangles;
for (const auto &contour : contours) {
vector<Point2f> points;
approxPolyDP(contour, points, 0.05 * arcLength(contour, true), true);
if (points.size() == 3) {
if (norm(Point2f((points[0].x + points[1].x + points[2].x) / 3,
(points[0].y + points[1].y + points[2].y) / 3) - posRobot1) > 20) {
triangles.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 (int 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 (int x = 0; x < 3; x++) {
for (int y = 0; y < 3; y++) {
distance.emplace_back(norm(mediaT0[x] - mediaTi[y]));
}
}
int pos = max_element(distance.begin(), distance.end()) - distance.begin();
int x = floor(pos/3);
int y = pos%3;
if (norm(triangles[0][(x+2)%3] - triangles[k][(y+2)%3]) < DISTANCE) {
Point2f media = (triangles[0][0] + triangles[0][1] + triangles[0][2]
+ triangles[k][0] + triangles[k][1] + triangles[k][2]) / 6;
double dist1 = norm(media - posRobot1);
double dist2 = norm(media - posRobot2);
double dist3 = norm(media - posRobot3);
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 windowClicked(int event, int x, int y, int, void *) {
if( event != EVENT_LBUTTONDOWN )
if (event != EVENT_LBUTTONDOWN)
return;
switch (Global::confStatus){
switch (Global::confStatus) {
case 0:
Global::corners.push_back(Point2f(x, y));
break;
......@@ -119,13 +328,11 @@ void windowClicked(int event, int x, int y, int, void*){
base.push_back(Point2f(comprimento * proporcao, altura * proporcao));
base.push_back(Point2f(comprimento * proporcao, 0));
std::sort(base.begin(), base.end(),
[](const cv::Point2f &a, const cv::Point2f &b)
{
[](const cv::Point2f &a, const cv::Point2f &b) {
return a.x < b.x;
});
std::sort(corners.begin(), corners.end(),
[](const cv::Point2f &a, const cv::Point2f &b)
{
[](const cv::Point2f &a, const cv::Point2f &b) {
return a.x < b.x;
});
Global::homography = findHomography(Global::corners, base);
......@@ -134,7 +341,7 @@ void windowClicked(int event, int x, int y, int, void*){
Global::confStatus++;
}
/*
int main() {
calibration();
VideoCapture c("/home/kramer/bolinha.avi");
......@@ -165,3 +372,59 @@ int main() {
if(waitKey(30) > 0) break;
}
}
*/
int main() {
VideoCapture c("../campo.mp4");
Mat frame;
namedWindow("OUTPUT");
int i = 0;
while (true) {
auto t1 = std::chrono::high_resolution_clock::now();
if (i == c.get(CAP_PROP_FRAME_COUNT)) {
c.set(CAP_PROP_POS_FRAMES, 0);
i = 0;
}
c >> frame;
Mat hsvSource;
cvtColor(frame, hsvSource, COLOR_BGR2HSV);
detectAlly(hsvSource);
vector<Point2f> enemies = findEnemies(hsvSource);
if (posRobot1.x != 0) {
circle(frame, posRobot1, 10, Scalar(0, 255, 0), -1);
circle(frame, posRobot2, 10, Scalar(0, 255, 0), -1);
circle(frame, posRobot3, 10, Scalar(0, 255, 0), -1);
}
Point2f ball = findBall(hsvSource);
for(auto & enemy : enemies){
circle(frame, enemy, 5, Scalar(255, 255, 255), -1);
}
line(frame, contornosRobot1[0], contornosRobot1[1], Scalar(255, 255, 255));
line(frame, contornosRobot1[1], contornosRobot1[2], Scalar(255, 255, 255));
line(frame, contornosRobot1[2], contornosRobot1[3], Scalar(255, 255, 255));
line(frame, contornosRobot1[3], contornosRobot1[0], Scalar(255, 255, 255));
line(frame, contornosRobot2[0], contornosRobot2[1], Scalar(255, 255, 255));
line(frame, contornosRobot2[1], contornosRobot2[2], Scalar(255, 255, 255));
line(frame, contornosRobot2[2], contornosRobot2[3], Scalar(255, 255, 255));
line(frame, contornosRobot2[3], contornosRobot2[0], Scalar(255, 255, 255));
line(frame, contornosRobot3[0], contornosRobot3[1], Scalar(255, 255, 255));
line(frame, contornosRobot3[1], contornosRobot3[2], Scalar(255, 255, 255));
line(frame, contornosRobot3[2], contornosRobot3[3], Scalar(255, 255, 255));
line(frame, contornosRobot3[3], contornosRobot3[0], Scalar(255, 255, 255));
circle(frame, ball, 5, Scalar(255, 0, 255), -1);
line(frame, posRobot1, frenteRobot1, Scalar(255, 0, 0), 10);
line(frame, posRobot2, frenteRobot2, Scalar(255, 0, 0), 10);
line(frame, posRobot3, frenteRobot3, Scalar(255, 0, 0), 10);
imshow("OUTPUT", frame);
auto t2 = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1).count();
cout << duration << endl;
if (waitKey(100) == 'e') break;
i++;
}
}
\ No newline at end of file
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment