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;
}
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;
}
