作業の備忘録

//http://forum.naturalpoint.com/showflat.php?Cat=0&Number=17391&an=0&page=2
// NP specific includes
#include "optitrack.h"
#include "stdio.h"

INPCameraCollection *pCameraCollection;
INPCamera *pCamera;
INPCameraFrame *pFrame;
INPVector *pVector;

#define HRCHECK(message) if (FAILED(hr)) { fprintf(stderr, "%s\n", message); return 0; }

int main(int argc, char* argv[])
{
 HRESULT hr;  LONG count;  LONG misc;  LONG waittime;
 VARIANT vx, vy, vz;  VARIANT vyaw, vpitch, vroll;  VARIANT voption;
 int running = 1;  int toerrors = 0;
 
 printf("Hello World!\n");  // probably not needed, but doesn't hurt: 
 
 VariantInit(&vx);  VariantInit(&vy);  VariantInit(&vz);
 VariantInit(&vyaw);  VariantInit(&vpitch);  VariantInit(&vroll);  VariantInit(&voption);
 CoInitialize(NULL);  // Startup Microsoft crap 
 
 // create CameraCollection and Vector objects: 
 hr = CoCreateInstance(__uuidof(NPCameraCollection), NULL, CLSCTX_ALL,
  __uuidof(INPCameraCollection), (void **) &pCameraCollection);
 printf("%x\n", hr);
 HRCHECK("CoCreateInstance for CameraCollection");

 hr = CoCreateInstance(__uuidof(NPVector), NULL, CLSCTX_ALL,
  __uuidof(INPVector), (void **) &pVector);
 HRCHECK("CoCreateInstance for Vector");  // find all the cameras:

 hr = pCameraCollection->Enum();
 HRCHECK("CameraCollection->Enum");

 hr = pCameraCollection->get_Count(&count);
 HRCHECK("CameraCollection->get_Count");
 if (count < hr =" pCameraCollection-">Item(0, &pCamera);
 HRCHECK("CameraCollection->Item");
 hr = pCamera->get_SerialNumber(&misc);
 HRCHECK("Camera->get_SerialNumber");
 printf("serial number = %ld\n", misc);
 hr = pCamera->Open();
 HRCHECK("Camera->Open");  // turn on illumination LEDs
 
 hr = pCamera->SetLED(NP_LED_ONE, VARIANT_TRUE);
 HRCHECK("Camera->SetLED");
#if 0
 voption.vt = VT_BOOL;
 voption.boolVal = VARIANT_TRUE;
 hr = pCamera->SetOption(NP_OPTION_SEND_EMPTY_FRAMES, voption);
 HRCHECK("Camera->SetOption");
#endif
 
 hr = pCamera->Start();
 
 HRCHECK("Camera->Start");
 waittime = 10; // value in milliseconds; 0 = don't wait; INFINITE = wait till data
 pVector->Reset();
 HRCHECK("Vector->Reset");
 running = 6000; // run ~1 minute for waittime=10 ms
 
 while(running--) {
  hr = pCamera->GetFrame(waittime, &pFrame);
  if (FAILED(hr)) {
   // fprintf(stderr,"Camera->GetFrame %x ", hr);
   switch(hr) {
    case S_OK:
     fprintf(stderr, "OK\n");
     break;
    case E_POINTER:
     fprintf(stderr, "E_POINTER\n");
     break;    
    case NP_E_DEVICE_DISCONNECTED:
     fprintf(stderr, "NP_E_DEVICE_DISCONNECTED\n");
     break;   
    case NP_E_DEVICE_NOT_SUPPORTED:
     fprintf(stderr, "NP_E_DEVICE_NOT_SUPPORTED\n");
     break;   
    case HRESULT_FROM_WIN32(ERROR_TIMEOUT): 
     /*fprintf(stderr, "HRESULT_FROM_WIN32(ERROR_TIMEOUT)\n");*/
     toerrors++;
     break; 
    default: 
     fprintf(stderr, "default\n");
     break;  
   }  
   continue;
  }
  hr = pVector->Update(pCamera, pFrame);
  HRCHECK("Vector->Update");
  
  hr = pVector->get_X(&vx);
  HRCHECK("Vector->get_X");
  
  hr = pVector->get_Y(&vy);
  HRCHECK("Vector->get_Y");  
  
  hr = pVector->get_Z(&vz);
  HRCHECK("Vector->get_Z"); 
  
  hr = pVector->get_Yaw(&vyaw);
  HRCHECK("Vector->get_Yaw");
  
  hr = pVector->get_Pitch(&vpitch);
  HRCHECK("Vector->get_Pitch");
  
  hr = pVector->get_Roll(&vroll); 
  HRCHECK("Vector->get_Roll"); 
  
  printf("x=%.3f  y=%.3f  z=%.3f   yaw=%.3f  pitch=%.3f  roll=%.3f  \r",
   vx.dblVal, vy.dblVal, vz.dblVal, vyaw.dblVal, vpitch.dblVal, vroll.dblVal);
  
  hr = pFrame->Free();
 }
 printf("\n\n%d timeouts\n", toerrors);
 printf("\nFin\n");  // shut everything down 
 
 pCamera->SetLED(NP_LED_ONE, VARIANT_FALSE);
 pCamera->Stop();
 pCamera->Close();
 CoUninitialize();
 return 0;
} 

posted by whashimo  # 9:52 午後 0 comments