add antoine latest modification, and code cleanup

svn path=/trunk/externals/pix_opencv/; revision=16382

2 files changed, 136 insertions, 60 deletions

diff --git a/pix_opencv_calibration.cc b/pix_opencv_calibration.cc
index f7681c3..053a6a5 100644
--- a/pix_opencv_calibration.cc
+++ b/pix_opencv_calibration.cc
@@ -19,8 +19,6 @@
 
 #include "pix_opencv_calibration.h"
 #include <stdio.h>
-//#include <stdlib.h>
-//#include <string.h>
 
 CPPEXTERN_NEW(pix_opencv_calibration)
 
@@ -34,6 +32,8 @@ CPPEXTERN_NEW(pix_opencv_calibration)
 /////////////////////////////////////////////////////////
 pix_opencv_calibration :: pix_opencv_calibration()
 { 
+	m_dataout = outlet_new(this->x_obj, 0);
+
 	find_rgb = NULL;
 	find_gray = NULL;
 	rgb = NULL;
@@ -58,23 +58,8 @@ pix_opencv_calibration :: pix_opencv_calibration()
 	intrinsic_matrix 		= 	cvCreateMat(3, 3, CV_32FC1);
 	distortion_coeffs 		= 	cvCreateMat(5, 1, CV_32FC1); 
 	
-	// make an "empty" intrinsinc matrix
-	CV_MAT_ELEM( *intrinsic_matrix, float, 0, 0 ) = 8;
-	CV_MAT_ELEM( *intrinsic_matrix, float, 1, 0 ) = 0;
-	CV_MAT_ELEM( *intrinsic_matrix, float, 2, 0 ) = 0;
-	CV_MAT_ELEM( *intrinsic_matrix, float, 0, 1 ) = 0;
-	CV_MAT_ELEM( *intrinsic_matrix, float, 1, 1 ) = 8;
-	CV_MAT_ELEM( *intrinsic_matrix, float, 2, 1 ) = 0;
-	CV_MAT_ELEM( *intrinsic_matrix, float, 0, 2 ) = 3;
-	CV_MAT_ELEM( *intrinsic_matrix, float, 1, 2 ) = 3;
-	CV_MAT_ELEM( *intrinsic_matrix, float, 2, 2 ) = 1;
-
-	// zeros distortion coeffs
-	for ( int i = 0 ; i < 5 ; i++ ) {
-	CV_MAT_ELEM( *distortion_coeffs, float, i, 0 ) = 0.0;
-	}
-
-	post("pix_opencv_calibration build on %s at %s", __DATE__, __TIME__);
+	pix_opencv_calibration :: resetCorrectionMatrix();
+	//~ post("pix_opencv_calibration build on %s at %s", __DATE__, __TIME__);
 }
 
 /////////////////////////////////////////////////////////
@@ -106,7 +91,6 @@ void pix_opencv_calibration :: processRGBAImage(imageStruct &image)
 { 
 	if ((this->comp_xsize!=image.xsize)||(this->comp_ysize!=image.ysize)||(!rgb)) { 
 	
-		// printf("process rgba image\n");
 		this->comp_xsize = image.xsize;
 		this->comp_ysize = image.ysize;
 		if ( calibration ) error ( "image size changed, calibration was cancelled");
@@ -125,7 +109,7 @@ void pix_opencv_calibration :: processRGBAImage(imageStruct &image)
 		find_gray = cvCreateImage(cvSize(this->comp_xsize,this->comp_ysize), IPL_DEPTH_8U, 1);
 
 		//create the images with new size
-		rgb = cvCreateImage(cvSize(this->comp_xsize,this->comp_ysize), IPL_DEPTH_8U, 4);
+		rgb = cvCreateImageHeader(cvSize(this->comp_xsize,this->comp_ysize), IPL_DEPTH_8U, 4);
 		tmp  = cvCreateImage(cvSize(this->comp_xsize,this->comp_ysize), IPL_DEPTH_8U, 4);
 		mapx = cvCreateImage(cvSize(this->comp_xsize,this->comp_ysize), IPL_DEPTH_32F, 1);
 		mapy = cvCreateImage(cvSize(this->comp_xsize,this->comp_ysize), IPL_DEPTH_32F, 1);
@@ -164,11 +148,16 @@ void pix_opencv_calibration :: processGrayImage(imageStruct &image)
 { 
 	if ((this->comp_xsize!=image.xsize)||(this->comp_ysize!=image.ysize)||(!gray)) { 
 		
-		// printf("process gray image\n");
 		this->comp_xsize = image.xsize;
 		this->comp_ysize = image.ysize;
-		if ( calibration ) error ( "image size changed, calibration was cancelled");
-		calibration = 0;
+		if ( calibration ) { 
+			error ( "image size changed, calibration was cancelled");
+			calibration = 0;
+			
+			t_atom data_out;
+			SETFLOAT(&data_out, calibration);
+			outlet_anything( this->m_dataout, gensym("calibration"), 1, &data_out);
+		}
 
 		if ( find_rgb ) cvReleaseImage(&find_rgb);
 		if ( find_gray ) cvReleaseImage(&find_gray);
@@ -183,7 +172,7 @@ void pix_opencv_calibration :: processGrayImage(imageStruct &image)
 		find_gray = cvCreateImage(cvSize(this->comp_xsize,this->comp_ysize), IPL_DEPTH_8U, 1);
 		
 		//create the images with new size
-		gray = cvCreateImage(cvSize(this->comp_xsize,this->comp_ysize), IPL_DEPTH_8U, 1);
+		gray = cvCreateImageHeader(cvSize(this->comp_xsize,this->comp_ysize), IPL_DEPTH_8U, 1);
 		tmp = cvCreateImage(cvSize(this->comp_xsize,this->comp_ysize), IPL_DEPTH_8U, 1);
 		mapx = cvCreateImage(cvSize(this->comp_xsize,this->comp_ysize), IPL_DEPTH_32F, 1);
 		mapy = cvCreateImage(cvSize(this->comp_xsize,this->comp_ysize), IPL_DEPTH_32F, 1);
@@ -221,12 +210,9 @@ void pix_opencv_calibration :: findCorners ( IplImage *image )
 	int					corner_count;
 	int					step;
 	CvSize				patternSize, image_size;
-	CvMat				in_cv;
 	
 	patternSize = cvSize( this->patternSize[0], this->patternSize[1] );
 	image_size = cvSize( image->width, image->height );
-	// cvGetImage	(&in_cv,	&in_image);	// create an IplImage from a CvMat
-	cvGetMat ( image, &in_cv ); // create a CvMat from IplImage
 
 	// find chessboard corners (gray or RGBA image...)
 	int found = cvFindChessboardCorners(image, 
@@ -235,6 +221,7 @@ void pix_opencv_calibration :: findCorners ( IplImage *image )
 										&corner_count, 
 										findChessFlag);
 	if (image->nChannels == 4) {
+		cvCopy(image, find_rgb) ;
 		cvCvtColor( image , find_gray , CV_RGBA2GRAY); // convert color to gray
 	} else {
 		cvCopy(image, find_gray) ;
@@ -248,13 +235,17 @@ void pix_opencv_calibration :: findCorners ( IplImage *image )
 					   cvSize(-1,-1), 
 					   cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1));
 
-	cvCvtColor( find_gray , find_rgb , CV_GRAY2RGBA); // convert gray to color
 
 	// draw chessboard corner (color image only)
-	cvDrawChessboardCorners(find_rgb, patternSize, corners, corner_count, found);
-
+	if (image->nChannels == 4) cvDrawChessboardCorners(find_rgb, patternSize, corners, corner_count, found);
+	else 
+	{
+		cvCvtColor( find_gray , find_rgb , CV_GRAY2RGBA); // convert gray to color
+		cvDrawChessboardCorners(find_rgb, patternSize, corners, corner_count, found);
+	}
+	
 	this->frame++;
-	if ( this->frame % this->wait_n_frame == 0 ) {
+	if ( this->frame % this->wait_n_frame == 0 ) { 
 	// update arrays
 
 		if( corner_count == board_point_nb ) {
@@ -271,7 +262,11 @@ void pix_opencv_calibration :: findCorners ( IplImage *image )
 
 			cvNot( find_rgb , find_rgb );
 		}
-		post("take : %d/%d\n", success_count, board_view_nb);
+		
+		t_atom data_out;
+		SETFLOAT(&data_out, success_count);
+		outlet_anything( this->m_dataout, gensym("take"), 1, &data_out);
+
 	}
 
 	// convert color to gray
@@ -304,7 +299,24 @@ void pix_opencv_calibration :: computeCalibration ( IplImage *image )
 	this->mapy = cvCreateImage( cvSize( image->width, image->height ), IPL_DEPTH_32F, 1 );
 
 	cvInitUndistortMap(this->intrinsic_matrix, this->distortion_coeffs, this->mapx, this->mapy);
+	
+	t_atom intra_out[9];
+	for ( int i = 0 ; i < 9 ; i++ ){
+		SETFLOAT(&intra_out[i], CV_MAT_ELEM( *intrinsic_matrix, float, i%3, i/3));
+	}		
+	outlet_anything( this->m_dataout, gensym("intrinsic_matrix"), 9, intra_out);
+	
+	t_atom dist_out[5];
+	for ( int i = 0 ; i < 5 ; i++ ){
+		SETFLOAT(&dist_out[i], CV_MAT_ELEM( *distortion_coeffs, float, i, 0));
+	}		
+	outlet_anything( this->m_dataout, gensym("distortion_coeffs"), 5, dist_out);
+	
 	calibration = 0;
+	
+	t_atom data_out;
+	SETFLOAT(&data_out, calibration);
+	outlet_anything( this->m_dataout, gensym("calibration"), 1, &data_out);
 }
 /////////////////////////////////////////////////////////
 // LoadMess
@@ -313,23 +325,31 @@ void pix_opencv_calibration :: computeCalibration ( IplImage *image )
 
 void pix_opencv_calibration :: loadIntraMess (t_symbol *filename)
 {
-	if ( filename == NULL ) { error("NULL pointer passed to function loadIntra"); return;}
-	this->intrinsic_matrix = (CvMat*)cvLoad(filename->s_name, 0, 0, 0);
+	if ( filename->s_name[0] == 0 ) {
+		error("no filename passed to loadIntra message");
+		return;
+	}
+	if ( filename == NULL ) { error("%s is not a valid matrix", filename->s_name); return;}
+	this->intrinsic_matrix = (CvMat*)cvLoad(filename->s_name, 0, 0, 0);// TODO crash when passing non-XML file
   
 	if (intrinsic_matrix == NULL) {
 		intrinsic_matrix = 	cvCreateMat(3, 3, CV_32FC1);
-		post("can't open file %s", filename->s_name); 
-		CV_MAT_ELEM( *intrinsic_matrix, float, 0, 0 ) = 8;
-		CV_MAT_ELEM( *intrinsic_matrix, float, 1, 0 ) = 0;
-		CV_MAT_ELEM( *intrinsic_matrix, float, 2, 0 ) = 0;
-		CV_MAT_ELEM( *intrinsic_matrix, float, 0, 1 ) = 0;
-		CV_MAT_ELEM( *intrinsic_matrix, float, 1, 1 ) = 8;
-		CV_MAT_ELEM( *intrinsic_matrix, float, 2, 1 ) = 0;
-		CV_MAT_ELEM( *intrinsic_matrix, float, 0, 2 ) = 3;
-		CV_MAT_ELEM( *intrinsic_matrix, float, 1, 2 ) = 3;
-		CV_MAT_ELEM( *intrinsic_matrix, float, 2, 2 ) = 1;
+		error("can't open file %s", filename->s_name);
+		resetCorrectionMatrix();
+	}
+	else if ( intrinsic_matrix->rows != 3 || intrinsic_matrix->cols != 3 || CV_MAT_TYPE(intrinsic_matrix->type) != CV_32FC1 ) {
+		error("%s is not a valid intrinsic matrix", filename->s_name);
+		cvReleaseMat(&intrinsic_matrix);
+		intrinsic_matrix = 	cvCreateMat(3, 3, CV_32FC1);
+		resetCorrectionMatrix();
 	}
 	else post("load transformation matrix from %s",filename->s_name);
+	
+	t_atom intra_out[9];
+	for ( int i = 0 ; i < 9 ; i++ ){
+		SETFLOAT(&intra_out[i], CV_MAT_ELEM( *intrinsic_matrix, float, i%3, i/3));
+	}		
+	outlet_anything( this->m_dataout, gensym("intrinsic_matrix"), 9, intra_out);
 
 	// reinitialise size to force reinitialisation of mapx and mapy on next frame
 	this->comp_xsize = 0;  
@@ -337,31 +357,43 @@ void pix_opencv_calibration :: loadIntraMess (t_symbol *filename)
 
 void pix_opencv_calibration :: loadDistMess (t_symbol *filename)
 {
+	if ( filename->s_name[0] == 0 ) {
+		error("no filename passed to loadDist message");
+		return;
+	}
 	if ( filename == NULL ) { error("NULL pointer passed to function loadDist"); return;}
-	distortion_coeffs = (CvMat*)cvLoad(filename->s_name);
-	if (distortion_coeffs == NULL){
-		distortion_coeffs = cvCreateMat(5, 1, CV_32FC1); 
-		post("can't open file %s", filename->s_name); 
-		// zeros distortion coeffs
-		for ( int i = 0 ; i < 5 ; i++ ) {
-		CV_MAT_ELEM( *distortion_coeffs, float, i, 0 ) = 0.0;
-		}
+	distortion_coeffs = (CvMat*)cvLoad(filename->s_name); // TODO crash when passing non-XML file
+	
+	if (distortion_coeffs == NULL) { 
+		distortion_coeffs = 	cvCreateMat(5, 1, CV_32FC1);
+		error("can't open file %s", filename->s_name);
+		resetCorrectionMatrix();
+	}
+	else if( distortion_coeffs->rows != 5 || distortion_coeffs->cols != 1 || CV_MAT_TYPE(distortion_coeffs->type) != CV_32FC1 ) {
+		error("%s is not a valid distortions coeffs file", filename->s_name);
+		cvReleaseMat(&distortion_coeffs);
+		distortion_coeffs = 	cvCreateMat(3, 3, CV_32FC1);
+		resetCorrectionMatrix();
 	}
 	else post("load distortion coefficients from %s",filename->s_name);
 
+	t_atom dist_out[5];
+	for ( int i = 0 ; i < 5 ; i++ ){
+		SETFLOAT(&dist_out[i], CV_MAT_ELEM( *distortion_coeffs, float, i, 0));
+	}		
+	outlet_anything( this->m_dataout, gensym("distortion_coeffs"), 5, dist_out);
+	
 	// reinitialise size to force reinitialisation of mapx and mapy on next frame
 	this->comp_xsize = 0;  
 }
 
 void pix_opencv_calibration :: writeIntraMess (t_symbol *filename)
 {
-	// printf("write intrinsic matrix\n");
 	cvSave(filename->s_name,intrinsic_matrix);
 }
 
 void pix_opencv_calibration :: writeDistMess (t_symbol *filename)
 {
-	// printf("write distorsion coeffs\n");
 	cvSave(filename->s_name,distortion_coeffs);
 }
 
@@ -372,15 +404,25 @@ void pix_opencv_calibration :: floatCalibrationhMess (float calib_flag)
 		this->success_count = 0;
 		this->frame = 0;
 	}
-	post("calibration : %d", this->calibration);
+	t_atom data_out;
+	SETFLOAT(&data_out, calibration);
+	outlet_anything( this->m_dataout, gensym("calibration"), 1, &data_out);
 }
 
 void pix_opencv_calibration :: patternSizeMess (float xsize, float ysize)
 {
-	if (calibration) error("you can't change pattern size during calibration"); return;
-	if ( xsize < 3 || ysize < 3 ) error("patternSize should be at least 3x3"); return;
+	if (calibration) {error("you can't change pattern size during calibration"); return;}
+	if ( xsize < 3 || ysize < 3 ) {error("patternSize should be at least 3x3"); return;}
 	this->patternSize[0]=xsize;
 	this->patternSize[1]=ysize;
+	
+	// reallocate matrix
+	cvReleaseMat(&image_points);
+	cvReleaseMat(&object_points);
+	cvReleaseMat(&point_counts);
+	image_points	=	cvCreateMat(patternSize[0]*patternSize[1]*board_view_nb, 2, CV_32FC1);
+	object_points	=	cvCreateMat(patternSize[0]*patternSize[1]*board_view_nb, 3, CV_32FC1);
+	point_counts	=	cvCreateMat(board_view_nb, 1, CV_32SC1);	
 }
 
 void pix_opencv_calibration :: viewMess (int view)
@@ -389,6 +431,7 @@ void pix_opencv_calibration :: viewMess (int view)
 	board_view_nb=view<2?2:view;
 	if (view < 2) error("view should be greater or equal to 2");
 
+	// reallocate matrix
 	cvReleaseMat(&image_points);
 	cvReleaseMat(&object_points);
 	cvReleaseMat(&point_counts);
@@ -433,8 +476,10 @@ void pix_opencv_calibration :: obj_setupCallback(t_class *classPtr)
   		  gensym("view"), A_FLOAT, A_NULL);  
   class_addmethod(classPtr, (t_method)&pix_opencv_calibration::waitMessCallback,
   		  gensym("wait"), A_FLOAT, A_NULL); 
-    class_addmethod(classPtr, (t_method)&pix_opencv_calibration::findChessFlagMessCallback,
+	class_addmethod(classPtr, (t_method)&pix_opencv_calibration::findChessFlagMessCallback,
   		  gensym("findChessFlag"), A_FLOAT, A_FLOAT, A_FLOAT, A_NULL);    
+    class_addmethod(classPtr, (t_method)&pix_opencv_calibration::resetMessCallback,
+  		  gensym("reset"), A_NULL);    
   		    		  	  
 }
 void pix_opencv_calibration :: loadIntraMessCallback(void *data, t_symbol* filename)
@@ -474,3 +519,29 @@ void pix_opencv_calibration :: findChessFlagMessCallback(void *data, t_floatarg
 		GetMyClass(data)->findChessFlagMess((int) adaptThres, (int) normalize, (int) filter);
 }
 
+void pix_opencv_calibration :: resetMessCallback(void *data)
+{
+		GetMyClass(data)->resetCorrectionMatrix();
+}
+
+void pix_opencv_calibration ::  resetCorrectionMatrix()
+{
+		// make an "empty" intrinsinc matrix
+	CV_MAT_ELEM( *intrinsic_matrix, float, 0, 0 ) = 8;
+	CV_MAT_ELEM( *intrinsic_matrix, float, 1, 0 ) = 0;
+	CV_MAT_ELEM( *intrinsic_matrix, float, 2, 0 ) = 0;
+	CV_MAT_ELEM( *intrinsic_matrix, float, 0, 1 ) = 0;
+	CV_MAT_ELEM( *intrinsic_matrix, float, 1, 1 ) = 8;
+	CV_MAT_ELEM( *intrinsic_matrix, float, 2, 1 ) = 0;
+	CV_MAT_ELEM( *intrinsic_matrix, float, 0, 2 ) = 3;
+	CV_MAT_ELEM( *intrinsic_matrix, float, 1, 2 ) = 3;
+	CV_MAT_ELEM( *intrinsic_matrix, float, 2, 2 ) = 1;
+
+	// zeros distortion coeffs
+	for ( int i = 0 ; i < 5 ; i++ ) {
+	CV_MAT_ELEM( *distortion_coeffs, float, i, 0) = 0.0;
+	}
+	
+	// reinitialise size to force reinitialisation of mapx and mapy on next frame
+	this->comp_xsize = 0;
+}
diff --git a/pix_opencv_calibration.h b/pix_opencv_calibration.h
index 7114288..ea90a47 100644
--- a/pix_opencv_calibration.h
+++ b/pix_opencv_calibration.h
@@ -72,7 +72,8 @@ class GEM_EXTERN pix_opencv_calibration : public GemPixObj
 	void 	viewMess (int view);
 	void 	waitMess (int wait);
 	void 	findChessFlagMess(int adaptThres, int normalize, int filter);
-	
+	void 	resetCorrectionMatrix();
+
 	// to detect changes in the image size
 	int 	comp_xsize;
 	int		comp_ysize;
@@ -88,6 +89,8 @@ class GEM_EXTERN pix_opencv_calibration : public GemPixObj
 
     private:
     
+    t_outlet *m_dataout;
+    
    	//////////
    	// Static member functions
 	static void	loadIntraMessCallback(void *data, t_symbol* filename);
@@ -99,7 +102,9 @@ class GEM_EXTERN pix_opencv_calibration : public GemPixObj
    	static void viewMessCallback(void *data, t_floatarg view);
     static void waitMessCallback(void *data, t_floatarg wait); 
     static void findChessFlagMessCallback(void *data, t_floatarg adaptThres, t_floatarg normalize, t_floatarg filter);
+    static void resetMessCallback(void *data);
 	/////////
+
 	// CvMat needed
 	CvMat 	*image_points, 
 			*object_points,