diff --git a/bin/CameraOrientationTracking b/bin/CameraOrientationTracking new file mode 100755 index 0000000..d29f141 Binary files /dev/null and b/bin/CameraOrientationTracking differ diff --git a/data/camera_chessboard_calibration_params.xml b/data/camera_chessboard_calibration_params.xml new file mode 100644 index 0000000..8fdbebb --- /dev/null +++ b/data/camera_chessboard_calibration_params.xml @@ -0,0 +1,52 @@ + + + + 0. 0. 0. 23. 0. 0. 46. 0. 0. 69. 0. 0. 92. 0. 0. 115. 0. 0. 138. 0. 0. + 161. 0. 0. 184. 0. 0. 0. 23. 0. 23. 23. 0. 46. 23. 0. 69. 23. 0. 92. + 23. 0. 115. 23. 0. 138. 23. 0. 161. 23. 0. 184. 23. 0. 0. 46. 0. 23. + 46. 0. 46. 46. 0. 69. 46. 0. 92. 46. 0. 115. 46. 0. 138. 46. 0. 161. + 46. 0. 184. 46. 0. 0. 69. 0. 23. 69. 0. 46. 69. 0. 69. 69. 0. 92. 69. + 0. 115. 69. 0. 138. 69. 0. 161. 69. 0. 184. 69. 0. 0. 92. 0. 23. 92. + 0. 46. 92. 0. 69. 92. 0. 92. 92. 0. 115. 92. 0. 138. 92. 0. 161. 92. + 0. 184. 92. 0. 0. 115. 0. 23. 115. 0. 46. 115. 0. 69. 115. 0. 92. 115. + 0. 115. 115. 0. 138. 115. 0. 161. 115. 0. 184. 115. 0. + + 5.88612000e+02 2.15689667e+02 5.54606873e+02 2.15611755e+02 + 5.20468445e+02 2.15258972e+02 4.85772522e+02 2.15278687e+02 + 4.51321503e+02 2.15120773e+02 4.16495636e+02 2.14783768e+02 + 3.81401917e+02 2.14718674e+02 3.46348175e+02 2.14802200e+02 + 3.10953491e+02 2.15044067e+02 5.88692505e+02 1.81895676e+02 + 5.54564270e+02 1.81450623e+02 5.20432983e+02 1.80957260e+02 + 4.85783722e+02 1.80860062e+02 4.51179932e+02 1.80639709e+02 + 4.16396912e+02 1.80453308e+02 3.81144226e+02 1.80382690e+02 + 3.45956238e+02 1.80322159e+02 3.10507690e+02 1.80347870e+02 + 5.88607361e+02 1.48160278e+02 5.54770813e+02 1.47601761e+02 + 5.20398499e+02 1.47191406e+02 4.85810608e+02 1.46545593e+02 + 4.50779694e+02 1.46543564e+02 4.15969879e+02 1.46196732e+02 + 3.80619965e+02 1.45882202e+02 3.45383270e+02 1.45875610e+02 + 3.09773987e+02 1.46258408e+02 5.89240051e+02 1.14398933e+02 + 5.54773438e+02 1.13560555e+02 5.20508728e+02 1.13239220e+02 + 4.85565887e+02 1.12632996e+02 4.50695831e+02 1.12478821e+02 + 4.15567413e+02 1.12141571e+02 3.80236389e+02 1.11906654e+02 + 3.44660980e+02 1.11700226e+02 3.09211731e+02 1.11827919e+02 + 5.89326904e+02 8.11330795e+01 5.55244568e+02 8.04520645e+01 + 5.20487244e+02 7.96654358e+01 4.85463074e+02 7.93521194e+01 + 4.50391876e+02 7.91115799e+01 4.15237152e+02 7.86507416e+01 + 3.79536346e+02 7.84776230e+01 3.43838593e+02 7.84099808e+01 + 3.08281067e+02 7.82369461e+01 5.89964478e+02 4.79190063e+01 + 5.55360474e+02 4.72442245e+01 5.20573486e+02 4.66231613e+01 + 4.85435333e+02 4.63138351e+01 4.50215057e+02 4.57072220e+01 + 4.14667877e+02 4.53788414e+01 3.78956207e+02 4.50921440e+01 + 3.43222778e+02 4.47188683e+01 3.07135529e+02 4.45427628e+01 + + 4 + 4 +
d
+ + -9.9999815881189502e-01 1.2697535215647785e-03 + 1.4387837969518850e-03 1.2288328754052873e+02 + -1.2718960354282296e-03 -9.9999808231089615e-01 + -1.4891793059053041e-03 -2.7774165634818203e+01 + 1.4368901471439603e-03 -1.4910065474532613e-03 + 9.9999785612079128e-01 5.2883271790406673e+02 0. 0. 0. 1. + diff --git a/lib/CameraOrientationTracking.o b/lib/CameraOrientationTracking.o new file mode 100644 index 0000000..69556e8 Binary files /dev/null and b/lib/CameraOrientationTracking.o differ diff --git a/makefile b/makefile index 4790545..2e0036e 100644 --- a/makefile +++ b/makefile @@ -1,7 +1,8 @@ -all: ReadWrite MonoCamCalib RedBallDetection RedBallTracking +all: ReadWrite MonoCamCalib RedBallDetection RedBallTracking CameraOrientationTracking 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` + g++ lib/CameraOrientationTracking.o lib/ReadWriteFunctions.o -o bin/CameraOrientationTracking -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 @@ -14,6 +15,9 @@ RedBallDetection: src/RedBallDetection.cpp RedBallTracking: src/RedBallTracking.cpp g++ -c src/RedBallTracking.cpp -o lib/RedBallTracking.o -I./include -I/usr/include/opencv4 + +CameraOrientationTracking: src/CameraOrientationTracking.cpp + g++ -c src/CameraOrientationTracking.cpp -o lib/CameraOrientationTracking.o -I./include -I/usr/include/opencv4 clean: rm lib/*.o diff --git a/src/CameraOrientationTracking.cpp b/src/CameraOrientationTracking.cpp new file mode 100644 index 0000000..fb2f0ee --- /dev/null +++ b/src/CameraOrientationTracking.cpp @@ -0,0 +1,232 @@ +#include +#include +#include +#include +#include + +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/imgproc/imgproc.hpp" +#include + +#include "ReadWriteFunctions.h" + +#define CAM_PARAMS_FILENAME "./data/camera_calibration_params.xml" +#define CAM_CHESSBOARD_CALIB_PARAMS_FILENAME "./data/camera_chessboard_calibration_params.xml" +#define FPS 5.0 +#define BOARDSIZE_WIDTH 9 +#define BOARDSIZE_HEIGHT 6 +#define WIN_SIZE 11 +#define BOARD_SQUARE_SIZE 23.4 +#define RESOLUTION_MAX 800 + +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; +} + +int main( int argc, char** argv ) +{ + // initializes main parameters + std::string sCameraParamFilename = CAM_PARAMS_FILENAME; + std::string sCameraChessboardCalibrationParamFilename = CAM_CHESSBOARD_CALIB_PARAMS_FILENAME; + float fFPS = FPS; + int iBoardSizeWidth = BOARDSIZE_WIDTH; + int iBoardSizeHeight = BOARDSIZE_HEIGHT; + int iWinSize = WIN_SIZE; + int iSquareSize = BOARD_SQUARE_SIZE; + int iMaxVideoResolution = RESOLUTION_MAX; + + // updates main parameters from arguments + int opt; + while ((opt = getopt (argc, argv, ":f:i:o:r:")) != -1) + { + switch (opt) + { + case 'f': + fFPS = atof(optarg); + break; + case 'i': + sCameraParamFilename = optarg; + break; + case 'o': + sCameraChessboardCalibrationParamFilename = optarg; + break; + case 'r': + iMaxVideoResolution = atoi(optarg); + break; + case '?': + if (optopt == 'f' || optopt == 'i' || optopt == 'o') + 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 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 + cv::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); + + // image resolution + std::cout << "Image resolution: (" << cap.get(cv::CAP_PROP_FRAME_WIDTH) << ", " << cap.get(cv::CAP_PROP_FRAME_HEIGHT) << ")" << std::endl; + + // checks if the camera was successfully opened + if ( !cap.isOpened() ) // if not success, exit program + { + std::cout << "[ERROR] Could not open the camera!" << std::endl; + return -1; + } + + // distorted/undistorted image + bool bIsImageUndistorted = true; + + // generates the chessboard 3D points + std::vector objectPoints; + for( int i = 0; i < iBoardSizeHeight; ++i ) + for( int j = 0; j < iBoardSizeWidth; ++j ) + objectPoints.push_back(cv::Point3f(j*iSquareSize, i*iSquareSize, 0)); + //std::cout << "objectPoints = " << objectPoints << std::endl; + + // creates a vector to store the determined image points + std::vector imagePoints; + + // creates a matrix to store the transformation matrix between the chessboard and the camera + cv::Mat cameraChessboardTransform = cv::Mat::eye(4, 4, cv::DataType::type); + + // 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); + } + + // finds chessboard in the grabbed frame + imagePoints.clear(); + int chessBoardFlags = cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE; + bool isChessboardFound = findChessboardCorners( imgOriginal, cv::Size(iBoardSizeWidth, iBoardSizeHeight), imagePoints, chessBoardFlags); + //std::cout << "imagePoints = " << imagePoints << std::endl; + + if (isChessboardFound) + { + // improve the found corners' coordinate accuracy for chessboard + cv::Mat imgGray; + cvtColor(imgOriginal, imgGray, cv::COLOR_BGR2GRAY); + cornerSubPix( imgGray, imagePoints, cv::Size(iWinSize,iWinSize), cv::Size(-1,-1), cv::TermCriteria( cv::TermCriteria::EPS+cv::TermCriteria::COUNT, 30, 0.0001 )); + + // draws the corners. + //cv::drawChessboardCorners( imgOriginal, cv::Size(iBoardSizeWidth, iBoardSizeHeight), cv::Mat(imagePoints), isChessboardFound ); + + // estimates the transform matrix: translation + rotation + cv::Mat rvec(3,1,cv::DataType::type); + cv::Mat tvec(3,1,cv::DataType::type); + cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec); + + std::cout << "(Rx, Ry, Rz)= (" << rvec.at(0,0)/M_PI*180.0 << ", " << rvec.at(1,0)/M_PI*180.0 << ", " << rvec.at(2,0)/M_PI*180.0 << ")" << std::endl; + //std::cout << "(Tx, Ty, Tz)= (" << tvec.at(0,0) << ", " << tvec.at(1,0) << ", " << tvec.at(2,0) << ")"<< std::endl; + + // generates the axes 3D points + std::vector axesPoints = {cv::Point3f(iSquareSize, 0.0, 0.0), cv::Point3f(0.0, iSquareSize, 0.0), cv::Point3f(0.0, 0.0, -iSquareSize)}; + + // projects them on the image plane + std::vector projectedAxesPoints; + cv::projectPoints(axesPoints, rvec, tvec, cameraMatrix, distCoeffs, projectedAxesPoints); + + // draws the axes on the image plane + cv::line(imgOriginal, imagePoints[0], projectedAxesPoints[0], cv::Scalar(0, 0, 255), 5); + cv::line(imgOriginal, imagePoints[0], projectedAxesPoints[1], cv::Scalar(0, 255, 0), 5); + cv::line(imgOriginal, imagePoints[0], projectedAxesPoints[2], cv::Scalar(255, 0, 0), 5); + //cv::drawFrameAxes(imgOriginal, cameraMatrix, distCoeffs, rvec, tvec, 1); + + // converts the rvec, tvec to a transform matrix + cv::Mat R; + cv::Rodrigues(rvec, R); // R is 3x3 + + cameraChessboardTransform(cv::Range(0,3), cv::Range(0,3)) = R*1; + cameraChessboardTransform(cv::Range(0,3), cv::Range(3,4)) = tvec*1; + + //std::cout << "R= " << R << std::endl; + //std::cout << "tvec= " << tvec << std::endl; + //std::cout << "Rt= " << Rt << std::endl; + + //~ /// FROM 3D world to 2D uv + //~ std::cout << "FROM 3D world to 2D uv" << std::endl; + //~ // creates a 4x1 matrix (homogeneous coordinates) to store the 3D points + //~ double data3D[4] = { -1*iSquareSize , -1*iSquareSize, 0, 1 }; + //~ cv::Mat pts3D = cv::Mat(4, 1, R.type(), data3D); + //~ std::cout << "(Xw, Yw, Zw) = (" << pts3D.at(0,0) << ", " << pts3D.at(1,0) << ", " << pts3D.at(2,0) << ") " << std::endl; + //~ // applies the transform matrix + //~ //cv::Mat pos2DPx = Rt * pts3D; + //~ cv::Mat pts2D = Rt * pts3D; + //~ std::cout << "(Xc, Yc, Zc) = (" << pts2D.at(0,0) << ", " << pts2D.at(1,0) << ", " << pts2D.at(2,0) << ") " << std::endl; + + + } + + // shows the original image with the tracking (red) lines + 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; + } + if (key == 's') + { + bool bIsCalibrationSaved = writeCameraChessboardCalibrationParameters(sCameraChessboardCalibrationParamFilename, objectPoints, imagePoints, cameraChessboardTransform); + if (bIsCalibrationSaved) + std::cout << "[INFO] The transformation matrix has been saved" << std::endl; + else + std::cout << "[ERROR] The transformation matrix was not saved" << std::endl; + } + + } + + return 0; +} \ No newline at end of file diff --git a/src/RedBallDetection.cpp b/src/RedBallDetection.cpp index c898940..4b92ed6 100644 --- a/src/RedBallDetection.cpp +++ b/src/RedBallDetection.cpp @@ -10,7 +10,8 @@ #include #define CAM_PARAMS_FILENAME "./data/camera_calibration_params.xml" -#define COLOR_PARAMS_FILENAME "./data/color_params.xml" +//#define COLOR_PARAMS_FILENAME "./data/color_params.xml" +#define COLOR_PARAMS_FILENAME "./data/color_params_RGB.xml" #define FPS 30.0 #define STRUCTURAL_ELEMENTS_SIZE 5 #define RESOLUTION_MAX 800 @@ -72,8 +73,7 @@ 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_RGB.xml"; + std::string sColorParamFilename = COLOR_PARAMS_FILENAME; int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE; float fFPS = FPS; int iMaxVideoResolution = RESOLUTION_MAX;