//==============================================
// Baseを原点とした場合のTargetの相対位置を取得
//==============================================
void cARTK::getMarkerPos( Vector3 *pvec3MarkerPos, int iPattIndexBase, int iPattIndexTarget )
{
double dInvBaseTransMat[3][4];
double dTargetTransMat[3][4];
arUtilMatInv( m_sMarkerInfo[iPattIndexBase].dTransMat, dInvBaseTransMat );
arUtilMatMul( dInvBaseTransMat, m_sMarkerInfo[iPattIndexTarget].dTransMat, dTargetTransMat );
pvec3MarkerPos->x = (float)dTargetTransMat[0][3];
pvec3MarkerPos->y = (float)dTargetTransMat[1][3];
pvec3MarkerPos->z = (float)dTargetTransMat[2][3];
Vector3Transform( pvec3MarkerPos, pvec3MarkerPos, m_matRotX );
pvec3MarkerPos->x *= (float)m_dViewScaleFactor;
pvec3MarkerPos->y *= (float)m_dViewScaleFactor;
pvec3MarkerPos->z *= (float)m_dViewScaleFactor;
}
osg::MatrixTransform* Worker::mt2mtCoordinates(osg::MatrixTransform* mtA, osg::MatrixTransform* mtB)
{
double matrix1[3][4]; double matrix2[3][4]; // Auxiliar double matrix to calculate distances between OSG objects(matrixTransforms).
double matrixA[3][4]; double matrixB[3][4]; // mtA and mtB in double matrix format.
osg::MatrixTransform* mtAfromB;
mtAfromB = new osg::MatrixTransform();
matrixTransform2matrixDouble(mtA,matrixA);
matrixTransform2matrixDouble(mtB,matrixB);
// if (osgArt->getMarker1()->valid() || osgArt->getMarker2()->valid())
// {
arUtilMatInv(matrixA, matrix1);// Now matrix1 is the inverse of matrixA
arUtilMatMul(matrix1, matrixB, matrix2);
matrixDouble2matrixTransform(matrix2, mtAfromB);
return mtAfromB;
// }
// else
// {
// std::cout<<"The ARToolKit marks are out of camera vision. Please show the 2 marks to the camera."<<std::endl;
// distance2mole1=9999;
// }
}
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 );
}
int arGetStereoMatching(AR3DStereoHandle *handle, ARdouble pos2dL[2], ARdouble pos2dR[2], ARdouble pos3d[3])
{
ARMat *matP, *matQ;
ARMat *matPt, *matR, *matS, *matT;
ARdouble wL[3][4], wR[3][4];
arUtilMatMul( handle->icpStereoHandle->matXcl2Ul, handle->icpStereoHandle->matC2L, wL );
arUtilMatMul( handle->icpStereoHandle->matXcr2Ur, handle->icpStereoHandle->matC2R, wR );
matP = arMatrixAlloc(4,3);
matPt = arMatrixAlloc(3,4);
matQ = arMatrixAlloc(4,1);
matR = arMatrixAlloc(3,3);
matS = arMatrixAlloc(3,1);
matT = arMatrixAlloc(3,1);
matP->m[0*3+0] = matPt->m[0*4+0] = wL[0][0] - wL[2][0] * pos2dL[0];
matP->m[0*3+1] = matPt->m[1*4+0] = wL[0][1] - wL[2][1] * pos2dL[0];
matP->m[0*3+2] = matPt->m[2*4+0] = wL[0][2] - wL[2][2] * pos2dL[0];
matP->m[1*3+0] = matPt->m[0*4+1] = wL[1][0] - wL[2][0] * pos2dL[1];
matP->m[1*3+1] = matPt->m[1*4+1] = wL[1][1] - wL[2][1] * pos2dL[1];
matP->m[1*3+2] = matPt->m[2*4+1] = wL[1][2] - wL[2][2] * pos2dL[1];
matP->m[2*3+0] = matPt->m[0*4+2] = wR[0][0] - wR[2][0] * pos2dR[0];
matP->m[2*3+1] = matPt->m[1*4+2] = wR[0][1] - wR[2][1] * pos2dR[0];
matP->m[2*3+2] = matPt->m[2*4+2] = wR[0][2] - wR[2][2] * pos2dR[0];
matP->m[3*3+0] = matPt->m[0*4+3] = wR[1][0] - wR[2][0] * pos2dR[1];
matP->m[3*3+1] = matPt->m[1*4+3] = wR[1][1] - wR[2][1] * pos2dR[1];
matP->m[3*3+2] = matPt->m[2*4+3] = wR[1][2] - wR[2][2] * pos2dR[1];
matQ->m[0] = wL[2][3] * pos2dL[0] - wL[0][3];
matQ->m[1] = wL[2][3] * pos2dL[1] - wL[1][3];
matQ->m[2] = wR[2][3] * pos2dR[0] - wR[0][3];
matQ->m[3] = wR[2][3] * pos2dR[1] - wR[1][3];
arMatrixMul( matR, matPt, matP );
arMatrixMul( matS, matPt, matQ );
if( arMatrixSelfInv( matR ) < 0 ) {
arMatrixFree( matP );
arMatrixFree( matPt);
arMatrixFree( matQ );
arMatrixFree( matR );
arMatrixFree( matS );
arMatrixFree( matT );
return -1;
}
arMatrixMul( matT, matR, matS );
pos3d[0] = matT->m[0];
pos3d[1] = matT->m[1];
pos3d[2] = matT->m[2];
arMatrixFree( matP );
arMatrixFree( matPt);
arMatrixFree( matQ );
arMatrixFree( matR );
arMatrixFree( matS );
arMatrixFree( matT );
return 0;
}
static ARdouble arGetTransMatMultiSquare2(AR3DHandle *handle, ARMarkerInfo *marker_info, int marker_num,
ARMultiMarkerInfoT *config, int robustFlag)
{
ARdouble *pos2d, *pos3d;
ARdouble trans1[3][4], trans2[3][4];
ARdouble err, err2;
int max, maxArea;
int vnum;
int dir;
int i, j, k;
//char mes[12];
//ARLOG("-- Pass1--\n");
for( i = 0; i < config->marker_num; i++ ) {
k = -1;
if( config->marker[i].patt_type == AR_MULTI_PATTERN_TYPE_TEMPLATE ) {
for( j = 0; j < marker_num; j++ ) {
if( marker_info[j].idPatt != config->marker[i].patt_id ) continue;
if( marker_info[j].cfPatt < config->cfPattCutoff ) continue;
if( k == -1 ) k = j;
else if( marker_info[k].cfPatt < marker_info[j].cfPatt ) k = j;
}
config->marker[i].visible = k;
if( k >= 0 ) marker_info[k].dir = marker_info[k].dirPatt;
}
else { // config->marker[i].patt_type == AR_MULTI_PATTERN_TYPE_MATRIX
for( j = 0; j < marker_num; j++ ) {
// Check if we need to examine the globalID rather than patt_id.
if (marker_info[j].idMatrix == 0 && marker_info[j].globalID != 0ULL) {
if( marker_info[j].globalID != config->marker[i].globalID ) continue;
} else {
if( marker_info[j].idMatrix != config->marker[i].patt_id ) continue;
}
if( marker_info[j].cfMatrix < config->cfMatrixCutoff ) continue;
if( k == -1 ) k = j;
else if( marker_info[k].cfMatrix < marker_info[j].cfMatrix ) k = j;
}
config->marker[i].visible = k;
if( k >= 0 ) marker_info[k].dir = marker_info[k].dirMatrix;
}
//if(k>=0) ARLOG(" *%d\n",i);
}
//ARLOG("-- Pass2--\n");
vnum = 0;
for( i = 0; i < config->marker_num; i++ ) {
if( (j=config->marker[i].visible) < 0 ) continue;
//glColor3f( 1.0, 1.0, 0.0 );
//sprintf(mes,"%d",i);
//argDrawStringsByIdealPos( mes, marker_info[j].pos[0], marker_info[j].pos[1] );
err = arGetTransMatSquare(handle, &marker_info[j], config->marker[i].width, trans2);
//ARLOG(" [%d:dir=%d] err = %f (%f,%f,%f)\n", i, marker_info[j].dir, err, trans2[0][3], trans2[1][3], trans2[2][3]);
if( err > AR_MULTI_POSE_ERROR_CUTOFF_EACH_DEFAULT ) {
config->marker[i].visible = -1;
if (marker_info[j].cutoffPhase == AR_MARKER_INFO_CUTOFF_PHASE_NONE) marker_info[j].cutoffPhase = AR_MARKER_INFO_CUTOFF_PHASE_POSE_ERROR;
continue;
}
//ARLOG(" *%d\n",i);
// Use the largest (in terms of 2D coordinates) marker's pose estimate as the
// input for the initial estimate for the pose estimator.
if( vnum == 0 || maxArea < marker_info[j].area ) {
maxArea = marker_info[j].area;
max = i;
for( j = 0; j < 3; j++ ) {
for( k = 0; k < 4; k++ ) trans1[j][k] = trans2[j][k];
}
}
vnum++;
}
if( vnum == 0 || vnum < config->min_submarker) {
config->prevF = 0;
return -1;
}
arUtilMatMul( (const ARdouble (*)[4])trans1, (const ARdouble (*)[4])config->marker[max].itrans, trans2 );
arMalloc(pos2d, ARdouble, vnum*4*2);
arMalloc(pos3d, ARdouble, vnum*4*3);
j = 0;
for( i = 0; i < config->marker_num; i++ ) {
if( (k=config->marker[i].visible) < 0 ) continue;
dir = marker_info[k].dir;
pos2d[j*8+0] = marker_info[k].vertex[(4-dir)%4][0];
pos2d[j*8+1] = marker_info[k].vertex[(4-dir)%4][1];
pos2d[j*8+2] = marker_info[k].vertex[(5-dir)%4][0];
pos2d[j*8+3] = marker_info[k].vertex[(5-dir)%4][1];
pos2d[j*8+4] = marker_info[k].vertex[(6-dir)%4][0];
pos2d[j*8+5] = marker_info[k].vertex[(6-dir)%4][1];
pos2d[j*8+6] = marker_info[k].vertex[(7-dir)%4][0];
pos2d[j*8+7] = marker_info[k].vertex[(7-dir)%4][1];
pos3d[j*12+0] = config->marker[i].pos3d[0][0];
pos3d[j*12+1] = config->marker[i].pos3d[0][1];
pos3d[j*12+2] = config->marker[i].pos3d[0][2];
pos3d[j*12+3] = config->marker[i].pos3d[1][0];
pos3d[j*12+4] = config->marker[i].pos3d[1][1];
pos3d[j*12+5] = config->marker[i].pos3d[1][2];
pos3d[j*12+6] = config->marker[i].pos3d[2][0];
pos3d[j*12+7] = config->marker[i].pos3d[2][1];
pos3d[j*12+8] = config->marker[i].pos3d[2][2];
pos3d[j*12+9] = config->marker[i].pos3d[3][0];
pos3d[j*12+10] = config->marker[i].pos3d[3][1];
pos3d[j*12+11] = config->marker[i].pos3d[3][2];
j++;
}
if( config->prevF == 0 ) {
if( robustFlag ) {
err = arGetTransMat( handle, trans2, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, config->trans );
if( err >= AR_MULTI_POSE_ERROR_CUTOFF_COMBINED_DEFAULT ) {
icpSetInlierProbability( handle->icpHandle, 0.8 );
err = arGetTransMatRobust( handle, trans2, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, config->trans );
if( err >= AR_MULTI_POSE_ERROR_CUTOFF_COMBINED_DEFAULT ) {
icpSetInlierProbability( handle->icpHandle, 0.6 );
err = arGetTransMatRobust( handle, trans2, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, config->trans );
if( err >= AR_MULTI_POSE_ERROR_CUTOFF_COMBINED_DEFAULT ) {
icpSetInlierProbability( handle->icpHandle, 0.4 );
err = arGetTransMatRobust( handle, trans2, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, config->trans );
if( err >= AR_MULTI_POSE_ERROR_CUTOFF_COMBINED_DEFAULT ) {
icpSetInlierProbability( handle->icpHandle, 0.0 );
err = arGetTransMatRobust( handle, trans2, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, config->trans );
}
}
}
}
}
else {
err = arGetTransMat( handle, trans2, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, config->trans );
}
free(pos3d);
free(pos2d);
}
else {
if( robustFlag ) {
err2 = arGetTransMat( handle, trans2, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, trans1 );
err = arGetTransMat( handle, config->trans, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, config->trans );
if( err2 < err ) {
for( j = 0; j < 3; j++ ) for( i = 0; i < 4; i++ ) config->trans[j][i] = trans1[j][i];
err = err2;
}
if( err >= AR_MULTI_POSE_ERROR_CUTOFF_COMBINED_DEFAULT ) {
icpSetInlierProbability( handle->icpHandle, 0.8 );
err2 = arGetTransMatRobust( handle, trans2, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, trans1 );
err = arGetTransMatRobust( handle, config->trans, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, config->trans );
if( err2 < err ) {
for( j = 0; j < 3; j++ ) for( i = 0; i < 4; i++ ) config->trans[j][i] = trans1[j][i];
err = err2;
}
if( err >= AR_MULTI_POSE_ERROR_CUTOFF_COMBINED_DEFAULT ) {
icpSetInlierProbability( handle->icpHandle, 0.6 );
err2 = arGetTransMatRobust( handle, trans2, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, trans1 );
err = arGetTransMatRobust( handle, config->trans, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, config->trans );
if( err2 < err ) {
for( j = 0; j < 3; j++ ) for( i = 0; i < 4; i++ ) config->trans[j][i] = trans1[j][i];
err = err2;
}
if( err >= AR_MULTI_POSE_ERROR_CUTOFF_COMBINED_DEFAULT ) {
icpSetInlierProbability( handle->icpHandle, 0.4 );
err2 = arGetTransMatRobust( handle, trans2, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, trans1 );
err = arGetTransMatRobust( handle, config->trans, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, config->trans );
if( err2 < err ) {
for( j = 0; j < 3; j++ ) for( i = 0; i < 4; i++ ) config->trans[j][i] = trans1[j][i];
err = err2;
}
if( err >= AR_MULTI_POSE_ERROR_CUTOFF_COMBINED_DEFAULT ) {
icpSetInlierProbability( handle->icpHandle, 0.0 );
err2 = arGetTransMatRobust( handle, trans2, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, trans1 );
err = arGetTransMatRobust( handle, config->trans, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, config->trans );
if( err2 < err ) {
for( j = 0; j < 3; j++ ) for( i = 0; i < 4; i++ ) config->trans[j][i] = trans1[j][i];
err = err2;
}
}
}
}
}
}
else {
err2 = arGetTransMat( handle, trans2, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, trans1 );
err = arGetTransMat( handle, config->trans, (ARdouble (*)[2])pos2d, (ARdouble (*)[3])pos3d, vnum*4, config->trans );
if( err2 < err ) {
for( j = 0; j < 3; j++ ) for( i = 0; i < 4; i++ ) config->trans[j][i] = trans1[j][i];
err = err2;
}
}
free(pos3d);
free(pos2d);
}
if (err < AR_MULTI_POSE_ERROR_CUTOFF_COMBINED_DEFAULT) config->prevF = 1;
else {
config->prevF = 0;
for (i = 0; i < config->marker_num; i++) {
if ((k = config->marker[i].visible) < 0) continue;
if (marker_info[k].cutoffPhase == AR_MARKER_INFO_CUTOFF_PHASE_NONE) marker_info[k].cutoffPhase = AR_MARKER_INFO_CUTOFF_PHASE_POSE_ERROR_MULTI;
}
}
return err;
}
int icpStereoPointRobust( ICPStereoHandleT *handle,
ICPStereoDataT *data,
ARdouble initMatXw2Xc[3][4],
ARdouble matXw2Xc[3][4],
ARdouble *err )
{
ICP2DCoordT U;
ARdouble *J_U_S;
ARdouble *dU, dx, dy;
ARdouble *E, *E2, K2, W;
ARdouble matXw2Ul[3][4];
ARdouble matXw2Ur[3][4];
ARdouble matXc2Ul[3][4];
ARdouble matXc2Ur[3][4];
ARdouble dS[6];
ARdouble err0, err1;
int inlierNum;
int i, j, k;
#if ICP_DEBUG
int l;
#endif
if( data->numL + data->numR < 4 ) return -1;
inlierNum = (int)((data->numL + data->numR) * handle->inlierProb) - 1;
if( inlierNum < 3 ) inlierNum = 3;
if( (J_U_S = (ARdouble *)malloc( sizeof(ARdouble)*12*(data->numL + data->numR) )) == NULL ) {
ARLOGe("Error: malloc\n");
return -1;
}
if( (dU = (ARdouble *)malloc( sizeof(ARdouble)*2*(data->numL + data->numR) )) == NULL ) {
ARLOGe("Error: malloc\n");
free(J_U_S);
return -1;
}
if( (E = (ARdouble *)malloc( sizeof(ARdouble)*(data->numL + data->numR) )) == NULL ) {
ARLOGe("Error: malloc\n");
free(J_U_S);
free(dU);
return -1;
}
if( (E2 = (ARdouble *)malloc( sizeof(ARdouble)*(data->numL + data->numR) )) == NULL ) {
ARLOGe("Error: malloc\n");
free(J_U_S);
free(dU);
free(E);
return -1;
}
for( j = 0; j < 3; j++ ) {
for( i = 0; i < 4; i++ ) matXw2Xc[j][i] = initMatXw2Xc[j][i];
}
arUtilMatMul( handle->matXcl2Ul, handle->matC2L, matXc2Ul );
arUtilMatMul( handle->matXcr2Ur, handle->matC2R, matXc2Ur );
for( i = 0;; i++ ) {
#if ICP_DEBUG
icpDispMat( "matXw2Xc", &(matXw2Xc[0][0]), 3, 4 );
#endif
arUtilMatMul( matXc2Ul, matXw2Xc, matXw2Ul );
arUtilMatMul( matXc2Ur, matXw2Xc, matXw2Ur );
for( j = 0; j < data->numL; j++ ) {
if( icpGetU_from_X_by_MatX2U( &U, matXw2Ul, &(data->worldCoordL[j]) ) < 0 ) {
icpStereoGetXw2XcCleanup("icpGetU_from_X_by_MatX2U",J_U_S,dU,E,E2);
return -1;
}
dx = data->screenCoordL[j].x - U.x;
dy = data->screenCoordL[j].y - U.y;
dU[j*2+0] = dx;
dU[j*2+1] = dy;
E[j] = E2[j] = dx*dx + dy*dy;
}
for( j = 0; j < data->numR; j++ ) {
if( icpGetU_from_X_by_MatX2U( &U, matXw2Ur, &(data->worldCoordR[j]) ) < 0 ) {
icpStereoGetXw2XcCleanup("icpGetU_from_X_by_MatX2U",J_U_S,dU,E,E2);
return -1;
}
dx = data->screenCoordR[j].x - U.x;
dy = data->screenCoordR[j].y - U.y;
dU[(data->numL+j)*2+0] = dx;
dU[(data->numL+j)*2+1] = dy;
E[data->numL+j] = E2[data->numL+j] = dx*dx + dy*dy;
}
qsort(E2, (data->numL + data->numR), sizeof(ARdouble), compE);
K2 = E2[inlierNum] * K2_FACTOR;
if( K2 < 16.0 ) K2 = 16.0;
err1 = 0.0;
for( j = 0; j < data->numL + data->numR; j++ ) {
if( E2[j] > K2 ) err1 += K2/6.0;
else err1 += K2/6.0 * (1.0 - (1.0-E2[j]/K2)*(1.0-E2[j]/K2)*(1.0-E2[j]/K2));
}
err1 /= (data->numL + data->numR);
#if ICP_DEBUG
ARLOG("Loop[%d]: k^2 = %f, err = %15.10f\n", i, K2, err1);
#endif
if( err1 < handle->breakLoopErrorThresh ) break;
if( i > 0 && err1 < ICP_BREAK_LOOP_ERROR_THRESH2 && err1/err0 > handle->breakLoopErrorRatioThresh ) break;
if( i == handle->maxLoop ) break;
err0 = err1;
k = 0;
#if ICP_DEBUG
l = 0;
#endif
for( j = 0; j < data->numL; j++ ) {
if( E[j] <= K2 ) {
if( icpGetJ_U_S( (ARdouble (*)[6])(&J_U_S[6*k]), matXc2Ul, matXw2Xc, &(data->worldCoordL[j]) ) < 0 ) {
icpStereoGetXw2XcCleanup("icpGetJ_U_S",J_U_S,dU,E,E2);
return -1;
}
#if ICP_DEBUG
icpDispMat( "J_U_S", (ARdouble *)(&J_U_S[6*k]), 2, 6 );
#endif
W = (1.0 - E[j]/K2)*(1.0 - E[j]/K2);
J_U_S[k*6+0] *= W;
J_U_S[k*6+1] *= W;
J_U_S[k*6+2] *= W;
J_U_S[k*6+3] *= W;
J_U_S[k*6+4] *= W;
J_U_S[k*6+5] *= W;
J_U_S[k*6+6] *= W;
J_U_S[k*6+7] *= W;
J_U_S[k*6+8] *= W;
J_U_S[k*6+9] *= W;
J_U_S[k*6+10] *= W;
J_U_S[k*6+11] *= W;
dU[k+0] = dU[j*2+0] * W;
dU[k+1] = dU[j*2+1] * W;
k+=2;
#if ICP_DEBUG
l++;
#endif
}
}
#if ICP_DEBUG
ARLOG("LEFT IN: %2d, OUT: %2d\n", l, data->numL-l);
#endif
#if ICP_DEBUG
l = 0;
#endif
for( j = 0; j < data->numR; j++ ) {
if( E[data->numL+j] <= K2 ) {
if( icpGetJ_U_S( (ARdouble (*)[6])(&J_U_S[6*k]), matXc2Ur, matXw2Xc, &(data->worldCoordR[j]) ) < 0 ) {
icpStereoGetXw2XcCleanup("icpGetJ_U_S",J_U_S,dU,E,E2);
return -1;
}
#if ICP_DEBUG
icpDispMat( "J_U_S", (ARdouble *)(&J_U_S[6*k]), 2, 6 );
#endif
W = (1.0 - E[data->numL+j]/K2)*(1.0 - E[data->numL+j]/K2);
J_U_S[k*6+0] *= W;
J_U_S[k*6+1] *= W;
J_U_S[k*6+2] *= W;
J_U_S[k*6+3] *= W;
J_U_S[k*6+4] *= W;
J_U_S[k*6+5] *= W;
J_U_S[k*6+6] *= W;
J_U_S[k*6+7] *= W;
J_U_S[k*6+8] *= W;
J_U_S[k*6+9] *= W;
J_U_S[k*6+10] *= W;
J_U_S[k*6+11] *= W;
dU[k+0] = dU[(data->numL+j)*2+0] * W;
dU[k+1] = dU[(data->numL+j)*2+1] * W;
k+=2;
#if ICP_DEBUG
l++;
#endif
}
}
#if ICP_DEBUG
ARLOG("RIGHT IN: %2d, OUT: %2d\n", l, data->numR-l);
#endif
if( k < 6 ) return -1;
if( icpGetDeltaS( dS, dU, (ARdouble (*)[6])J_U_S, k ) < 0 ) {
icpStereoGetXw2XcCleanup("icpGetS",J_U_S,dU,E,E2);
return -1;
}
icpUpdateMat( matXw2Xc, dS );
}
#if ICP_DEBUG
ARLOG("*********** %f\n", err1);
ARLOG("Loop = %d\n", i);
#endif
*err = err1;
free(J_U_S);
free(dU);
free(E);
free(E2);
return 0;
}
static ARdouble arGetTransMatMultiSquareStereo2(AR3DStereoHandle *handle,
ARMarkerInfo *marker_infoL, int marker_numL,
ARMarkerInfo *marker_infoR, int marker_numR,
ARMultiMarkerInfoT *config, int robustFlag)
{
ARdouble *pos2dL = NULL, *pos3dL = NULL;
ARdouble *pos2dR = NULL, *pos3dR = NULL;
ARdouble trans1[3][4], trans2[3][4];
ARdouble err, err2;
int max, maxArea;
int vnumL, vnumR;
int dir;
int i, j, k;
for( i = 0; i < config->marker_num; i++ ) {
k = -1;
if( config->marker[i].patt_type == AR_MULTI_PATTERN_TYPE_TEMPLATE ) {
for( j = 0; j < marker_numL; j++ ) {
if( marker_infoL[j].idPatt != config->marker[i].patt_id ) continue;
if( marker_infoL[j].cfPatt < 0.50 ) continue;
if( k == -1 ) k = j;
else if( marker_infoL[k].cfPatt < marker_infoL[j].cfPatt ) k = j;
}
config->marker[i].visible = k;
if( k >= 0 ) {
marker_infoL[k].dir = marker_infoL[k].dirPatt;
}
}
else {
for( j = 0; j < marker_numL; j++ ) {
if( marker_infoL[j].idMatrix != config->marker[i].patt_id ) continue;
if( marker_infoL[j].cfMatrix < 0.50 ) continue;
if( k == -1 ) k = j;
else if( marker_infoL[k].cfMatrix < marker_infoL[j].cfMatrix ) k = j;
}
config->marker[i].visible = k;
if( k >= 0 ) {
marker_infoL[k].dir = marker_infoL[k].dirMatrix;
}
}
k = -1;
if( config->marker[i].patt_type == AR_MULTI_PATTERN_TYPE_TEMPLATE ) {
for( j = 0; j < marker_numR; j++ ) {
if( marker_infoR[j].idPatt != config->marker[i].patt_id ) continue;
if( marker_infoR[j].cfPatt < 0.50 ) continue;
if( k == -1 ) k = j;
else if( marker_infoR[k].cfPatt < marker_infoR[j].cfPatt ) k = j;
}
config->marker[i].visibleR = k;
if( k >= 0 ) {
marker_infoR[k].dir = marker_infoR[k].dirPatt;
}
}
else {
for( j = 0; j < marker_numR; j++ ) {
if( marker_infoR[j].idMatrix != config->marker[i].patt_id ) continue;
if( marker_infoR[j].cfMatrix < 0.50 ) continue;
if( k == -1 ) k = j;
else if( marker_infoR[k].cfMatrix < marker_infoR[j].cfMatrix ) k = j;
}
config->marker[i].visibleR = k;
if( k >= 0 ) {
marker_infoR[k].dir = marker_infoR[k].dirMatrix;
}
}
}
vnumL = 0;
for( i = 0; i < config->marker_num; i++ ) {
if( (j=config->marker[i].visible) == -1 ) continue;
err = arGetTransMatSquareStereo( handle, &marker_infoL[j], NULL, config->marker[i].width, trans2 );
if( err > THRESH_1 ) {
config->marker[i].visible = -1;
//ARLOG("err = %f\n", err);
continue;
}
if( vnumL == 0 || maxArea < marker_infoL[j].area ) {
maxArea = marker_infoL[j].area;
max = i;
for( j = 0; j < 3; j++ ) {
for( k = 0; k < 4; k++ ) trans1[j][k] = trans2[j][k];
}
}
vnumL++;
}
vnumR = 0;
for( i = 0; i < config->marker_num; i++ ) {
if( (j=config->marker[i].visibleR) == -1 ) continue;
err = arGetTransMatSquareStereo( handle, NULL, &marker_infoR[j], config->marker[i].width, trans2 );
if( err > THRESH_1 ) {
config->marker[i].visibleR = -1;
//ARLOG("err = %f\n", err);
continue;
}
if( (vnumL == 0 && vnumR == 0) || maxArea < marker_infoR[j].area ) {
maxArea = marker_infoR[j].area;
max = i;
for( j = 0; j < 3; j++ ) {
for( k = 0; k < 4; k++ ) trans1[j][k] = trans2[j][k];
}
}
vnumR++;
}
//ARLOG("vnumL=%d, vnumR=%d\n", vnumL, vnumR);
if( (vnumL == 0 && vnumR == 0) || (vnumL < config->min_submarker && vnumR < config->min_submarker) ) {
config->prevF = 0;
//ARLOG("**** NG.\n");
return -1;
}
if(vnumL > 0) {
arMalloc(pos2dL, ARdouble, vnumL*4*2);
arMalloc(pos3dL, ARdouble, vnumL*4*3);
}
if(vnumR > 0) {
arMalloc(pos2dR, ARdouble, vnumR*4*2);
arMalloc(pos3dR, ARdouble, vnumR*4*3);
}
j = 0;
for( i = 0; i < config->marker_num; i++ ) {
if( (k=config->marker[i].visible) < 0 ) continue;
dir = marker_infoL[k].dir;
pos2dL[j*8+0] = marker_infoL[k].vertex[(4-dir)%4][0];
pos2dL[j*8+1] = marker_infoL[k].vertex[(4-dir)%4][1];
pos2dL[j*8+2] = marker_infoL[k].vertex[(5-dir)%4][0];
pos2dL[j*8+3] = marker_infoL[k].vertex[(5-dir)%4][1];
pos2dL[j*8+4] = marker_infoL[k].vertex[(6-dir)%4][0];
pos2dL[j*8+5] = marker_infoL[k].vertex[(6-dir)%4][1];
pos2dL[j*8+6] = marker_infoL[k].vertex[(7-dir)%4][0];
pos2dL[j*8+7] = marker_infoL[k].vertex[(7-dir)%4][1];
pos3dL[j*12+0] = config->marker[i].pos3d[0][0];
pos3dL[j*12+1] = config->marker[i].pos3d[0][1];
pos3dL[j*12+2] = config->marker[i].pos3d[0][2];
pos3dL[j*12+3] = config->marker[i].pos3d[1][0];
pos3dL[j*12+4] = config->marker[i].pos3d[1][1];
pos3dL[j*12+5] = config->marker[i].pos3d[1][2];
pos3dL[j*12+6] = config->marker[i].pos3d[2][0];
pos3dL[j*12+7] = config->marker[i].pos3d[2][1];
pos3dL[j*12+8] = config->marker[i].pos3d[2][2];
pos3dL[j*12+9] = config->marker[i].pos3d[3][0];
pos3dL[j*12+10] = config->marker[i].pos3d[3][1];
pos3dL[j*12+11] = config->marker[i].pos3d[3][2];
j++;
}
j = 0;
for( i = 0; i < config->marker_num; i++ ) {
if( (k=config->marker[i].visibleR) < 0 ) continue;
dir = marker_infoR[k].dir;
pos2dR[j*8+0] = marker_infoR[k].vertex[(4-dir)%4][0];
pos2dR[j*8+1] = marker_infoR[k].vertex[(4-dir)%4][1];
pos2dR[j*8+2] = marker_infoR[k].vertex[(5-dir)%4][0];
pos2dR[j*8+3] = marker_infoR[k].vertex[(5-dir)%4][1];
pos2dR[j*8+4] = marker_infoR[k].vertex[(6-dir)%4][0];
pos2dR[j*8+5] = marker_infoR[k].vertex[(6-dir)%4][1];
pos2dR[j*8+6] = marker_infoR[k].vertex[(7-dir)%4][0];
pos2dR[j*8+7] = marker_infoR[k].vertex[(7-dir)%4][1];
pos3dR[j*12+0] = config->marker[i].pos3d[0][0];
pos3dR[j*12+1] = config->marker[i].pos3d[0][1];
pos3dR[j*12+2] = config->marker[i].pos3d[0][2];
pos3dR[j*12+3] = config->marker[i].pos3d[1][0];
pos3dR[j*12+4] = config->marker[i].pos3d[1][1];
pos3dR[j*12+5] = config->marker[i].pos3d[1][2];
pos3dR[j*12+6] = config->marker[i].pos3d[2][0];
pos3dR[j*12+7] = config->marker[i].pos3d[2][1];
pos3dR[j*12+8] = config->marker[i].pos3d[2][2];
pos3dR[j*12+9] = config->marker[i].pos3d[3][0];
pos3dR[j*12+10] = config->marker[i].pos3d[3][1];
pos3dR[j*12+11] = config->marker[i].pos3d[3][2];
j++;
}
if( config->prevF == 0 ) {
arUtilMatMul( (const ARdouble (*)[4])trans1, (const ARdouble (*)[4])config->marker[max].itrans, trans2 );
if( robustFlag ) {
err = arGetTransMatStereo( handle, trans2, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, config->trans );
if( err >= THRESH_2 ) {
icpStereoSetInlierProbability( handle->icpStereoHandle, 0.8 );
err = arGetTransMatStereoRobust( handle, trans2, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, config->trans );
if( err >= THRESH_2 ) {
icpStereoSetInlierProbability( handle->icpStereoHandle, 0.6 );
err = arGetTransMatStereoRobust( handle, trans2, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, config->trans );
if( err >= THRESH_2 ) {
icpStereoSetInlierProbability( handle->icpStereoHandle, 0.4 );
err = arGetTransMatStereoRobust( handle, trans2, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, config->trans );
if( err >= THRESH_2 ) {
icpStereoSetInlierProbability( handle->icpStereoHandle, 0.0 );
err = arGetTransMatStereoRobust( handle, trans2, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, config->trans );
}
}
}
}
}
else {
err = arGetTransMatStereo( handle, trans2, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, config->trans );
}
}
else {
arUtilMatMul( (const ARdouble (*)[4])trans1, (const ARdouble (*)[4])config->marker[max].itrans, trans2 );
if( robustFlag ) {
err2 = arGetTransMatStereo( handle, trans2, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, trans1 );
err = arGetTransMatStereo( handle, config->trans, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, config->trans );
if( err2 < err ) {
for( j = 0; j < 3; j++ ) for( i = 0; i < 4; i++ ) config->trans[j][i] = trans1[j][i];
err = err2;
}
if( err >= THRESH_2 ) {
icpStereoSetInlierProbability( handle->icpStereoHandle, 0.8 );
err2 = arGetTransMatStereoRobust( handle, trans2, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, trans1 );
err = arGetTransMatStereoRobust( handle, config->trans, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, config->trans );
if( err2 < err ) {
for( j = 0; j < 3; j++ ) for( i = 0; i < 4; i++ ) config->trans[j][i] = trans1[j][i];
err = err2;
}
if( err >= THRESH_2 ) {
icpStereoSetInlierProbability( handle->icpStereoHandle, 0.6 );
err2 = arGetTransMatStereoRobust( handle, trans2, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, trans1 );
err = arGetTransMatStereoRobust( handle, config->trans, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, config->trans );
if( err2 < err ) {
for( j = 0; j < 3; j++ ) for( i = 0; i < 4; i++ ) config->trans[j][i] = trans1[j][i];
err = err2;
}
if( err >= THRESH_2 ) {
icpStereoSetInlierProbability( handle->icpStereoHandle, 0.4 );
err2 = arGetTransMatStereoRobust( handle, trans2, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, trans1 );
err = arGetTransMatStereoRobust( handle, config->trans, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, config->trans );
if( err2 < err ) {
for( j = 0; j < 3; j++ ) for( i = 0; i < 4; i++ ) config->trans[j][i] = trans1[j][i];
err = err2;
}
if( err >= THRESH_2 ) {
icpStereoSetInlierProbability( handle->icpStereoHandle, 0.0 );
err2 = arGetTransMatStereoRobust( handle, trans2, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, trans1 );
err = arGetTransMatStereoRobust( handle, config->trans, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, config->trans );
if( err2 < err ) {
for( j = 0; j < 3; j++ ) for( i = 0; i < 4; i++ ) config->trans[j][i] = trans1[j][i];
err = err2;
}
}
}
}
}
}
else {
err2 = arGetTransMatStereo( handle, trans2, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, trans1 );
err = arGetTransMatStereo( handle, config->trans, (ARdouble (*)[2])pos2dL, (ARdouble (*)[3])pos3dL, vnumL*4,
(ARdouble (*)[2])pos2dR, (ARdouble (*)[3])pos3dR, vnumR*4, config->trans );
if( err2 < err ) {
for( j = 0; j < 3; j++ ) for( i = 0; i < 4; i++ ) config->trans[j][i] = trans1[j][i];
err = err2;
}
}
}
if( vnumL > 0 ) {
free(pos3dL);
free(pos2dL);
}
if( vnumR > 0 ) {
free(pos3dR);
free(pos2dR);
}
if( err < THRESH_2 ) {
config->prevF = 1;
}
else {
config->prevF = 0;
}
return err;
}
/* main loop */
static void mainLoop(void)
{
ARUint8 *dataPtr;
ARMarkerInfo *marker_info;
int marker_num;
int i, j, k;
/* grab a vide frame */
if( (dataPtr = (ARUint8 *)arVideoGetImage()) == NULL ) {
arUtilSleep(2);
return;
}
if( count == 0 ) arUtilTimerReset();
count++;
argDrawMode2D();
argDispImage( dataPtr, 0,0 );
/* detect the markers in the video frame */
if( arDetectMarker(dataPtr, thresh, &marker_info, &marker_num) < 0 ) {
cleanup();
exit(0);
}
arVideoCapNext();
argDrawMode3D();
argDraw3dCamera( 0, 0 );
glClearDepth( 1.0 );
glClear(GL_DEPTH_BUFFER_BIT);
/* check for object visibility */
for( i = 0; i < 2; i++ ) {
k = -1;
for( j = 0; j < marker_num; j++ ) {
if( object[i].patt_id == marker_info[j].id ) {
if( k == -1 ) k = j;
else if( marker_info[k].cf < marker_info[j].cf ) k = j;
}
}
object[i].visible = k;
if( k >= 0 ) {
arGetTransMat(&marker_info[k],
object[i].center, object[i].width,
object[i].trans);
draw( object[i].model_id, object[i].trans );
}
}
argSwapBuffers();
if( object[0].visible >= 0
&& object[1].visible >= 0 ) {
double wmat1[3][4], wmat2[3][4];
arUtilMatInv(object[0].trans, wmat1);
arUtilMatMul(wmat1, object[1].trans, wmat2);
for( j = 0; j < 3; j++ ) {
for( i = 0; i < 4; i++ ) printf("%8.4f ", wmat2[j][i]);
printf("\n");
}
printf("\n\n");
}
}
static void draw( void )
{
static int before_visible[2] = {0, 0};
static int Wire_count = -1; // 出力する Wire の選択(0〜4)
static int flag_first1 = 0; // 一度でも hiro が見つかったら立つ
int i;
static double begin[3];
double wtrans[3][4];
double gl_para[16];
/* 3Dオブジェクトを描画するための準備 */
argDrawMode3D();
argDraw3dCamera(0, 0);
/* Zバッファなどの設定 */
glClear( GL_DEPTH_BUFFER_BIT ); // 陰面処理の設定
glEnable( GL_DEPTH_TEST ); // 陰面処理の適用
mySetLight(); // 光源の設定
glEnable( GL_LIGHTING ); // 光源の適用
/* マーカの出現と消失の処理 */
/* マーカ1の処理 */
if( myCheck_appear(before_visible[PTT1_MARK_ID], object[PTT1_MARK_ID].visible) ){
begin[0] = object[PTT1_MARK_ID].patt_trans[0][3];
begin[1] = object[PTT1_MARK_ID].patt_trans[1][3];
begin[2] = object[PTT1_MARK_ID].patt_trans[2][3];
points[L_P_count].flag_after_change = 0;
}
if( myCheck_disappear(before_visible[PTT1_MARK_ID], object[PTT1_MARK_ID].visible) ){
myCopyTrans( object[PTT1_MARK_ID].patt_trans, points[L_P_count].trans);
points[L_P_count].start_point[0] = begin[0] - object[PTT1_MARK_ID].patt_trans[0][3];
points[L_P_count].start_point[1] = -( begin[1] - object[PTT1_MARK_ID].patt_trans[1][3] ); // Y軸はカメラに対して反転しているので
points[L_P_count].start_point[2] = -( begin[2] - object[PTT1_MARK_ID].patt_trans[2][3] ); // Z軸はカメラに対して反転しているので
for(i=0; i<3; i++){
points[L_P_count].end_point[i] = 0.0;
}
myMoveBallInit( &points[L_P_count] );
L_P_count++;
if(MAX_LINES <= L_P_count){
L_P_count = 0;
}
}
/* マーカ2の出現と消失 */
if( myCheck_appear(before_visible[PTT2_MARK_ID], object[PTT2_MARK_ID].visible) ){
for(i=0; i < L_P_count; i++){
arUtilMatMul( itrans2, points[i].trans, wtrans);
myCopyTrans( wtrans, points[i].trans);
points[i].flag_after_change = 1;
}
}
if( myCheck_disappear(before_visible[PTT2_MARK_ID], object[PTT2_MARK_ID].visible) ){
for(i=0; i < L_P_count; i++){
arUtilMatMul( object[PTT2_MARK_ID].patt_trans, points[i].trans, wtrans);
myCopyTrans( wtrans, points[i].trans );
}
}
/* 3Dオブジェクトの描画 */
/* 過去に描いた線分の描画 */
if(flag_first1 && L_P_count >= 1 && !(object[PTT2_MARK_ID].visible) ){
mySelectColor( 1.0, 0.0, 0.0); // 材質特性の設定
glLineWidth( 5.0);
for(i=0; i < L_P_count; i++){
glPushMatrix(); // 念のため
myMatrix( points[i].trans);
myLineLoop(i, i+1);
glPopMatrix(); // 念のため
}
}
/* マーカ1(Hiro) が映ったときの処理 */
if(object[PTT1_MARK_ID].visible){
flag_first1 = 1;
/* 現在書いている線分の描画 */
myMatrix(object[PTT1_MARK_ID].patt_trans);
mySelectColor( 1.0, 0.8, 0.0); // 材質特性の設定
glLineWidth( 5.0);
glBegin( GL_LINES );
glVertex3f( begin[0] - object[PTT1_MARK_ID].patt_trans[0][3], -( begin[1] - object[PTT1_MARK_ID].patt_trans[1][3] ),
-( begin[2] - object[PTT1_MARK_ID].patt_trans[2][3] ) );
glVertex3f( 0.0, 0.0, 0.0 );
glEnd();
/* hiro のマーカを縁取り */
myMatrix(object[PTT1_MARK_ID].patt_trans);
mySelectColor( 0.0, 0.0, 1.0); // 材質特性の設定
glLineWidth( 3.0);
myMarkSquare (PTT1_MARK_ID);
}
/* マーカ2(kanji) が表示されているとき */
if(object[PTT2_MARK_ID].visible){
/* マーカ2の縁取り */
myMatrix(object[PTT2_MARK_ID].patt_trans);
mySelectColor( 0.0, 1.0, 0.0); // 材質特性の設定
glLineWidth( 3.0);
myMarkSquare (PTT2_MARK_ID);
/* 座標変換 */
glLineWidth( 5.0);
for(i=0; i < L_P_count; i++){
glPushMatrix();
mySelectColor( 0.0, 0.0, 1.0); // 材質特性の設定
argConvGlpara( points[i].trans, gl_para );
glMultMatrixd( gl_para );
myLineLoop(i, i+1);
if( flag_animation){
myMoveBall( &points[i] );
}
else{
myMoveBall2( &points[i], i );
}
glPopMatrix();
}
}
visible_log(PTT_NUM, before_visible); // 各マーカの visible 状態の記録
glDisable( GL_LIGHTING ); // 光源の無効化
glDisable( GL_DEPTH_TEST ); // 陰面処理の無効化
}
void ARBundlePublisher::findTransformToCenter(double camera_to_marker_trans[3][4], int knownPatternCount) {
arUtilMatMul(camera_to_marker_trans, tfs[knownPatternCount], master_trans_);
}
int icpCalibStereo( ICPCalibDataT data[], int num,
ARdouble matXcl2Ul[3][4], ARdouble matXcr2Ur[3][4], ARdouble initMatL2R[3][4],
ARdouble matL2R[3][4],
ARdouble *err )
{
ARdouble *J, *dU, *dTS;
ARdouble *matXw2Xcl;
ARdouble matXw2Ul[3][4];
ARdouble matXw2Ur[3][4];
ARdouble matXcl2Ur[3][4];
ARdouble J_UL_S[2][6];
ARdouble oldErr;
ICP3DCoordT cameraCoordL;
ICP2DCoordT screenCoord;
int Jrow, Jclm;
int dataNum;
int i, j, k, l;
int i1;
int ret = 0;
if( num <= 0 ) {
ARLOGe("Data num error!!\n");
return -1;
}
dataNum = 0;
for( i = 0; i < num; i++ ) {
if( data[i].numL <= 0 || data[i].numR <= 0 ) {
ARLOGe("Data num error!!\n");
return -1;
}
dataNum += data[i].numL;
dataNum += data[i].numR;
}
Jrow = dataNum * 2;
Jclm = (num + 1) * 6;
//ARLOGe("Jrow = %d, Jclm = %d\n", Jrow, Jclm );
J = (ARdouble *)malloc(sizeof(ARdouble)*Jrow*Jclm);
if( J == NULL ) {
ARLOGe("Out of memory!!\n");
return -1;
}
dU = (ARdouble *)malloc(sizeof(ARdouble)*Jrow);
if( dU == NULL ) {
ARLOGe("Out of memory!!\n");
free(J);
return -1;
}
dTS = (ARdouble *)malloc(sizeof(ARdouble)*Jclm);
if( dTS == NULL ) {
ARLOGe("Out of memory!!\n");
free(J);
free(dU);
return -1;
}
matXw2Xcl = (ARdouble *)malloc(sizeof(ARdouble)*3*4*num);
if( matXw2Xcl == NULL ) {
ARLOGe("Out of memory!!\n");
free(J);
free(dU);
free(dTS);
return -1;
}
for( j = 0; j < 3; j++ ) {
for( i = 0; i < 4; i++ ) matL2R[j][i] = initMatL2R[j][i];
}
for( k = 0; k < num; k++ ) {
for( j = 0; j < 3; j++ ) {
for( i = 0; i < 4; i++ ) {
matXw2Xcl[k*12+j*4+i] = data[k].initMatXw2Xcl[j][i];
}
}
}
for( l = 0; l < ICP_CALIB_STEREO_MAX_LOOP; l++ ) {
k = 0;
for( i = 0; i < Jrow*Jclm; i++ ) J[i] = 0.0;
for( i = 0; i < Jrow; i++ ) dU[i] = 0.0;
arUtilMatMul( (const ARdouble (*)[4])matXcr2Ur, (const ARdouble (*)[4])matL2R, matXcl2Ur );
*err = 0.0;
for( j = 0; j < num; j++ ) {
arUtilMatMul( (const ARdouble (*)[4])matXcl2Ul, (const ARdouble (*)[4])&(matXw2Xcl[j*12]), matXw2Ul );
arUtilMatMul( (const ARdouble (*)[4])matXcl2Ur, (const ARdouble (*)[4])&(matXw2Xcl[j*12]), matXw2Ur );
for( i = 0; i < data[j].numL; i++ ) {
if( icpGetJ_U_S( J_UL_S, matXcl2Ul, (ARdouble (*)[4])&(matXw2Xcl[j*12]), &(data[j].worldCoordL[i]) ) < 0 ) {
ARLOGe("Error icpGetJ_U_S\n");
ret = -1;
break;
}
for( i1 = 0; i1 < 6; i1++ ) {
J[(k*2 )*Jclm+(1+j)*6+i1] = J_UL_S[0][i1];
J[(k*2+1)*Jclm+(1+j)*6+i1] = J_UL_S[1][i1];
}
if( icpGetU_from_X_by_MatX2U( &screenCoord, matXw2Ul, &(data[j].worldCoordL[i]) ) < 0 ) {
ARLOGe("Error icpGetU_from_X_by_MatX2U\n");
ret = -1;
break;
}
dU[k*2 ] = data[j].screenCoordL[i].x - screenCoord.x;
dU[k*2+1] = data[j].screenCoordL[i].y - screenCoord.y;
*err += dU[k*2]*dU[k*2] + dU[k*2+1]*dU[k*2+1];
k++;
}
if( ret == -1 ) break;
for( i = 0; i < data[j].numR; i++ ) {
if( icpGetJ_U_S( J_UL_S, matXcl2Ur, (ARdouble (*)[4])&(matXw2Xcl[j*12]), &(data[j].worldCoordR[i]) ) < 0 ) {
ARLOGe("Error icpGetJ_U_S\n");
ret = -1;
break;
}
for( i1 = 0; i1 < 6; i1++ ) {
J[(k*2 )*Jclm+(1+j)*6+i1] = J_UL_S[0][i1];
J[(k*2+1)*Jclm+(1+j)*6+i1] = J_UL_S[1][i1];
}
if( icpGetU_from_X_by_MatX2U( &screenCoord, matXw2Ur, &(data[j].worldCoordR[i]) ) < 0 ) {
ARLOGe("Error icpGetU_from_X_by_MatX2U\n");
ret = -1;
break;
}
dU[k*2 ] = data[j].screenCoordR[i].x - screenCoord.x;
dU[k*2+1] = data[j].screenCoordR[i].y - screenCoord.y;
*err += dU[k*2]*dU[k*2] + dU[k*2+1]*dU[k*2+1];
if( icpGetXc_from_Xw_by_MatXw2Xc( &cameraCoordL, (ARdouble (*)[4])&(matXw2Xcl[j*12]), &(data[j].worldCoordR[i]) ) < 0 ) {
ARLOGe("Error icpGetXc_from_Xw_by_MatXw2Xc\n");
ret = -1;
break;
}
if( icpGetJ_U_S( J_UL_S, matXcr2Ur, matL2R, &cameraCoordL ) < 0 ) {
ARLOGe("Error icpGetJ_U_S\n");
ret = -1;
break;
}
for( i1 = 0; i1 < 6; i1++ ) {
J[(k*2 )*Jclm+i1] = J_UL_S[0][i1];
J[(k*2+1)*Jclm+i1] = J_UL_S[1][i1];
}
k++;
}
if( ret == -1 ) break;
}
if( ret == -1 ) break;
*err /= dataNum;
ARLOGe("Error = %f\n", *err);
if( *err < ICP_CALIB_STEREO_BREAK_LOOP_ERROR_THRESH ) break;
if( l > 0 && *err/oldErr > ICP_CALIB_STEREO_BREAK_LOOP_ERROR_RATIO_THRESH ) break;
oldErr = *err;
if( icp2GetTS( dTS, dU, J, Jrow, Jclm ) < 0 ) {
ARLOGe("Error icp2GetTS\n");
ret = -1;
break;
}
icpUpdateMat( matL2R, &(dTS[0]) );
for( j = 0; j < num; j++ ) {
icpUpdateMat( (ARdouble (*)[4])&(matXw2Xcl[j*12]), &(dTS[6*(j+1)]) );
}
}
free(J);
free(dU);
free(dTS);
free(matXw2Xcl);
return ret;
}
