Index: trunk/PtGreyCameras/Flea3Grabber.cpp
===================================================================
--- trunk/PtGreyCameras/Flea3Grabber.cpp	(revision 51)
+++ trunk/PtGreyCameras/Flea3Grabber.cpp	(revision 53)
@@ -15,9 +15,5 @@
 
 #include <iomanip>
-#include "opencv2/objdetect/objdetect.hpp"
-#include "opencv2/calib3d/calib3d.hpp"
-#include "opencv2/imgproc/imgproc.hpp"
-#include "opencv2/core/core.hpp"
-#include "opencv2/highgui/highgui.hpp"
+#include <iostream>
 
 #include "Pacpus/kernel/DbiteException.h"
@@ -176,6 +172,23 @@
 
 	if (config.getProperty("use_shmem") != QString::null)
-			this->use_shmem = (bool)config.getProperty("use_shmem").toInt();
-
+		this->use_shmem = (bool)config.getProperty("use_shmem").toInt();
+
+	if (config.getProperty("shmem_corrected") != QString::null)
+		this->shmem_corrected = (bool)config.getProperty("shmem_corrected").toInt();
+
+	if (config.getProperty("cam_translationx") != QString::null)
+		trans_[0] = config.getProperty("cam_translationx").toFloat();
+	if (config.getProperty("cam_translationy") != QString::null)
+		trans_[1] = config.getProperty("cam_translationy").toFloat();
+	if (config.getProperty("cam_translationz") != QString::null)
+		trans_[2] = config.getProperty("cam_translationz").toFloat();
+
+	if (config.getProperty("cam_rotationx") != QString::null)
+		rot_[0] = config.getProperty("cam_rotationx").toFloat();
+	if (config.getProperty("cam_rotationy") != QString::null)
+		rot_[1] = config.getProperty("cam_rotationy").toFloat();
+	if (config.getProperty("cam_rotationz") != QString::null)
+		rot_[2] = config.getProperty("cam_rotationz").toFloat();
+	
 	for (int i = 0; i < nbrCamera_; i++)
 	{
@@ -277,4 +290,20 @@
 		if (config.getProperty("cam_strobe_duration" + QString::number(i)) != QString::null)
 			settings_[i].cam_strobe_duration = config.getProperty("cam_strobe_duration" + QString::number(i)).toInt();
+
+		//-------------------------------- Camera settings --------------------------------------------------
+
+		if (config.getProperty("cam_fx" + QString::number(i)) != QString::null)
+			settings_[i].tmp_matrix[0][0] = config.getProperty("cam_fx" + QString::number(i)).toDouble();// / settings_[i].cam_width;
+		if (config.getProperty("cam_fy" + QString::number(i)) != QString::null)
+			settings_[i].tmp_matrix[1][1] = config.getProperty("cam_fy" + QString::number(i)).toDouble();// / settings_[i].cam_height;
+		if (config.getProperty("cam_cx" + QString::number(i)) != QString::null)
+			settings_[i].tmp_matrix[0][2] = config.getProperty("cam_cx" + QString::number(i)).toDouble();// / settings_[i].cam_width;
+		if (config.getProperty("cam_cy" + QString::number(i)) != QString::null)
+			settings_[i].tmp_matrix[1][2] = config.getProperty("cam_cy" + QString::number(i)).toDouble();// / settings_[i].cam_height;
+		settings_[i].tmp_matrix[2][2] = 1.0;
+
+		for (int j = 0; j < 8; j++) // 8 max parameters for the distortion in OpenCV
+			if (config.getProperty("cam_distk" + QString::number(j + 1) + QString::number(i)) != QString::null)
+				settings_[i].distCoe[j] = config.getProperty("cam_distk" + QString::number(j + 1) + QString::number(i)).toDouble();
 
 		//-------------------------------- Recording options ------------------------------------------------
@@ -381,4 +410,11 @@
 	this->settings_[indice].image_compact = 0;				// Don't compact the image data
 	this->settings_[indice].save2dbt = 0;
+
+	for (int i = 0; i < 3; i++)
+		for (int j = 0; j < 3; j++)
+			settings_[indice].tmp_matrix[i][j] = 0.0;
+
+	for (int i = 0; i < 8; i++) // 8 max for dist coeffs
+		settings_[indice].distCoe[i] = 0.0;
 }
 
@@ -393,5 +429,43 @@
 	this->is_running = true;
 
+	// Tmp matrice used because of intelligent pointer of OpenCV and "intelligent" memory free 
+	cv::Mat_<double> tmp_cameraMatrix(3,3); // 3x3 matrix
+	cv::Mat_<double> tmp_distCoeffs(8,1);   // 5x1 matrix
+	cv::Mat_<double> R(3,1);            // 3x1 matrix, rotation left to right camera
+	cv::Mat_<double> T(3,1);            // 3x1 matrix, translation left to right proj. center
+	cv::Mat R1, R2, P1, P2, Q;
+	std::vector<cv::Mat> map1(nbrCamera_);
+	std::vector<cv::Mat> map2(nbrCamera_);
+	std::vector<cv::Mat> correctedImg(nbrCamera_);
+
+	for (int i = 0; i < nbrCamera_; i++)
+	{
+		correctedImg.push_back(cv::Mat(settings_[i].cam_height, settings_[i].cam_width, CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels)));
+		map1.push_back(cv::Mat());
+		map2.push_back(cv::Mat());
+
+		tmp_cameraMatrix << settings_[i].tmp_matrix[0][0], settings_[i].tmp_matrix[0][1], settings_[i].tmp_matrix[0][2], settings_[i].tmp_matrix[1][0], settings_[i].tmp_matrix[1][1], settings_[i].tmp_matrix[1][2], settings_[i].tmp_matrix[2][0], settings_[i].tmp_matrix[2][1], settings_[i].tmp_matrix[2][2];
+		settings_[i].matrix = tmp_cameraMatrix;
+		LOG_INFO(settings_[i].matrix);
+		tmp_distCoeffs << settings_[i].distCoe[0], settings_[i].distCoe[1], settings_[i].distCoe[2], settings_[i].distCoe[3], settings_[i].distCoe[4], settings_[i].distCoe[5], settings_[i].distCoe[6], settings_[i].distCoe[7];
+		settings_[i].distCoeffs = tmp_distCoeffs;
+		LOG_INFO(settings_[i].distCoeffs);
+	}
+
+	// Used only for two cameras
+	if (nbrCamera_ == 2)
+	{
+		// Initialisation des matrices pour les calculs OpenCV
+		R << rot_[0], rot_[1], rot_[2];
+		T << trans_[0], trans_[1], trans_[2];
+		
+		cv::stereoRectify(settings_[0].matrix, settings_[0].distCoeffs, settings_[1].matrix, settings_[1].distCoeffs, cv::Size(settings_[0].cam_width, settings_[0].cam_height), R, T, R1, R2, P1, P2, Q, CV_CALIB_ZERO_DISPARITY);
+
+		cv::initUndistortRectifyMap(settings_[0].matrix, settings_[0].distCoeffs, R1, P1, cv::Size(settings_[0].cam_width, settings_[0].cam_height), CV_16SC2, map1.at(0), map2.at(0));
+		cv::initUndistortRectifyMap(settings_[1].matrix, settings_[1].distCoeffs, R2, P2, cv::Size(settings_[1].cam_width, settings_[1].cam_height), CV_16SC2, map1.at(1), map2.at(1));	
+	}
+
 	std::vector<FlyCapture2::Camera *> cams;
+	std::vector<FlyCapture2::Image> convertedImage;
 
 	//==================================== Camera initialization =================================================
@@ -414,5 +488,5 @@
 
 	for (int i = 0; i < this->nbrCamera_; i++)
-	{
+	{	
 		if (this->use_shmem)
 		{
@@ -460,5 +534,5 @@
 
 		// Just for visualization
-		printf(
+		/*printf(
 			"\n*** CAMERA INFORMATION ***\n"
 			"Serial number - %u\n"
@@ -475,5 +549,5 @@
 			camInfo.sensorResolution,
 			camInfo.firmwareVersion,
-			camInfo.firmwareBuildTime);
+			camInfo.firmwareBuildTime);*/
 	    
 		// Query for available Format 7 modes
@@ -660,21 +734,39 @@
 		const int kNumberWidth = 5;
 		//--------------------------------------------------------------------------------------------
+		FlyCapture2::Image tmp_img;
+		convertedImage.push_back(tmp_img);
 	}
 	
 	// Raw image
-	FlyCapture2::Image rawImage;
-	FlyCapture2::Image convertedImage;
+	FlyCapture2::Image rawImage;	
 
 	//bool shmen_init = false;
+	// Because the API doesn't work without (saw this with FlyCapture2.exe)
+	if (settings_[masterCamera_].cam_trigger_enable)
+	{
+		cams[masterCamera_]->FireSoftwareTrigger();
+		usleep(50000);
+		cams[masterCamera_]->FireSoftwareTrigger();
+		usleep(50000);
+	}
 
 	road_time_t time;
-		
+	
 	while (THREAD_ALIVE) 
 	{
-		cams[masterCamera_]->FireSoftwareTrigger();
-
-		time = road_time();
-
-		for (int i = 1; i < this->nbrCamera_; i++)
+		if (settings_[masterCamera_].cam_trigger_enable)
+		{
+			error = cams[masterCamera_]->FireSoftwareTrigger();
+			if (error != FlyCapture2::PGRERROR_OK)
+			{
+					LOG_ERROR(error.GetDescription());
+					continue;
+			}
+			time = road_time();
+		}
+
+		//cout << settings_[1].timeStamp.microSeconds - settings_[0].timeStamp.microSeconds << endl;
+
+		for (int i = 0; i < this->nbrCamera_; i++)
 		{
 			// Retrieve an image
@@ -687,5 +779,5 @@
 
 			// Image timestamp
-			//FlyCapture2::TimeStamp timeStamp = rawImage.GetTimeStamp();
+			//settings_[i].timeStamp = rawImage.GetTimeStamp();
 
 			//time = road_time(); // Use the same time base of the other sensors
@@ -694,5 +786,5 @@
 			{
 				// Convert the raw image
-				error = rawImage.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &convertedImage);
+				error = rawImage.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &convertedImage[i]);
 				if (error != FlyCapture2::PGRERROR_OK)
 				{
@@ -704,5 +796,5 @@
 			{
 				// Convert the raw image
-				error = rawImage.Convert(FlyCapture2::PIXEL_FORMAT_BGR, &convertedImage);
+				error = rawImage.Convert(FlyCapture2::PIXEL_FORMAT_BGR, &convertedImage[i]);
 				if (error != FlyCapture2::PGRERROR_OK)
 				{
@@ -712,4 +804,6 @@
 			}
 
+			cv::remap(cv::Mat(convertedImage[i].GetRows(), convertedImage[i].GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), convertedImage[i].GetData(), (int)((double)convertedImage[i].GetReceivedDataSize()/(double)convertedImage[i].GetRows())), correctedImg[i], map1.at(i), map2.at(i), CV_INTER_LINEAR);
+		
 			if (this->use_shmem)
 			{
@@ -720,8 +814,23 @@
 				// Copy images header and data to memory
 				memcpy(this->settings_[i].img_mem, &settings_[i].ImageHeader, sizeof(TimestampedStructImage)); 
-				memcpy((void*)((TimestampedStructImage*)this->settings_[i].img_mem + 1), (void*)convertedImage.GetData(), this->settings_[i].ImageHeader.image.data_size); 
+				
+				if (shmem_corrected)
+					memcpy((void*)((TimestampedStructImage*)this->settings_[i].img_mem + 1), (void*)correctedImg[i].data, this->settings_[i].ImageHeader.image.data_size); 
+				else
+					memcpy((void*)((TimestampedStructImage*)this->settings_[i].img_mem + 1), (void*)convertedImage[i].GetData(), this->settings_[i].ImageHeader.image.data_size); 
+				
 				this->shmem_images_[i]->write(this->settings_[i].img_mem, this->settings_[i].img_mem_size);
 			}
 
+			if (this->settings_[i].showdebug)
+			{
+				cv::namedWindow(QString("Flea3Component - Current Reference Frame " + QString::number(i)).toStdString(), CV_WINDOW_NORMAL);
+				cv::imshow(QString("Flea3Component - Current Reference Frame " + QString::number(i)).toStdString(), cv::Mat(convertedImage[i].GetRows(), convertedImage[i].GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), convertedImage[i].GetData(), (int)((double)convertedImage[i].GetReceivedDataSize()/(double)convertedImage[i].GetRows())));
+
+				cv::namedWindow(QString("Flea3Component - Current Corrected Frame " + QString::number(i)).toStdString(), CV_WINDOW_NORMAL);
+				cv::imshow(QString("Flea3Component - Current Corrected Frame " + QString::number(i)).toStdString(), cv::Mat(convertedImage[i].GetRows(), convertedImage[i].GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), correctedImg[i].data, (int)((double)convertedImage[i].GetReceivedDataSize()/(double)convertedImage[i].GetRows())));
+				cv::waitKey(1);
+			}
+		
 			/////////////////////
 			// Write images to disk
@@ -833,14 +942,4 @@
 			//LOG_TRACE("image no. " << imageCount << " acquired successfully");
 			//++imageCount;
-
-			if (this->settings_[i].showdebug)
-			{
-				unsigned int rowBytes = (int)((double)convertedImage.GetReceivedDataSize()/(double)convertedImage.GetRows()); 
-				cv::Mat Img_BGR = cv::Mat(convertedImage.GetRows(), convertedImage.GetCols(), CV_MAKE_TYPE(CV_8U, this->settings_[i].cam_channels), convertedImage.GetData(), rowBytes);
-			
-				cv::namedWindow("Flea3Component - Current Reference Frame", CV_WINDOW_KEEPRATIO);
-				cv::imshow("Flea3Component - Current Reference Frame", Img_BGR);
-				cv::waitKey(1);
-			}
 		}
    	}
