/* thread to execute display of camera frames */ void *showCameraWindow(void *arg) { CameraData_t *myCam = (CameraData_t *)arg; Convexctx_t *cameraCtx; IplImage *img = NULL; const char *windowName = TT_CameraName(myCam->i); cvNamedWindow(windowName,CV_WINDOW_AUTOSIZE); for( ;key != KEY_ESC; ){ TT_CameraFrameBuffer(myCam->i, W, H, 0, 8, (unsigned char *)myCam->displayImage->imageData); pthread_mutex_lock(&keyMutex); if(myCam->i == 2){ img = compute_ContourTree(myCam->displayImage); if(img) cvShowImage(windowName, img); else cvShowImage(windowName, myCam->displayImage); key = cvWaitKey(15); //delay atleast n ms for TT firmware propogation delay if(img) cvReleaseImage(&img); } else cvShowImage(windowName, myCam->displayImage); pthread_mutex_unlock(&keyMutex); } cvReleaseImage(&myCam->displayImage); pthread_exit(NULL); }
// Main application int main( int argc, char* argv[] ) { printf("== NaturalPoint Tracking Tools API Marker Sample =======---\n"); printf("== (C) NaturalPoint, Inc.\n\n"); printf("Initializing NaturalPoint Devices\n"); TT_Initialize(); // Do an update to pick up any recently-arrived cameras. TT_Update(); // Load a project file from the executable directory. printf( "Loading Project: project.ttp\n\n" ); CheckResult( TT_LoadProject("project.ttp") ); // List all detected cameras. printf( "Cameras:\n" ); for( int i = 0; i < TT_CameraCount(); i++) { printf( "\t%s\n", TT_CameraName(i) ); } printf("\n"); // List all defined rigid bodies. printf("Rigid Bodies:\n"); for( int i = 0; i < TT_TrackableCount(); i++) { printf("\t%s\n", TT_TrackableName(i)); } printf("\n"); int frameCounter = 0; // Poll API data until the user hits a keyboard key. while( !_kbhit() ) { if( TT_Update() == NPRESULT_SUCCESS ) { frameCounter++; // Update tracking information every 100 frames (for example purposes). if( (frameCounter%100) == 0 ) { float yaw,pitch,roll; float x,y,z; float qx,qy,qz,qw; bool tracked; printf( "Frame #%d: (Markers: %d)\n", frameCounter, TT_FrameMarkerCount() ); for( int i = 0; i < TT_TrackableCount(); i++ ) { TT_TrackableLocation( i, &x,&y,&z, &qx,&qy,&qz,&qw, &yaw,&pitch,&roll ); if( TT_IsTrackableTracked( i ) ) { printf( "\t%s: Pos (%.3f, %.3f, %.3f) Orient (%.1f, %.1f, %.1f)\n", TT_TrackableName( i ), x, y, z, yaw, pitch, roll ); TransformMatrix xRot( TransformMatrix::RotateX( -roll * kRadToDeg ) ); TransformMatrix yRot( TransformMatrix::RotateY( -yaw * kRadToDeg ) ); TransformMatrix zRot( TransformMatrix::RotateZ( -pitch * kRadToDeg ) ); // Compose the local-to-world rotation matrix in XZY (roll, pitch, yaw) order. TransformMatrix worldTransform = xRot * zRot * yRot; // Inject world-space coordinates of the origin. worldTransform.SetTranslation( x, y, z ); // Invert the transform matrix to convert from a local-to-world to a world-to-local. worldTransform.Invert(); float mx, my, mz; int markerCount = TT_TrackableMarkerCount( i ); for( int j = 0; j < markerCount; ++j ) { // Get the world-space coordinates of each rigid body marker. TT_TrackablePointCloudMarker( i, j, tracked, mx, my, mz ); // Transform the rigid body point from world coordinates to local rigid body coordinates. // Any world-space point can be substituted here to transform it into the local space of // the rigid body. Point4 worldPnt( mx, my, mz, 1.0f ); Point4 localPnt = worldTransform * worldPnt; printf( "\t\t%d: World (%.3f, %.3f, %.3f) Local (%.3f, %.3f, %.3f)\n", j + 1, mx, my, mz, localPnt[0], localPnt[1], localPnt[2] ); } } else { printf( "\t%s: Not Tracked\n", TT_TrackableName( i ) ); } } } } Sleep(2); } printf( "Shutting down NaturalPoint Tracking Tools\n" ); CheckResult( TT_Shutdown() ); printf( "Complete\n" ); while( !_kbhit() ) { Sleep(20); } TT_FinalCleanup(); return 0; }