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,7 +13,6 @@
 using namespace cv;
 using namespace std;
 
-
 // Display the help for the programm
 void help(const char *programName);
 
@@ -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,8 +47,6 @@ int main( int argc, char** argv )
     // Default pattern is chessboard
     Pattern pattern = CHESSBOARD;
 
-
-
     /******************************************************************/
     /* READ THE INPUT PARAMETERS - DO NOT MODIFY                      */
     /******************************************************************/
@@ -62,9 +57,6 @@ int main( int argc, char** argv )
         return EXIT_FAILURE;
     }
 
-
-
-
     /******************************************************************/
     /* PART TO DEVELOP                                                  */
     /******************************************************************/
@@ -72,12 +64,12 @@ 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();
@@ -86,7 +78,7 @@ int main( int argc, char** argv )
     // 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();
@@ -94,26 +86,26 @@ int main( int argc, char** argv )
     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);
 
-
     return EXIT_SUCCESS;
 }
 
-
 // Display the help for the programm
 
 void help(const char *programName)
@@ -138,7 +130,6 @@ bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, Pa
         return false;
     }
 
-
     // Read the input arguments
     for (int i = 1; i < argc; i++)
     {
@@ -187,4 +178,3 @@ bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, Pa
 
     return true;
 }
-

src/tp/checkerboardvideo.cpp

@@ -48,8 +48,6 @@ int main( int argc, char** argv )
     // Used to load the video and get the frames
     VideoCapture capture;
 
-
-
     /******************************************************************/
     /* READ THE INPUT PARAMETERS - DO NOT MODIFY                      */
     /******************************************************************/
@@ -60,50 +58,46 @@ int main( int argc, char** argv )
         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)
     {
-        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();
@@ -112,7 +106,7 @@ int main( int argc, char** argv )
         // 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();
@@ -124,30 +118,31 @@ int main( int argc, char** argv )
         // 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)
+        {
             break;
-
+        }
     }
 
     /******************************************************************/
     // release the video resource
     /******************************************************************/
 
-
-
     return EXIT_SUCCESS;
 }
 
-
 // Display the help for the program
 
 void help(const char *programName)
@@ -172,7 +167,6 @@ bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, Pa
         return false;
     }
 
-
     // Read the input arguments
     for (int i = 1; i < argc; i++)
     {
@@ -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,33 +116,31 @@ 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)
@@ -155,7 +150,7 @@ int main( int argc, char** argv )
         /******************************************************************/
         // 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,30 +16,32 @@ 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;
@@ -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,14 +69,14 @@ 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;
@@ -70,8 +85,7 @@ bool ChessboardCameraTracker::process( cv::Mat &view, cv::Mat &pose, const Camer
         //******************************************************************/
         // decompose the homography
         //******************************************************************/
-
+        decomposeHomography(H, cam.matK, pose);
-
     }
 
     return found;

src/tp/tracker/ChessboardCameraTrackerKLT.cpp

@@ -31,7 +31,8 @@ bool ChessboardCameraTrackerKLT::process( cv::Mat &view, cv::Mat &pose, const Ca
     // 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
                 line( view, _corners[ i ], currPts[ i ], Scalar( 255, 0, 0 ), 1 );
                 circle( view, currPts[ i ], 3, Scalar( 255, 0, 255 ), -1, 8 );
                 #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                                              */
 /******************************************************************/
@@ -37,7 +35,7 @@ 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 )
@@ -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;
@@ -99,7 +96,6 @@ bool detectChessboard( const Mat &rgbimage, vector<Point2f> &pointbuf, const Siz
  */
 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);
-
 }
 
 /**
@@ -188,19 +176,25 @@ void calcChessboardCorners( const Size &boardSize, const float &squareSize, vect
     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++)
             {
                 /******************************************************************/
@@ -208,8 +202,13 @@ void calcChessboardCorners( const Size &boardSize, const float &squareSize, vect
                 // 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;
 
@@ -227,74 +226,80 @@ void calcChessboardCorners( const Size &boardSize, const float &squareSize, vect
  */
 void decomposeHomography(const Mat &H, const Mat &matK, Mat &poseMat)
 {
-
     Mat temp;
 
     //******************************************************************/
     // temp contains inv(K)*H
     //******************************************************************/
-
+    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
-    //******************************************************************/
-
-
     //******************************************************************/
     // 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
 
@@ -322,9 +327,13 @@ void calcChessboardCorners3D( const Size &boardSize, const float &squareSize, ve
                 // 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;
@@ -338,9 +347,13 @@ void calcChessboardCorners3D( const Size &boardSize, const float &squareSize, ve
                 // 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;
 
@@ -383,7 +396,6 @@ void mySolvePnPRansac( cv::InputArray objectPoints, cv::InputArray imagePoints,
 #endif
 }
 
-
 bool getVideoSizeAndType(const std::string &videoFilename, cv::VideoCapture capture, cv::Size &singleSize, int &imgInType)
 {
     if (!capture.isOpened())

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,18 +29,15 @@
 #include <chrono>
 #include <thread>
 
-
 using namespace std;
 using namespace cv;
 
-
 // Display the help for the programm
 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);
 
-
 /*
  * Common globals
  */
@@ -64,9 +60,6 @@ bool stop = false;
 // the size of the video frame
 Size singleSize;
 
-
-
-
 // OpenGL initialization
 
 void glInit()
@@ -74,8 +67,7 @@ void glInit( )
     //******************************************************************
     // enable the depth test
     //******************************************************************
-
+    glEnable(GL_DEPTH_TEST);
-
 
     glEnable(GL_TEXTURE_2D);
     glGenTextures(1, &gCameraTextureId);
@@ -83,14 +75,12 @@ void glInit( )
     //******************************************************************
     // 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,38 +88,37 @@ 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()
@@ -189,10 +178,11 @@ void displayFunc( )
 
     // render the background image from camera texture
     glEnable(GL_TEXTURE_2D);
+
     //******************************************************************
     // disable the lighting before drawing the background
     //******************************************************************
-
+    glDisable(GL_LIGHTING);
 
     drawBackground();
 
@@ -209,7 +199,7 @@ void displayFunc( )
     //******************************************************************
     // apply the modelview matrix gModelViewMatrix using glMultMatrixf
     //******************************************************************
-
+    glMultMatrixf(gModelViewMatrix);
 
     // enable the texture for a nice effect ;)
     glDisable(GL_TEXTURE_2D);
@@ -217,12 +207,12 @@ void displayFunc( )
     //******************************************************************
     // enable the lighting before drawing the teapot/the object
     //******************************************************************
-
+    glEnable(GL_LIGHTING);
 
     //******************************************************************
     // draw the teapot (the solid version)
     //******************************************************************
-
+    glutSolidTeapot(1);
 
     glutSwapBuffers();
     glutPostRedisplay();
@@ -257,7 +247,6 @@ void keyFunc( unsigned char key, int x, int y )
 
 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;
@@ -282,15 +277,12 @@ int main( int argc, char** argv )
 
     // 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>(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))
     {
         cerr << "Aborting..." << endl;
@@ -300,13 +292,16 @@ 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);
 
@@ -324,12 +319,12 @@ int main( int argc, char** argv )
 
     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);
@@ -346,6 +341,11 @@ int main( int argc, char** argv )
             Mat view0;
             capture >> view0;
 
+            if (!tracker.process(view0, cameraPose, cam, boardSize, pattern))
+            {
+                continue;
+            }
+
             // get a copy of the frame
             if (view0.empty())
             {
@@ -360,9 +360,20 @@ int main( int argc, char** argv )
             // 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;
         }
@@ -379,12 +390,10 @@ int main( int argc, char** argv )
 
         // sleep for 35ms
         std::this_thread::sleep_for(std::chrono::milliseconds(35));
-
     }
 
     capture.release();
 
-
     return EXIT_SUCCESS;
 }
 
@@ -402,7 +411,6 @@ 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)
@@ -414,7 +422,6 @@ bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, st
         return false;
     }
 
-
     // Read the input arguments
     for (int i = 1; i < argc; i++)
     {