ENSEEIHT/TP-augmented-reality
1
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

complet

Browse source
This commit is contained in:
Laureηt 2023-06-27 19:41:19 +02:00
parent f61d4cfb48
commit 4bda1107d1
Signed by: Laurent
SSH key fingerprint: SHA256:kZEpW8cMJ54PDeCvOhzreNr4FSh6R13CMGH/POoO8DI
14 changed files with 627 additions and 612 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
.gitignore vendored
View file

@ -1 +1,3 @@
build/
deps/
3rdparty/

12
.vscode/settings.json vendored Normal file
View file

@ -0,0 +1,12 @@
{
"files.associations": {
"*.lp": "plaintext",
"*.dat": "gmpl",
"ostream": "cpp",
"*.ipp": "cpp",
"cmath": "cpp",
"complex": "cpp",
"vector": "cpp",
"iostream": "cpp"
}
}

88
src/tp/checkerboard.cpp
View file

@ -13,14 +13,13 @@
using namespace cv;
using namespace std;
// Display the help for the programm
void help( const char* programName );
void help(const char *programName);
// parse the input command line arguments
bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, Pattern &pattern );
bool parseArgs(int argc, char **argv, Size &boardSize, string &inputFilename, Pattern &pattern);
int main( int argc, char** argv )
int main(int argc, char **argv)
{
/******************************************************************/
/* CONSTANTS to use */
@ -29,14 +28,12 @@ int main( int argc, char** argv )
// the name of the window
const string WINDOW_NAME = "Image View";
/******************************************************************/
/* VARIABLES TO use */
/******************************************************************/
Mat view; // it will contain the original image loaded from file
vector<Point2f> pointbuf; // it will contain the detected corners on the chessboard
bool found; // it will be true if a chessboard is found, false otherwise
@ -50,21 +47,16 @@ int main( int argc, char** argv )
// Default pattern is chessboard
Pattern pattern = CHESSBOARD;
/******************************************************************/
/* READ THE INPUT PARAMETERS - DO NOT MODIFY */
/******************************************************************/
if( !parseArgs( argc, argv, boardSize, inputFilename, pattern ) )
if (!parseArgs(argc, argv, boardSize, inputFilename, pattern))
{
cerr << "Aborting..." << endl;
return EXIT_FAILURE;
}
/******************************************************************/
/* PART TO DEVELOP */
/******************************************************************/
@ -72,105 +64,104 @@ int main( int argc, char** argv )
/******************************************************************/
// create a window to display the image --> see namedWindow
/******************************************************************/
namedWindow(WINDOW_NAME, WINDOW_AUTOSIZE);
/******************************************************************/
// read the input image from file into "view" --> see imread
/******************************************************************/
view = imread(inputFilename, CV_LOAD_IMAGE_COLOR);
//Measure the execution time, get time before function call
auto t = ( double ) getTickCount( );
// Measure the execution time, get time before function call
auto t = (double)getTickCount();
/******************************************************************/
// call the function that detects the chessboard on the image
// found = detectChessboard...
/******************************************************************/
found = detectChessboard(view, pointbuf, boardSize, pattern);
// get time after function call and display info
t = ( ( double ) getTickCount( ) - t ) / getTickFrequency( );
t = ((double)getTickCount() - t) / getTickFrequency();
cout << ( ( !found ) ? ( "No " ) : ( "" ) ) << "chessboard detected!" << endl;
cout << ((!found) ? ("No ") : ("")) << "chessboard detected!" << endl;
cout << "Chessboard detection took " << t * 1000 << "ms" << endl;
/******************************************************************/
// if the chessboard is found draw the cornerns on top of it
// --> see drawChessboardCorners
/******************************************************************/
if (found)
{
drawChessboardCorners(view, boardSize, Mat(pointbuf), found);
}
/******************************************************************/
// show the image inside the window --> see imshow
/******************************************************************/
imshow(WINDOW_NAME, view);
// wait for user input before ending --> see waitKey
waitKey( -1 );
waitKey(-1);
return EXIT_SUCCESS;
}
// Display the help for the programm
void help( const char* programName )
void help(const char *programName)
{
cout << "Detect a chessboard in a given image" << endl
<< "Usage: " << programName << endl
<< " -w <board_width> # the number of inner corners per one of board dimension" << endl
<< " -h <board_height> # the number of inner corners per another board dimension" << endl
<< " [-pt <pattern=[circles|acircles|chess]>] # the type of pattern: chessboard or circles' grid" << endl
<< " <image file> " << endl
<< endl;
<< "Usage: " << programName << endl
<< " -w <board_width> # the number of inner corners per one of board dimension" << endl
<< " -h <board_height> # the number of inner corners per another board dimension" << endl
<< " [-pt <pattern=[circles|acircles|chess]>] # the type of pattern: chessboard or circles' grid" << endl
<< " <image file> " << endl
<< endl;
}
// parse the input command line arguments
bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, Pattern &pattern )
bool parseArgs(int argc, char **argv, Size &boardSize, string &inputFilename, Pattern &pattern)
{
// check the minimum number of arguments
if( argc < 3 )
if (argc < 3)
{
help( argv[0] );
help(argv[0]);
return false;
}
// Read the input arguments
for( int i = 1; i < argc; i++ )
for (int i = 1; i < argc; i++)
{
const char* s = argv[i];
if( strcmp( s, "-w" ) == 0 )
const char *s = argv[i];
if (strcmp(s, "-w") == 0)
{
if( sscanf( argv[++i], "%u", &boardSize.width ) != 1 || boardSize.width <= 0 )
if (sscanf(argv[++i], "%u", &boardSize.width) != 1 || boardSize.width <= 0)
{
cerr << "Invalid board width" << endl;
return false;
}
}
else if( strcmp( s, "-h" ) == 0 )
else if (strcmp(s, "-h") == 0)
{
if( sscanf( argv[++i], "%u", &boardSize.height ) != 1 || boardSize.height <= 0 )
if (sscanf(argv[++i], "%u", &boardSize.height) != 1 || boardSize.height <= 0)
{
cerr << "Invalid board height" << endl;
return false;
}
}
else if( s[0] != '-' )
else if (s[0] != '-')
{
inputFilename.assign( s );
inputFilename.assign(s);
}
else if( strcmp( s, "-pt" ) == 0 )
else if (strcmp(s, "-pt") == 0)
{
i++;
if( !strcmp( argv[i], "circles" ) )
if (!strcmp(argv[i], "circles"))
pattern = CIRCLES_GRID;
else if( !strcmp( argv[i], "acircles" ) )
else if (!strcmp(argv[i], "acircles"))
pattern = ASYMMETRIC_CIRCLES_GRID;
else if( !strcmp( argv[i], "chess" ) )
else if (!strcmp(argv[i], "chess"))
pattern = CHESSBOARD;
else
{
@ -187,4 +178,3 @@ bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, Pa
return true;
}

119
src/tp/checkerboardvideo.cpp
View file

@ -14,12 +14,12 @@ using namespace cv;
using namespace std;
// Display the help for the program
void help( const char* programName );
void help(const char *programName);
// parse the input command line arguments
bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, Pattern &pattern );
bool parseArgs(int argc, char **argv, Size &boardSize, string &inputFilename, Pattern &pattern);
int main( int argc, char** argv )
int main(int argc, char **argv)
{
/******************************************************************/
/* CONSTANTS to use */
@ -48,163 +48,157 @@ int main( int argc, char** argv )
// Used to load the video and get the frames
VideoCapture capture;
/******************************************************************/
/* READ THE INPUT PARAMETERS - DO NOT MODIFY */
/******************************************************************/
if( !parseArgs( argc, argv, boardSize, inputFilename, pattern ) )
if (!parseArgs(argc, argv, boardSize, inputFilename, pattern))
{
cerr << "Aborting..." << endl;
return EXIT_FAILURE;
}
/******************************************************************/
/* PART TO DEVELOP */
/* PART TO DEVELOP */
/******************************************************************/
/******************************************************************/
// create a window to display the image --> see namedWindow
/******************************************************************/
namedWindow(WINDOW_NAME, CV_WINDOW_AUTOSIZE);
/******************************************************************/
// read the input video with capture
/******************************************************************/
capture.open(inputFilename);
/******************************************************************/
// check it is really opened
/******************************************************************/
if (!capture.isOpened())
{
cerr << "Could not open the video file " << inputFilename << endl;
return EXIT_FAILURE;
}
// processing loop
while(true)
while (true)
{
Mat view;
Mat frame;
/******************************************************************/
// get the new frame from capture and copy it to view
// get the new frame from capture and copy it to frame
/******************************************************************/
capture >> frame;
// capture.retrieve(frame);
/******************************************************************/
// if no more images to process exit the loop
/******************************************************************/
if (frame.empty())
{
break;
}
//Measure the execution time, get time before function call
auto t = ( double ) getTickCount( );
// Measure the execution time, get time before function call
auto t = (double)getTickCount();
/******************************************************************/
// call the function that detects the chessboard on the image
// found = detectChessboard...
/******************************************************************/
found = detectChessboard(frame, pointbuf, boardSize, pattern);
// get time after function call and display info
t = ( ( double ) getTickCount( ) - t ) / getTickFrequency( );
t = ((double)getTickCount() - t) / getTickFrequency();
cout << ( ( !found ) ? ( "No " ) : ( "" ) ) << "chessboard detected!" << endl;
cout << ((!found) ? ("No ") : ("")) << "chessboard detected!" << endl;
cout << "Chessboard detection took " << t * 1000 << "ms" << endl;
/******************************************************************/
// if the chessboard is found draw the corners on top of it
// --> see drawChessboardCorners
/******************************************************************/
if (found)
{
drawChessboardCorners(frame, boardSize, Mat(pointbuf), found);
}
/******************************************************************/
// show the image inside the window --> see imshow
/******************************************************************/
imshow(WINDOW_NAME, frame);
// wait 20ms for user input before processing the next frame
// Any user input will stop the execution
if( waitKey( 10 ) >= 0 )
if (waitKey(10) >= 0)
{
break;
}
}
/******************************************************************/
// release the video resource
/******************************************************************/
return EXIT_SUCCESS;
}
// Display the help for the program
void help( const char* programName )
void help(const char *programName)
{
cout << "Detect a chessboard in a given video" << endl
<< "Usage: " << programName << endl
<< " -w <board_width> # the number of inner corners per one of board dimension" << endl
<< " -h <board_height> # the number of inner corners per another board dimension" << endl
<< " [-pt <pattern=[circles|acircles|chess]>] # the type of pattern: chessboard or circles' grid" << endl
<< " <video file> " << endl
<< endl;
<< "Usage: " << programName << endl
<< " -w <board_width> # the number of inner corners per one of board dimension" << endl
<< " -h <board_height> # the number of inner corners per another board dimension" << endl
<< " [-pt <pattern=[circles|acircles|chess]>] # the type of pattern: chessboard or circles' grid" << endl
<< " <video file> " << endl
<< endl;
}
// parse the input command line arguments
bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, Pattern &pattern )
bool parseArgs(int argc, char **argv, Size &boardSize, string &inputFilename, Pattern &pattern)
{
// check the minimum number of arguments
if( argc < 3 )
if (argc < 3)
{
help( argv[0] );
help(argv[0]);
return false;
}
// Read the input arguments
for( int i = 1; i < argc; i++ )
for (int i = 1; i < argc; i++)
{
const char* s = argv[i];
if( strcmp( s, "-w" ) == 0 )
const char *s = argv[i];
if (strcmp(s, "-w") == 0)
{
if( sscanf( argv[++i], "%u", &boardSize.width ) != 1 || boardSize.width <= 0 )
if (sscanf(argv[++i], "%u", &boardSize.width) != 1 || boardSize.width <= 0)
{
cerr << "Invalid board width" << endl;
return false;
}
}
else if( strcmp( s, "-h" ) == 0 )
else if (strcmp(s, "-h") == 0)
{
if( sscanf( argv[++i], "%u", &boardSize.height ) != 1 || boardSize.height <= 0 )
if (sscanf(argv[++i], "%u", &boardSize.height) != 1 || boardSize.height <= 0)
{
cerr << "Invalid board height" << endl;
return false;
}
}
else if( s[0] != '-' )
else if (s[0] != '-')
{
inputFilename.assign( s );
inputFilename.assign(s);
}
else if( strcmp( s, "-pt" ) == 0 )
else if (strcmp(s, "-pt") == 0)
{
i++;
if( !strcmp( argv[i], "circles" ) )
if (!strcmp(argv[i], "circles"))
pattern = CIRCLES_GRID;
else if( !strcmp( argv[i], "acircles" ) )
else if (!strcmp(argv[i], "acircles"))
pattern = ASYMMETRIC_CIRCLES_GRID;
else if( !strcmp( argv[i], "chess" ) )
else if (!strcmp(argv[i], "chess"))
pattern = CHESSBOARD;
else
{
@ -221,4 +215,3 @@ bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, Pa
return true;
}

45
src/tp/checkerboardvideoRectified.cpp
View file

@ -75,33 +75,33 @@ int main( int argc, char** argv )
/******************************************************************/
// create a window using WINDOW_NAME as name to display the image --> see namedWindow
/******************************************************************/
namedWindow(WINDOW_NAME, CV_WINDOW_AUTOSIZE);
/******************************************************************/
// create a second window using WINDOW_RECTIFIED as name to display the rectified image
/******************************************************************/
namedWindow(WINDOW_RECTIFIED, CV_WINDOW_AUTOSIZE);
/******************************************************************/
// read the input video with capture (same as before)
/******************************************************************/
capture.open(inputFilename);
/******************************************************************/
// check it is really opened
/******************************************************************/
if(!capture.isOpened())
{
cerr << "Could not open the video file " << inputFilename << endl;
return EXIT_FAILURE;
}
/******************************************************************/
// create the set of 2D (arbitrary) points of the checkerboard, let's say the
// size of the squares is 25
// call to calcChessboardCorners
/******************************************************************/
calcChessboardCorners(boardSize, 25, objectPoints, pattern);
// processing loop
while( true )
@ -110,19 +110,18 @@ int main( int argc, char** argv )
/******************************************************************/
// get the new frame from capture and copy it to view
/******************************************************************/
capture >> view;
/******************************************************************/
// if no more images to process exit the loop
/******************************************************************/
if(view.empty())
break;
/******************************************************************/
// call the function that detects the chessboard on the image
/******************************************************************/
// found = detectChessboard...
found = detectChessboard(view, pointbuf, boardSize, pattern);
cout << ( ( !found ) ? ( "No " ) : ( "" ) ) << "chessboard detected!" << endl;
@ -134,14 +133,14 @@ int main( int argc, char** argv )
// --> see findHomography
// http://docs.opencv.org/2.4.13.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?highlight=homography#findhomography
/******************************************************************/
Mat H = findHomography(pointbuf, objectPoints, CV_RANSAC);
/******************************************************************/
// use the estimated homography to rectify the image
// --> see warpPerspective
// http://docs.opencv.org/2.4.13.4/modules/imgproc/doc/geometric_transformations.html#void warpPerspective(InputArray src, OutputArray dst, InputArray M, Size dsize, int flags, int borderMode, const Scalar& borderValue)
/******************************************************************/
warpPerspective(view, rectified, H, view.size());
}
else
{
@ -149,26 +148,25 @@ int main( int argc, char** argv )
// otherwise copy the original image in rectified
// Mat.copyTo()
/******************************************************************/
view.copyTo(rectified);
}
/******************************************************************/
// if the chessboard is found draw the cornerns on top of it
// --> see drawChessboardCorners
/******************************************************************/
if(found)
drawChessboardCorners(view, boardSize, Mat(pointbuf), found);
/******************************************************************/
// show the image inside the window --> see imshow
/******************************************************************/
imshow(WINDOW_NAME, view);
/******************************************************************/
// show the rectified image inside the window --> see imshow
/******************************************************************/
imshow(WINDOW_RECTIFIED, rectified);
// wait 20ms for user input before processing the next frame
// Any user input will stop the execution
@ -179,8 +177,7 @@ int main( int argc, char** argv )
/******************************************************************/
// release the video resource
/******************************************************************/
capture.release();
return EXIT_SUCCESS;
}

66
src/tp/checkerboardvideoUndistort.cpp
View file

@ -35,31 +35,32 @@ bool loadCameraParameters( const string &calibFilename, Mat &matK, Mat &dist )
{
// object that will parse the file
FileStorage fs;
/******************************************************************/
// open the file to read the parameters
// --> see method open() of FileStorage
/******************************************************************/
fs.open(calibFilename, FileStorage::READ);
/******************************************************************/
// check if the file has been found/opened
// --> see isOpened()
/******************************************************************/
if (!fs.isOpened())
{
cerr << "Could not open the calibration file" << endl;
return false;
}
/******************************************************************/
// load the camera matrix from the tag "camera_matrix" of the file
/******************************************************************/
fs["camera_matrix"] >> matK;
/******************************************************************/
// load the distortion coefficients from the tag "distortion_coefficients" of the file
/******************************************************************/
fs["distortion_coefficients"] >> dist;
cout << matK << endl;
cout << dist << endl;
@ -98,7 +99,6 @@ int main( int argc, char** argv )
// variable used to read the user input
int mode = 'o';
/******************************************************************/
/* READ THE INPUT PARAMETERS - DO NOT MODIFY */
/******************************************************************/
@ -109,9 +109,6 @@ int main( int argc, char** argv )
return EXIT_FAILURE;
}
/******************************************************************/
/* PART TO DEVELOP */
/******************************************************************/
@ -119,43 +116,41 @@ int main( int argc, char** argv )
/******************************************************************/
// create a window using WINDOW_NAME as name to display the image --> see namedWindow
/******************************************************************/
namedWindow( WINDOW_NAME, CV_WINDOW_AUTOSIZE );
/******************************************************************/
// read the input video with capture (same as before)
/******************************************************************/
capture.open(inputFilename);
/******************************************************************/
// check it is really opened
/******************************************************************/
if( !capture.isOpened() )
{
cerr << "Could not open the input video: " << inputFilename << endl;
return EXIT_FAILURE;
}
/******************************************************************/
// call to loadCameraParameters. we want to read the calibration
// matrix in matK and the distortion coefficients in dist
/******************************************************************/
if( !loadCameraParameters( calibFilename, matK, dist ) )
{
cerr << "Aborting..." << endl;
return EXIT_FAILURE;
}
// processing loop
while( true )
while(true)
{
Mat view;
/******************************************************************/
// get the new frame from capture and copy it to view
/******************************************************************/
capture >> view;
if( view.empty( ) )
break;
@ -174,21 +169,22 @@ int main( int argc, char** argv )
/******************************************************************/
// copy the original image into temp --> see Mat.clone()
/******************************************************************/
view.copyTo(temp);
/******************************************************************/
// apply the undistortion and store the new image in view
// --> see undistort
/******************************************************************/
undistort(temp, view, matK, dist);
/******************************************************************/
// compute the difference between the two images and store the result in view
// see --> absdiff
/******************************************************************/
absdiff(temp, view, view);
}
// if we want to see the undistorted image
// if we want to see the undistorted image
else if( mode == 'u' )
{
msg = "(o)riginal, (d)ifference";
@ -198,13 +194,13 @@ int main( int argc, char** argv )
/******************************************************************/
// copy the original image into temp --> see Mat.clone()
/******************************************************************/
view.copyTo(temp);
/******************************************************************/
// apply the undistortion and store the new image in view
// --> see undistort
/******************************************************************/
undistort(temp, view, matK, dist);
}
else
{
@ -221,7 +217,7 @@ int main( int argc, char** argv )
/******************************************************************/
// show view inside the window --> see imshow
/******************************************************************/
imshow(WINDOW_NAME, view);
// wait 20ms for user input before processing the next frame
// Any user input will stop the execution

24
src/tp/tracker/Camera.cpp
View file

@ -16,33 +16,35 @@ bool Camera::init( const std::string& calibFilename )
/******************************************************************/
// open the file storage with the given filename
/******************************************************************/
FileStorage fs( calibFilename, FileStorage::READ );
/******************************************************************/
// check if the file storage has been opened correclty
/******************************************************************/
if( !fs.isOpened() )
{
cerr << "Could not open the calibration file: " << calibFilename << endl;
return false;
}
/******************************************************************/
// load the camera_matrix in matK
/******************************************************************/
fs["camera_matrix"] >> matK;
/******************************************************************/
// load the distortion_coefficients in distCoeff
/******************************************************************/
fs["distortion_coefficients"] >> distCoeff;
/******************************************************************/
// load image_width and image_height in imageSize.[width|height]
/******************************************************************/
fs["image_width"] >> imageSize.width;
fs["image_height"] >> imageSize.height;
// cout << matK << endl;
// cout << distCoeff << endl;
// cout << matK << endl;
// cout << distCoeff << endl;
return true;
}
@ -56,8 +58,6 @@ bool Camera::init( const std::string& calibFilename )
*/
void Camera::getOGLProjectionMatrix( float *proj, float znear, float zfar ) const
{
// With window_coords==’y down’, we have:
//
// [2*K00/width, -2*K01/width, (width - 2*K02 + 2*x0)/width, 0]

36
src/tp/tracker/ChessboardCameraTracker.cpp
View file

@ -31,13 +31,29 @@ bool ChessboardCameraTracker::process( cv::Mat &view, cv::Mat &pose, const Camer
// undistort the input image. view at the end must contain the undistorted version
// of the image.
//******************************************************************/
Mat temp;
view.copyTo(temp);
undistort(temp, view, cam.matK, cam.distCoeff);
temp.copyTo(view);
//******************************************************************/
// detect the chessboard
//******************************************************************/
switch (pattern)
{
case CHESSBOARD:
found = findChessboardCorners(view, boardSize, corners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);
break;
case CIRCLES_GRID:
found = findCirclesGrid(view, boardSize, corners);
break;
case ASYMMETRIC_CIRCLES_GRID:
found = findCirclesGrid(view, boardSize, corners, CALIB_CB_ASYMMETRIC_GRID);
break;
default:
found = false;
break;
}
// cout << ( (!found ) ? ( "No " ) : ("") ) << "chessboard detected!" << endl;
@ -46,7 +62,6 @@ bool ChessboardCameraTracker::process( cv::Mat &view, cv::Mat &pose, const Camer
//******************************************************************/
if( found )
{
// contains the points on the chessboard
vector<Point2f> objectPoints;
@ -54,24 +69,23 @@ bool ChessboardCameraTracker::process( cv::Mat &view, cv::Mat &pose, const Camer
// create the set of 2D (arbitrary) points of the checkerboard
// call to calcChessboardCorners
//******************************************************************/
calcChessboardCorners(boardSize, 1, objectPoints, pattern);
//******************************************************************/
// estimate the homography
// --> see findHomography
// http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?highlight=homography#findhomography
//******************************************************************/
Mat H = findHomography(objectPoints, corners, CV_RANSAC);
// cout << "H = " << H << endl << endl;
// cout << "corners =" << corners << endl << endl;
// cout << "ptsOb =" << objectPoints << endl << endl;
// cout << "H = " << H << endl << endl;
// cout << "corners =" << corners << endl << endl;
// cout << "ptsOb =" << objectPoints << endl << endl;
//******************************************************************/
// decompose the homography
//******************************************************************/
decomposeHomography(H, cam.matK, pose);
}
return found;

52
src/tp/tracker/ChessboardCameraTrackerKLT.cpp
View file

@ -25,13 +25,14 @@ bool ChessboardCameraTrackerKLT::process( cv::Mat &view, cv::Mat &pose, const Ca
// true if the chessboard is found
bool found = false;
Mat temp = view.clone( ); // used for correcting the optical distortion
Mat temp = view.clone(); // used for correcting the optical distortion
//******************************************************************/
// undistort the input image. view at the end must contain the undistorted version
// of the image.
//******************************************************************/
Mat viewUndistorted;
undistort( view, viewUndistorted, cam.matK, cam.distCoeff );
// contains the grey version of the current frame
Mat viewGrey;
@ -39,7 +40,7 @@ bool ChessboardCameraTrackerKLT::process( cv::Mat &view, cv::Mat &pose, const Ca
//******************************************************************/
// convert the current left frame into greylevel image
//******************************************************************/
cvtColor( viewUndistorted, viewGrey, CV_BGR2GRAY );
// if we have too few points or none
if( _corners.size( ) < 10 )
@ -47,7 +48,7 @@ bool ChessboardCameraTrackerKLT::process( cv::Mat &view, cv::Mat &pose, const Ca
//******************************************************************/
// detect the chessboard
//******************************************************************/
found = findChessboardCorners( viewGrey, boardSize, _corners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS );
if( found )
{
@ -55,27 +56,24 @@ bool ChessboardCameraTrackerKLT::process( cv::Mat &view, cv::Mat &pose, const Ca
// generate the points on the chessboard, this time 3D points
// see --> calcChessboardCorners3D
//******************************************************************/
calcChessboardCorners3D( boardSize, 1, _objectPoints, pattern );
//******************************************************************/
// compute the pose of the camera using mySolvePnPRansac (utility.hpp))
//******************************************************************/
mySolvePnPRansac(_objectPoints, _corners, cam.matK, cam.distCoeff, pose);
}
}
else
{
// use klt to track the points
else
{ // use klt to track the points
// some parameters for the optical flow algorithm
Size winSize( 11, 11 );
TermCriteria termcrit( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03 );
// vector where the estimated tracked points of the new frame will be stored
vector<Point2f> currPts;
// auxiliary stuff for the optical flow computation
// status has will have the same length as currPts and is 0 if the
// optical flow estimation for the corresponding new point is not good
@ -85,7 +83,7 @@ bool ChessboardCameraTrackerKLT::process( cv::Mat &view, cv::Mat &pose, const Ca
//******************************************************************/
// estimate the new position of the tracked points using calcOpticalFlowPyrLK
//******************************************************************/
calcOpticalFlowPyrLK( _prevGrey, viewGrey, _corners, currPts, status, err, winSize, 3, termcrit, 0, 0.001 );
//******************************************************************/
// Filter currPts and update the lists _corners and _objectPoints: if
@ -103,31 +101,30 @@ bool ChessboardCameraTrackerKLT::process( cv::Mat &view, cv::Mat &pose, const Ca
// if it's a good point copy it in _corners and also copy keep the
// corresponding _objectPoints
//******************************************************************/
// if..
if (status[i] > 0)
{
#if DEBUGGING
#if DEBUGGING
line( view, _corners[ i ], currPts[ i ], Scalar( 255, 0, 0 ), 1 );
circle( view, currPts[ i ], 3, Scalar( 255, 0, 255 ), -1, 8 );
#endif
#endif
//******************************************************************/
// copy the current point in _corners
//******************************************************************/
_corners[k] = currPts[i];
//******************************************************************/
// copy the corresponding _objectPoints
//******************************************************************/
_objectPoints[k] = _objectPoints[i];
//******************************************************************/
// update k
//******************************************************************/
++k;
}
}
// resize the two vector to the size k, the number of "well" tracked features
_corners.resize( k );
_objectPoints.resize( k );
@ -138,9 +135,10 @@ bool ChessboardCameraTrackerKLT::process( cv::Mat &view, cv::Mat &pose, const Ca
//******************************************************************/
// compute the pose of the camera using mySolvePnPRansac (utility.hpp))
//******************************************************************/
mySolvePnPRansac(_objectPoints, _corners, cam.matK, cam.distCoeff, pose, idxInl);
// _vecCorners.push_back(_corners);
//// _vecObjectPoints.push_back(_objectPoints);
// _vecObjectPoints.push_back(_objectPoints);
// _vecIdx.push_back(_indices);
//******************************************************************/
@ -148,15 +146,13 @@ bool ChessboardCameraTrackerKLT::process( cv::Mat &view, cv::Mat &pose, const Ca
// Filter both the image points and the 3D reference points
//******************************************************************/
found = true;
}
//******************************************************************/
// update _prevGrey with the current grey frame
//******************************************************************/
viewGrey.copyTo( _prevGrey );
return found;
}

2
src/tp/tracker/tracker/utility.hpp
View file

@ -7,7 +7,7 @@
#include <iostream>
#define DEBUGGING 1
#define DEBUGGING 0
#if DEBUGGING
#define PRINTVAR( a ) std::cout << #a << " = " << (a) << endl << endl;

410
src/tp/tracker/utility.cpp
View file

@ -8,8 +8,6 @@
using namespace cv;
using namespace std;
/******************************************************************/
/* FUNCTIONS TO DEVELOP */
/******************************************************************/
@ -18,71 +16,70 @@ using namespace std;
* Detect a chessboard in a given image
*
* @param[in] rgbimage The rgb image to process
* @param[out] pointbuf the set of 2D image corner detected on the chessboard
* @param[out] pointbuf the set of 2D image corner detected on the chessboard
* @param[in] boardSize the size of the board in terms of corners (width X height)
* @param[in] patternType The type of chessboard pattern to look for
* @return true if the chessboard is detected inside the image, false otherwise
* @return true if the chessboard is detected inside the image, false otherwise
*/
bool detectChessboard( const Mat &rgbimage, vector<Point2f> &pointbuf, const Size &boardSize, Pattern patternType )
bool detectChessboard(const Mat &rgbimage, vector<Point2f> &pointbuf, const Size &boardSize, Pattern patternType)
{
// it contains the value to return
bool found = false;
switch( patternType )
switch (patternType)
{
// detect a classic chessboard
case CHESSBOARD:
// detect a classic chessboard
case CHESSBOARD:
/******************************************************************/
// detect the chessboard --> see findChessboardCorners
// found = ...
/******************************************************************/
found = findChessboardCorners(rgbimage, boardSize, pointbuf, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);
// if a chessboard is found refine the position of the points in a window 11x11 pixel
// use the default value for the termination criteria --> TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 )
if (found)
{
Mat viewGrey; // it will contain the graylevel version of the image
/******************************************************************/
// detect the chessboard --> see findChessboardCorners
// found = ...
// convert the image in "rgbimage" to gray level and save it in "viewGrey"
// --> cvtColor with CV_BGR2GRAY option
/******************************************************************/
cvtColor(rgbimage, viewGrey, CV_BGR2GRAY);
// if a chessboard is found refine the position of the points in a window 11x11 pixel
// use the default value for the termination criteria --> TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 )
if( found )
{
Mat viewGrey; // it will contain the graylevel version of the image
/******************************************************************/
// convert the image in "rgbimage" to gray level and save it in "viewGrey"
// --> cvtColor with CV_BGR2GRAY option
/******************************************************************/
/******************************************************************/
// refine the corner location in "pointbuf" using "viewGrey"
// --> see cornerSubPix
/******************************************************************/
}
break;
// detect a regular grid made of circles
case CIRCLES_GRID:
/******************************************************************/
// detect the circles --> see findCirclesGrid
// found = ...
// refine the corner location in "pointbuf" using "viewGrey"
// --> see cornerSubPix
/******************************************************************/
cornerSubPix(viewGrey, pointbuf, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
}
break;
// detect a regular grid made of circles
case CIRCLES_GRID:
/******************************************************************/
// detect the circles --> see findCirclesGrid
// found = ...
/******************************************************************/
found = findCirclesGrid(rgbimage, boardSize, pointbuf, CALIB_CB_SYMMETRIC_GRID);
break;
break;
// detect an asymmetric grid made of circles
case ASYMMETRIC_CIRCLES_GRID:
/******************************************************************/
// detect the circles --> see findCirclesGrid using the options CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING
// found = ...
/******************************************************************/
// detect an asymmetric grid made of circles
case ASYMMETRIC_CIRCLES_GRID:
/******************************************************************/
// detect the circles --> see findCirclesGrid using the options CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING
// found = ...
/******************************************************************/
found = findCirclesGrid(rgbimage, boardSize, pointbuf, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
break;
break;
default:
cerr << "Unknown pattern type" << endl;
return found;
default:
cerr << "Unknown pattern type" << endl;
return found;
}
return found;
@ -97,20 +94,19 @@ bool detectChessboard( const Mat &rgbimage, vector<Point2f> &pointbuf, const Siz
* @param[in] scale A scale factor for the unit vectors to draw
* @param[in] alreadyUndistorted A boolean value that tells if the input image rgbimage is already undistorted or we are working on a distorted image
*/
void drawReferenceSystem( cv::Mat &rgbimage, const Camera& cam, const cv::Mat &poseMat, const int &thickness, const double &scale, bool alreadyUndistorted )
void drawReferenceSystem(cv::Mat &rgbimage, const Camera &cam, const cv::Mat &poseMat, const int &thickness, const double &scale, bool alreadyUndistorted)
{
// contains the points to project to draw the 3 axis
vector<Point3f> vertex3D;
//******************************************************************/
// Add the four 3D points (Point3f) that we can use to draw
// the reference system to vertex3D. Use <scale> as unit
//******************************************************************/
vertex3D.push_back(Point3f(0, 0, 0));
vertex3D.push_back(Point3f(0, scale, 0));
vertex3D.push_back(Point3f(scale, 0, 0));
vertex3D.push_back(Point3f(0, 0, -scale));
// contains the projected 3D points on the image
vector<Point2f> imgRefPts;
@ -121,14 +117,7 @@ void drawReferenceSystem( cv::Mat &rgbimage, const Camera& cam, const cv::Mat &p
// if it is true we pass a 1x5 zero vector, otherwise the distortion
// parameter of cam
//******************************************************************/
myProjectPoints(vertex3D, poseMat, cam.matK, alreadyUndistorted ? Mat::zeros(1, 5, CV_64F) : cam.distCoeff, imgRefPts);
// cout << "vertex3D" << vertex3D << endl;
// cout << "imgRefPts" << imgRefPts << endl;
@ -136,38 +125,37 @@ void drawReferenceSystem( cv::Mat &rgbimage, const Camera& cam, const cv::Mat &p
//******************************************************************/
// draw the line of the x-axis and put "X" at the end
//******************************************************************/
line(rgbimage, imgRefPts[0], imgRefPts[1], Scalar(0, 0, 255), thickness);
putText(rgbimage, "X", imgRefPts[1], FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 255), thickness);
//******************************************************************/
// draw the line of the y-axis and put "Y" at the end
//******************************************************************/
line(rgbimage, imgRefPts[0], imgRefPts[2], Scalar(0, 255, 0), thickness);
putText(rgbimage, "Y", imgRefPts[2], FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 255, 0), thickness);
//******************************************************************/
// draw the line of the z-axis and put "Z" at the end
//******************************************************************/
line(rgbimage, imgRefPts[0], imgRefPts[3], Scalar(255, 0, 0), thickness);
putText(rgbimage, "Z", imgRefPts[3], FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 0, 0), thickness);
}
/**
* Wrapper around the original opencv's projectPoints
*
*
* @param objectPoints the 3D points
* @param poseMat the pose matrix
* @param cameraMatrix the calibration matrix
* @param distCoeffs the distortion coeffi
* @param imagePoints
*/
void myProjectPoints( InputArray objectPoints, const Mat &poseMat, InputArray cameraMatrix, InputArray distCoeffs, OutputArray imagePoints )
void myProjectPoints(InputArray objectPoints, const Mat &poseMat, InputArray cameraMatrix, InputArray distCoeffs, OutputArray imagePoints)
{
Mat rvec;
Rodrigues( poseMat.colRange( 0, 3 ), rvec );
Rodrigues(poseMat.colRange(0, 3), rvec);
// projectPoints( Mat( vertex3D.t( ) ).reshape( 3, 1 ), rvec, Tvec, K, dist, imgRefPts );
projectPoints( objectPoints, rvec, poseMat.col( 3 ), cameraMatrix, distCoeffs, imagePoints );
projectPoints(objectPoints, rvec, poseMat.col(3), cameraMatrix, distCoeffs, imagePoints);
}
/**
@ -175,46 +163,57 @@ void myProjectPoints( InputArray objectPoints, const Mat &poseMat, InputArray ca
*
* @param[in] boardSize the size of the board in terms of corners (width X height)
* @param[in] squareSize the size in mm of the each square of the chessboard
* @param[out] corners the set of 2D points on the chessboard
* @param[out] corners the set of 2D points on the chessboard
* @param[in] patternType The type of chessboard pattern to look for
*/
void calcChessboardCorners( const Size &boardSize, const float &squareSize, vector<Point2f>& corners, Pattern patternType )
void calcChessboardCorners(const Size &boardSize, const float &squareSize, vector<Point2f> &corners, Pattern patternType)
{
corners.resize( 0 );
corners.reserve( boardSize.height * boardSize.width );
corners.resize(0);
corners.reserve(boardSize.height * boardSize.width);
switch( patternType )
switch (patternType)
{
case CHESSBOARD:
case CIRCLES_GRID:
for( int i = 0; i < boardSize.height; i++ )
for( int j = 0; j < boardSize.width; j++ )
{
/******************************************************************/
// create a Point2f(x,y) according to the position j,i and a square
// size of squareSize. Add it to corners (using push_back...)
/******************************************************************/
case CHESSBOARD:
case CIRCLES_GRID:
for (int i = 0; i < boardSize.height; i++)
{
for (int j = 0; j < boardSize.width; j++)
{
/******************************************************************/
// create a Point2f(x,y) according to the position j,i and a square
// size of squareSize. Add it to corners (using push_back...)
/******************************************************************/
corners.push_back(
Point2f(
j * squareSize,
i * squareSize
)
);
}
}
break;
case ASYMMETRIC_CIRCLES_GRID:
for (int i = 0; i < boardSize.height; i++)
{
for (int j = 0; j < boardSize.width; j++)
{
/******************************************************************/
// create a Point2f(x,y) according to the position j,i considering
// that x is generate using the formula (2*j + i % 2)*squareSize
// Add it to corners (using push_back...)
/******************************************************************/
corners.push_back(
Point2f(
(2 * j + i % 2) * squareSize,
i * squareSize
)
);
}
}
break;
}
break;
case ASYMMETRIC_CIRCLES_GRID:
for( int i = 0; i < boardSize.height; i++ )
for( int j = 0; j < boardSize.width; j++ )
{
/******************************************************************/
// create a Point2f(x,y) according to the position j,i considering
// that x is generate using the formula (2*j + i % 2)*squareSize
// Add it to corners (using push_back...)
/******************************************************************/
}
break;
default:
CV_Error( CV_StsBadArg, "Unknown pattern type\n" );
default:
CV_Error(CV_StsBadArg, "Unknown pattern type\n");
}
}
@ -225,133 +224,147 @@ void calcChessboardCorners( const Size &boardSize, const float &squareSize, vect
* @param[in] matK The 3x3 calibration matrix K
* @param[out] poseMat the 3x4 pose matrix [R t]
*/
void decomposeHomography( const Mat &H, const Mat& matK, Mat& poseMat )
void decomposeHomography(const Mat &H, const Mat &matK, Mat &poseMat)
{
Mat temp;
//******************************************************************/
//temp contains inv(K)*H
// temp contains inv(K)*H
//******************************************************************/
// PRINTVAR( temp );
temp = matK.inv() * H;
// PRINTVAR( temp );
Mat r1, r2, r3, t;
//******************************************************************/
// get r1 and r2 from temp
//******************************************************************/
r1 = temp.col(0);
r2 = temp.col(1);
t = temp.col(2);
// cout << "r1: " << r1 << endl;
// cout << "r2: " << r2 << endl;
// cout << "t: " << t << endl;
//******************************************************************/
// compute lambda
//******************************************************************/
float lambda;
Mat prod = r1.t() * r1;
// cout << "prod: " << prod << endl;
lambda = sqrt(prod.at<double>(0));
// cout << "lambda: " << lambda << endl;
//******************************************************************/
// normalize r1 and r2
// normalize r1 r2 and t
//******************************************************************/
if (lambda != 0)
{
r1 = r1 / lambda;
r2 = r2 / lambda;
t = t / lambda;
}
//******************************************************************/
// compute r3
//******************************************************************/
r3 = r1.cross(r2);
// PRINTVAR( r3 );
//******************************************************************/
// compute t
//******************************************************************/
// PRINTVAR( r3 );
//******************************************************************/
// create a 3x4 matrix (float) for poseMat
//******************************************************************/
poseMat = Mat::zeros(3, 4, CV_32F);
//******************************************************************/
// fill the columns of poseMat with r1 r2 r3 and t
//******************************************************************/
// cout << "*r1: " << r1 << endl;
// cout << "*r2: " << r2 << endl;
// cout << "*t: " << t << endl;
poseMat.at<float>(0, 0) = r1.at<double>(0);
poseMat.at<float>(1, 0) = r1.at<double>(1);
poseMat.at<float>(2, 0) = r1.at<double>(2);
poseMat.at<float>(0, 1) = r2.at<double>(0);
poseMat.at<float>(1, 1) = r2.at<double>(1);
poseMat.at<float>(2, 1) = r2.at<double>(2);
poseMat.at<float>(0, 2) = r3.at<double>(0);
poseMat.at<float>(1, 2) = r3.at<double>(1);
poseMat.at<float>(2, 2) = r3.at<double>(2);
poseMat.at<float>(0, 3) = t.at<double>(0);
poseMat.at<float>(1, 3) = t.at<double>(1);
poseMat.at<float>(2, 3) = t.at<double>(2);
// PRINTVAR( poseMat );
// cout << "poseMat: " << poseMat << endl;
}
/******************************************************************************/
//KLT ONLY
// KLT ONLY
/**
* Generate the set of 3D points of a chessboard
*
* @param[in] boardSize the size of the board in terms of corners (width X height)
* @param[in] squareSize the size in mm of the each square of the chessboard
* @param[out] corners the set of 3D points on the chessboard
* @param[out] corners the set of 3D points on the chessboard
* @param[in] patternType The type of chessboard pattern to look for
*/
void calcChessboardCorners3D( const Size &boardSize, const float &squareSize, vector<Point3f>& corners, Pattern patternType )
void calcChessboardCorners3D(const Size &boardSize, const float &squareSize, vector<Point3f> &corners, Pattern patternType)
{
corners.resize( 0 );
corners.reserve( boardSize.height * boardSize.width );
corners.resize(0);
corners.reserve(boardSize.height * boardSize.width);
switch( patternType )
switch (patternType)
{
case CHESSBOARD:
case CIRCLES_GRID:
for( int i = 0; i < boardSize.height; i++ )
for( int j = 0; j < boardSize.width; j++ )
{
/******************************************************************/
// create a Point3f(x,y,0) according to the position j,i and a square
// size of squareSize. Add it to corners (using push_back...)
/******************************************************************/
case CHESSBOARD:
case CIRCLES_GRID:
for (int i = 0; i < boardSize.height; i++)
for (int j = 0; j < boardSize.width; j++)
{
/******************************************************************/
// create a Point3f(x,y,0) according to the position j,i and a square
// size of squareSize. Add it to corners (using push_back...)
/******************************************************************/
corners.push_back(
Point3f(
j * squareSize,
i * squareSize,
0
)
);
}
break;
case ASYMMETRIC_CIRCLES_GRID:
for (int i = 0; i < boardSize.height; i++)
for (int j = 0; j < boardSize.width; j++)
{
/******************************************************************/
// create a Point3f(x,y,0) according to the position j,i considering
// that x is generate using the formula (2*j + i % 2)*squareSize
// Add it to corners (using push_back...)
/******************************************************************/
corners.push_back(
Point3f(
(2 * j + i % 2) * squareSize,
i * squareSize,
0
)
);
}
break;
}
break;
case ASYMMETRIC_CIRCLES_GRID:
for( int i = 0; i < boardSize.height; i++ )
for( int j = 0; j < boardSize.width; j++ )
{
/******************************************************************/
// create a Point3f(x,y,0) according to the position j,i considering
// that x is generate using the formula (2*j + i % 2)*squareSize
// Add it to corners (using push_back...)
/******************************************************************/
}
break;
default:
CV_Error( CV_StsBadArg, "Unknown pattern type\n" );
default:
CV_Error(CV_StsBadArg, "Unknown pattern type\n");
}
}
/**
* Wrapper around the original opencv's solvePnPRansac
*
*
* @param[in] objectPoints the 3D points
* @param[in] imagePoints the image points
* @param[in] cameraMatrix the calibration matrix
@ -359,34 +372,33 @@ void calcChessboardCorners3D( const Size &boardSize, const float &squareSize, ve
* @param[out] poseMat the pose matrix
* @param[out] inliers the list of indices of the inliers points
*/
void mySolvePnPRansac( cv::InputArray objectPoints, cv::InputArray imagePoints, cv::InputArray cameraMatrix, cv::InputArray distCoeffs, cv::Mat &poseMat, OutputArray inliers )
void mySolvePnPRansac(cv::InputArray objectPoints, cv::InputArray imagePoints, cv::InputArray cameraMatrix, cv::InputArray distCoeffs, cv::Mat &poseMat, OutputArray inliers)
{
Mat currR, currT;
solvePnPRansac( objectPoints, imagePoints, cameraMatrix, distCoeffs, currR, currT, false, 100, 4, 100, inliers );
solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs, currR, currT, false, 100, 4, 100, inliers);
poseMat = Mat( 3, 4, CV_32F );
poseMat = Mat(3, 4, CV_32F);
Mat Rot;
Rodrigues( currR, Rot );
Rodrigues(currR, Rot);
#if CV_MINOR_VERSION < 4
// apparently older versions does not support direct copy
Mat temp;
Rot.convertTo( temp, CV_32F );
Mat a1 = poseMat.colRange( 0, 3 );
temp.copyTo( a1 );
a1 = poseMat.col( 3 );
currT.convertTo( temp, CV_32F );
temp.copyTo( a1 );
Rot.convertTo(temp, CV_32F);
Mat a1 = poseMat.colRange(0, 3);
temp.copyTo(a1);
a1 = poseMat.col(3);
currT.convertTo(temp, CV_32F);
temp.copyTo(a1);
#else
Rot.copyTo( poseMat.colRange( 0, 3 ) );
currT.copyTo( poseMat.col( 3 ) );
Rot.copyTo(poseMat.colRange(0, 3));
currT.copyTo(poseMat.col(3));
#endif
}
bool getVideoSizeAndType(const std::string &videoFilename, cv::VideoCapture capture, cv::Size &singleSize, int &imgInType)
{
if( !capture.isOpened( ) )
if (!capture.isOpened())
{
cerr << "Could not open video file " << videoFilename << endl;
return false;
@ -395,17 +407,17 @@ bool getVideoSizeAndType(const std::string &videoFilename, cv::VideoCapture capt
// open video file and get the first image just to get the image size
Mat view0;
capture >> view0;
if( view0.empty( ) )
if (view0.empty())
{
cerr << "Could not get the first frame of the video file " << videoFilename << endl;
return false;
}
imgInType = view0.type( );
singleSize = view0.size( );
imgInType = view0.type();
singleSize = view0.size();
// close capture...
capture.release( );
capture.release();
// and re-open it so that we will start from the first frame again
return capture.open(videoFilename);
}

28
src/tp/tracking.cpp
View file

@ -56,8 +56,6 @@ int main( int argc, char** argv )
// 3x4 camera pose matrix [R t]
Mat cameraPose;
/******************************************************************/
/* READ THE INPUT PARAMETERS - DO NOT MODIFY */
/******************************************************************/
@ -68,9 +66,6 @@ int main( int argc, char** argv )
return EXIT_FAILURE;
}
/******************************************************************/
/* PART TO DEVELOP */
/******************************************************************/
@ -97,8 +92,11 @@ int main( int argc, char** argv )
//******************************************************************/
// init the Camera loading the calibration parameters
//******************************************************************/
if( !cam.init( calibFilename ) )
{
cerr << "Could not load calibration file " << calibFilename << endl;
return EXIT_FAILURE;
}
// processing loop
while(true)
@ -107,29 +105,30 @@ int main( int argc, char** argv )
/******************************************************************/
// get the new frame from capture and copy it to view
/******************************************************************/
capture >> view;
/******************************************************************/
// if no more images to process exit the loop
/******************************************************************/
if( view.empty() )
break;
//******************************************************************/
// process the image with the process method
//******************************************************************/
//if...
if ( tracker.process( view, cameraPose, cam, boardSize, pattern ) )
{
//******************************************************************/
// draw the reference on top of the image
//******************************************************************/
drawReferenceSystem( view, cam, cameraPose, 2, 5, false );
}
/******************************************************************/
// show the image inside the window --> see imshow
/******************************************************************/
imshow( WINDOW_NAME, view );
// wait 20ms for user input before processing the next frame
// Any user input will stop the execution
@ -145,8 +144,7 @@ int main( int argc, char** argv )
/******************************************************************/
// release the video resource
/******************************************************************/
capture.release();
return EXIT_SUCCESS;
}

22
src/tp/trackingKLT.cpp
View file

@ -55,8 +55,6 @@ int main( int argc, char** argv )
// 3x4 camera pose matrix [R t]
Mat cameraPose;
/******************************************************************/
/* READ THE INPUT PARAMETERS - DO NOT MODIFY */
/******************************************************************/
@ -66,9 +64,6 @@ int main( int argc, char** argv )
return EXIT_FAILURE;
}
/******************************************************************/
/* PART TO DEVELOP */
/******************************************************************/
@ -95,8 +90,7 @@ int main( int argc, char** argv )
//******************************************************************/
// init the Camera loading the calibration parameters
//******************************************************************/
cam.init( calibFilename );
// processing loop
while( true )
@ -105,28 +99,33 @@ int main( int argc, char** argv )
/******************************************************************/
// get the new frame from capture and copy it to view
/******************************************************************/
capture >> view;
/******************************************************************/
// if no more images to process exit the loop
/******************************************************************/
if( view.empty( ) )
{
break;
}
//******************************************************************/
// process the image
//******************************************************************/
//if...
if ( tracker.process( view, cameraPose, cam, boardSize, pattern ) )
{
//******************************************************************/
// draw the reference on top of the image
//******************************************************************/
// drawCameraPose( view, cam, cameraPose, boardSize, pattern );
drawReferenceSystem( view, cam, cameraPose, 2, 5, true );
}
/******************************************************************/
// show the image inside the window --> see imshow
/******************************************************************/
imshow( WINDOW_NAME, view );
// wait 20ms for user input before processing the next frame
@ -145,7 +144,6 @@ int main( int argc, char** argv )
/******************************************************************/
capture.release( );
return EXIT_SUCCESS;
}

333
src/tp/videoOGLTeapot.cpp
View file

@ -21,26 +21,22 @@
#include <GL/freeglut.h>
#endif
#include <glm.h>
#include <cstdlib>
#include <iostream>
#include <iostream>
#include <cstdio>
#include <chrono>
#include <thread>
using namespace std;
using namespace cv;
// Display the help for the programm
void help( const char* programName );
void help(const char *programName);
// parse the input command line arguments
bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, string &calibFile, string &objFile );
bool parseArgs(int argc, char **argv, Size &boardSize, string &inputFilename, string &calibFile, string &objFile);
/*
* Common globals
@ -64,33 +60,27 @@ bool stop = false;
// the size of the video frame
Size singleSize;
// OpenGL initialization
void glInit( )
void glInit()
{
//******************************************************************
// enable the depth test
//******************************************************************
glEnable(GL_DEPTH_TEST);
glEnable( GL_TEXTURE_2D );
glGenTextures( 1, &gCameraTextureId );
glEnable(GL_TEXTURE_2D);
glGenTextures(1, &gCameraTextureId);
//******************************************************************
// set the Gouraud shading
//******************************************************************
glShadeModel(GL_SMOOTH);
//******************************************************************
// set the LIGHT0 as a simple white, directional light with direction [1,2,-2]
//******************************************************************
GLfloat light0_position[] = {1.0, 2.0, -2.0, 0.0};
//******************************************************************
// set the material properties for the teapot
@ -98,166 +88,165 @@ void glInit( )
// as you prefer. The teapot in the figure has is mainly gray with
// ambient 0.7, diffuse 0.8, specular 1.0 and shininess 100
//******************************************************************
GLfloat mat_ambient[] = {0.7, 0.7, 0.7, 1.0};
GLfloat mat_diffuse[] = {0.8, 0.8, 0.8, 1.0};
GLfloat mat_specular[] = {1.0, 1.0, 1.0, 1.0};
GLfloat mat_shininess[] = {100.0};
// //******************************************************************
// // enable the lights
// //******************************************************************
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
//******************************************************************
// enable the lights
//******************************************************************
glLightfv(GL_LIGHT0, GL_AMBIENT, mat_ambient);
glLightfv(GL_LIGHT0, GL_DIFFUSE, mat_diffuse);
glLightfv(GL_LIGHT0, GL_SPECULAR, mat_specular);
glLightfv(GL_LIGHT0, GL_POSITION, light0_position);
//******************************************************************
// set the opengl projection matrix to gProjectionMatrix:
// load the identity and multiply it by gProjectionMatrix using glMultMatrixf
//******************************************************************
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glMultMatrixf(gProjectionMatrix);
//******************************************************************
// set back the modelview mode
//******************************************************************
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
}
// Updates texture handle gCameraTextureId with OpenCV image in cv::Mat from gResultImage
void updateTexture( )
void updateTexture()
{
glBindTexture( GL_TEXTURE_2D, gCameraTextureId );
glBindTexture(GL_TEXTURE_2D, gCameraTextureId);
// set texture filter to linear - we do not build mipmaps for speed
glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );
glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR );
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
// create the texture from OpenCV image data
glTexImage2D( GL_TEXTURE_2D, 0, GL_RGB8, singleSize.width, singleSize.height, 0,
#if _WIN32
GL_BGR_EXT,
#else
GL_BGR,
#endif
GL_UNSIGNED_BYTE, gResultImage.data );
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB8, singleSize.width, singleSize.height, 0,
#if _WIN32
GL_BGR_EXT,
#else
GL_BGR,
#endif
GL_UNSIGNED_BYTE, gResultImage.data);
}
/**
* Draw the background from the camera image
*/
void drawBackground( )
void drawBackground()
{
// set up the modelview matrix so that the view is between [-1,-1] and [1,1]
glMatrixMode( GL_PROJECTION );
glPushMatrix( );
glLoadIdentity( );
glOrtho( -1, 1, -1, 1, 0, 1 );
glMatrixMode( GL_MODELVIEW );
glLoadIdentity( );
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glOrtho(-1, 1, -1, 1, 0, 1);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
// draw the quad textured with the camera image
glBindTexture( GL_TEXTURE_2D, gCameraTextureId );
glBegin( GL_QUADS );
glTexCoord2f( 0, 1 );
glVertex2f( -1, -1 );
glTexCoord2f( 0, 0 );
glVertex2f( -1, 1 );
glTexCoord2f( 1, 0 );
glVertex2f( 1, 1 );
glTexCoord2f( 1, 1 );
glVertex2f( 1, -1 );
glEnd( );
glBindTexture(GL_TEXTURE_2D, gCameraTextureId);
glBegin(GL_QUADS);
glTexCoord2f(0, 1);
glVertex2f(-1, -1);
glTexCoord2f(0, 0);
glVertex2f(-1, 1);
glTexCoord2f(1, 0);
glVertex2f(1, 1);
glTexCoord2f(1, 1);
glVertex2f(1, -1);
glEnd();
// reset the projection matrix
glMatrixMode( GL_PROJECTION );
glPopMatrix( );
glMatrixMode( GL_MODELVIEW );
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
}
/** OpenGL display callback */
void displayFunc( )
void displayFunc()
{
glClear( GL_COLOR_BUFFER_BIT );
glClear(GL_COLOR_BUFFER_BIT);
// render the background image from camera texture
glEnable( GL_TEXTURE_2D );
glEnable(GL_TEXTURE_2D);
//******************************************************************
// disable the lighting before drawing the background
//******************************************************************
glDisable(GL_LIGHTING);
drawBackground( );
drawBackground();
// clear th depth buffer bit so that the background is overdrawn
glClear( GL_DEPTH_BUFFER_BIT );
glClear(GL_DEPTH_BUFFER_BIT);
// everything will be white
glColor3f( 1, 1, 1 );
glColor3f(1, 1, 1);
// start with fresh modelview matrix and apply the transform of the plane
glMatrixMode( GL_MODELVIEW );
glLoadIdentity( );
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
//******************************************************************
// apply the modelview matrix gModelViewMatrix using glMultMatrixf
//******************************************************************
glMultMatrixf(gModelViewMatrix);
// enable the texture for a nice effect ;)
glDisable( GL_TEXTURE_2D );
glDisable(GL_TEXTURE_2D);
//******************************************************************
// enable the lighting before drawing the teapot/the object
//******************************************************************
glEnable(GL_LIGHTING);
//******************************************************************
// draw the teapot (the solid version)
//******************************************************************
glutSolidTeapot(1);
glutSwapBuffers( );
glutPostRedisplay( );
glutSwapBuffers();
glutPostRedisplay();
}
// Windows resize callback
// Windows resize callback
void reshape( GLint width, GLint height )
void reshape(GLint width, GLint height)
{
glViewport( 0, 0, width, height );
glViewport(0, 0, width, height);
}
// Keyboard callback
// Keyboard callback
void keyFunc( unsigned char key, int x, int y )
void keyFunc(unsigned char key, int x, int y)
{
cout << key << " pressed" << endl;
switch( key )
switch (key)
{
case 27:
gFinished = true;
break;
case 27:
gFinished = true;
break;
case 's':
stop = !stop;
break;
default:
break;
case 's':
stop = !stop;
break;
default:
break;
}
}
int main( int argc, char** argv )
int main(int argc, char **argv)
{
/******************************************************************/
/* VARIABLES TO USE */
/******************************************************************/
@ -271,6 +260,12 @@ int main( int argc, char** argv )
// the camera
Camera cam;
// Camera Tracker object
ChessboardCameraTrackerKLT tracker;
// 3x4 camera pose matrix [R t]
Mat cameraPose;
// the filenames for the video and the calibration
string videoFilename, calibFilename, objFile;
int imgInType;
@ -281,17 +276,14 @@ int main( int argc, char** argv )
VideoCapture capture;
// the video capture a dummy matrix used to show the teapot in a fix position of the image
Mat dummyMatrix = Mat::eye( 4, 4, CV_32F );
dummyMatrix.at<float>( 1, 1 ) = -1;
dummyMatrix.at<float>( 2, 3 ) = 50;
cout << dummyMatrix << endl;
Mat dummyMatrix = Mat::eye(4, 4, CV_32F);
// dummyMatrix.at<float>(1, 1) = -1;
// dummyMatrix.at<float>(2, 3) = 50;
/******************************************************************/
/* READ THE INPUT PARAMETERS - DO NOT MODIFY */
/******************************************************************/
if( !parseArgs( argc, argv, boardSize, videoFilename, calibFilename, objFile ) )
if (!parseArgs(argc, argv, boardSize, videoFilename, calibFilename, objFile))
{
cerr << "Aborting..." << endl;
return EXIT_FAILURE;
@ -300,160 +292,175 @@ int main( int argc, char** argv )
//******************************************************************
// init the Camera loading the calibration parameters
//******************************************************************
if (!cam.init(calibFilename))
{
cerr << "Aborting..." << endl;
return EXIT_FAILURE;
}
//******************************************************************
// get the corresponding projection matrix in OGL format
//******************************************************************
cam.getOGLProjectionMatrix(gProjectionMatrix, 0.25f, 500.0f);
capture.open( videoFilename );
capture.open(videoFilename);
// check if capture has opened the video
if( !capture.isOpened( ) )
if (!capture.isOpened())
{
cerr << "Could not open video file " << videoFilename << endl;
return EXIT_FAILURE;
}
if( !getVideoSizeAndType(videoFilename, capture, singleSize, imgInType ) )
if (!getVideoSizeAndType(videoFilename, capture, singleSize, imgInType))
{
cerr << "Something wrong while checking the size and type of the video " << videoFilename << endl;
return EXIT_FAILURE;
}
gResultImage = Mat( singleSize, imgInType );
gResultImage = Mat(singleSize, imgInType);
// Setup GLUT rendering and callbacks
glutInit( &argc, argv );
glutCreateWindow( "Main" );
glutKeyboardFunc( keyFunc );
glutReshapeFunc( reshape );
// reshape the window with the size of the image
glutReshapeWindow( singleSize.width, singleSize.height );
glutDisplayFunc( displayFunc );
glutInit(&argc, argv);
glutCreateWindow("Main");
glutKeyboardFunc(keyFunc);
glutReshapeFunc(reshape);
glInit( );
// reshape the window with the size of the image
glutReshapeWindow(singleSize.width, singleSize.height);
glutDisplayFunc(displayFunc);
glInit();
gFinished = false;
while( !gFinished )
while (!gFinished)
{
if( !stop )
if (!stop)
{
Mat view0;
capture >> view0;
if (!tracker.process(view0, cameraPose, cam, boardSize, pattern))
{
continue;
}
// get a copy of the frame
if( view0.empty( ) )
if (view0.empty())
{
cerr << "no more images available" << endl;
gFinished = true;
break;
}
view0.copyTo( gResultImage );
view0.copyTo(gResultImage);
// set the gModelViewMatrix with the content of the dummy matrix
// OpenGL uses a column-major order for storing the matrix element, while OpenCV uses
// a row major order for storing the elements. Hence we need first to convert the dummy matrix
// to its transpose and only then pass the data pointer to gModelViewMatrix
gModelViewMatrix = ( float* ) Mat( dummyMatrix.t( ) ).data;
// Mat tmp;
// cameraPose.convertTo(tmp, CV_32F);
// tmp.copyTo(dummyMatrix);
// gModelViewMatrix = (float *)Mat(dummyMatrix.t()).data;
Mat temp;
cameraPose.convertTo(temp, CV_32F);
PRINTVAR(temp);
temp.copyTo(dummyMatrix.rowRange(0, 3));
PRINTVAR(dummyMatrix);
gModelViewMatrix = ( float* ) Mat( dummyMatrix.t( ) ).data;
cout << endl << endl << "****************** frame " << frameNumber << " ******************" << endl;
cout << endl
<< endl
<< "****************** frame " << frameNumber << " ******************" << endl;
++frameNumber;
}
// update the texture to be displayed in OPENGL
updateTexture( );
updateTexture();
// force Opengl to call the displayFunc
#if __APPLE__
glutCheckLoop( );
glutCheckLoop();
#else
glutMainLoopEvent( );
glutMainLoopEvent();
#endif
// sleep for 35ms
std::this_thread::sleep_for(std::chrono::milliseconds(35));
}
capture.release( );
capture.release();
return EXIT_SUCCESS;
}
// Display the help for the programm
void help( const char* programName )
void help(const char *programName)
{
cout << "Detect a chessboard in a given video and visualize a teapot on top of it" << endl
<< "Usage: " << programName << endl
<< " -w <board_width> # the number of inner corners per one of board dimension" << endl
<< " -h <board_height> # the number of inner corners per another board dimension" << endl
<< " -c <calib file> # the name of the calibration file" << endl
<< " [-o <obj file>] # the optional obj file containing the 3D model to display" << endl
<< " <video file> # the name of the video file" << endl
<< endl;
<< "Usage: " << programName << endl
<< " -w <board_width> # the number of inner corners per one of board dimension" << endl
<< " -h <board_height> # the number of inner corners per another board dimension" << endl
<< " -c <calib file> # the name of the calibration file" << endl
<< " [-o <obj file>] # the optional obj file containing the 3D model to display" << endl
<< " <video file> # the name of the video file" << endl
<< endl;
}
// parse the input command line arguments
bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, string &calibFile, string &objFile )
bool parseArgs(int argc, char **argv, Size &boardSize, string &inputFilename, string &calibFile, string &objFile)
{
// check the minimum number of arguments
if( argc < 3 )
if (argc < 3)
{
help( argv[0] );
help(argv[0]);
return false;
}
// Read the input arguments
for( int i = 1; i < argc; i++ )
for (int i = 1; i < argc; i++)
{
const char* s = argv[i];
const char *s = argv[i];
if( strcmp( s, "-w" ) == 0 )
if (strcmp(s, "-w") == 0)
{
if( sscanf( argv[++i], "%u", &boardSize.width ) != 1 || boardSize.width <= 0 )
if (sscanf(argv[++i], "%u", &boardSize.width) != 1 || boardSize.width <= 0)
{
cerr << "Invalid board width" << endl;
return false;
}
}
else if( strcmp( s, "-h" ) == 0 )
else if (strcmp(s, "-h") == 0)
{
if( sscanf( argv[++i], "%u", &boardSize.height ) != 1 || boardSize.height <= 0 )
if (sscanf(argv[++i], "%u", &boardSize.height) != 1 || boardSize.height <= 0)
{
cerr << "Invalid board height" << endl;
return false;
}
}
else if( s[0] != '-' )
else if (s[0] != '-')
{
inputFilename.assign( s );
inputFilename.assign(s);
}
else if( strcmp( s, "-c" ) == 0 )
else if (strcmp(s, "-c") == 0)
{
if( i + 1 < argc )
calibFile.assign( argv[++i] );
if (i + 1 < argc)
calibFile.assign(argv[++i]);
else
{
cerr << "Missing argument for option " << s << endl;
return false;
}
}
else if( strcmp( s, "-o" ) == 0 )
else if (strcmp(s, "-o") == 0)
{
if( i + 1 < argc )
objFile.assign( argv[++i] );
if (i + 1 < argc)
objFile.assign(argv[++i]);
else
{
cerr << "Missing argument for the obj file " << s << endl;