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