diff --git a/.gitignore b/.gitignore
index 567609b..7907dac 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1 +1,3 @@
 build/
+deps/
+3rdparty/
\ No newline at end of file
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..fb61c26
--- /dev/null
+++ b/.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"
+    }
+}
\ No newline at end of file
diff --git a/src/tp/checkerboard.cpp b/src/tp/checkerboard.cpp
index 8789b83..b722ef7 100644
--- a/src/tp/checkerboard.cpp
+++ b/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,105 +64,104 @@ int main( int argc, char** argv )
     /******************************************************************/
     // create a window to display the image --> see namedWindow
     /******************************************************************/
-
+    namedWindow(WINDOW_NAME, WINDOW_AUTOSIZE);
 
     /******************************************************************/
     // read the input image from file into "view" --> see imread
     /******************************************************************/
+    view = imread(inputFilename, CV_LOAD_IMAGE_COLOR);
 
-
-    //Measure the execution time, get time before function call
-    auto t = ( double ) getTickCount( );
+    // Measure the execution time, get time before function call
+    auto t = (double)getTickCount();
 
     /******************************************************************/
     // call the function that detects the chessboard on the image
     // found = detectChessboard...
     /******************************************************************/
-
+    found = detectChessboard(view, pointbuf, boardSize, pattern);
 
     // get time after function call and display info
-    t = ( ( double ) getTickCount( ) - t ) / getTickFrequency( );
+    t = ((double)getTickCount() - t) / getTickFrequency();
 
-    cout << ( ( !found ) ? ( "No " ) : ( "" ) ) << "chessboard detected!" << endl;
+    cout << ((!found) ? ("No ") : ("")) << "chessboard detected!" << endl;
     cout << "Chessboard detection took " << t * 1000 << "ms" << endl;
 
-
     /******************************************************************/
     // if the chessboard is found draw the cornerns on top of it
     // --> see drawChessboardCorners
     /******************************************************************/
-
+    if (found)
+    {
+        drawChessboardCorners(view, boardSize, Mat(pointbuf), found);
+    }
 
     /******************************************************************/
     // show the image inside the window --> see imshow
     /******************************************************************/
-
+    imshow(WINDOW_NAME, view);
 
     // wait for user input before ending --> see waitKey
-    waitKey( -1 );
-
+    waitKey(-1);
 
     return EXIT_SUCCESS;
 }
 
-
 // Display the help for the programm
 
-void help( const char* programName )
+void help(const char *programName)
 {
     cout << "Detect a chessboard in a given image" << endl
-            << "Usage: " << programName << endl
-            << "     -w <board_width>                                  # the number of inner corners per one of board dimension" << endl
-            << "     -h <board_height>                                 # the number of inner corners per another board dimension" << endl
-            << "     [-pt <pattern=[circles|acircles|chess]>]          # the type of pattern: chessboard or circles' grid" << endl
-            << "     <image file> " << endl
-            << endl;
+         << "Usage: " << programName << endl
+         << "     -w <board_width>                                  # the number of inner corners per one of board dimension" << endl
+         << "     -h <board_height>                                 # the number of inner corners per another board dimension" << endl
+         << "     [-pt <pattern=[circles|acircles|chess]>]          # the type of pattern: chessboard or circles' grid" << endl
+         << "     <image file> " << endl
+         << endl;
 }
 
 // parse the input command line arguments
 
-bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, Pattern &pattern )
+bool parseArgs(int argc, char **argv, Size &boardSize, string &inputFilename, Pattern &pattern)
 {
     // check the minimum number of arguments
-    if( argc < 3 )
+    if (argc < 3)
     {
-        help( argv[0] );
+        help(argv[0]);
         return false;
     }
 
-
     // Read the input arguments
-    for( int i = 1; i < argc; i++ )
+    for (int i = 1; i < argc; i++)
     {
-        const char* s = argv[i];
-        if( strcmp( s, "-w" ) == 0 )
+        const char *s = argv[i];
+        if (strcmp(s, "-w") == 0)
         {
-            if( sscanf( argv[++i], "%u", &boardSize.width ) != 1 || boardSize.width <= 0 )
+            if (sscanf(argv[++i], "%u", &boardSize.width) != 1 || boardSize.width <= 0)
             {
                 cerr << "Invalid board width" << endl;
                 return false;
             }
         }
-        else if( strcmp( s, "-h" ) == 0 )
+        else if (strcmp(s, "-h") == 0)
         {
-            if( sscanf( argv[++i], "%u", &boardSize.height ) != 1 || boardSize.height <= 0 )
+            if (sscanf(argv[++i], "%u", &boardSize.height) != 1 || boardSize.height <= 0)
             {
                 cerr << "Invalid board height" << endl;
                 return false;
             }
         }
-        else if( s[0] != '-' )
+        else if (s[0] != '-')
         {
-            inputFilename.assign( s );
+            inputFilename.assign(s);
         }
-        else if( strcmp( s, "-pt" ) == 0 )
+        else if (strcmp(s, "-pt") == 0)
         {
             i++;
-            if( !strcmp( argv[i], "circles" ) )
+            if (!strcmp(argv[i], "circles"))
                 pattern = CIRCLES_GRID;
-            else if( !strcmp( argv[i], "acircles" ) )
+            else if (!strcmp(argv[i], "acircles"))
                 pattern = ASYMMETRIC_CIRCLES_GRID;
-            else if( !strcmp( argv[i], "chess" ) )
+            else if (!strcmp(argv[i], "chess"))
                 pattern = CHESSBOARD;
             else
             {
@@ -187,4 +178,3 @@ bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, Pa
 
     return true;
 }
-
diff --git a/src/tp/checkerboardvideo.cpp b/src/tp/checkerboardvideo.cpp
index 9ae9216..39f783e 100644
--- a/src/tp/checkerboardvideo.cpp
+++ b/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,163 +48,157 @@ int main( int argc, char** argv )
     // Used to load the video and get the frames
     VideoCapture capture;
 
-
-
     /******************************************************************/
     /* READ THE INPUT PARAMETERS - DO NOT MODIFY                      */
     /******************************************************************/
 
-    if( !parseArgs( argc, argv, boardSize, inputFilename, pattern ) )
+    if (!parseArgs(argc, argv, boardSize, inputFilename, pattern))
     {
         cerr << "Aborting..." << endl;
         return EXIT_FAILURE;
     }
 
-
-
-
     /******************************************************************/
-    /* PART TO DEVELOP                                                  */
+    /* PART TO DEVELOP                                                */
     /******************************************************************/
 
-
-
     /******************************************************************/
     // create a window to display the image --> see namedWindow
     /******************************************************************/
-
+    namedWindow(WINDOW_NAME, CV_WINDOW_AUTOSIZE);
 
     /******************************************************************/
     // read the input video with capture
     /******************************************************************/
-
+    capture.open(inputFilename);
 
     /******************************************************************/
     // check it is really opened
     /******************************************************************/
-
-
-
-
-
-
-
+    if (!capture.isOpened())
+    {
+        cerr << "Could not open the video file " << inputFilename << endl;
+        return EXIT_FAILURE;
+    }
 
     // processing loop
-    while(true)
+    while (true)
     {
-        Mat view;
+        Mat frame;
         /******************************************************************/
-        // get the new frame from capture and copy it to view
+        // get the new frame from capture and copy it to frame
         /******************************************************************/
-
+        capture >> frame;
+        // capture.retrieve(frame);
 
         /******************************************************************/
         // if no more images to process exit the loop
         /******************************************************************/
+        if (frame.empty())
+        {
+            break;
+        }
 
-
-
-        //Measure the execution time, get time before function call
-        auto t = ( double ) getTickCount( );
+        // Measure the execution time, get time before function call
+        auto t = (double)getTickCount();
 
         /******************************************************************/
         // call the function that detects the chessboard on the image
         // found = detectChessboard...
         /******************************************************************/
-
+        found = detectChessboard(frame, pointbuf, boardSize, pattern);
 
         // get time after function call and display info
-        t = ( ( double ) getTickCount( ) - t ) / getTickFrequency( );
+        t = ((double)getTickCount() - t) / getTickFrequency();
 
-        cout << ( ( !found ) ? ( "No " ) : ( "" ) ) << "chessboard detected!" << endl;
+        cout << ((!found) ? ("No ") : ("")) << "chessboard detected!" << endl;
         cout << "Chessboard detection took " << t * 1000 << "ms" << endl;
 
         /******************************************************************/
         // if the chessboard is found draw the corners on top of it
         // --> see drawChessboardCorners
         /******************************************************************/
-
+        if (found)
+        {
+            drawChessboardCorners(frame, boardSize, Mat(pointbuf), found);
+        }
 
         /******************************************************************/
         // show the image inside the window --> see imshow
         /******************************************************************/
-
+        imshow(WINDOW_NAME, frame);
 
         // wait 20ms for user input before processing the next frame
         // Any user input will stop the execution
-        if( waitKey( 10 ) >= 0 )
+        if (waitKey(10) >= 0)
+        {
             break;
-
+        }
     }
 
     /******************************************************************/
     // release the video resource
     /******************************************************************/
 
-
-
     return EXIT_SUCCESS;
 }
 
-
 // Display the help for the program
 
-void help( const char* programName )
+void help(const char *programName)
 {
     cout << "Detect a chessboard in a given video" << endl
-            << "Usage: " << programName << endl
-            << "     -w <board_width>                                  # the number of inner corners per one of board dimension" << endl
-            << "     -h <board_height>                                 # the number of inner corners per another board dimension" << endl
-            << "     [-pt <pattern=[circles|acircles|chess]>]          # the type of pattern: chessboard or circles' grid" << endl
-            << "     <video file> " << endl
-            << endl;
+         << "Usage: " << programName << endl
+         << "     -w <board_width>                                  # the number of inner corners per one of board dimension" << endl
+         << "     -h <board_height>                                 # the number of inner corners per another board dimension" << endl
+         << "     [-pt <pattern=[circles|acircles|chess]>]          # the type of pattern: chessboard or circles' grid" << endl
+         << "     <video file> " << endl
+         << endl;
 }
 
 // parse the input command line arguments
 
-bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, Pattern &pattern )
+bool parseArgs(int argc, char **argv, Size &boardSize, string &inputFilename, Pattern &pattern)
 {
     // check the minimum number of arguments
-    if( argc < 3 )
+    if (argc < 3)
     {
-        help( argv[0] );
+        help(argv[0]);
         return false;
     }
 
-
     // Read the input arguments
-    for( int i = 1; i < argc; i++ )
+    for (int i = 1; i < argc; i++)
     {
-        const char* s = argv[i];
-        if( strcmp( s, "-w" ) == 0 )
+        const char *s = argv[i];
+        if (strcmp(s, "-w") == 0)
         {
-            if( sscanf( argv[++i], "%u", &boardSize.width ) != 1 || boardSize.width <= 0 )
+            if (sscanf(argv[++i], "%u", &boardSize.width) != 1 || boardSize.width <= 0)
             {
                 cerr << "Invalid board width" << endl;
                 return false;
             }
         }
-        else if( strcmp( s, "-h" ) == 0 )
+        else if (strcmp(s, "-h") == 0)
         {
-            if( sscanf( argv[++i], "%u", &boardSize.height ) != 1 || boardSize.height <= 0 )
+            if (sscanf(argv[++i], "%u", &boardSize.height) != 1 || boardSize.height <= 0)
             {
                 cerr << "Invalid board height" << endl;
                 return false;
             }
         }
-        else if( s[0] != '-' )
+        else if (s[0] != '-')
         {
-            inputFilename.assign( s );
+            inputFilename.assign(s);
         }
-        else if( strcmp( s, "-pt" ) == 0 )
+        else if (strcmp(s, "-pt") == 0)
         {
             i++;
-            if( !strcmp( argv[i], "circles" ) )
+            if (!strcmp(argv[i], "circles"))
                 pattern = CIRCLES_GRID;
-            else if( !strcmp( argv[i], "acircles" ) )
+            else if (!strcmp(argv[i], "acircles"))
                 pattern = ASYMMETRIC_CIRCLES_GRID;
-            else if( !strcmp( argv[i], "chess" ) )
+            else if (!strcmp(argv[i], "chess"))
                 pattern = CHESSBOARD;
             else
             {
@@ -221,4 +215,3 @@ bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, Pa
 
     return true;
 }
-
diff --git a/src/tp/checkerboardvideoRectified.cpp b/src/tp/checkerboardvideoRectified.cpp
index b4a0147..54e78e3 100644
--- a/src/tp/checkerboardvideoRectified.cpp
+++ b/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;
 }
diff --git a/src/tp/checkerboardvideoUndistort.cpp b/src/tp/checkerboardvideoUndistort.cpp
index b7e1df5..7eea6bb 100644
--- a/src/tp/checkerboardvideoUndistort.cpp
+++ b/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,21 +169,22 @@ int main( int argc, char** argv )
             /******************************************************************/
             // copy the original image into temp --> see Mat.clone()
             /******************************************************************/
-
+            view.copyTo(temp);
 
             /******************************************************************/
             // apply the undistortion and store the new image in view
             // --> see undistort
             /******************************************************************/
-
+            undistort(temp, view, matK, dist);
 
             /******************************************************************/
             // compute the difference between the two images and store the result in view
             // see --> absdiff
             /******************************************************************/
-
+            absdiff(temp, view, view);
         }
-            // if we want to see the undistorted image
+        
+        // if we want to see the undistorted image
         else if( mode == 'u' )
         {
             msg = "(o)riginal, (d)ifference";
@@ -198,13 +194,13 @@ int main( int argc, char** argv )
             /******************************************************************/
             // copy the original image into temp --> see Mat.clone()
             /******************************************************************/
-
+            view.copyTo(temp);
 
             /******************************************************************/
             // apply the undistortion and store the new image in view
             // --> see undistort
             /******************************************************************/
-
+            undistort(temp, view, matK, dist);
         }
         else
         {
@@ -221,7 +217,7 @@ int main( int argc, char** argv )
         /******************************************************************/
         // show view inside the window --> see imshow
         /******************************************************************/
-
+        imshow(WINDOW_NAME, view);
 
         // wait 20ms for user input before processing the next frame
         // Any user input will stop the execution
diff --git a/src/tp/tracker/Camera.cpp b/src/tp/tracker/Camera.cpp
index b9017f8..5fa2dd8 100644
--- a/src/tp/tracker/Camera.cpp
+++ b/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]
diff --git a/src/tp/tracker/ChessboardCameraTracker.cpp b/src/tp/tracker/ChessboardCameraTracker.cpp
index 6677362..77c0302 100644
--- a/src/tp/tracker/ChessboardCameraTracker.cpp
+++ b/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 << "corners =" << corners << endl << endl;
-//         cout << "ptsOb =" << objectPoints << endl << endl;
+        // cout << "H = " << H << endl << endl;
+        // cout << "corners =" << corners << endl << endl;
+        // cout << "ptsOb =" << objectPoints << endl << endl;
 
         //******************************************************************/
         // decompose the homography
         //******************************************************************/
-
-
+        decomposeHomography(H, cam.matK, pose);
     }
 
     return found;
diff --git a/src/tp/tracker/ChessboardCameraTrackerKLT.cpp b/src/tp/tracker/ChessboardCameraTrackerKLT.cpp
index 0ad90df..358bb90 100644
--- a/src/tp/tracker/ChessboardCameraTrackerKLT.cpp
+++ b/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,27 +56,24 @@ bool ChessboardCameraTrackerKLT::process( cv::Mat &view, cv::Mat &pose, const Ca
             // generate the points on the chessboard, this time 3D points
             // see --> calcChessboardCorners3D
             //******************************************************************/
-
-
+            calcChessboardCorners3D( boardSize, 1, _objectPoints, pattern );
 
             //******************************************************************/
             // compute the pose of the camera using mySolvePnPRansac (utility.hpp))
             //******************************************************************/
-
+            mySolvePnPRansac(_objectPoints, _corners, cam.matK, cam.distCoeff, pose);
         }
-
     }
-    else
-    {
-        // use klt to track the points
-
+    else 
+    { // use klt to track the points
+        
         // some parameters for the optical flow algorithm
         Size winSize( 11, 11 );
         TermCriteria termcrit( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03 );
-
+        
         // vector where the estimated tracked points of the new frame will be stored
         vector<Point2f> currPts;
-
+        
         // auxiliary stuff for the optical flow computation
         // status has will have the same length as currPts and is 0 if the
         // optical flow estimation for the corresponding new point is not good
@@ -85,7 +83,7 @@ bool ChessboardCameraTrackerKLT::process( cv::Mat &view, cv::Mat &pose, const Ca
         //******************************************************************/
         // estimate the new position of the tracked points using calcOpticalFlowPyrLK
         //******************************************************************/
-
+        calcOpticalFlowPyrLK( _prevGrey, viewGrey, _corners, currPts, status, err, winSize, 3, termcrit, 0, 0.001 );
 
         //******************************************************************/
         // Filter currPts and update the lists _corners and _objectPoints: if
@@ -103,31 +101,30 @@ bool ChessboardCameraTrackerKLT::process( cv::Mat &view, cv::Mat &pose, const Ca
             // if it's a good point copy it in _corners and also copy keep the
             // corresponding _objectPoints
             //******************************************************************/
-            // if..
+            if (status[i] > 0)
             {
-#if DEBUGGING
+                #if DEBUGGING
                 line( view, _corners[ i ], currPts[ i ], Scalar( 255, 0, 0 ), 1 );
                 circle( view, currPts[ i ], 3, Scalar( 255, 0, 255 ), -1, 8 );
-#endif
+                #endif
+                
                 //******************************************************************/
                 // copy the current point in _corners
                 //******************************************************************/
-
+                _corners[k] = currPts[i];
 
                 //******************************************************************/
                 // copy the corresponding _objectPoints
                 //******************************************************************/
-
-
+                _objectPoints[k] = _objectPoints[i];
+                
                 //******************************************************************/
                 // update k
                 //******************************************************************/
                 ++k;
-
             }
-
         }
-
+        
         // resize the two vector to the size k, the number of "well" tracked features
         _corners.resize( k );
         _objectPoints.resize( k );
@@ -138,9 +135,10 @@ bool ChessboardCameraTrackerKLT::process( cv::Mat &view, cv::Mat &pose, const Ca
         //******************************************************************/
         // compute the pose of the camera using mySolvePnPRansac (utility.hpp))
         //******************************************************************/
+        mySolvePnPRansac(_objectPoints, _corners, cam.matK, cam.distCoeff, pose, idxInl);
 
 //        _vecCorners.push_back(_corners);
-////        _vecObjectPoints.push_back(_objectPoints);
+//        _vecObjectPoints.push_back(_objectPoints);
 //        _vecIdx.push_back(_indices);
 
         //******************************************************************/
@@ -148,15 +146,13 @@ bool ChessboardCameraTrackerKLT::process( cv::Mat &view, cv::Mat &pose, const Ca
         // Filter both the image points and the 3D reference points
         //******************************************************************/
 
-
-
         found = true;
     }
 
     //******************************************************************/
     // update _prevGrey with the current grey frame
     //******************************************************************/
-
+    viewGrey.copyTo( _prevGrey );
 
     return found;
 }
diff --git a/src/tp/tracker/tracker/utility.hpp b/src/tp/tracker/tracker/utility.hpp
index 13593dc..ad92a6c 100644
--- a/src/tp/tracker/tracker/utility.hpp
+++ b/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;
diff --git a/src/tp/tracker/utility.cpp b/src/tp/tracker/utility.cpp
index c2fea50..ecc2e5f 100644
--- a/src/tp/tracker/utility.cpp
+++ b/src/tp/tracker/utility.cpp
@@ -8,8 +8,6 @@
 using namespace cv;
 using namespace std;
 
-
-
 /******************************************************************/
 /* FUNCTIONS TO DEVELOP                                              */
 /******************************************************************/
@@ -18,71 +16,70 @@ using namespace std;
  * Detect a chessboard in a given image
  *
  * @param[in] rgbimage The rgb image to process
- * @param[out] pointbuf the set of 2D image corner detected on the chessboard 
+ * @param[out] pointbuf the set of 2D image corner detected on the chessboard
  * @param[in] boardSize the size of the board in terms of corners (width X height)
  * @param[in] patternType The type of chessboard pattern to look for
- * @return true if the chessboard is detected inside the image, false otherwise 
+ * @return true if the chessboard is detected inside the image, false otherwise
  */
-bool detectChessboard( const Mat &rgbimage, vector<Point2f> &pointbuf, const Size &boardSize, Pattern patternType )
+bool detectChessboard(const Mat &rgbimage, vector<Point2f> &pointbuf, const Size &boardSize, Pattern patternType)
 {
     // it contains the value to return
     bool found = false;
 
-    switch( patternType )
+    switch (patternType)
     {
-            // detect a classic chessboard
-        case CHESSBOARD:
+        // detect a classic chessboard
+    case CHESSBOARD:
+
+        /******************************************************************/
+        // detect the chessboard --> see findChessboardCorners
+        // found = ...
+        /******************************************************************/
+        found = findChessboardCorners(rgbimage, boardSize, pointbuf, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);
+
+        // if a chessboard is found refine the position of the points in a window 11x11 pixel
+        // use the default value for the termination criteria --> TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 )
+        if (found)
+        {
+            Mat viewGrey; // it will contain the graylevel version of the image
 
             /******************************************************************/
-            // detect the chessboard --> see findChessboardCorners
-            // found = ...
+            // convert the image in "rgbimage" to gray level and save it in "viewGrey"
+            // --> cvtColor with CV_BGR2GRAY option
             /******************************************************************/
+            cvtColor(rgbimage, viewGrey, CV_BGR2GRAY);
 
-
-            // if a chessboard is found refine the position of the points in a window 11x11 pixel
-            // use the default value for the termination criteria --> TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 )
-            if( found )
-            {
-                Mat viewGrey; // it will contain the graylevel version of the image
-
-                /******************************************************************/
-                // convert the image in "rgbimage" to gray level and save it in "viewGrey"
-                // --> cvtColor with CV_BGR2GRAY option
-                /******************************************************************/
-
-
-                /******************************************************************/
-                // refine the corner location in "pointbuf" using "viewGrey"
-                // --> see cornerSubPix
-                /******************************************************************/
-
-            }
-            break;
-
-            // detect a regular grid made of circles
-        case CIRCLES_GRID:
             /******************************************************************/
-            // detect the circles --> see findCirclesGrid
-            // found = ...
+            // refine the corner location in "pointbuf" using "viewGrey"
+            // --> see cornerSubPix
             /******************************************************************/
+            cornerSubPix(viewGrey, pointbuf, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
+        }
+        break;
 
+        // detect a regular grid made of circles
+    case CIRCLES_GRID:
+        /******************************************************************/
+        // detect the circles --> see findCirclesGrid
+        // found = ...
+        /******************************************************************/
+        found = findCirclesGrid(rgbimage, boardSize, pointbuf, CALIB_CB_SYMMETRIC_GRID);
 
-            break;
+        break;
 
-            // detect an asymmetric grid made of circles
-        case ASYMMETRIC_CIRCLES_GRID:
-            /******************************************************************/
-            // detect the circles --> see findCirclesGrid using the options CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING
-            // found = ...
-            /******************************************************************/
+        // detect an asymmetric grid made of circles
+    case ASYMMETRIC_CIRCLES_GRID:
+        /******************************************************************/
+        // detect the circles --> see findCirclesGrid using the options CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING
+        // found = ...
+        /******************************************************************/
+        found = findCirclesGrid(rgbimage, boardSize, pointbuf, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
 
+        break;
 
-            break;
-
-
-        default:
-            cerr << "Unknown pattern type" << endl;
-            return found;
+    default:
+        cerr << "Unknown pattern type" << endl;
+        return found;
     }
 
     return found;
@@ -97,20 +94,19 @@ bool detectChessboard( const Mat &rgbimage, vector<Point2f> &pointbuf, const Siz
  * @param[in] scale A scale factor for the unit vectors to draw
  * @param[in] alreadyUndistorted A boolean value that tells if the input image rgbimage is already undistorted or we are working on a distorted image
  */
-void drawReferenceSystem( cv::Mat &rgbimage, const Camera& cam, const cv::Mat &poseMat, const int &thickness, const double &scale, bool alreadyUndistorted )
+void drawReferenceSystem(cv::Mat &rgbimage, const Camera &cam, const cv::Mat &poseMat, const int &thickness, const double &scale, bool alreadyUndistorted)
 {
-
     // contains the points to project to draw the 3 axis
     vector<Point3f> vertex3D;
-
+    
     //******************************************************************/
     // Add the four 3D points (Point3f) that we can use to draw
     // the reference system to vertex3D. Use <scale> as unit
     //******************************************************************/
-
-
-
-
+    vertex3D.push_back(Point3f(0, 0, 0));
+    vertex3D.push_back(Point3f(0, scale, 0));
+    vertex3D.push_back(Point3f(scale, 0, 0));
+    vertex3D.push_back(Point3f(0, 0, -scale));
 
     // contains the projected 3D points on the image
     vector<Point2f> imgRefPts;
@@ -121,14 +117,7 @@ void drawReferenceSystem( cv::Mat &rgbimage, const Camera& cam, const cv::Mat &p
     // if it is true we pass a 1x5 zero vector, otherwise the distortion
     //  parameter of cam
     //******************************************************************/
-
-
-
-
-
-
-
-
+    myProjectPoints(vertex3D, poseMat, cam.matK, alreadyUndistorted ? Mat::zeros(1, 5, CV_64F) : cam.distCoeff, imgRefPts);
 
     //    cout << "vertex3D" << vertex3D << endl;
     //    cout << "imgRefPts" << imgRefPts << endl;
@@ -136,38 +125,37 @@ void drawReferenceSystem( cv::Mat &rgbimage, const Camera& cam, const cv::Mat &p
     //******************************************************************/
     // draw the line of the x-axis and put "X" at the end
     //******************************************************************/
-
-
+    line(rgbimage, imgRefPts[0], imgRefPts[1], Scalar(0, 0, 255), thickness);
+    putText(rgbimage, "X", imgRefPts[1], FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 255), thickness);
 
     //******************************************************************/
     // draw the line of the y-axis and put "Y" at the end
     //******************************************************************/
-
-
+    line(rgbimage, imgRefPts[0], imgRefPts[2], Scalar(0, 255, 0), thickness);
+    putText(rgbimage, "Y", imgRefPts[2], FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 255, 0), thickness);
 
     //******************************************************************/
     // draw the line of the z-axis and put "Z" at the end
     //******************************************************************/
-
-
-
+    line(rgbimage, imgRefPts[0], imgRefPts[3], Scalar(255, 0, 0), thickness);
+    putText(rgbimage, "Z", imgRefPts[3], FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 0, 0), thickness);
 }
 
 /**
  * Wrapper around the original opencv's projectPoints
- * 
+ *
  * @param objectPoints the 3D points
  * @param poseMat the pose matrix
  * @param cameraMatrix the calibration matrix
  * @param distCoeffs the distortion coeffi
  * @param imagePoints
  */
-void myProjectPoints( InputArray objectPoints, const Mat &poseMat, InputArray cameraMatrix, InputArray distCoeffs, OutputArray imagePoints )
+void myProjectPoints(InputArray objectPoints, const Mat &poseMat, InputArray cameraMatrix, InputArray distCoeffs, OutputArray imagePoints)
 {
     Mat rvec;
-    Rodrigues( poseMat.colRange( 0, 3 ), rvec );
+    Rodrigues(poseMat.colRange(0, 3), rvec);
     // projectPoints( Mat( vertex3D.t( ) ).reshape( 3, 1 ), rvec, Tvec, K, dist, imgRefPts );
-    projectPoints( objectPoints, rvec, poseMat.col( 3 ), cameraMatrix, distCoeffs, imagePoints );
+    projectPoints(objectPoints, rvec, poseMat.col(3), cameraMatrix, distCoeffs, imagePoints);
 }
 
 /**
@@ -175,46 +163,57 @@ void myProjectPoints( InputArray objectPoints, const Mat &poseMat, InputArray ca
  *
  * @param[in] boardSize the size of the board in terms of corners (width X height)
  * @param[in] squareSize the size in mm of the each square of the chessboard
- * @param[out] corners the set of 2D points on the chessboard 
+ * @param[out] corners the set of 2D points on the chessboard
  * @param[in] patternType The type of chessboard pattern to look for
  */
-void calcChessboardCorners( const Size &boardSize, const float &squareSize, vector<Point2f>& corners, Pattern patternType )
+void calcChessboardCorners(const Size &boardSize, const float &squareSize, vector<Point2f> &corners, Pattern patternType)
 {
-    corners.resize( 0 );
-    corners.reserve( boardSize.height * boardSize.width );
+    corners.resize(0);
+    corners.reserve(boardSize.height * boardSize.width);
 
-    switch( patternType )
+    switch (patternType)
     {
-        case CHESSBOARD:
-        case CIRCLES_GRID:
-            for( int i = 0; i < boardSize.height; i++ )
-                for( int j = 0; j < boardSize.width; j++ )
-                {
-                    /******************************************************************/
-                    // create a Point2f(x,y) according to the position j,i and a square
-                    // size of squareSize. Add it to corners (using push_back...)
-                    /******************************************************************/
+    case CHESSBOARD:
+    case CIRCLES_GRID:
+        for (int i = 0; i < boardSize.height; i++)
+        {
+            for (int j = 0; j < boardSize.width; j++)
+            {
+                /******************************************************************/
+                // create a Point2f(x,y) according to the position j,i and a square
+                // size of squareSize. Add it to corners (using push_back...)
+                /******************************************************************/
+                corners.push_back(
+                    Point2f(
+                        j * squareSize,
+                        i * squareSize
+                    )
+                );
+            }
+        }
+        break;
+    case ASYMMETRIC_CIRCLES_GRID:
+        for (int i = 0; i < boardSize.height; i++)
+        {
+            for (int j = 0; j < boardSize.width; j++)
+            {
+                /******************************************************************/
+                // create a Point2f(x,y) according to the position j,i considering
+                // that x is generate using the formula (2*j + i % 2)*squareSize
+                //  Add it to corners (using push_back...)
+                /******************************************************************/
+                corners.push_back(
+                    Point2f(
+                        (2 * j + i % 2) * squareSize, 
+                        i * squareSize
+                    )
+                );
+            }
+        }
+        break;
 
-
-                }
-            break;
-
-        case ASYMMETRIC_CIRCLES_GRID:
-            for( int i = 0; i < boardSize.height; i++ )
-                for( int j = 0; j < boardSize.width; j++ )
-                {
-                    /******************************************************************/
-                    // create a Point2f(x,y) according to the position j,i considering
-                    // that x is generate using the formula (2*j + i % 2)*squareSize
-                    //  Add it to corners (using push_back...)
-                    /******************************************************************/
-
-
-                }
-            break;
-
-        default:
-            CV_Error( CV_StsBadArg, "Unknown pattern type\n" );
+    default:
+        CV_Error(CV_StsBadArg, "Unknown pattern type\n");
     }
 }
 
@@ -225,133 +224,147 @@ void calcChessboardCorners( const Size &boardSize, const float &squareSize, vect
  * @param[in] matK The 3x3 calibration matrix K
  * @param[out] poseMat the 3x4 pose matrix [R t]
  */
-void decomposeHomography( const Mat &H, const Mat& matK, Mat& poseMat )
+void decomposeHomography(const Mat &H, const Mat &matK, Mat &poseMat)
 {
-
     Mat temp;
 
     //******************************************************************/
-    //temp contains inv(K)*H
+    // temp contains inv(K)*H
     //******************************************************************/
-
-//     PRINTVAR( temp );
+    temp = matK.inv() * H;
+    // PRINTVAR( temp );
 
     Mat r1, r2, r3, t;
     //******************************************************************/
     // get r1 and r2 from temp
     //******************************************************************/
-
-
+    r1 = temp.col(0);
+    r2 = temp.col(1);
+    t = temp.col(2);
+    
+    // cout << "r1: " << r1 << endl;
+    // cout << "r2: " << r2 << endl;
+    // cout << "t: " << t << endl;
 
     //******************************************************************/
     // compute lambda
     //******************************************************************/
-
+    float lambda;
+    Mat prod = r1.t() * r1;
+    // cout << "prod: " << prod << endl;
+    lambda = sqrt(prod.at<double>(0));
+    // cout << "lambda: " << lambda << endl;
 
     //******************************************************************/
-    // normalize r1 and r2
+    // normalize r1 r2 and t
     //******************************************************************/
+    if (lambda != 0)
+    {
 
-
+        r1 = r1 / lambda;
+        r2 = r2 / lambda;
+        t = t / lambda;
+    }
 
     //******************************************************************/
     // compute r3
     //******************************************************************/
+    r3 = r1.cross(r2);
 
-//     PRINTVAR( r3 );
-
-    //******************************************************************/
-    // compute t
-    //******************************************************************/
-
+    // PRINTVAR( r3 );
 
     //******************************************************************/
     // create a 3x4 matrix (float) for poseMat
     //******************************************************************/
-
+    poseMat = Mat::zeros(3, 4, CV_32F);
 
     //******************************************************************/
     // fill the columns of poseMat with r1 r2 r3 and t
     //******************************************************************/
+    // cout << "*r1: " << r1 << endl;
+    // cout << "*r2: " << r2 << endl;
+    // cout << "*t: " << t << endl;
+    poseMat.at<float>(0, 0) = r1.at<double>(0);
+    poseMat.at<float>(1, 0) = r1.at<double>(1);
+    poseMat.at<float>(2, 0) = r1.at<double>(2);
+    poseMat.at<float>(0, 1) = r2.at<double>(0);
+    poseMat.at<float>(1, 1) = r2.at<double>(1);
+    poseMat.at<float>(2, 1) = r2.at<double>(2);
+    poseMat.at<float>(0, 2) = r3.at<double>(0);
+    poseMat.at<float>(1, 2) = r3.at<double>(1);
+    poseMat.at<float>(2, 2) = r3.at<double>(2);    
+    poseMat.at<float>(0, 3) = t.at<double>(0);
+    poseMat.at<float>(1, 3) = t.at<double>(1);
+    poseMat.at<float>(2, 3) = t.at<double>(2);
 
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-    //      PRINTVAR( poseMat );
-
+    // cout << "poseMat: " << poseMat << endl;
 }
 
-
 /******************************************************************************/
-//KLT ONLY
+// KLT ONLY
 
 /**
  * Generate the set of 3D points of a chessboard
  *
  * @param[in] boardSize the size of the board in terms of corners (width X height)
  * @param[in] squareSize the size in mm of the each square of the chessboard
- * @param[out] corners the set of 3D points on the chessboard 
+ * @param[out] corners the set of 3D points on the chessboard
  * @param[in] patternType The type of chessboard pattern to look for
  */
-void calcChessboardCorners3D( const Size &boardSize, const float &squareSize, vector<Point3f>& corners, Pattern patternType )
+void calcChessboardCorners3D(const Size &boardSize, const float &squareSize, vector<Point3f> &corners, Pattern patternType)
 {
-    corners.resize( 0 );
-    corners.reserve( boardSize.height * boardSize.width );
+    corners.resize(0);
+    corners.reserve(boardSize.height * boardSize.width);
 
-    switch( patternType )
+    switch (patternType)
     {
-        case CHESSBOARD:
-        case CIRCLES_GRID:
-            for( int i = 0; i < boardSize.height; i++ )
-                for( int j = 0; j < boardSize.width; j++ )
-                {
-                    /******************************************************************/
-                    // create a Point3f(x,y,0) according to the position j,i and a square
-                    // size of squareSize. Add it to corners (using push_back...)
-                    /******************************************************************/
+    case CHESSBOARD:
+    case CIRCLES_GRID:
+        for (int i = 0; i < boardSize.height; i++)
+            for (int j = 0; j < boardSize.width; j++)
+            {
+                /******************************************************************/
+                // create a Point3f(x,y,0) according to the position j,i and a square
+                // size of squareSize. Add it to corners (using push_back...)
+                /******************************************************************/
+                corners.push_back(
+                    Point3f(
+                        j * squareSize,
+                        i * squareSize,
+                        0
+                    )
+                );
+            }
 
+        break;
 
+    case ASYMMETRIC_CIRCLES_GRID:
+        for (int i = 0; i < boardSize.height; i++)
+            for (int j = 0; j < boardSize.width; j++)
+            {
+                /******************************************************************/
+                // create a Point3f(x,y,0) according to the position j,i considering
+                // that x is generate using the formula (2*j + i % 2)*squareSize
+                //  Add it to corners (using push_back...)
+                /******************************************************************/
+                corners.push_back(
+                    Point3f(
+                        (2 * j + i % 2) * squareSize, 
+                        i * squareSize,
+                        0
+                    )
+                );
+            }
+        break;
 
-                }
-
-            break;
-
-        case ASYMMETRIC_CIRCLES_GRID:
-            for( int i = 0; i < boardSize.height; i++ )
-                for( int j = 0; j < boardSize.width; j++ )
-                {
-                    /******************************************************************/
-                    // create a Point3f(x,y,0) according to the position j,i considering
-                    // that x is generate using the formula (2*j + i % 2)*squareSize
-                    //  Add it to corners (using push_back...)
-                    /******************************************************************/
-
-
-
-                }
-            break;
-
-        default:
-            CV_Error( CV_StsBadArg, "Unknown pattern type\n" );
+    default:
+        CV_Error(CV_StsBadArg, "Unknown pattern type\n");
     }
 }
 
 /**
  * Wrapper around the original opencv's solvePnPRansac
- * 
+ *
  * @param[in] objectPoints the 3D points
  * @param[in] imagePoints the image points
  * @param[in] cameraMatrix the calibration matrix
@@ -359,34 +372,33 @@ void calcChessboardCorners3D( const Size &boardSize, const float &squareSize, ve
  * @param[out] poseMat the pose matrix
  * @param[out] inliers the list of indices of the inliers points
  */
-void mySolvePnPRansac( cv::InputArray objectPoints, cv::InputArray imagePoints, cv::InputArray cameraMatrix, cv::InputArray distCoeffs, cv::Mat &poseMat, OutputArray inliers )
+void mySolvePnPRansac(cv::InputArray objectPoints, cv::InputArray imagePoints, cv::InputArray cameraMatrix, cv::InputArray distCoeffs, cv::Mat &poseMat, OutputArray inliers)
 {
     Mat currR, currT;
-    solvePnPRansac( objectPoints, imagePoints, cameraMatrix, distCoeffs, currR, currT, false, 100, 4, 100, inliers );
+    solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs, currR, currT, false, 100, 4, 100, inliers);
 
-    poseMat = Mat( 3, 4, CV_32F );
+    poseMat = Mat(3, 4, CV_32F);
 
     Mat Rot;
-    Rodrigues( currR, Rot );
+    Rodrigues(currR, Rot);
 #if CV_MINOR_VERSION < 4
     // apparently older versions does not support direct copy
     Mat temp;
-    Rot.convertTo( temp, CV_32F );
-    Mat a1 = poseMat.colRange( 0, 3 );
-    temp.copyTo( a1 );
-    a1 = poseMat.col( 3 );
-    currT.convertTo( temp, CV_32F );
-    temp.copyTo( a1 );
+    Rot.convertTo(temp, CV_32F);
+    Mat a1 = poseMat.colRange(0, 3);
+    temp.copyTo(a1);
+    a1 = poseMat.col(3);
+    currT.convertTo(temp, CV_32F);
+    temp.copyTo(a1);
 #else
-    Rot.copyTo( poseMat.colRange( 0, 3 ) );
-    currT.copyTo( poseMat.col( 3 ) );
+    Rot.copyTo(poseMat.colRange(0, 3));
+    currT.copyTo(poseMat.col(3));
 #endif
 }
 
-
 bool getVideoSizeAndType(const std::string &videoFilename, cv::VideoCapture capture, cv::Size &singleSize, int &imgInType)
 {
-    if( !capture.isOpened( ) )
+    if (!capture.isOpened())
     {
         cerr << "Could not open video file " << videoFilename << endl;
         return false;
@@ -395,17 +407,17 @@ bool getVideoSizeAndType(const std::string &videoFilename, cv::VideoCapture capt
     // open video file and get the first image just to get the image size
     Mat view0;
     capture >> view0;
-    if( view0.empty( ) )
+    if (view0.empty())
     {
         cerr << "Could not get the first frame of the video file " << videoFilename << endl;
         return false;
     }
 
-    imgInType = view0.type( );
-    singleSize = view0.size( );
+    imgInType = view0.type();
+    singleSize = view0.size();
 
     // close capture...
-    capture.release( );
+    capture.release();
     // and re-open it so that we will start from the first frame again
     return capture.open(videoFilename);
 }
diff --git a/src/tp/tracking.cpp b/src/tp/tracking.cpp
index 1e0821a..feee110 100644
--- a/src/tp/tracking.cpp
+++ b/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;
 }
diff --git a/src/tp/trackingKLT.cpp b/src/tp/trackingKLT.cpp
index 28960fe..861ef49 100644
--- a/src/tp/trackingKLT.cpp
+++ b/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;
 }
 
diff --git a/src/tp/videoOGLTeapot.cpp b/src/tp/videoOGLTeapot.cpp
index 0313ee6..79978de 100644
--- a/src/tp/videoOGLTeapot.cpp
+++ b/src/tp/videoOGLTeapot.cpp
@@ -21,26 +21,22 @@
 #include <GL/freeglut.h>
 #endif
 
-
 #include <glm.h>
 
 #include <cstdlib>
-#include <iostream> 
+#include <iostream>
 #include <cstdio>
 #include <chrono>
 #include <thread>
 
-
 using namespace std;
 using namespace cv;
 
-
 // Display the help for the programm
-void help( const char* programName );
+void help(const char *programName);
 
 // parse the input command line arguments
-bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, string &calibFile, string &objFile );
-
+bool parseArgs(int argc, char **argv, Size &boardSize, string &inputFilename, string &calibFile, string &objFile);
 
 /*
  * Common globals
@@ -64,33 +60,27 @@ bool stop = false;
 // the size of the video frame
 Size singleSize;
 
-
-
-
 // OpenGL initialization
 
-void glInit( )
+void glInit()
 {
     //******************************************************************
     // enable the depth test
     //******************************************************************
+    glEnable(GL_DEPTH_TEST);
 
-
-
-    glEnable( GL_TEXTURE_2D );
-    glGenTextures( 1, &gCameraTextureId );
+    glEnable(GL_TEXTURE_2D);
+    glGenTextures(1, &gCameraTextureId);
 
     //******************************************************************
     // set the Gouraud shading
     //******************************************************************
-
+    glShadeModel(GL_SMOOTH);
 
     //******************************************************************
     // set the LIGHT0 as a simple white, directional light with direction [1,2,-2]
     //******************************************************************
-
-
-
+    GLfloat light0_position[] = {1.0, 2.0, -2.0, 0.0};
 
     //******************************************************************
     // set the material properties for the teapot
@@ -98,166 +88,165 @@ void glInit( )
     // as you prefer. The teapot in the figure has is mainly gray with
     // ambient 0.7, diffuse 0.8, specular 1.0 and shininess 100
     //******************************************************************
+    GLfloat mat_ambient[] = {0.7, 0.7, 0.7, 1.0};
+    GLfloat mat_diffuse[] = {0.8, 0.8, 0.8, 1.0};
+    GLfloat mat_specular[] = {1.0, 1.0, 1.0, 1.0};
+    GLfloat mat_shininess[] = {100.0};
 
+    // //******************************************************************
+    // // enable the lights
+    // //******************************************************************
+    glEnable(GL_LIGHTING);
+    glEnable(GL_LIGHT0);
 
-
-
-
-
-
-
-
-
-    //******************************************************************
-    // enable the lights
-    //******************************************************************
-
-
+    glLightfv(GL_LIGHT0, GL_AMBIENT, mat_ambient);
+    glLightfv(GL_LIGHT0, GL_DIFFUSE, mat_diffuse);
+    glLightfv(GL_LIGHT0, GL_SPECULAR, mat_specular);
+    glLightfv(GL_LIGHT0, GL_POSITION, light0_position);
 
     //******************************************************************
     // set the opengl projection matrix to gProjectionMatrix:
     // load the identity and multiply it by gProjectionMatrix using glMultMatrixf
     //******************************************************************
-
-
-
+    glMatrixMode(GL_PROJECTION);
+    glLoadIdentity();
+    glMultMatrixf(gProjectionMatrix);
 
     //******************************************************************
     // set back the modelview mode
     //******************************************************************
-
-
+    glMatrixMode(GL_MODELVIEW);
+    glLoadIdentity();
 }
 
-
 // Updates texture handle gCameraTextureId with OpenCV image in cv::Mat from gResultImage
 
-void updateTexture( )
+void updateTexture()
 {
-    glBindTexture( GL_TEXTURE_2D, gCameraTextureId );
+    glBindTexture(GL_TEXTURE_2D, gCameraTextureId);
 
     // set texture filter to linear - we do not build mipmaps for speed
-    glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );
-    glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR );
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
 
     // create the texture from OpenCV image data
-    glTexImage2D( GL_TEXTURE_2D, 0, GL_RGB8, singleSize.width, singleSize.height, 0,
-    #if _WIN32
-            GL_BGR_EXT,
-    #else
-            GL_BGR,
-    #endif
-            GL_UNSIGNED_BYTE, gResultImage.data );
+    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB8, singleSize.width, singleSize.height, 0,
+#if _WIN32
+                 GL_BGR_EXT,
+#else
+                  GL_BGR,
+#endif
+                 GL_UNSIGNED_BYTE, gResultImage.data);
 }
 
 /**
  * Draw the background from the camera image
  */
-void drawBackground( )
+void drawBackground()
 {
     // set up the modelview matrix so that the view is between [-1,-1] and [1,1]
-    glMatrixMode( GL_PROJECTION );
-    glPushMatrix( );
-    glLoadIdentity( );
-    glOrtho( -1, 1, -1, 1, 0, 1 );
-    glMatrixMode( GL_MODELVIEW );
-    glLoadIdentity( );
+    glMatrixMode(GL_PROJECTION);
+    glPushMatrix();
+    glLoadIdentity();
+    glOrtho(-1, 1, -1, 1, 0, 1);
+    glMatrixMode(GL_MODELVIEW);
+    glLoadIdentity();
 
     // draw the quad textured with the camera image
-    glBindTexture( GL_TEXTURE_2D, gCameraTextureId );
-    glBegin( GL_QUADS );
-    glTexCoord2f( 0, 1 );
-    glVertex2f( -1, -1 );
-    glTexCoord2f( 0, 0 );
-    glVertex2f( -1, 1 );
-    glTexCoord2f( 1, 0 );
-    glVertex2f( 1, 1 );
-    glTexCoord2f( 1, 1 );
-    glVertex2f( 1, -1 );
-    glEnd( );
+    glBindTexture(GL_TEXTURE_2D, gCameraTextureId);
+    glBegin(GL_QUADS);
+    glTexCoord2f(0, 1);
+    glVertex2f(-1, -1);
+    glTexCoord2f(0, 0);
+    glVertex2f(-1, 1);
+    glTexCoord2f(1, 0);
+    glVertex2f(1, 1);
+    glTexCoord2f(1, 1);
+    glVertex2f(1, -1);
+    glEnd();
 
     // reset the projection matrix
-    glMatrixMode( GL_PROJECTION );
-    glPopMatrix( );
-    glMatrixMode( GL_MODELVIEW );
+    glMatrixMode(GL_PROJECTION);
+    glPopMatrix();
+    glMatrixMode(GL_MODELVIEW);
 }
 
 /** OpenGL display callback */
-void displayFunc( )
+void displayFunc()
 {
-    glClear( GL_COLOR_BUFFER_BIT );
+    glClear(GL_COLOR_BUFFER_BIT);
 
     // render the background image from camera texture
-    glEnable( GL_TEXTURE_2D );
+    glEnable(GL_TEXTURE_2D);
+
     //******************************************************************
     // disable the lighting before drawing the background
     //******************************************************************
+    glDisable(GL_LIGHTING);
 
-
-    drawBackground( );
+    drawBackground();
 
     // clear th depth buffer bit so that the background is overdrawn
-    glClear( GL_DEPTH_BUFFER_BIT );
+    glClear(GL_DEPTH_BUFFER_BIT);
 
     // everything will be white
-    glColor3f( 1, 1, 1 );
+    glColor3f(1, 1, 1);
 
     // start with fresh modelview matrix and apply the transform of the plane
-    glMatrixMode( GL_MODELVIEW );
-    glLoadIdentity( );
+    glMatrixMode(GL_MODELVIEW);
+    glLoadIdentity();
 
     //******************************************************************
     // apply the modelview matrix gModelViewMatrix using glMultMatrixf
     //******************************************************************
-
+    glMultMatrixf(gModelViewMatrix);
 
     // enable the texture for a nice effect ;)
-    glDisable( GL_TEXTURE_2D );
+    glDisable(GL_TEXTURE_2D);
 
     //******************************************************************
     // enable the lighting before drawing the teapot/the object
     //******************************************************************
-
+    glEnable(GL_LIGHTING);
 
     //******************************************************************
     // draw the teapot (the solid version)
     //******************************************************************
+    glutSolidTeapot(1);
 
-
-    glutSwapBuffers( );
-    glutPostRedisplay( );
+    glutSwapBuffers();
+    glutPostRedisplay();
 }
 
-// Windows resize callback 
+// Windows resize callback
 
-void reshape( GLint width, GLint height )
+void reshape(GLint width, GLint height)
 {
-    glViewport( 0, 0, width, height );
+    glViewport(0, 0, width, height);
 }
 
-// Keyboard callback 
+// Keyboard callback
 
-void keyFunc( unsigned char key, int x, int y )
+void keyFunc(unsigned char key, int x, int y)
 {
     cout << key << " pressed" << endl;
 
-    switch( key )
+    switch (key)
     {
-        case 27:
-            gFinished = true;
-            break;
+    case 27:
+        gFinished = true;
+        break;
 
-        case 's':
-            stop = !stop;
-            break;
-        default:
-            break;
+    case 's':
+        stop = !stop;
+        break;
+    default:
+        break;
     }
 }
 
-int main( int argc, char** argv )
+int main(int argc, char **argv)
 {
-
     /******************************************************************/
     /* VARIABLES TO USE                                                  */
     /******************************************************************/
@@ -271,6 +260,12 @@ int main( int argc, char** argv )
     // the camera
     Camera cam;
 
+    // Camera Tracker object
+    ChessboardCameraTrackerKLT tracker;
+
+    // 3x4 camera pose matrix [R t]
+    Mat cameraPose;
+
     // the filenames for the video and the calibration
     string videoFilename, calibFilename, objFile;
     int imgInType;
@@ -281,17 +276,14 @@ int main( int argc, char** argv )
     VideoCapture capture;
 
     // the video capture a dummy matrix used to show the teapot in a fix position of the image
-    Mat dummyMatrix = Mat::eye( 4, 4, CV_32F );
-    dummyMatrix.at<float>( 1, 1 ) = -1;
-    dummyMatrix.at<float>( 2, 3 ) = 50;
-
-    cout << dummyMatrix << endl;
+    Mat dummyMatrix = Mat::eye(4, 4, CV_32F);
+    // dummyMatrix.at<float>(1, 1) = -1;
+    // dummyMatrix.at<float>(2, 3) = 50;
 
     /******************************************************************/
     /* READ THE INPUT PARAMETERS - DO NOT MODIFY                      */
     /******************************************************************/
-
-    if( !parseArgs( argc, argv, boardSize, videoFilename, calibFilename, objFile ) )
+    if (!parseArgs(argc, argv, boardSize, videoFilename, calibFilename, objFile))
     {
         cerr << "Aborting..." << endl;
         return EXIT_FAILURE;
@@ -300,160 +292,175 @@ int main( int argc, char** argv )
     //******************************************************************
     // init the Camera loading the calibration parameters
     //******************************************************************
-
+    if (!cam.init(calibFilename))
+    {
+        cerr << "Aborting..." << endl;
+        return EXIT_FAILURE;
+    }
 
     //******************************************************************
     // get the corresponding projection matrix in OGL format
     //******************************************************************
+    cam.getOGLProjectionMatrix(gProjectionMatrix, 0.25f, 500.0f);
 
-
-
-    capture.open( videoFilename );
+    capture.open(videoFilename);
 
     // check if capture has opened the video
-    if( !capture.isOpened( ) )
+    if (!capture.isOpened())
     {
         cerr << "Could not open video file " << videoFilename << endl;
         return EXIT_FAILURE;
     }
-    if( !getVideoSizeAndType(videoFilename, capture, singleSize, imgInType ) )
+    if (!getVideoSizeAndType(videoFilename, capture, singleSize, imgInType))
     {
         cerr << "Something wrong while checking the size and type of the video " << videoFilename << endl;
         return EXIT_FAILURE;
     }
 
-    gResultImage = Mat( singleSize, imgInType );
-
+    gResultImage = Mat(singleSize, imgInType);
 
     // Setup GLUT rendering and callbacks
-    glutInit( &argc, argv );
-    glutCreateWindow( "Main" );
-    glutKeyboardFunc( keyFunc );
-    glutReshapeFunc( reshape );
-    // reshape the window with the size of the image
-    glutReshapeWindow( singleSize.width, singleSize.height );
-    glutDisplayFunc( displayFunc );
+    glutInit(&argc, argv);
+    glutCreateWindow("Main");
+    glutKeyboardFunc(keyFunc);
+    glutReshapeFunc(reshape);
 
-    glInit( );
+    // reshape the window with the size of the image
+    glutReshapeWindow(singleSize.width, singleSize.height);
+    glutDisplayFunc(displayFunc);
+
+    glInit();
 
     gFinished = false;
 
-    while( !gFinished )
+    while (!gFinished)
     {
 
-        if( !stop )
+        if (!stop)
         {
             Mat view0;
             capture >> view0;
 
+            if (!tracker.process(view0, cameraPose, cam, boardSize, pattern))
+            {
+                continue;
+            }
+
             // get a copy of the frame
-            if( view0.empty( ) )
+            if (view0.empty())
             {
                 cerr << "no more images available" << endl;
                 gFinished = true;
                 break;
             }
 
-            view0.copyTo( gResultImage );
+            view0.copyTo(gResultImage);
 
             // set the gModelViewMatrix with the content of the dummy matrix
             // OpenGL uses a column-major order for storing the matrix element, while OpenCV uses
             // a row major order for storing the elements. Hence we need first to convert the dummy matrix
             // to its transpose and only then pass the data pointer to gModelViewMatrix
-            gModelViewMatrix = ( float* ) Mat( dummyMatrix.t( ) ).data;
+            // Mat tmp;
+            // cameraPose.convertTo(tmp, CV_32F);
+            // tmp.copyTo(dummyMatrix);
+            // gModelViewMatrix = (float *)Mat(dummyMatrix.t()).data;
+            Mat temp;
+            cameraPose.convertTo(temp, CV_32F);
+            PRINTVAR(temp);
+            temp.copyTo(dummyMatrix.rowRange(0, 3));
+            PRINTVAR(dummyMatrix);
+            gModelViewMatrix = ( float* ) Mat( dummyMatrix.t( )  ).data;
 
-            cout << endl << endl << "****************** frame " << frameNumber << " ******************" << endl;
+            cout << endl
+                 << endl
+                 << "****************** frame " << frameNumber << " ******************" << endl;
 
             ++frameNumber;
         }
 
         // update the texture to be displayed in OPENGL
-        updateTexture( );
+        updateTexture();
 
         // force Opengl to call the displayFunc
 #if __APPLE__
-        glutCheckLoop( );
+        glutCheckLoop();
 #else
-        glutMainLoopEvent( );
+        glutMainLoopEvent();
 #endif
 
         // sleep for 35ms
         std::this_thread::sleep_for(std::chrono::milliseconds(35));
-
     }
 
-    capture.release( );
-
+    capture.release();
 
     return EXIT_SUCCESS;
 }
 
 // Display the help for the programm
 
-void help( const char* programName )
+void help(const char *programName)
 {
     cout << "Detect a chessboard in a given video and visualize a teapot on top of it" << endl
-            << "Usage: " << programName << endl
-            << "     -w <board_width>                                  # the number of inner corners per one of board dimension" << endl
-            << "     -h <board_height>                                 # the number of inner corners per another board dimension" << endl
-            << "     -c <calib file>                                   # the name of the calibration file" << endl
-            << "     [-o <obj file>]                                   # the optional obj file containing the 3D model to display" << endl
-            << "     <video file>                                      # the name of the video file" << endl
-            << endl;
+         << "Usage: " << programName << endl
+         << "     -w <board_width>                                  # the number of inner corners per one of board dimension" << endl
+         << "     -h <board_height>                                 # the number of inner corners per another board dimension" << endl
+         << "     -c <calib file>                                   # the name of the calibration file" << endl
+         << "     [-o <obj file>]                                   # the optional obj file containing the 3D model to display" << endl
+         << "     <video file>                                      # the name of the video file" << endl
+         << endl;
 }
 
-
 // parse the input command line arguments
 
-bool parseArgs( int argc, char**argv, Size &boardSize, string &inputFilename, string &calibFile, string &objFile )
+bool parseArgs(int argc, char **argv, Size &boardSize, string &inputFilename, string &calibFile, string &objFile)
 {
     // check the minimum number of arguments
-    if( argc < 3 )
+    if (argc < 3)
     {
-        help( argv[0] );
+        help(argv[0]);
         return false;
     }
 
-
     // Read the input arguments
-    for( int i = 1; i < argc; i++ )
+    for (int i = 1; i < argc; i++)
     {
-        const char* s = argv[i];
+        const char *s = argv[i];
 
-        if( strcmp( s, "-w" ) == 0 )
+        if (strcmp(s, "-w") == 0)
         {
-            if( sscanf( argv[++i], "%u", &boardSize.width ) != 1 || boardSize.width <= 0 )
+            if (sscanf(argv[++i], "%u", &boardSize.width) != 1 || boardSize.width <= 0)
             {
                 cerr << "Invalid board width" << endl;
                 return false;
             }
         }
-        else if( strcmp( s, "-h" ) == 0 )
+        else if (strcmp(s, "-h") == 0)
         {
-            if( sscanf( argv[++i], "%u", &boardSize.height ) != 1 || boardSize.height <= 0 )
+            if (sscanf(argv[++i], "%u", &boardSize.height) != 1 || boardSize.height <= 0)
             {
                 cerr << "Invalid board height" << endl;
                 return false;
             }
         }
-        else if( s[0] != '-' )
+        else if (s[0] != '-')
         {
-            inputFilename.assign( s );
+            inputFilename.assign(s);
         }
-        else if( strcmp( s, "-c" ) == 0 )
+        else if (strcmp(s, "-c") == 0)
         {
-            if( i + 1 < argc )
-                calibFile.assign( argv[++i] );
+            if (i + 1 < argc)
+                calibFile.assign(argv[++i]);
             else
             {
                 cerr << "Missing argument for option " << s << endl;
                 return false;
             }
         }
-        else if( strcmp( s, "-o" ) == 0 )
+        else if (strcmp(s, "-o") == 0)
         {
-            if( i + 1 < argc )
-                objFile.assign( argv[++i] );
+            if (i + 1 < argc)
+                objFile.assign(argv[++i]);
             else
             {
                 cerr << "Missing argument for the obj file " << s << endl;