예제 #1
0
int main()
{
	
	TT_Initialize(); //setup TT cameras
	printf("Opening Calibration: %s\n", 
		TT_LoadCalibration("CalibrationResult 2010-11-02 5.03pm.cal") == NPRESULT_SUCCESS ?
		"PASS" : "ERROR");
	int cameraCount = TT_CameraCount();
	CameraData_t cameras[MAX_NUM_CAMERAS];
	pthread_t threads[MAX_NUM_CAMERAS]; 
	
	assert(MAX_NUM_CAMERAS == cameraCount);

	TT_SetCameraSettings(0, NPVIDEOTYPE_PRECISION,300, 150, 15);
	TT_SetCameraSettings(1, NPVIDEOTYPE_PRECISION,300, 150, 15);
	TT_SetCameraSettings(2, NPVIDEOTYPE_PRECISION,300, 150, 15);
	/* 1. Change camera settings ^
	   2. Allocate space for the displays 
	*/
	for(int i = 0; i < cameraCount; i++){
		cameras[i].i = i;
		cameras[i].displayImage = cvCreateImage(cvSize(W,H), IPL_DEPTH_8U, 1);
	}

	/* call the threads for display of camera data */
	
	pthread_mutex_init(&keyMutex, NULL);

	for(int i = 0; i < cameraCount; i++){
		if(pthread_create(&threads[i], NULL, showCameraWindow, (void*)&cameras[i])){
			printf("\aThread couldn't be created!");
			TT_Shutdown();
			TT_FinalCleanup();
			exit(0);
		}
	}
	
	printf("Press any Key to Exit!\n"); 
	while(!_kbhit()){
		int result = TT_Update();
		if(result != NPRESULT_SUCCESS)
			Sleep(70UL); //wait for updated frame 1/sleeptime[ms] = frame-rate
	}

	for(int i = 0; i < cameraCount; i++)
		pthread_join(threads[i], NULL);
	
	pthread_mutex_destroy(&keyMutex);
	cvDestroyAllWindows();
	TT_Shutdown();
	TT_FinalCleanup();
	return 0;
}
예제 #2
0
//=======================================================
// InitializeCameras
//=======================================================
bool mitk::OptitrackTrackingDevice::InitializeCameras()
{
  MITK_DEBUG << "Initialize Optitrack";
  int result;

  result = TT_Initialize(); // Initialize the cameras

  if(result == NPRESULT_SUCCESS)
  {
    MITK_DEBUG << "Optitrack Initialization Succeed";
    return true;
  }
  else
  {
    MITK_DEBUG << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(result);
  // If not succeed after OPTITRACK_ATTEMPTS times launch exception
  MITK_INFO << "Optitrack Tracking System cannot be initialized \n" << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(result);
  mitkThrowException(mitk::IGTException) << "Optitrack Tracking System cannot be initialized \n" << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(result);
  return false;
  }
}
예제 #3
0
// 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;
}