calibrtwocamadlg.cpp

这是前段时间自己写的一个给两个摄像机定标的对话框程序

	CvFont dfont;
	cvInitFont (&dfont, CV_FONT_VECTOR0, 0.3, 0.3, 0.0f, 1);

	CvPoint onecorner; 

	int numcorners = m_nNumCorners;

	CvPoint2D64d* uv  = new CvPoint2D64d[m_nImageNumber * m_nNumCorners];
	CvPoint3D64d* XYZ = new CvPoint3D64d[m_nImageNumber * m_nNumCorners];

	CvPoint2D32f* corners = new CvPoint2D32f[m_nNumCorners]; //declare an array for the corner points
	CvMemStorage* storage = 0; // allocate some storage for ??

	m_nEffectiveImageNumber=-1;

	for( int imgnum=0; imgnum<m_nImageNumber; imgnum++ )
	{
		numcorners = m_nNumCorners;

		img = p2p_InputImages[imgnum];
		
		imgsize.width = img->width;
		imgsize.height= img->height;

		img0 = cvCloneImage(img); // make copy of the image
		img1 = cvCloneImage(img); // make another copy of the image
		greyimg = cvCreateImageHeader(imgsize,IPL_DEPTH_8U,1); 
		cvCreateImageData(greyimg);

		cvCvtColor(img, greyimg, CV_RGB2GRAY); // convert color image to grey
		img0 = cvCloneImage(greyimg); 

		// Find Corners
		cvFindChessBoardCornerGuesses(greyimg, 
									  img0, 
									  storage, 
									  cvSize(m_nXHeight,m_nXWidth),
									  corners,
									  &numcorners);

		if( numcorners != m_nNumCorners ) {
			// release images
			//cvReleaseImage( &img );
			cvReleaseImage( &img0 );
			cvReleaseImage( &img1 );
			cvReleaseImage( &greyimg );
			continue;
		}
		else
			m_nEffectiveImageNumber++;

		// draw a circle at each corner found
		for( int t = 0; t < numcorners; t++ ) 
		{
			onecorner.x = (int)corners[t].x;
			onecorner.y = (int)corners[t].y;

			cvCircle(img1, onecorner, 8, CV_RGB(0,255,0),2);
			// image, center, radius, color, thickness 
		}

		// Find sub-corners
		cvFindCornerSubPix(greyimg,
						   corners, 
						   numcorners, 
						   cvSize (m_nXHeight,m_nXWidth), 
						   cvSize(-1, -1), 
						   cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 10, 0.1));

		// correct order
		if( m_btnApplyOrder ){
			CvPoint3D32f init = FindRectangleCorner( corners, numcorners );
			if( init.z < m_nXHeight*m_nXWidth )
				SortPoints(corners, numcorners, &init);
			else
			{
				AfxMessageBox("SortError");
				// release images
				cvReleaseImage( &img0 );
				cvReleaseImage( &img1 );
				cvReleaseImage( &greyimg );
				m_nEffectiveImageNumber--;
				continue;
			}
		}

		//draw a circle and put the corner number at each subcorner found
		for( t = 0; t < numcorners; t++ ) 
		{
			onecorner.x = (int)corners[t].x;
			onecorner.y = (int)corners[t].y;

			cvCircle(img1, onecorner, 3, CV_RGB(255,0,0),1);

			cvPutText(img1,numbers[t], cvPoint(onecorner.x, onecorner.y + 20), &dfont, CV_RGB(255,0,0)); 
		}

		// CAMERA CALIBRATION PART 
		for( int currPoint=0; currPoint < numcorners; currPoint++ ) 
		{ 
			// Copy image points data 
			uv[ m_nEffectiveImageNumber*numcorners + currPoint].x = corners[currPoint].x;
			uv[ m_nEffectiveImageNumber*numcorners + currPoint].y = corners[currPoint].y;
		}

		int index;
		for( int i = 0; i < m_nXWidth; i++ )
		{
			for( int j = 0; j < m_nXHeight; j++ )
			{
				index = m_nEffectiveImageNumber*numcorners + i*m_nXHeight+j;
				
				XYZ[ index ].x = m_dGridWidth *(m_nXWidth -i);
				XYZ[ index ].y = m_dGridHeight*(m_nXHeight-j);
				XYZ[ index ].z = 0; 
			}
		}

		if( m_bDisplayCorners )
		{
			cvvNamedWindow( "image", 1 ); //create window with the name "image"
			cvvShowImage("image",img1);
			cvvWaitKey(0); // wait for key press before displaying next image
			cvDestroyWindow( "image" );
		}

		// release images
//		cvReleaseImage( &img );
		cvReleaseImage( &img0 );
		cvReleaseImage( &img1 );
		cvReleaseImage( &greyimg );
 
	} //loop to next image 

	// clear memory storage - reset free space position
	free (corners);

	m_nEffectiveImageNumber++;

	delete []uveff ; uveff  =NULL;
	delete []XYZeff; XYZeff = NULL;

	if( m_nImageNumber == m_nEffectiveImageNumber ){
		uveff  = uv;  uv  = NULL;
		XYZeff = XYZ; XYZ = NULL;
	}
	else
	{
		int size = m_nEffectiveImageNumber * m_nNumCorners;
		uveff  = new CvPoint2D64d[size];
		XYZeff = new CvPoint3D64d[size];

		for(int ph=0; ph<size; ph++)
		{
			uveff [ph] = uv [ph];
			XYZeff[ph] = XYZ[ph];
		}
		delete []uv;  uv  = NULL;
		delete []XYZ; XYZ = NULL;
	}

}

CvPoint3D32f CCalibrtwocamaDlg::FindRectangleCorner(CvPoint2D32f *points, int n)
{

	CvPoint3D32f retVal;
	CvPoint2D32f out;
	
	for( int k=0; k<n; k++ )
	{
		out = CalculateAngleRespectTo( points, n,  points[k], m_nXHeight-2, m_nXWidth-2, true, true);
		if( out.x == INT_MIN || out.y == INT_MIN )
			continue;
		else
			break;
	}
	
	out = CalculateAngleRespectTo( points, n,  points[k], m_nXHeight-2, m_nXWidth-2, false, true);
	
	retVal.x = out.x;
	retVal.y = out.y;
	retVal.z = (float)k;
	
	if( k >= m_nXWidth*m_nXHeight )
		return retVal; // error
	
	/////////////////////////////////////////////////////////////
	//	iterate the corner to the top-left coordinate  //////////
	/////////////////////////////////////////////////////////////
	
	int tempIndex=k;
	double degree1, degree2;
	for( int i=0; i<n; i++ )
	{
		degree1 = atan2(points[k].y-points[i].y,points[i].x-points[k].x);
		degree2 = fabs(degree1 - retVal.y);
		
		if( degree2 < PI/90 || degree1 == 0 )
		{
			if( points[i].x < points[tempIndex].x  )
			{
				tempIndex = i;
			}
		}
	}
	k = tempIndex;
	
	// find the the most repeting angle in the y direction
	out = CalculateAngleRespectTo( points, n, points[k], m_nXHeight-2);
	if( out.x == INT_MIN ) AfxMessageBox("error after CalculateAngleRespectTo");
	retVal.x = out.x; // orientation of y-alignment
	
	for( i=0; i<n; i++ )
	{
		degree1 = atan2(points[k].y-points[i].y,points[i].x-points[k].x);
		degree2 = fabs(degree1 - retVal.x);
		
		if( degree2 < PI/90 || degree1 == 0 )
		{
			if( points[i].y < points[tempIndex].y  )
			{
				tempIndex = i;
			}
		}
	}
	k = tempIndex; // TOP-LEFT FOUND
	
	// Find the angles of alignment	
	out = CalculateAngleRespectTo( points, n, points[tempIndex], m_nXHeight-2, m_nXWidth-2, false, true);
	if( out.x == INT_MIN ) AfxMessageBox("error after CalculateAngleRespectTo");
	
	retVal.x = out.x;
	retVal.y = out.y;
	retVal.z = (float)tempIndex;
	
	return retVal;
}

void CCalibrtwocamaDlg::SortPoints(CvPoint2D32f *&points, int n, CvPoint3D32f *init)
{

	CvPoint2D32f* output   = new CvPoint2D32f[n         ];
	CvPoint2D32f* longrow  = new CvPoint2D32f[m_nXHeight];
	CvPoint2D32f* shortcol = new CvPoint2D32f[m_nXWidth ];
	
	int startPointIndex = (int)init[0].z;
	
	int scounter = 0 ;
	
	double degree1, degree2;
	
	for( int i=0; i<n; i++)
	{
		degree1 = atan2(points[startPointIndex].y-points[i].y,points[i].x-points[startPointIndex].x);
		degree2 = fabs(degree1 - init[0].y);
		
		if( degree2 < PI/90 || degree1 == 0 )
		{
			shortcol[scounter] = points[i];
			scounter++;
		}
	}
	if( scounter != m_nXWidth ) AfxMessageBox("Error in CZhangImplementation_1Dlg::SortPoints(CvPoint2D32f *, int, CvPoint3D32f *)");
	
	// sort 6-points top-to-bottom
	SortRespectTo( shortcol, points[startPointIndex], m_nXWidth );
	
	CvPoint2D32f anchor, out;
	int lcounter = 0;
	
	for( i=0; i<scounter; i++ )
	{
		anchor = shortcol[i];
		
		out = CalculateAngleRespectTo(points, n, anchor, m_nXHeight-2);
		if( out.x == INT_MIN ) 
			AfxMessageBox("error:  SortPoints");
		
		lcounter = 0;
		for( int j=0; j<n; j++ )
		{
			degree1 = atan2(anchor.y-points[j].y,points[j].x-anchor.x);
			degree2 = fabs(degree1 - out.x);
			
			if( degree2 < PI/90 || degree1 == 0 )
			{
				longrow[lcounter] = points[j];
				lcounter++;
				if( lcounter == m_nXHeight ) 
					break;
			}
		}
		if( lcounter != m_nXHeight ) AfxMessageBox("Error in CZhangImplementation_1Dlg::SortPoints(CvPoint2D32f *, int, CvPoint3D32f *)");
		
		// sort 8-points left-to-right
		SortRespectTo( longrow, anchor, m_nXHeight );
		
		// insert sorted entires to the output array;
		for( j=0; j<m_nXHeight; j++)
		{
			output[i*m_nXHeight+j] = longrow[j];
		}
	}
	
	delete []shortcol; shortcol = NULL;
	delete []longrow ; longrow  = NULL;
	delete []points  ; points   = output;
}

void CCalibrtwocamaDlg::SortRespectTo(CvPoint2D32f *&arr, CvPoint2D32f anchor, int n)
{

	//  N*N sort scheme
	
	CvPoint2D32f *out  = new CvPoint2D32f[n];
	
	double *dist  = new double[n];
	int    *order = new int[n];
	
	out[0] = anchor;
	
	for( int i=0; i<n; i++)
	{
		dist[i] = sqrt( pow(arr[i].y-anchor.y,2) + pow(arr[i].x-anchor.x,2) );
	}
	
	int minIndex;
	double min;
	
	for( i=0; i<n; i++ )
	{
		min = INT_MAX;
		minIndex = 0;
		for( int j=0; j<n; j++ )
		{
			if( dist[j] < min )
			{
				min = dist[j];
				minIndex = j;
			}
		}
		
		order[i] = minIndex;
		dist[minIndex] = INT_MAX;
	}
	
	for( i=0; i<n; i++)
	{
		out[i] = arr[ order[i] ];
	}
	
	delete []dist;
	delete []order;
	delete []arr; arr = out;
}

CvPoint2D32f CCalibrtwocamaDlg::CalculateAngleRespectTo(CvPoint2D32f *arr, int n, CvPoint2D32f anchor, int thr1, int thr2, bool exact, bool vertAlso)
{

	CvPoint2D32f retVal;
	retVal.x = INT_MIN;
	retVal.y = INT_MIN;
	
	double angle1,angle2;
	
	double *angles		 = new double[n];
	BYTE   *angleCounter = new BYTE  [n];
	
	int		max_1, max_2, maxIndex_1, maxIndex_2;
	double	degree,degree1,degree2;
	
	for( int i=0; i<n; i++ )
	{
		if( anchor.x == arr[i].x && anchor.y == arr[i].y )
			angles[i] = -10000;
		else
			angles[i]	= atan2( anchor.y-arr[i].y , arr[i].x-anchor.x );
	}
	
	for( i=0; i<n; i++) angleCounter[i] = 0;
	
	for( i=0; i<n; i++ )
	{
		if( angles[i]==1000) continue;
		
		for(int j=i+1; j<n; j++ )
		{
			degree = KMathematica::MapAnglePi2mPi(angles[i]-angles[j]);
			if( fabs(degree) <= PI/90 )
			{
				angleCounter[i]++;
				angles[j] = 1000;
			}
		}
	}
	
	max_1	   = INT_MIN;
	maxIndex_1 = 0;
	angle1	   = 0;
	
	for( i=0; i<n; i++ )
	{
		if( angleCounter[i] > max_1 )
		{
			max_1 = angleCounter[i];
			maxIndex_1 = i;
			angle1 = angles[i];
		}
	}
	
	if( max_1 >= thr1 )
		retVal.x = (float)angle1; 
	
	double diff = 0;
	
	if( vertAlso )