int trackingInitGetResult( THREAD_HANDLE_T *threadHandle, float trans[3][4], int *page )
{
TrackingInitHandle *trackingInitHandle;
int i, j;
if (!threadHandle || !trans || !page) {
ARLOGe("trackingInitGetResult(): Error: NULL threadHandle or trans or page.\n");
return (-1);
}
if( threadGetStatus( threadHandle ) == 0 ) return 0;
threadEndWait( threadHandle );
trackingInitHandle = (TrackingInitHandle *)threadGetArg(threadHandle);
if (!trackingInitHandle) return (-1);
if( trackingInitHandle->flag ) {
for (j = 0; j < 3; j++) for (i = 0; i < 4; i++) trans[j][i] = trackingInitHandle->trans[j][i];
*page = trackingInitHandle->page;
return 1;
}
return -1;
}
int surfSubGetDescriptors( SurfSubIPointArrayT *iPointArray, int *image, int width, int height, int border, int maxNum, int threadNum )
{
#if 1
int k = 0;
for(int i = 0; i < iPointArray->num; i++) {
int x, y, s;
x = (int)(iPointArray->iPoint[i].x+0.5);
y = (int)(iPointArray->iPoint[i].y+0.5);
s = (int)(iPointArray->iPoint[i].scale+0.5);
if( x-s*20 < 0 || y-s*20 < 0 || x+s*20 >= width || y+s*20 >= height ) {
continue;
}
if( i != k ) {
iPointArray->iPoint[k] = iPointArray->iPoint[i];
}
k++;
}
iPointArray->num = k;
if( iPointArray->num == 0 ) return 0;
#endif
if( maxNum > 0 ) {
if( iPointArray->num > maxNum ) {
qsort( iPointArray->iPoint, iPointArray->num, sizeof(SurfSubIPointT), compSurfP );
iPointArray->num = maxNum;
}
}
static int initF = 0;
static int threadMax;
static THREAD_HANDLE_T **threadHandle;
static SurfSubGetDescriptorsParam *arg;
if( initF == 0 ) {
threadMax = threadGetCPU();
threadHandle = (THREAD_HANDLE_T **)malloc(sizeof(THREAD_HANDLE_T*)*threadMax);
if( threadHandle == NULL ) {ARLOGe("Malloc error: surfSubGetDescriptors.\n"); exit(0);}
arg = (SurfSubGetDescriptorsParam *)malloc(sizeof(SurfSubGetDescriptorsParam)*threadMax);
if( arg == NULL ) {ARLOGe("Malloc error: surfSubGetDescriptors.\n"); exit(0);}
for(int i = 0; i < threadMax; i++ ) {
threadHandle[i] = threadInit(i, &(arg[i]), surfSubGetDescriptorsSub);
}
initF = 1;
}
if( threadNum < 0 ) threadNum = threadMax;
if( threadNum > threadMax ) threadNum = threadMax;
char *flag = (char *)malloc(iPointArray->num);
int i = iPointArray->num/threadNum;
int j = iPointArray->num%threadNum;
k = 0;
for(int l = 0; l < j; l++ ) {
arg[l].iPoint = iPointArray->iPoint;
arg[l].flag = flag;
arg[l].image = image;
arg[l].width = width;
arg[l].height = height;
arg[l].border = border;
arg[l].startNum = k;
arg[l].endNum = k + i;
threadStartSignal( threadHandle[l] );
k += (i+1);
}
for(int l = j; l < threadNum; l++ ) {
arg[l].iPoint = iPointArray->iPoint;
arg[l].flag = flag;
arg[l].image = image;
arg[l].width = width;
arg[l].height = height;
arg[l].border = border;
arg[l].startNum = k;
arg[l].endNum = k + i - 1;
threadStartSignal( threadHandle[l] );
k += i;
}
for(int l = 0; l < threadNum; l++ ) {
threadEndWait( threadHandle[l] );
}
k = 0;
for(int i = 0; i < iPointArray->num; i++) {
if( flag[i] == 0 ) continue;
if( i != k ) {
iPointArray->iPoint[k] = iPointArray->iPoint[i];
}
k++;
}
iPointArray->num = k;
free(flag);
/*
for(int i = 0; i < iPointArray->num; i++) {
if( getOrientation(&(iPointArray->iPoint[i]), image, width, height, border) < 0 ) {
for( int j = i+1; j < iPointArray->num; j++ ) iPointArray->iPoint[j-1] = iPointArray->iPoint[j];
iPointArray->num--;
i--;
continue;
}
if( getDescriptor(&(iPointArray->iPoint[i]), image, width, height, border) < 0 ) {
for( int j = i+1; j < iPointArray->num; j++ ) iPointArray->iPoint[j-1] = iPointArray->iPoint[j];
iPointArray->num--;
i--;
continue;
}
}
*/
return 0;
}
int ar2Tracking( AR2HandleT *ar2Handle, AR2SurfaceSetT *surfaceSet, ARUint8 *dataPtr, float trans[3][4], float *err )
{
AR2TemplateCandidateT *candidatePtr;
AR2TemplateCandidateT *cp[AR2_THREAD_MAX];
#if AR2_CAPABLE_ADAPTIVE_TEMPLATE
float aveBlur;
#endif
int num, num2;
int i, j, k;
if (!ar2Handle || !surfaceSet || !dataPtr || !trans || !err) return (-1);
if( surfaceSet->contNum <= 0 ) {
ARLOGd("ar2Tracking() error: ar2SetInitTrans() must be called first.\n");
return -2;
}
*err = 0.0F;
for( i = 0; i < surfaceSet->num; i++ ) {
arUtilMatMulf( (const float (*)[4])surfaceSet->trans1, (const float (*)[4])surfaceSet->surface[i].trans, ar2Handle->wtrans1[i] );
if( surfaceSet->contNum > 1 ) arUtilMatMulf( (const float (*)[4])surfaceSet->trans2, (const float (*)[4])surfaceSet->surface[i].trans, ar2Handle->wtrans2[i] );
if( surfaceSet->contNum > 2 ) arUtilMatMulf( (const float (*)[4])surfaceSet->trans3, (const float (*)[4])surfaceSet->surface[i].trans, ar2Handle->wtrans3[i] );
}
if( ar2Handle->trackingMode == AR2_TRACKING_6DOF ) {
extractVisibleFeatures(ar2Handle->cparamLT, ar2Handle->wtrans1, surfaceSet, ar2Handle->candidate, ar2Handle->candidate2);
}
else {
extractVisibleFeaturesHomography(ar2Handle->xsize, ar2Handle->ysize, ar2Handle->wtrans1, surfaceSet, ar2Handle->candidate, ar2Handle->candidate2);
}
candidatePtr = ar2Handle->candidate;
#if AR2_CAPABLE_ADAPTIVE_TEMPLATE
aveBlur = 0.0F;
#endif
i = 0; // Counts up to searchFeatureNum.
num = 0;
while( i < ar2Handle->searchFeatureNum ) {
num2 = num;
for( j = 0; j < ar2Handle->threadNum; j++ ) {
if( i == ar2Handle->searchFeatureNum ) break;
k = ar2SelectTemplate( candidatePtr, surfaceSet->prevFeature, num2, ar2Handle->pos, ar2Handle->xsize, ar2Handle->ysize );
if( k < 0 ) {
if( candidatePtr == ar2Handle->candidate ) {
candidatePtr = ar2Handle->candidate2;
k = ar2SelectTemplate( candidatePtr, surfaceSet->prevFeature, num2, ar2Handle->pos, ar2Handle->xsize, ar2Handle->ysize );
if( k < 0 ) break; // PRL 2012-05-15: Give up if we can't select template from alternate candidate either.
}
else break;
}
cp[j] = &(candidatePtr[k]);
ar2Handle->pos[num2][0] = candidatePtr[k].sx;
ar2Handle->pos[num2][1] = candidatePtr[k].sy;
ar2Handle->arg[j].ar2Handle = ar2Handle;
ar2Handle->arg[j].surfaceSet = surfaceSet;
ar2Handle->arg[j].candidate = &(candidatePtr[k]);
ar2Handle->arg[j].dataPtr = dataPtr;
threadStartSignal( ar2Handle->threadHandle[j] );
num2++;
if( num2 == 5 ) num2 = num;
i++;
}
k = j;
if( k == 0 ) break;
for( j = 0; j < k; j++ ) {
threadEndWait( ar2Handle->threadHandle[j] );
if( ar2Handle->arg[j].ret == 0 && ar2Handle->arg[j].result.sim > ar2Handle->simThresh ) {
if( ar2Handle->trackingMode == AR2_TRACKING_6DOF ) {
#ifdef ARDOUBLE_IS_FLOAT
arParamObserv2Ideal(ar2Handle->cparamLT->param.dist_factor,
ar2Handle->arg[j].result.pos2d[0], ar2Handle->arg[j].result.pos2d[1],
&ar2Handle->pos2d[num][0], &ar2Handle->pos2d[num][1], ar2Handle->cparamLT->param.dist_function_version);
#else
ARdouble pos2d0, pos2d1;
arParamObserv2Ideal(ar2Handle->cparamLT->param.dist_factor,
(ARdouble)(ar2Handle->arg[j].result.pos2d[0]), (ARdouble)(ar2Handle->arg[j].result.pos2d[1]),
&pos2d0, &pos2d1, ar2Handle->cparamLT->param.dist_function_version);
ar2Handle->pos2d[num][0] = (float)pos2d0;
ar2Handle->pos2d[num][1] = (float)pos2d1;
#endif
}
else {
ar2Handle->pos2d[num][0] = ar2Handle->arg[j].result.pos2d[0];
ar2Handle->pos2d[num][1] = ar2Handle->arg[j].result.pos2d[1];
}
ar2Handle->pos3d[num][0] = ar2Handle->arg[j].result.pos3d[0];
ar2Handle->pos3d[num][1] = ar2Handle->arg[j].result.pos3d[1];
ar2Handle->pos3d[num][2] = ar2Handle->arg[j].result.pos3d[2];
ar2Handle->pos[num][0] = cp[j]->sx;
ar2Handle->pos[num][1] = cp[j]->sy;
ar2Handle->usedFeature[num].snum = cp[j]->snum;
ar2Handle->usedFeature[num].level = cp[j]->level;
ar2Handle->usedFeature[num].num = cp[j]->num;
ar2Handle->usedFeature[num].flag = 0;
#if AR2_CAPABLE_ADAPTIVE_TEMPLATE
aveBlur += ar2Handle->arg[j].result.blurLevel;
#endif
num++;
}
}
}
for( i = 0; i < num; i++ ) {
surfaceSet->prevFeature[i] = ar2Handle->usedFeature[i];
}
surfaceSet->prevFeature[num].flag = -1;
//ARLOG("------\nNum = %d\n", num);
if( ar2Handle->trackingMode == AR2_TRACKING_6DOF ) {
if( num < 3 ) {
surfaceSet->contNum = 0;
return -3;
}
*err = ar2GetTransMat( ar2Handle->icpHandle, surfaceSet->trans1, ar2Handle->pos2d, ar2Handle->pos3d, num, trans, 0 );
//ARLOG("outlier 0%%: err = %f, num = %d\n", *err, num);
if( *err > ar2Handle->trackingThresh ) {
icpSetInlierProbability( ar2Handle->icpHandle, 0.8F );
*err = ar2GetTransMat( ar2Handle->icpHandle, trans, ar2Handle->pos2d, ar2Handle->pos3d, num, trans, 1 );
//ARLOG("outlier 20%%: err = %f, num = %d\n", *err, num);
if( *err > ar2Handle->trackingThresh ) {
icpSetInlierProbability( ar2Handle->icpHandle, 0.6F );
*err = ar2GetTransMat( ar2Handle->icpHandle, trans, ar2Handle->pos2d, ar2Handle->pos3d, num, trans, 1 );
//ARLOG("outlier 60%%: err = %f, num = %d\n", *err, num);
if( *err > ar2Handle->trackingThresh ) {
icpSetInlierProbability( ar2Handle->icpHandle, 0.4F );
*err = ar2GetTransMat( ar2Handle->icpHandle, trans, ar2Handle->pos2d, ar2Handle->pos3d, num, trans, 1 );
//ARLOG("outlier 60%%: err = %f, num = %d\n", *err, num);
if( *err > ar2Handle->trackingThresh ) {
icpSetInlierProbability( ar2Handle->icpHandle, 0.0F );
*err = ar2GetTransMat( ar2Handle->icpHandle, trans, ar2Handle->pos2d, ar2Handle->pos3d, num, trans, 1 );
//ARLOG("outlier Max: err = %f, num = %d\n", *err, num);
if( *err > ar2Handle->trackingThresh ) {
surfaceSet->contNum = 0;
#if AR2_CAPABLE_ADAPTIVE_TEMPLATE
if( ar2Handle->blurMethod == AR2_ADAPTIVE_BLUR ) ar2Handle->blurLevel = AR2_DEFAULT_BLUR_LEVEL; // Reset the blurLevel.
#endif
return -4;
}
}
}
}
}
}
else {
if( num < 3 ) {
surfaceSet->contNum = 0;
return -3;
}
*err = ar2GetTransMatHomography( surfaceSet->trans1, ar2Handle->pos2d, ar2Handle->pos3d, num, trans, 0, 1.0F );
//ARLOG("outlier 0%%: err = %f, num = %d\n", *err, num);
if( *err > ar2Handle->trackingThresh ) {
*err = ar2GetTransMatHomography( trans, ar2Handle->pos2d, ar2Handle->pos3d, num, trans, 1, 0.8F );
//ARLOG("outlier 20%%: err = %f, num = %d\n", *err, num);
if( *err > ar2Handle->trackingThresh ) {
*err = ar2GetTransMatHomography( trans, ar2Handle->pos2d, ar2Handle->pos3d, num, trans, 1, 0.6F );
//ARLOG("outlier 40%%: err = %f, num = %d\n", *err, num);
if( *err > ar2Handle->trackingThresh ) {
*err = ar2GetTransMatHomography( trans, ar2Handle->pos2d, ar2Handle->pos3d, num, trans, 1, 0.4F );
//ARLOG("outlier 60%%: err = %f, num = %d\n", *err, num);
if( *err > ar2Handle->trackingThresh ) {
*err = ar2GetTransMatHomography( trans, ar2Handle->pos2d, ar2Handle->pos3d, num, trans, 1, 0.0F );
//ARLOG("outlier Max: err = %f, num = %d\n", *err, num);
if( *err > ar2Handle->trackingThresh ) {
surfaceSet->contNum = 0;
#if AR2_CAPABLE_ADAPTIVE_TEMPLATE
if( ar2Handle->blurMethod == AR2_ADAPTIVE_BLUR ) ar2Handle->blurLevel = AR2_DEFAULT_BLUR_LEVEL; // Reset the blurLevel.
#endif
return -4;
}
}
}
}
}
}
#if AR2_CAPABLE_ADAPTIVE_TEMPLATE
if( ar2Handle->blurMethod == AR2_ADAPTIVE_BLUR ) {
aveBlur = aveBlur/num + 0.5F;
ar2Handle->blurLevel += (int)aveBlur - 1;
if( ar2Handle->blurLevel < 1 ) ar2Handle->blurLevel = 1;
if( ar2Handle->blurLevel >= AR2_BLUR_IMAGE_MAX-1 ) ar2Handle->blurLevel = AR2_BLUR_IMAGE_MAX-2;
}
#endif
surfaceSet->contNum++;
for( j = 0; j < 3; j++ ) {
for( i = 0; i < 4; i++ ) surfaceSet->trans3[j][i] = surfaceSet->trans2[j][i];
}
for( j = 0; j < 3; j++ ) {
for( i = 0; i < 4; i++ ) surfaceSet->trans2[j][i] = surfaceSet->trans1[j][i];
}
for( j = 0; j < 3; j++ ) {
for( i = 0; i < 4; i++ ) surfaceSet->trans1[j][i] = trans[j][i];
}
return 0;
}
