static MRESULT EXPENTRY procOpts(HWND hwnd, ULONG msg, MPARAM mp1, MPARAM mp2)
{
ULONG ret ;
switch (msg) {
case WM_INITDLG :
if (SessDlgCenter) {
dialogAtCenter(hwnd) ;
} else {
dialogAtMouse(hwnd, DID_OK) ;
}
setOptions(hwnd) ;
return (MRESULT) 0 ;
case WM_COMMAND :
switch (ret = SHORT1FROMMP(mp1)) {
case DID_OK :
case DID_CANCEL :
if (getOptions(hwnd) != TRUE) {
return (MRESULT) 0 ;
}
saveParam() ;
WinDismissDlg(hwnd, ret) ;
return (MRESULT) 0 ;
default :
return (MRESULT) 0 ;
}
}
return WinDefDlgProc(hwnd, msg, mp1, mp2) ;
}
void EditParams::buttonBoxClicked(QAbstractButton* bbox)
{
QDialogButtonBox::StandardButton button=m_ui->buttonBox->standardButton(bbox);
if(button==QDialogButtonBox::RestoreDefaults)
{
for(int i=0;i<fields.size();i++)
fields[i]->reset();
}
else if(button==QDialogButtonBox::Cancel)
close();
else if(button==QDialogButtonBox::Save)
{
saveParam();
accept();
}
}
static void calib(void)
{
ICPHandleT *icpHandleL;
ICPHandleT *icpHandleR;
ICPDataT icpData;
ARdouble initMatXw2Xc[3][4];
ARdouble initTransL2R[3][4], matL[3][4], matR[3][4], invMatL[3][4];
ARdouble transL2R[3][4];
ARdouble err;
int i;
if( (icpHandleL = icpCreateHandle(paramL.mat)) == NULL ) {
ARLOG("Error!! icpCreateHandle\n");
return;
}
icpSetBreakLoopErrorThresh( icpHandleL, 0.00001 );
if( (icpHandleR = icpCreateHandle(paramR.mat)) == NULL ) {
ARLOG("Error!! icpCreateHandle\n");
return;
}
icpSetBreakLoopErrorThresh( icpHandleR, 0.00001 );
for( i = 0; i < calibImageNum; i++ ) {
if( icpGetInitXw2Xc_from_PlanarData(paramL.mat, calibData[i].screenCoordL, calibData[i].worldCoordL, calibData[i].numL, calibData[i].initMatXw2Xcl) < 0 ) {
ARLOG("Error!! icpGetInitXw2Xc_from_PlanarData\n");
return;
}
icpData.screenCoord = calibData[i].screenCoordL;
icpData.worldCoord = calibData[i].worldCoordL;
icpData.num = calibData[i].numL;
}
if( icpGetInitXw2Xc_from_PlanarData(paramL.mat, calibData[0].screenCoordL, calibData[0].worldCoordL, calibData[0].numL, initMatXw2Xc) < 0 ) {
ARLOG("Error!! icpGetInitXw2Xc_from_PlanarData\n");
return;
}
icpData.screenCoord = calibData[0].screenCoordL;
icpData.worldCoord = calibData[0].worldCoordL;
icpData.num = calibData[0].numL;
if( icpPoint(icpHandleL, &icpData, initMatXw2Xc, matL, &err) < 0 ) {
ARLOG("Error!! icpPoint\n");
return;
}
if( icpGetInitXw2Xc_from_PlanarData(paramR.mat, calibData[0].screenCoordR, calibData[0].worldCoordR, calibData[0].numR, matR) < 0 ) {
ARLOG("Error!! icpGetInitXw2Xc_from_PlanarData\n");
return;
}
icpData.screenCoord = calibData[0].screenCoordR;
icpData.worldCoord = calibData[0].worldCoordR;
icpData.num = calibData[0].numR;
if( icpPoint(icpHandleR, &icpData, initMatXw2Xc, matR, &err) < 0 ) {
ARLOG("Error!! icpPoint\n");
return;
}
arUtilMatInv( matL, invMatL );
arUtilMatMul( matR, invMatL, initTransL2R );
if( icpCalibStereo(calibData, calibImageNum, paramL.mat, paramR.mat, initTransL2R, transL2R, &err) < 0 ) {
ARLOG("Calibration error!!\n");
return;
}
ARLOG("Estimated transformation matrix from Left to Right.\n");
arParamDispExt( transL2R );
saveParam( transL2R );
}
static BOOL saveProfile(HAB hab)
{
#ifdef DEBUG
TRACE("in saveProfile ... ") ;
#endif
int entry, i, id ;
HINI hini ;
BOOL stat ;
ULONG len ;
UCHAR key[32] ;
#ifdef DEBUG
TRACE("done in saveProfile\n") ;
#endif
/*
* save to profile
*/
if ((hini = PrfOpenProfile(hab, ProfilePath)) == NULLHANDLE) {
#ifdef DEBUG
TRACE("saveProfile - failed to open %s\n", ProfilePath) ;
#endif
return FALSE ;
}
/*
* re-load new profile
*/
len = sizeof(profOrder) ;
stat = PrfQueryProfileData(hini, ProgramName, "ORDER", profOrder, &len) ;
if (stat != TRUE || len != sizeof(profOrder)) {
memset(profOrder, 0xff, sizeof(profOrder)) ;
}
for (i = 0 ; i < MAXHOSTS ; i++) {
if ((id = profOrder[i]) == 0xff) {
continue ;
}
sprintf(key, "HOST%02d", id) ;
len = sizeof(HOSTREC) ;
stat = PrfQueryProfileData(hini,
ProgramName, key, &profHosts[id], &len) ;
if (stat != TRUE || len != sizeof(HOSTREC)) {
profOrder[i] = 0xff ;
}
}
/*
* select new entry
*/
if ((entry = saveParam()) < 0) {
PrfCloseProfile(hini) ;
return FALSE ;
}
/*
* save new order and host data
*/
sprintf(key, "HOST%02d", entry) ;
PrfWriteProfileData(hini, ProgramName,
"ORDER", profOrder, sizeof(profOrder)) ;
PrfWriteProfileData(hini, ProgramName,
key, &profHosts[entry], sizeof(HOSTREC)) ;
PrfCloseProfile(hini) ;
#ifdef DEBUG
TRACE("saveProfile ... saved to %d\n", entry) ;
#endif
return TRUE ;
}
static void calib(void)
{
ARParam param;
CvMat *objectPoints;
CvMat *imagePoints;
CvMat *pointCounts;
CvMat *intrinsics;
CvMat *distortionCoeff;
CvMat *rotationVectors;
CvMat *translationVectors;
CvMat *rotationVector;
CvMat *rotationMatrix;
float intr[3][4];
float dist[4];
ARdouble trans[3][4];
ARdouble cx, cy, cz, hx, hy, h, sx, sy, ox, oy, err;
int i, j, k, l;
objectPoints = cvCreateMat(capturedImageNum * chessboardCornerNumX * chessboardCornerNumY, 3, CV_32FC1);
imagePoints = cvCreateMat(capturedImageNum * chessboardCornerNumX * chessboardCornerNumY, 2, CV_32FC1);
pointCounts = cvCreateMat(capturedImageNum, 1, CV_32SC1);
intrinsics = cvCreateMat(3, 3, CV_32FC1);
distortionCoeff = cvCreateMat(1, 4, CV_32FC1);
rotationVectors = cvCreateMat(capturedImageNum, 3, CV_32FC1);
translationVectors = cvCreateMat(capturedImageNum, 3, CV_32FC1);
rotationVector = cvCreateMat(1, 3, CV_32FC1);
rotationMatrix = cvCreateMat(3, 3, CV_32FC1);
l = 0;
for (k = 0; k < capturedImageNum; k++)
{
for (i = 0; i < chessboardCornerNumX; i++)
{
for (j = 0; j < chessboardCornerNumY; j++)
{
((float*)(objectPoints->data.ptr + objectPoints->step * l))[0] = patternWidth * i;
((float*)(objectPoints->data.ptr + objectPoints->step * l))[1] = patternWidth * j;
((float*)(objectPoints->data.ptr + objectPoints->step * l))[2] = 0.0f;
((float*)(imagePoints->data.ptr + imagePoints->step * l))[0] = cornerSet[l].x;
((float*)(imagePoints->data.ptr + imagePoints->step * l))[1] = cornerSet[l].y;
l++;
}
}
((int*)(pointCounts->data.ptr))[k] = chessboardCornerNumX * chessboardCornerNumY;
}
cvCalibrateCamera2(objectPoints, imagePoints, pointCounts, cvSize(xsize, ysize),
intrinsics, distortionCoeff, rotationVectors, translationVectors, 0);
for (j = 0; j < 3; j++)
{
for (i = 0; i < 3; i++)
{
intr[j][i] = ((float*)(intrinsics->data.ptr + intrinsics->step * j))[i];
}
intr[j][3] = 0.0f;
}
for (i = 0; i < 4; i++)
{
dist[i] = ((float*)(distortionCoeff->data.ptr))[i];
}
convParam(intr, dist, xsize, ysize, ¶m); // COVHI10434 ignored.
arParamDisp(¶m);
l = 0;
for (k = 0; k < capturedImageNum; k++)
{
for (i = 0; i < 3; i++)
{
((float*)(rotationVector->data.ptr))[i] = ((float*)(rotationVectors->data.ptr + rotationVectors->step * k))[i];
}
cvRodrigues2(rotationVector, rotationMatrix);
for (j = 0; j < 3; j++)
{
for (i = 0; i < 3; i++)
{
trans[j][i] = ((float*)(rotationMatrix->data.ptr + rotationMatrix->step * j))[i];
}
trans[j][3] = ((float*)(translationVectors->data.ptr + translationVectors->step * k))[j];
}
// arParamDispExt(trans);
err = 0.0;
for (i = 0; i < chessboardCornerNumX; i++)
{
for (j = 0; j < chessboardCornerNumY; j++)
{
cx = trans[0][0] * patternWidth * i + trans[0][1] * patternWidth * j + trans[0][3];
cy = trans[1][0] * patternWidth * i + trans[1][1] * patternWidth * j + trans[1][3];
cz = trans[2][0] * patternWidth * i + trans[2][1] * patternWidth * j + trans[2][3];
hx = param.mat[0][0] * cx + param.mat[0][1] * cy + param.mat[0][2] * cz + param.mat[0][3];
hy = param.mat[1][0] * cx + param.mat[1][1] * cy + param.mat[1][2] * cz + param.mat[1][3];
h = param.mat[2][0] * cx + param.mat[2][1] * cy + param.mat[2][2] * cz + param.mat[2][3];
if (h == 0.0)
continue;
sx = hx / h;
sy = hy / h;
arParamIdeal2Observ(param.dist_factor, sx, sy, &ox, &oy, param.dist_function_version);
sx = ((float*)(imagePoints->data.ptr + imagePoints->step * l))[0];
sy = ((float*)(imagePoints->data.ptr + imagePoints->step * l))[1];
err += (ox - sx) * (ox - sx) + (oy - sy) * (oy - sy);
l++;
}
}
err = sqrt(err / (chessboardCornerNumX * chessboardCornerNumY));
ARLOG("Err[%2d]: %f[pixel]\n", k + 1, err);
}
saveParam(¶m);
cvReleaseMat(&objectPoints);
cvReleaseMat(&imagePoints);
cvReleaseMat(&pointCounts);
cvReleaseMat(&intrinsics);
cvReleaseMat(&distortionCoeff);
cvReleaseMat(&rotationVectors);
cvReleaseMat(&translationVectors);
cvReleaseMat(&rotationVector);
cvReleaseMat(&rotationMatrix);
}
