//=======================================================
// OpenConnection
//=======================================================
bool mitk::OptitrackTrackingDevice::OpenConnection()
{
// Not initialize the system twice.
if(!m_initialized)
{
MITK_DEBUG << "Initialize Optitrack Tracking System";
if( this->InitializeCameras() )
{
m_initialized = true; // Set the initialized variable to true
this->SetState(mitk::TrackingDevice::Ready);
if(this->m_calibrationPath.empty()){
MITK_INFO << "Numer of connected cameras = " << TT_CameraCount();
MITK_WARN << "Attention: No calibration File defined !!";
return m_initialized;
}
else
{
this->LoadCalibration();
}
}
else
{
m_initialized = false; // Set the initialized variable to false
this->SetState(mitk::TrackingDevice::Setup); // Set the State to Setup
MITK_INFO << "Device initialization failed. Device is still in setup state";
mitkThrowException(mitk::IGTException) << "Device initialization failed. Device is still in setup state";
}
}
//this->LoadCalibration();
return m_initialized;
}
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;
}
// 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;
}
//=======================================================
// SetCameraParams
//=======================================================
bool mitk::OptitrackTrackingDevice::SetCameraParams(int exposure, int threshold , int intensity, int videoType )
{
MITK_DEBUG << "SetCameraParams";
if(this->m_initialized)
{
int num_cams = 0;
int resultUpdate;
bool resultSetCameraSettings;
for( int i=OPTITRACK_ATTEMPTS; i>0; i--)
{
resultUpdate = TT_Update(); // Get Update for the Optitrack API
if(resultUpdate == NPRESULT_SUCCESS)
{
MITK_DEBUG << "Update Succeed";
num_cams = TT_CameraCount();
i = 0;
}
else
{
MITK_DEBUG << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultUpdate);
MITK_DEBUG << "Trying again...";
Sleep(30);
}
}
// If no cameras are connected
if(num_cams == 0)
{
MITK_DEBUG << "No cameras are connected to the device";
return false;
mitkThrowException(mitk::IGTException) << "No cameras are connected to the device";
}
for(int cam = 0; cam < num_cams; cam++) // for all connected cameras
{
for( int i=OPTITRACK_ATTEMPTS; i>0; i--)
{
resultUpdate = TT_Update(); // Get Update for the Optitrack API
if(resultUpdate == NPRESULT_SUCCESS)
{
MITK_DEBUG << "Update Succeed for camera number " << cam;
resultSetCameraSettings = TT_SetCameraSettings(cam,videoType,exposure,threshold,intensity);
if(resultSetCameraSettings)
{
MITK_INFO << "Camera # "<<cam<< " params are set";
i = 0; // End attempts for camera #cam
}
else
{
MITK_DEBUG << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultSetCameraSettings);
if(i == 1)
mitkThrowException(mitk::IGTException) << "Camera number " << cam << " failed during setting the params. Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultSetCameraSettings);
}
}
else
{
MITK_DEBUG << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultUpdate);
MITK_DEBUG << "Update: Trying again...";
}
}
}
}
else
{
MITK_INFO << "System is not Initialized -> System is not ready to perform the Camera Parameters Setting";
mitkThrowException(mitk::IGTException) << "System is not Initialized -> System is not ready to perform the Camera Parameters Setting";
return false;
}
return true;
}
