
//=============================================================================
//
// main.cpp
// This program uses the StereoRig class to perform stereo undistortion
// and rectification for dense disparity maps
// Author: Pablo F. Alcantarilla
// Institution: Department of Electronics. University of Alcala
// Date: 14/10/2011
// Email: pablo.alcantarilla@depeca.uah.es
//=============================================================================

#include "main.h"

//******************************************************************************
//******************************************************************************

/** Main Function */
int main( int argc, char *argv[] )
{
   //***************************************************************************
   // Variables declaration
   //***************************************************************************
   unsigned int nImages = 0;
   char fileLeft[MAX_NUMBER_CHARACTERS], fileRight[MAX_NUMBER_CHARACTERS], fileDisp[MAX_NUMBER_CHARACTERS]; 
   char key = 0;
   tcamera_calib stereo_calib;

   // IplImages
   IplImage *img_left_input, *img_right_input;

   //***************************************************************************
   //***************************************************************************
   
   // Check that the input arguments
   if( argc != 4 )
   {
	   cout << "Error introducing input arguments!!" << endl;
	   cout << "Format is: ./stereo_disparity /left_images_path /right_images_path calibration_file.txt" << endl;
	   return -1;
   }
   
   // Parse the configuration file with the stereo rig info
   Parse_Configuration_File(&stereo_calib,argv[3]);
   
   // Create the Stereo Rig class
   StereoRig stereo(&stereo_calib);
   
   // Create OpenCV windows in case visualization is active
   if( VISUALIZATION_MODE == 1 )
   {
	   cvNamedWindow("Rectified Left Image", CV_WINDOW_NORMAL);
	   cvNamedWindow("Rectified Right Image", CV_WINDOW_NORMAL);
	   cvNamedWindow("Disparity Map",CV_WINDOW_NORMAL);	
   }

   // Read the first stereo image
   sprintf(fileLeft,"%s/image%05d.pgm",argv[1],nImages);   
   sprintf(fileRight,"%s/image%05d.pgm",argv[2],nImages); 
   
   // Loop to read the stereo images from the folders
   while(1)
   {
	   // get the distorted images
	   img_left_input = cvLoadImage(fileLeft,CV_LOAD_IMAGE_GRAYSCALE);
	   img_right_input = cvLoadImage(fileRight,CV_LOAD_IMAGE_GRAYSCALE);
	
	   if( img_left_input == NULL || img_right_input == NULL )
	   {
			break;
	   }

	   // Do the stereo processing here: undistortion, rectification, disparity map
	   stereo.Do_Stereo_Processing(img_left_input,img_right_input);
	   
	   if( VISUALIZATION_MODE == 1 )
	   {
		   cvShowImage("Rectified Left Image",stereo.Get_Left_Image());
		   cvMoveWindow("Rectified Left Image",WPOS1X,WPOS1Y);

		   cvShowImage("Rectified Right Image",stereo.Get_Right_Image());
		   cvMoveWindow("Rectified Right Image",WPOS2X,WPOS2Y);

		   cvShowImage("Disparity Map",stereo.Get_Disparity_Color_Image());
		   cvMoveWindow("Disparity Map",WPOS3X,WPOS3Y);
			
		   key = cvWaitKey(10);
	   }
	   
	   // For saving the output images!!
	   if( SAVE_OUPUT_IMAGES )
	   {
		   sprintf(fileLeft,"./out_images/left/image%05d.jpg",nImages);	
		   cvSaveImage(fileLeft,stereo.Get_Left_Image());
		   
		   sprintf(fileRight,"./out_images/right/image%05d.jpg",nImages);	
		   cvSaveImage(fileRight,stereo.Get_Right_Image());
				   
		   sprintf(fileDisp,"./out_images/disp/image%05d.jpg",nImages);	
   		   cvSaveImage(fileDisp,stereo.Get_Disparity_Color_Image());
	   }

	   // Get ready for the next stereo frame
	   nImages++;
	   sprintf(fileLeft,"%s/image%05d.pgm",argv[1],nImages);
	   sprintf(fileRight,"%s/image%05d.pgm",argv[2],nImages);
	   
	   // Release the allocated memory for the images
	   cvReleaseImage(&img_left_input);
	   cvReleaseImage(&img_right_input);

	   // To exit and quit the program press q over a window
	   if( key == 'q' )
	   {
		   break;
	   }
   }
   
   // Destroy the OpenCV windows
   if( VISUALIZATION_MODE == 1 )
   {
	   cvDestroyWindow("Rectified Left Image");
	   cvDestroyWindow("Rectified Right Image");
	   cvDestroyWindow("Disparity Map");	   
   }
}

//*************************************************************************************
//*************************************************************************************

/** This function is for parsing the .txt calibration file							 */
/** The function saves the data in a tcamera_calib structure						 */
void Parse_Configuration_File(tcamera_calib *stereo, char *calib_file)
{
	setlocale(LC_ALL,"C");

	ifstream infile;
	infile.exceptions(ifstream::eofbit | ifstream::failbit | ifstream::badbit );
	infile.open(calib_file,ifstream::in);

	// Initialize the values of some matrixes
	stereo->ML = Zeros;
	stereo->MR = Zeros;
	stereo->ML[2][2] = 1.0;
	stereo->MR[2][2] = 1.0;	
	stereo->DL[4] = 0.0;
	stereo->DR[4] = 0.0;
	
	// Now read the information from the txt file
	infile.getline(stereo->sequence_info,MAX_NUMBER_CHARACTERS);
	
	infile >> stereo->width;
	infile >> stereo->height;
	infile >> stereo->ML[0][0];
	infile >> stereo->ML[1][1];
	infile >> stereo->ML[0][2];
	infile >> stereo->ML[1][2];
	
	infile >> stereo->MR[0][0];
	infile >> stereo->MR[1][1];
	infile >> stereo->MR[0][2];
	infile >> stereo->MR[1][2];
	
	infile >> stereo->R[0][0];
	infile >> stereo->R[0][1];
	infile >> stereo->R[0][2];
	infile >> stereo->R[1][0];
	infile >> stereo->R[1][1];
	infile >> stereo->R[1][2];
	infile >> stereo->R[2][0];
	infile >> stereo->R[2][1];
	infile >> stereo->R[2][2];
	
	infile >> stereo->T[0];
	infile >> stereo->T[1];
	infile >> stereo->T[2];
	
	infile >> stereo->DL[0];
	infile >> stereo->DL[1];
	infile >> stereo->DL[2];
	infile >> stereo->DL[3];
	
	infile >> stereo->DR[0];
	infile >> stereo->DR[1];
	infile >> stereo->DR[2];
	infile >> stereo->DR[3];
	
	// Compute the essential and fundamental matrixes
	Compute_Essential_Matrix(stereo);
	Compute_Fundamental_Matrix(stereo);
	
	infile.close();
	
}

//*************************************************************************************
//*************************************************************************************

/** This function computes the essential matrix of the stereo rig					 */
void Compute_Essential_Matrix(tcamera_calib *stereo)
{
	TooN::Matrix<3,3> Taux;
	Taux = Zeros;
	
	Taux[0][1] = -stereo->T[2];
	Taux[0][2] = stereo->T[1];
	Taux[1][0] = stereo->T[2];
	Taux[1][2] = -stereo->T[0];
	Taux[2][0] = -stereo->T[1];
	Taux[2][1] = stereo->T[0];
	
	stereo->E = Taux*stereo->R;	
}

//*************************************************************************************
//*************************************************************************************

/** This function computes the fundamental matrix of the stereo rig					 */
void Compute_Fundamental_Matrix(tcamera_calib *stereo)
{
	TooN::Matrix<3,3> b;
	LU<3> MLu(stereo->ML);
	LU<3> MRu(stereo->MR);
	b = Identity;
	
    // Compute the inverse with LU decomposition
    stereo->ML_inv = MLu.backsub(b);
	stereo->MR_inv = MRu.backsub(b);

	// Finally compute the fundamental matrix
	stereo->F = stereo->MR_inv.T()*stereo->E*stereo->ML_inv;
}