Merge branch 'develop'

color_params_RGB.xml

@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<opencv_storage>
+<lowR>25</lowR>
+<highR>152</highR>
+<lowG>64</lowG>
+<highG>237</highG>
+<lowB>220</lowB>
+<highB>255</highB>
+</opencv_storage>

makefile

@@ -1,6 +1,7 @@
-all:  ReadWrite MonoCamCalib RedBallDetection
+all:  ReadWrite MonoCamCalib RedBallDetection RedBallTracking
 	g++ lib/MonoCameraCalibration.o lib/ReadWriteFunctions.o -o bin/MonoCameraCalibration -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
 	g++ lib/RedBallDetection.o -o bin/RedBallDetection -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
+	g++ lib/RedBallTracking.o -o bin/RedBallTracking -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
 	
 MonoCamCalib: src/MonoCameraCalibration.cpp
 	g++ -c src/MonoCameraCalibration.cpp -o lib/MonoCameraCalibration.o -I./include -I/usr/include/opencv4
@@ -11,6 +12,8 @@ ReadWrite: src/ReadWriteFunctions.cpp
 RedBallDetection: src/RedBallDetection.cpp
 	g++ -c src/RedBallDetection.cpp -o lib/RedBallDetection.o -I./include -I/usr/include/opencv4
 
+RedBallTracking: src/RedBallTracking.cpp
+	g++ -c src/RedBallTracking.cpp -o lib/RedBallTracking.o -I./include -I/usr/include/opencv4
 
 clean:
 	rm lib/*.o

src/RedBallDetection.cpp

@@ -30,7 +30,8 @@ bool readCameraParameters(std::string filename, cv::Mat &camMatrix, cv::Mat & di
 	return true;
 }
 
-bool writeColorParameters(std::string filename, int iLowH, int iHighH, int iLowS, int iHighS, int iLowV, int iHighV)
+//bool writeColorParameters(std::string filename, int iLowH, int iHighH, int iLowS, int iHighS, int iLowV, int iHighV)
+bool writeColorParameters(std::string filename, int iLowR, int iHighR, int iLowG, int iHighG, int iLowB, int iHighB)
 {
 	cv::FileStorage fs(filename, cv::FileStorage::WRITE);
 	if (!fs.isOpened())
@@ -38,6 +39,9 @@ bool writeColorParameters(std::string filename, int iLowH, int iHighH, int iLowS
 		std::cout << "[ERROR] Could not open the file storage: " <<  filename << " !"<< std::endl;
 		return false;
 	}
+	
+	/*
+	
 	fs << "lowH" << iLowH;
 	fs << "highH" << iHighH;
 	
@@ -47,6 +51,17 @@ bool writeColorParameters(std::string filename, int iLowH, int iHighH, int iLowS
 	fs << "lowV" << iLowV;
 	fs << "highV" << iHighV;
 	
+	*/
+	
+	fs << "lowR" << iLowR;
+	fs << "highR" << iHighR;
+	
+	fs << "lowG" << iLowG;
+	fs << "highG" << iHighG;
+	
+	fs << "lowB" << iLowB;
+	fs << "highB" << iHighB;
+	
 	// releases the writer
         fs.release();
 
@@ -57,7 +72,8 @@ int main(int argc, char** argv)
 {	
 	// initializes main parameters
 	std::string sCameraParamFilename = CAM_PARAMS_FILENAME;
-	std::string sColorParamFilename = COLOR_PARAMS_FILENAME;
+	//std::string sColorParamFilename = COLOR_PARAMS_FILENAME;
+	std::string sColorParamFilename = "color_params_RGB.xml";
 	int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE;
 	float fFPS = FPS;
 	int iMaxVideoResolution = RESOLUTION_MAX;
@@ -125,6 +141,7 @@ int main(int argc, char** argv)
 	cv::namedWindow("Control", cv::WINDOW_AUTOSIZE); //create a window called "Control"
 
 	// sets min/max value for HSV color representation
+	/*
 	int iLowH = 0;
 	int iHighH = 179;
 
@@ -133,7 +150,16 @@ int main(int argc, char** argv)
 
 	int iLowV = 0;
 	int iHighV = 255;
+	*/
+	int iLowR = 0;
+	int iHighR = 255;
 
+	int iLowG = 0; 
+	int iHighG = 255;
+
+	int iLowB = 0;
+	int iHighB = 255;
+/*
 	// creates trackbars in "Control" window
 	cv::createTrackbar("LowH", "Control", &iLowH, 179); //Hue (0 - 179)
 	cv::createTrackbar("HighH", "Control", &iHighH, 179);
@@ -143,6 +169,18 @@ int main(int argc, char** argv)
 
 	cv::createTrackbar("LowV", "Control", &iLowV, 255); //Value (0 - 255)
 	cv::createTrackbar("HighV", "Control", &iHighV, 255);
+	*/
+	// creates trackbars in "Control" window
+	cv::createTrackbar("LowR", "Control", &iLowR, 255); //Red (0 - 255)
+	cv::createTrackbar("HighR", "Control", &iHighR, 255);
+
+	cv::createTrackbar("LowG", "Control", &iLowG, 255); //Green (0 - 255)
+	cv::createTrackbar("HighG", "Control", &iHighG, 255);
+
+	cv::createTrackbar("LowB", "Control", &iLowB, 255); //Blue (0 - 255)
+	cv::createTrackbar("HighB", "Control", &iHighB, 255);
+	
+	
 
 	while (true)
 	{
@@ -168,7 +206,8 @@ int main(int argc, char** argv)
 
 		//Threshold the image based on the trackbar values
 		cv::Mat imgThresholded;
-		inRange(imgHSV, cv::Scalar(iLowH, iLowS, iLowV), cv::Scalar(iHighH, iHighS, iHighV), imgThresholded); 
+		//inRange(imgHSV, cv::Scalar(iLowH, iLowS, iLowV), cv::Scalar(iHighH, iHighS, iHighV), imgThresholded);
+		inRange(imgOriginal, cv::Scalar(iLowR, iLowG, iLowB), cv::Scalar(iHighR, iHighG, iHighB), imgThresholded); 
 
 		//morphological opening (remove small objects from the foreground)
 		cv::erode(imgThresholded, imgThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) );
@@ -197,7 +236,8 @@ int main(int argc, char** argv)
 		}
 		if (key == 's')
 		{
-			writeColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV);
+			//writeColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV);
+			writeColorParameters(sColorParamFilename, iLowR, iHighR, iLowG, iHighG, iLowB, iHighB);
 			std::cout << "[INFO] Color parameters saved to file: " <<  sColorParamFilename << std::endl;
 		}
 		

src/RedBallTracking.cpp

@@ -0,0 +1,235 @@
+#include <iostream>
+#include <ctype.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/imgproc/imgproc.hpp"
+#include <opencv2/calib3d.hpp>
+
+#define CAM_PARAMS_FILENAME "./data/camera_calibration_params.xml"
+#define COLOR_PARAMS_FILENAME "./data/color_params.xml"
+#define FPS 30.0
+#define STRUCTURAL_ELEMENTS_SIZE 5
+#define AREA_THRESOLD 1000
+#define RESOLUTION_MAX 800
+
+using namespace cv;
+using namespace std;
+
+bool readCameraParameters(std::string filename, cv::Mat &camMatrix, cv::Mat & distCoeffs)
+{
+	cv::FileStorage fs(filename, cv::FileStorage::READ);
+	if (!fs.isOpened())
+	{
+		std::cout << "[ERROR] Could not open the camera parameter file storage: " <<  filename << " !"<< std::endl;
+		return false;
+	}
+	
+	fs["camera_matrix"] >> camMatrix;
+	fs["distortion_coefficients"] >> distCoeffs;
+	
+	return true;
+}
+
+bool readColorParameters(std::string filename, int& iLowH, int& iHighH, int& iLowS, int& iHighS, int& iLowV, int& iHighV)
+{
+	cv::FileStorage fs(filename, cv::FileStorage::READ);
+	if (!fs.isOpened())
+	{
+		std::cout << "[ERROR] Could not open the color paramter file storage: " <<  filename << " !"<< std::endl;
+		return false;
+	}
+	
+	fs["lowH"] >> iLowH;
+	fs["highH"] >> iHighH;
+	
+	fs["lowS"] >> iLowS;
+	fs["highS"] >> iHighS;
+	
+	fs["lowV"] >> iLowV;
+	fs["highV"] >> iHighV;
+	
+	return true;
+}
+
+ int main( int argc, char** argv )
+ {
+	// initializes main parameters
+	std::string sCameraParamFilename = CAM_PARAMS_FILENAME;
+	std::string sColorParamFilename = COLOR_PARAMS_FILENAME;
+	float fFPS = FPS;
+	int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE;
+	int iAreaThresold = AREA_THRESOLD;
+	 int iMaxVideoResolution = RESOLUTION_MAX;
+	 
+	// updates main parameters from arguments
+	int opt;
+	while ((opt = getopt (argc, argv, ":c:f:s:a:i:r:")) != -1)
+	{
+		switch (opt)
+		{
+			case 'c':
+				sColorParamFilename = optarg;
+				break;
+			case 'f':
+				fFPS = atof(optarg); 
+				break;
+			case 's':
+				iStructuralElementSize = atoi(optarg);
+				break;
+			case 'a':
+				iAreaThresold = atoi(optarg);
+				break;
+			case 'i':
+				sCameraParamFilename = optarg;
+				break;
+			case 'r':
+				iMaxVideoResolution = atoi(optarg);
+				break;
+			case '?':
+				if (optopt == 'c' || optopt == 'f' || optopt == 's' || optopt == 'a' | optopt == 'r')
+					fprintf (stderr, "Option -%c requires an argument.\n", optopt);
+				else if (isprint (optopt))
+					fprintf (stderr, "Unknown option `-%c'.\n", optopt);
+				else
+					fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt);
+				return 1;
+			default:
+				abort ();
+		}
+	}
+
+	// reads color parameters from the file storage
+	int iLowH, iHighH, iLowS, iHighS, iLowV, iHighV;
+	bool isColorParamsSet = readColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV);
+	
+	// checks if the color parameters were successfully read
+	if (!isColorParamsSet)
+	{
+		std::cout << "[ERROR] Color parameters could not be loaded!" << std::endl;
+		return -1;
+	}
+	
+	// distorted/undistorted image
+	bool bIsImageUndistorted = true;
+	
+	// reads camera intrinsic parameters
+	cv::Mat cameraMatrix, distCoeffs;
+	bool isCamParamsSet = readCameraParameters(sCameraParamFilename, cameraMatrix, distCoeffs);
+	
+	// checks if the camera parameters were successfully read
+	if (!isCamParamsSet)
+	{
+		std::cout << "[WARNING] Camera intrinsic parameters could not be loaded!" << std::endl;
+	}
+	 
+	// creates a camera grabber
+	VideoCapture cap(0, cv::CAP_V4L2); //capture the video from webcam
+	
+	// changes image resolution to maximum (e.g. 1920x1080 if possible)
+	cap.set(cv::CAP_PROP_FRAME_HEIGHT, iMaxVideoResolution); cap.set(cv::CAP_PROP_FRAME_WIDTH, iMaxVideoResolution);
+	
+	// checks if the camera was successfully opened
+	if ( !cap.isOpened() )  // if not success, exit program
+	{
+		cout << "[ERROR] Could not open the camera!" << endl;
+		return -1;
+	}
+
+	// inits previous x,y location of the ball
+	int iLastX = -1; 
+	int iLastY = -1;
+
+	// captures a temporary image from the camera
+	Mat imgTmp;
+	cap.read(imgTmp); 
+
+	// creates a black image with the size as the camera output
+	Mat imgLines = Mat::zeros( imgTmp.size(), CV_8UC3 );
+
+	// main loop launched every FPS
+	while (true)
+	{
+		// reads a new frame from video
+		cv::Mat imgOriginal;
+		bool bSuccess = cap.read(imgOriginal);
+
+		// checks if a new frame was grabbed
+		if (!bSuccess) //if not success, break loop
+		{
+			std::cout << "[WARNING] Could not read a new frame from video stream" << std::endl;
+			break;
+		}
+		
+		if (bIsImageUndistorted && isCamParamsSet)
+		{
+			cv::Mat temp = imgOriginal.clone();
+			cv::undistort(temp, imgOriginal, cameraMatrix, distCoeffs);
+		}
+
+		// converts the captured frame from BGR to HSV
+		cv::Mat imgHSV;
+		cvtColor(imgOriginal, imgHSV, cv::COLOR_BGR2HSV); 
+
+		// thresholds the image based on the trackbar values
+		cv::Mat imgThresholded;
+		inRange(imgHSV, cv::Scalar(iLowH, iLowS, iLowV), cv::Scalar(iHighH, iHighS, iHighV), imgThresholded); 
+
+		// applies morphological opening (removes small objects from the foreground)
+		cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) );
+		cv::dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) ); 
+
+		// applies morphological closing (removes small holes from the foreground)
+		cv::dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) ); 
+		cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) );
+
+		// calculates the moments of the thresholded image
+		Moments oMoments = moments(imgThresholded);
+		double dM01 = oMoments.m01;
+		double dM10 = oMoments.m10;
+		double dArea = oMoments.m00;
+
+		// if the area <= iAreaThresold, considers that the there are no object in the image and it's because of the noise, the area is not zero 
+		if (dArea > iAreaThresold)
+		{
+			// calculates the position of the ball
+			int posX = dM10 / dArea;
+			int posY = dM01 / dArea;        
+
+			if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0)
+			{
+				// draww a red line from the previous point to the current point
+				line(imgLines, Point(posX, posY), Point(iLastX, iLastY), Scalar(0,0,255), 2);
+			}
+
+			// stores the current position for enxt frame
+			iLastX = posX;
+			iLastY = posY;
+		}
+
+		// displays the thresholded image
+		imshow("Thresholded Image", imgThresholded); 
+
+		// shows the original image with the tracking (red) lines
+		imgOriginal = imgOriginal + imgLines;
+		imshow("Original", imgOriginal); 
+
+		// waits for awhile depending on the FPS value
+		char key = (char)cv::waitKey(1000.0/fFPS);
+		// checks if ESC was pressed to exit
+		if (key == 27) // if 'esc' key is pressed, break loop
+		{
+			std::cout << "[INFO] esc key is pressed by user -> Shuting down!" << std::endl;
+			break; 
+		}
+		if (key == 'u')
+		{
+			bIsImageUndistorted = !bIsImageUndistorted;
+			std::cout << "[INFO] Image undistorted: " <<  bIsImageUndistorted<< std::endl;
+		}
+	}
+
+	return 0;
+}