static void *surfSubGetDescriptorsSub( THREAD_HANDLE_T *threadHandle )
{
SurfSubGetDescriptorsParam *arg;
int startNum;
int endNum;
int i;
arg = (SurfSubGetDescriptorsParam *)threadGetArg(threadHandle);
for(;;) {
if( threadStartWait(threadHandle) < 0 ) break;
startNum = arg->startNum;
endNum = arg->endNum;
for( i = startNum; i <= endNum; i++ ) {
if( getOrientation(&(arg->iPoint[i]), arg->image, arg->width, arg->height, arg->border) < 0 ) {
arg->flag[i] = 0;
continue;
}
if( getDescriptor(&(arg->iPoint[i]), arg->image, arg->width, arg->height, arg->border) < 0 ) {
arg->flag[i] = 0;
continue;
}
arg->flag[i] = 1;
}
threadEndSignal(threadHandle);
}
return NULL;
}
void *ar2Tracking2d( THREAD_HANDLE_T *threadHandle )
{
AR2Tracking2DParamT *arg;
int ID;
arg = (AR2Tracking2DParamT *)threadGetArg(threadHandle);
ID = threadGetID(threadHandle);
ARLOGi("Start tracking_thread #%d.\n", ID);
for(;;) {
if( threadStartWait(threadHandle) < 0 ) break;
#if AR2_CAPABLE_ADAPTIVE_TEMPLATE
arg->ret = ar2Tracking2dSub( arg->ar2Handle, arg->surfaceSet, arg->candidate,
arg->dataPtr, arg->mfImage, &(arg->templ), &(arg->templ2), &(arg->result) );
#else
arg->ret = ar2Tracking2dSub( arg->ar2Handle, arg->surfaceSet, arg->candidate,
arg->dataPtr, arg->mfImage, &(arg->templ), &(arg->result) );
#endif
threadEndSignal(threadHandle);
}
ARLOGi("End tracking_thread #%d.\n", ID);
return NULL;
}
static void *trackingInitMain( THREAD_HANDLE_T *threadHandle )
{
TrackingInitHandle *trackingInitHandle;
KpmHandle *kpmHandle;
KpmResult *kpmResult = NULL;
int kpmResultNum;
ARUint8 *imagePtr;
float err;
int i, j, k;
if (!threadHandle) {
ARLOGe("Error starting tracking thread: empty THREAD_HANDLE_T.\n");
return (NULL);
}
trackingInitHandle = (TrackingInitHandle *)threadGetArg(threadHandle);
if (!threadHandle) {
ARLOGe("Error starting tracking thread: empty trackingInitHandle.\n");
return (NULL);
}
kpmHandle = trackingInitHandle->kpmHandle;
imagePtr = trackingInitHandle->imagePtr;
if (!kpmHandle || !imagePtr) {
ARLOGe("Error starting tracking thread: empty kpmHandle/imagePtr.\n");
return (NULL);
}
ARLOGi("Start tracking thread.\n");
kpmGetResult( kpmHandle, &kpmResult, &kpmResultNum );
for(;;) {
if( threadStartWait(threadHandle) < 0 ) break;
kpmMatching(kpmHandle, imagePtr);
trackingInitHandle->flag = 0;
for( i = 0; i < kpmResultNum; i++ ) {
if( kpmResult[i].camPoseF != 0 ) continue;
ARLOGd("kpmGetPose OK.\n");
if( trackingInitHandle->flag == 0 || err > kpmResult[i].error ) { // Take the first or best result.
trackingInitHandle->flag = 1;
trackingInitHandle->page = kpmResult[i].pageNo;
for (j = 0; j < 3; j++) for (k = 0; k < 4; k++) trackingInitHandle->trans[j][k] = kpmResult[i].camPose[j][k];
err = kpmResult[i].error;
}
}
threadEndSignal(threadHandle);
}
ARLOGi("End tracking thread.\n");
return (NULL);
}
int trackingInitQuit( THREAD_HANDLE_T **threadHandle_p )
{
TrackingInitHandle *trackingInitHandle;
if (!threadHandle_p) {
ARLOGe("trackingInitQuit(): Error: NULL threadHandle_p.\n");
return (-1);
}
if (!*threadHandle_p) return 0;
threadWaitQuit( *threadHandle_p );
trackingInitHandle = (TrackingInitHandle *)threadGetArg(*threadHandle_p);
if (trackingInitHandle) {
free( trackingInitHandle->imagePtr );
free( trackingInitHandle );
}
threadFree( threadHandle_p );
return 0;
}
int trackingInitStart( THREAD_HANDLE_T *threadHandle, ARUint8 *imagePtr )
{
TrackingInitHandle *trackingInitHandle;
if (!threadHandle || !imagePtr) {
ARLOGe("trackingInitStart(): Error: NULL threadHandle or imagePtr.\n");
return (-1);
}
trackingInitHandle = (TrackingInitHandle *)threadGetArg(threadHandle);
if (!trackingInitHandle) {
ARLOGe("trackingInitStart(): Error: NULL trackingInitHandle.\n");
return (-1);
}
memcpy( trackingInitHandle->imagePtr, imagePtr, trackingInitHandle->imageSize );
threadStartSignal( threadHandle );
return 0;
}
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;
}
