www.pudn.com > ManualCameraCalibrationSourceCode_v1.0.rar > ZhangImplementation_1Dlg.cpp
/**********************************************************************
Camera Calibration Software v1.0
Copyright 2004 Engin Tola
--
Engin Tola
March 20, 2004
--
This piece of program contained in Camera Calibration Software v1.0 can be used,
copied, modified, merged, published, and/or have copies distributed for academic
or research purposes only without restriction under the following conditions:
1. The above header and this permission notice shall be included in all copies
or substantial portions of the program
2. The software is provided "as is", without warranty of any kind, express or
implied, including but not limited to the warranties of merchantability, fitness
for a particular purpose and non-infringement. In no event shall the author(s) be
liable for any claim, damages or liability, whether in an action of contract, tort
or otherwise, arising from, out of or in connection with Camera Calibration
Software v1.0 or the use or other dealings in Camera Calibration Software v1.0
3. If you use this piece of code for research purposes, refer to
Tola, Engin. 2006 June 12. Homepage.
4. An acknowledgement note should be included as:
"The software used here was originally created by Tola, Engin. 2006 June 12.
Homepage.
**********************************************************************/
// ZhangImplementation_1Dlg.cpp : implementation file
//
#include "stdafx.h"
#include "ZhangImplementation_1.h"
#include "ZhangImplementation_1Dlg.h"
#include "math.h"
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif
/////////////////////////////////////////////////////////////////////////////
// CAboutDlg dialog used for App About
FILE *cps; //file to write corner points
//for visual number effect to see the corners that were found
char* numbers[] =
{ "1", "2", "3", "4", "5", "6", "7", "8", "9", "10",
"11", "12", "13", "14", "15", "16", "17", "18", "19", "20",
"21", "22", "23", "24", "25", "26", "27", "28", "29", "30",
"31", "32", "33", "34", "35", "36", "37", "38", "39", "40",
"41", "42", "43", "44", "45", "46", "47", "48", "49", "50",
"51", "52", "53", "54", "55", "56", "57", "58", "59", "60",
"61", "62", "63", "64", "65", "66", "67", "68", "69", "70",
"71", "72", "73", "74", "75", "76", "77", "78", "79", "80",
"81", "82", "83", "84", "85", "86", "87", "88", "89", "90"
};
class CAboutDlg : public CDialog
{
public:
CAboutDlg();
// Dialog Data
//{{AFX_DATA(CAboutDlg)
enum { IDD = IDD_ABOUTBOX };
//}}AFX_DATA
// ClassWizard generated virtual function overrides
//{{AFX_VIRTUAL(CAboutDlg)
protected:
virtual void DoDataExchange(CDataExchange* pDX); // DDX/DDV support
//}}AFX_VIRTUAL
// Implementation
protected:
//{{AFX_MSG(CAboutDlg)
virtual BOOL OnInitDialog();
//}}AFX_MSG
DECLARE_MESSAGE_MAP()
};
CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD)
{
//{{AFX_DATA_INIT(CAboutDlg)
//}}AFX_DATA_INIT
}
void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
CDialog::DoDataExchange(pDX);
//{{AFX_DATA_MAP(CAboutDlg)
//}}AFX_DATA_MAP
}
BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)
//{{AFX_MSG_MAP(CAboutDlg)
//}}AFX_MSG_MAP
END_MESSAGE_MAP()
/////////////////////////////////////////////////////////////////////////////
// CZhangImplementation_1Dlg dialog
CZhangImplementation_1Dlg::CZhangImplementation_1Dlg(CWnd* pParent /*=NULL*/)
: CDialog(CZhangImplementation_1Dlg::IDD, pParent)
{
//{{AFX_DATA_INIT(CZhangImplementation_1Dlg)
m_nImageNumber = 0;
m_nXHeight = 0;
m_nXWidth = 0;
m_dGridHeight = 0.0;
m_dGridWidth = 0.0;
m_bDisplayCorners = FALSE;
m_bDisplayRT = FALSE;
m_nGroupNo = 0;
m_btnApplyOrder = FALSE;
m_bCombination = FALSE;
m_bGenerateMFile = FALSE;
m_bCompansateDistortion = FALSE;
//}}AFX_DATA_INIT
// Note that LoadIcon does not require a subsequent DestroyIcon in Win32
m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
m_nImageNumber = 0;
p2p_InputImages = 0;
m_cstrImageNames = NULL;
m_distortion = NULL;
m_cameraMatrix = NULL;
m_rotMatrs = NULL;
m_transVects = NULL;
uveff = NULL;
XYZeff = NULL;
}
CZhangImplementation_1Dlg::~CZhangImplementation_1Dlg()
{
delete[]m_distortion ;
delete[]m_cameraMatrix;
delete[]m_transVects ;
delete[]m_rotMatrs ;
delete[]uveff ;
delete[]XYZeff ;
delete[]m_cstrImageNames;
if( p2p_InputImages != NULL )
{
for( int i=0; iSetWindowText("Manual Camera Calibration Software v1.0");
CDialog::OnInitDialog();
// Add "About..." menu item to system menu.
// IDM_ABOUTBOX must be in the system command range.
ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
ASSERT(IDM_ABOUTBOX < 0xF000);
CMenu* pSysMenu = GetSystemMenu(FALSE);
if (pSysMenu != NULL)
{
CString strAboutMenu;
strAboutMenu.LoadString(IDS_ABOUTBOX);
if (!strAboutMenu.IsEmpty())
{
pSysMenu->AppendMenu(MF_SEPARATOR);
pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
}
}
// Set the icon for this dialog. The framework does this automatically
// when the application's main window is not a dialog
SetIcon(m_hIcon, TRUE); // Set big icon
SetIcon(m_hIcon, FALSE); // Set small icon
// initializations of interface parameters.
m_nImageNumber = 0;
m_nXHeight = 8;
m_nXWidth = 6;
m_nGroupNo = 4;
m_dGridHeight = 25;
m_dGridWidth = 25;
m_bDisplayCorners = false;
m_btnApplyOrder = true;
m_bGenerateMFile = true;
m_bCombination = true;
m_bCompansateDistortion = true;
UpdateData(false);
return TRUE; // return TRUE unless you set the focus to a control
}
void CZhangImplementation_1Dlg::OnSysCommand(UINT nID, LPARAM lParam)
{
if ((nID & 0xFFF0) == IDM_ABOUTBOX)
{
CAboutDlg dlgAbout;
dlgAbout.DoModal();
}
else
{
CDialog::OnSysCommand(nID, lParam);
}
}
// If you add a minimize button to your dialog, you will need the code below
// to draw the icon. For MFC applications using the document/view model,
// this is automatically done for you by the framework.
void CZhangImplementation_1Dlg::OnPaint()
{
if (IsIconic())
{
CPaintDC dc(this); // device context for painting
SendMessage(WM_ICONERASEBKGND, (WPARAM) dc.GetSafeHdc(), 0);
// Center icon in client rectangle
int cxIcon = GetSystemMetrics(SM_CXICON);
int cyIcon = GetSystemMetrics(SM_CYICON);
CRect rect;
GetClientRect(&rect);
int x = (rect.Width() - cxIcon + 1) / 2;
int y = (rect.Height() - cyIcon + 1) / 2;
// Draw the icon
dc.DrawIcon(x, y, m_hIcon);
}
else
{
CDialog::OnPaint();
}
}
// The system calls this to obtain the cursor to display while the user drags
// the minimized window.
HCURSOR CZhangImplementation_1Dlg::OnQueryDragIcon()
{
return (HCURSOR) m_hIcon;
}
//----------------------------------------------------------------------------//
//
// Function: OnLoad
// Purpose : Load Input Images which will be used to calibrate the camera. Input is
// taken automatically once the input seed is taken. The selected file is
// composed into seed and numeric. Specified number of images are extracted
// from the folder starting from the 0th image.(Ex: seed: image,
// m_nImageNumber = 4; extracted images = image0, image1,image2,image3)
// Output : p2p_InputImages : is a pointer to the array of image pointers of type
// IplImage.
//
//----------------------------------------------------------------------------//
void CZhangImplementation_1Dlg::OnLoad()
{
// get the number of images to process from the "Number of Images" box
UpdateData();
if( m_nImageNumber == 0 || m_nXHeight == 0 || m_nXWidth == 0)
return;
///Get root file name that will be used to generate 5 filenames of the images
CFileDialog dlg (TRUE,
"*.bmp",
"*.*",
OFN_FILEMUSTEXIST |
OFN_HIDEREADONLY,
"BMP Files (*.bmp)|*.bmp|JPG Files (*.jpg)|*.jpg|TIFF Files (*.tif;*.tiff)|*.tif; *.tiff|All Files (*.*)|*.*||",
this);
dlg.m_ofn.lpstrTitle = "Open File...";
if (dlg.DoModal () == IDCANCEL)
{
return;
}
CString tempString;
m_ImageRoot.path = dlg.GetPathName ();
m_ImageRoot.GenerateFileProps();
// form the root names....
m_ImageRoot.title = m_ImageRoot.title.Left(m_ImageRoot.title.GetLength()-1);
m_ImageRoot.name = m_ImageRoot.title + '.' + m_ImageRoot.extension;
m_ImageRoot.path = m_ImageRoot.folderPath + '\\' + m_ImageRoot.name;
char buffer[5];
p2p_InputImages = new IplImage *[m_nImageNumber];
delete []m_cstrImageNames; m_cstrImageNames = NULL;
m_cstrImageNames = new CString [m_nImageNumber];
// load images as IplImage*'s
for( int i=0; iwidth;
imgsize.height = p2p_InputImages[0]->height;
}
}
}
//----------------------------------------------------------------------------//
//
// Function: OnCalibrate
// Purpose : Finds the A matrix(m_cameraMatrix) and the m_distortion vector from the
// loaded images. There must be at least 4 images in order to get a correct
// A matrix. The more the number of the images used the more exact results
// are obtained. This function also calculates the recpective rotation and
// translation vectors for the inputted calibration images used. By using these
// information pixel error is calculated.
// Output : m_cameraMatrix : a0 = horizontal scaling, a4= vertical scaling, a1 = skew
// (a2,a5) = principal point.
// m_distortion : first two entries are radial m_distortion and last two entries
// are tangential distoriton parameters.
//----------------------------------------------------------------------------//
void CZhangImplementation_1Dlg::OnCalibrate()
{
UpdateData();
if( m_nImageNumber == 0 )
return;
m_nNumCorners = m_nXHeight*m_nXWidth;
// Extract World and Image Coordinates
FindCorners();
// Find CameraMatrix, Distortion Params, Rotation Matrices, Translation Vectors
CalibrateCamera();
// Backproject Points and Calculate Errors on X and Y direction
CalculateCalibrationErrors( true );
// Generates Reports concerning Calibration and Error
GenerateReport();
// Displays Results.
DisplayReport();
}
//----------------------------------------------------------------------------//
//
// Function: OnFindCorners
// Purpose : This function finds the corner points in the calibration images using
// opencv functions cvFindChessBoardCornerGuesses and cvFindCornerSubPix.
// After image pixel coordinates are found their respective world coordinates
// are assigned. (z coordinate is always zero). When the function cannot find
// the specified number of coordinates in the image, the image is discarded
// because of the added complexity in assigning their world coordinates.
// Output : uveff : coordinates of the chessborder in image plane.
// XYZeff: coordinates of uveff w.r.t World Coordinate System
// m_nEffectiveImageNumber : number of images processed.
//----------------------------------------------------------------------------//
void CZhangImplementation_1Dlg::FindCorners()
{
m_nNumCorners = m_nXWidth * m_nXHeight;
IplImage* img = 0; // initialize some image arrays
IplImage* img0 = 0;
IplImage* img1 = 0;
IplImage* greyimg = 0;
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; imgnumwidth;
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; phwidth;
imgsize.height = p2p_InputImages[0]->height;
}
m_cstrImageNames[i] = tempFileName.Right(tempFileName.GetLength()-tempFileName.ReverseFind('\\')-1);
}
}
UpdateData(false);
delete []buffer; buffer = NULL;
}
void CZhangImplementation_1Dlg::OnTest()
{
}
void CZhangImplementation_1Dlg::OnOpenLogFile()
{
if( strcmp(m_ImageRoot.folderPath.GetBuffer(300),"") == 0 )
return;
CString WE_Command = "Notepad.exe " + m_ImageRoot.folderPath+'\\'+ "calibration_result.txt";
if (WinExec(WE_Command, SW_SHOWNORMAL) <= 31)
MessageBox("Error running Notepad", "Zhang Calibration Project", MB_OK | MB_ICONERROR);
}
void CZhangImplementation_1Dlg::OnDisplCoordinates()
{
if( strcmp(m_ImageRoot.folderPath.GetBuffer(300),"") == 0 )
return;
CString WE_Command = "Notepad.exe " + m_ImageRoot.folderPath+'\\'+ "backprojection.txt";
if (WinExec(WE_Command, SW_SHOWNORMAL) <= 31)
MessageBox("Error running Notepad", "Zhang Calibration Project", MB_OK | MB_ICONERROR);
}
CvPoint3D32f CZhangImplementation_1Dlg::FindRectangleCorner(CvPoint2D32f *points, int n )
{
CvPoint3D32f retVal;
CvPoint2D32f out;
for( int k=0; 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 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 )
{
max_2 = INT_MIN;
maxIndex_2 = -1;
angle2 = 0;
for( i=0; i 500 ) continue;
degree1 = fabs(KMathematica::MapAnglePi2mPi(angles[i]-angle1));
degree2 = fabs(degree1 - PI/2);
if( angleCounter[i] >= max_2 && ((degree2 < PI/6)&&exact || (!exact)&&( degree1 > diff) ) )
{
max_2 = angleCounter[i];
maxIndex_2 = i;
angle2 = angles[i];
diff = degree1;
}
}
if( max_2 >= thr2 && maxIndex_2 != -1 )
retVal.y = (float)angle2;
}
// delete []angles;
delete []angleCounter;
return retVal;
}
//----------------------------------------------------------------------------//
//
// Function: CalibrateCamera
// Purpose : Finds the A matrix(m_cameraMatrix) and the m_distortion vector from the
// loaded images. There must be at least 4 images in order to get a correct
// A matrix. The more the number of the images used the more exact results
// are obtained. This function also calculates the recpective rotation and
// translation vectors for the input calibration images used.
// Output : m_cameraMatrix : a0 = horizontal scaling, a4= vertical scaling, a1 = skew
// (a2,a5) = principal point.
// m_distortion : first two entries are radial m_distortion and last two entries
// are tangential distoriton parameters.
//----------------------------------------------------------------------------//
void CZhangImplementation_1Dlg::CalibrateCamera()
{
UpdateData();
if( m_nEffectiveImageNumber == 0 )
return;
int* numPoints = new int[m_nEffectiveImageNumber];
for( int k=0; k < m_nEffectiveImageNumber; k++ ) numPoints[k] = m_nNumCorners;
delete[]m_distortion ; m_distortion = new double[4]; for( k=0; k<4; k++ ) m_distortion [k] = 0.0;
delete[]m_cameraMatrix; m_cameraMatrix = new double[9]; for( k=0; k<9; k++ ) m_cameraMatrix[k] = 0.0;
delete[]m_transVects ; m_transVects = new double[m_nEffectiveImageNumber*3];
delete[]m_rotMatrs ; m_rotMatrs = new double[m_nEffectiveImageNumber*9];
int useIntrinsicGuess = 0;
CvPoint2D64d* uvinv = new CvPoint2D64d[m_nEffectiveImageNumber * m_nNumCorners];
CvPoint3D64d* XYZinv = new CvPoint3D64d[m_nEffectiveImageNumber * m_nNumCorners];
int size = m_nEffectiveImageNumber * m_nNumCorners;
int index;
for(int i=0 ; i m_dErrorMax.x ) m_dErrorMax.x = fabs(diffx);
if( fabs(diffy) > m_dErrorMax.y ) m_dErrorMax.y = fabs(diffy);
if( print )
{
fprintf( fp, "%d\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t\n",
index,
uveff[index].x,
uveff[index].y,
uvapp[index].x,
uvapp[index].y,
diffx,
diffy );
}
}
}
if( print )
{
fclose(fp);
}
m_dErrorPower.x = sqrt(m_dErrorPower.x);
m_dErrorPower.y = sqrt(m_dErrorPower.y);
m_dErrorPower.x /= m_nEffectiveImageNumber*m_nNumCorners;
m_dErrorPower.y /= m_nEffectiveImageNumber*m_nNumCorners;
m_dErrorMean.x /= m_nEffectiveImageNumber*m_nNumCorners;
m_dErrorMean.y /= m_nEffectiveImageNumber*m_nNumCorners;
delete []uvapp; uvapp = NULL;
}
void CZhangImplementation_1Dlg::DisplayReport()
{
UpdateData();
if( m_nEffectiveImageNumber == 0 ) return;
CString path = m_ImageRoot.folderPath + '\\' + "calibration_result.txt";
CStdioFile file(path,CFile::modeRead|CFile::typeText);
CString strLine,strText;
while(file.ReadString(strLine)){
if( strcmp(strLine, "R & T Matrices") == 0 )
break;
strText+=strLine;
strText+="\n";
}
MessageBox(strText, "Calibration Results");
if( m_bDisplayRT )
{
strText = "";
while(file.ReadString(strLine)){
strText+=strLine;
strText+="\n";
}
MessageBox(strText,"Rotation and Translation Vectors");
}
}
void CZhangImplementation_1Dlg::OnTestInputSweepRun()
{
// this part of the program will run on all combinations of the input
// images in groups of ...
UpdateData();
if( m_nImageNumber == 0 )
return;
int** inputOrder;
int size;
CString path;
m_nNumCorners = m_nXHeight*m_nXWidth;
// Extract World and Image Coordinates
FindCorners();
int* arr = new int[m_nEffectiveImageNumber];
for(int i=0; iSetWindowText("About Manual Camera Calibration Software v1.0");
CDialog::OnInitDialog();
// TODO: Add extra initialization here
return TRUE; // return TRUE unless you set the focus to a control
// EXCEPTION: OCX Property Pages should return FALSE
}