complet

.gitignore

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

.vscode/settings.json

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

src/tp/checkerboard.cpp

@@ -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,51 +64,51 @@ 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
-    //Measure the execution time, get time before function call
+    auto t = (double)getTickCount();
-    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
@@ -129,48 +121,47 @@ 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)
 {
     // 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, "-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;
 }
-

src/tp/checkerboardvideo.cpp

@@ -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,109 +48,104 @@ 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                                                */
     /******************************************************************/
 
-
-
     /******************************************************************/
     // 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
@@ -163,48 +158,47 @@ 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)
 {
     // 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, "-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;
 }
-

src/tp/checkerboardvideoRectified.cpp

@@ -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;
 }

src/tp/checkerboardvideoUndistort.cpp

@@ -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,20 +169,21 @@ 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
         else if( mode == 'u' )
         {
@@ -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

src/tp/tracker/Camera.cpp

@@ -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]

src/tp/tracker/ChessboardCameraTracker.cpp

@@ -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 << "H = " << H << endl << endl;
+        // cout << "corners =" << corners << endl << endl;
-//         cout << "corners =" << corners << endl << endl;
+        // cout << "ptsOb =" << objectPoints << endl << endl;
-//         cout << "ptsOb =" << objectPoints << endl << endl;
 
         //******************************************************************/
         // decompose the homography
         //******************************************************************/
-
+        decomposeHomography(H, cam.matK, pose);
-
     }
 
     return found;

src/tp/tracker/ChessboardCameraTrackerKLT.cpp

@@ -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,19 +56,16 @@ 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
-        // use klt to track the points
         
         // some parameters for the optical flow algorithm
         Size winSize( 11, 11 );
@@ -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,29 +101,28 @@ 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
@@ -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;
 }

src/tp/tracker/tracker/utility.hpp

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

src/tp/tracker/utility.cpp

@@ -8,8 +8,6 @@
 using namespace cv;
 using namespace std;
 
-
-
 /******************************************************************/
 /* FUNCTIONS TO DEVELOP                                              */
 /******************************************************************/
@@ -23,12 +21,12 @@ using namespace std;
  * @param[in] patternType The type of chessboard pattern to look for
  * @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:
@@ -37,11 +35,11 @@ bool detectChessboard( const Mat &rgbimage, vector<Point2f> &pointbuf, const Siz
         // 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 )
+        if (found)
         {
             Mat viewGrey; // it will contain the graylevel version of the image
 
@@ -49,13 +47,13 @@ bool detectChessboard( const Mat &rgbimage, vector<Point2f> &pointbuf, const Siz
             // convert the image in "rgbimage" to gray level and save it in "viewGrey"
             // --> cvtColor with CV_BGR2GRAY option
             /******************************************************************/
-
+            cvtColor(rgbimage, viewGrey, CV_BGR2GRAY);
 
             /******************************************************************/
             // 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;
 
@@ -65,7 +63,7 @@ bool detectChessboard( const Mat &rgbimage, vector<Point2f> &pointbuf, const Siz
         // detect the circles --> see findCirclesGrid
         // found = ...
         /******************************************************************/
-
+        found = findCirclesGrid(rgbimage, boardSize, pointbuf, CALIB_CB_SYMMETRIC_GRID);
 
         break;
 
@@ -75,11 +73,10 @@ bool detectChessboard( const Mat &rgbimage, vector<Point2f> &pointbuf, const Siz
         // 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;
 
-
     default:
         cerr << "Unknown pattern type" << endl;
         return found;
@@ -97,9 +94,8 @@ 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;
     
@@ -107,10 +103,10 @@ void drawReferenceSystem( cv::Mat &rgbimage, const Camera& cam, const cv::Mat &p
     // 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,21 +125,20 @@ 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);
-
 }
 
 /**
@@ -162,12 +150,12 @@ void drawReferenceSystem( cv::Mat &rgbimage, const Camera& cam, const cv::Mat &p
  * @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);
 }
 
 /**
@@ -178,43 +166,54 @@ void myProjectPoints( InputArray objectPoints, const Mat &poseMat, InputArray ca
  * @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.resize(0);
-    corners.reserve( boardSize.height * boardSize.width );
+    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 i = 0; i < boardSize.height; i++)
-                for( int j = 0; j < boardSize.width; j++ )
+        {
+            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 i = 0; i < boardSize.height; i++)
-                for( int j = 0; j < boardSize.width; j++ )
+        {
+            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;
 
     default:
-            CV_Error( CV_StsBadArg, "Unknown pattern type\n" );
+        CV_Error(CV_StsBadArg, "Unknown pattern type\n");
     }
 }
 
@@ -225,78 +224,84 @@ 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
     //******************************************************************/
-
+    temp = matK.inv() * H;
-//     PRINTVAR( temp );
+    // 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 );
+    // PRINTVAR( r3 );
-
-    //******************************************************************/
-    // compute t
-    //******************************************************************/
-
 
     //******************************************************************/
     // 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);
 
-
+    // cout << "poseMat: " << poseMat << endl;
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-    //      PRINTVAR( poseMat );
-
 }
 
-
 /******************************************************************************/
-//KLT ONLY
+// KLT ONLY
 
 /**
  * Generate the set of 3D points of a chessboard
@@ -306,46 +311,54 @@ void decomposeHomography( const Mat &H, const Mat& matK, Mat& poseMat )
  * @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.resize(0);
-    corners.reserve( boardSize.height * boardSize.width );
+    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 i = 0; i < boardSize.height; i++)
-                for( int j = 0; j < boardSize.width; j++ )
+            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 i = 0; i < boardSize.height; i++)
-                for( int j = 0; j < boardSize.width; j++ )
+            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;
 
     default:
-            CV_Error( CV_StsBadArg, "Unknown pattern type\n" );
+        CV_Error(CV_StsBadArg, "Unknown pattern type\n");
     }
 }
 
@@ -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 );
+    Rot.convertTo(temp, CV_32F);
-    Mat a1 = poseMat.colRange( 0, 3 );
+    Mat a1 = poseMat.colRange(0, 3);
-    temp.copyTo( a1 );
+    temp.copyTo(a1);
-    a1 = poseMat.col( 3 );
+    a1 = poseMat.col(3);
-    currT.convertTo( temp, CV_32F );
+    currT.convertTo(temp, CV_32F);
-    temp.copyTo( a1 );
+    temp.copyTo(a1);
 #else
-    Rot.copyTo( poseMat.colRange( 0, 3 ) );
+    Rot.copyTo(poseMat.colRange(0, 3));
-    currT.copyTo( poseMat.col( 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( );
+    imgInType = view0.type();
-    singleSize = view0.size( );
+    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);
 }

src/tp/tracking.cpp

@@ -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;
 }

src/tp/trackingKLT.cpp

@@ -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;
 }
 

src/tp/videoOGLTeapot.cpp

@@ -21,7 +21,6 @@
 #include <GL/freeglut.h>
 #endif
 
-
 #include <glm.h>
 
 #include <cstdlib>
@@ -30,17 +29,14 @@
 #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,150 +88,150 @@ 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);
 
-
+    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);
-
-
-
-
-    //******************************************************************
-    // enable the lights
-    //******************************************************************
-
-
 
     //******************************************************************
     // 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_MAG_FILTER, GL_LINEAR);
-    glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_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,
+    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB8, singleSize.width, singleSize.height, 0,
-    #if _WIN32
+#if _WIN32
                  GL_BGR_EXT,
-    #else
+#else
                   GL_BGR,
-    #endif
+#endif
-            GL_UNSIGNED_BYTE, gResultImage.data );
+                 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 );
+    glMatrixMode(GL_PROJECTION);
-    glPushMatrix( );
+    glPushMatrix();
-    glLoadIdentity( );
+    glLoadIdentity();
-    glOrtho( -1, 1, -1, 1, 0, 1 );
+    glOrtho(-1, 1, -1, 1, 0, 1);
-    glMatrixMode( GL_MODELVIEW );
+    glMatrixMode(GL_MODELVIEW);
-    glLoadIdentity( );
+    glLoadIdentity();
 
     // draw the quad textured with the camera image
-    glBindTexture( GL_TEXTURE_2D, gCameraTextureId );
+    glBindTexture(GL_TEXTURE_2D, gCameraTextureId);
-    glBegin( GL_QUADS );
+    glBegin(GL_QUADS);
-    glTexCoord2f( 0, 1 );
+    glTexCoord2f(0, 1);
-    glVertex2f( -1, -1 );
+    glVertex2f(-1, -1);
-    glTexCoord2f( 0, 0 );
+    glTexCoord2f(0, 0);
-    glVertex2f( -1, 1 );
+    glVertex2f(-1, 1);
-    glTexCoord2f( 1, 0 );
+    glTexCoord2f(1, 0);
-    glVertex2f( 1, 1 );
+    glVertex2f(1, 1);
-    glTexCoord2f( 1, 1 );
+    glTexCoord2f(1, 1);
-    glVertex2f( 1, -1 );
+    glVertex2f(1, -1);
-    glEnd( );
+    glEnd();
 
     // reset the projection matrix
-    glMatrixMode( GL_PROJECTION );
+    glMatrixMode(GL_PROJECTION);
-    glPopMatrix( );
+    glPopMatrix();
-    glMatrixMode( GL_MODELVIEW );
+    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 );
+    glMatrixMode(GL_MODELVIEW);
-    glLoadIdentity( );
+    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();
-    glutSwapBuffers( );
+    glutPostRedisplay();
-    glutPostRedisplay( );
 }
 
 // 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
 
-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;
@@ -255,9 +245,8 @@ void keyFunc( unsigned char key, int x, int y )
     }
 }
 
-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 );
+    Mat dummyMatrix = Mat::eye(4, 4, CV_32F);
-    dummyMatrix.at<float>( 1, 1 ) = -1;
+    // dummyMatrix.at<float>(1, 1) = -1;
-    dummyMatrix.at<float>( 2, 3 ) = 50;
+    // dummyMatrix.at<float>(2, 3) = 50;
-
-    cout << dummyMatrix << endl;
 
     /******************************************************************/
     /* 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,97 +292,114 @@ 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 );
+    glutInit(&argc, argv);
-    glutCreateWindow( "Main" );
+    glutCreateWindow("Main");
-    glutKeyboardFunc( keyFunc );
+    glutKeyboardFunc(keyFunc);
-    glutReshapeFunc( reshape );
+    glutReshapeFunc(reshape);
-    // reshape the window with the size of the image
-    glutReshapeWindow( singleSize.width, singleSize.height );
-    glutDisplayFunc( displayFunc );
 
-    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
+            // 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
@@ -402,58 +411,56 @@ void help( const char* programName )
          << 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 )
+            if (i + 1 < argc)
-                calibFile.assign( argv[++i] );
+                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 )
+            if (i + 1 < argc)
-                objFile.assign( argv[++i] );
+                objFile.assign(argv[++i]);
             else
             {
                 cerr << "Missing argument for the obj file " << s << endl;