Merge branch 'develop'
This commit is contained in:
ros
commit
a28b91f204
Binary file not shown.
|
|
@ -0,0 +1,52 @@
|
|||
<?xml version="1.0"?>
|
||||
<opencv_storage>
|
||||
<objPoints>
|
||||
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.</objPoints>
|
||||
<imagePoints>
|
||||
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</imagePoints>
|
||||
<cameraChessboardTransform type_id="opencv-matrix">
|
||||
<rows>4</rows>
|
||||
<cols>4</cols>
|
||||
<dt>d</dt>
|
||||
<data>
|
||||
-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.</data></cameraChessboardTransform>
|
||||
</opencv_storage>
|
||||
Binary file not shown.
6
makefile
6
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
|
||||
|
|
@ -15,6 +16,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
|
||||
rm bin/*
|
||||
|
|
|
|||
|
|
@ -0,0 +1,232 @@
|
|||
#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>
|
||||
|
||||
#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<cv::Point3f> 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<cv::Point2f> 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<double>::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<double>::type);
|
||||
cv::Mat tvec(3,1,cv::DataType<double>::type);
|
||||
cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
|
||||
|
||||
std::cout << "(Rx, Ry, Rz)= (" << rvec.at<double>(0,0)/M_PI*180.0 << ", " << rvec.at<double>(1,0)/M_PI*180.0 << ", " << rvec.at<double>(2,0)/M_PI*180.0 << ")" << std::endl;
|
||||
//std::cout << "(Tx, Ty, Tz)= (" << tvec.at<double>(0,0) << ", " << tvec.at<double>(1,0) << ", " << tvec.at<double>(2,0) << ")"<< std::endl;
|
||||
|
||||
// generates the axes 3D points
|
||||
std::vector<cv::Point3f> 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<cv::Point2f> 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<double>(0,0) << ", " << pts3D.at<double>(1,0) << ", " << pts3D.at<double>(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<double>(0,0) << ", " << pts2D.at<double>(1,0) << ", " << pts2D.at<double>(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;
|
||||
}
|
||||
|
|
@ -10,7 +10,8 @@
|
|||
#include <opencv2/calib3d.hpp>
|
||||
|
||||
#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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue