vcs.maemo.org Git - opencv/blob - otherlibs/PTGrey/PTGrey.cpp

   1 // PTGrey.cpp : Defines the entry point for the DLL application.
   2 //
   3
   4 #include "windows.h"
   5
   6 #include "PGControl.h"
   7 #include <time.h>
   8 #include <stdio.h>
   9 #include <string.h>
  10 #include <stdlib.h>
  11 #include <math.h>
  12 #include <assert.h>
  13 #include "ipl.h"
  14 #include "cv.h"
  15 extern "C"
  16 {
  17 #include "highgui.h"
  18 }
  19
  20 // PGR include files
  21 #include "Digiclops.h"
  22 #include "Triclops.h"
  23 #if ( DIGICLOPS_VERSION > 200 )
  24 #include "pgrgui.h"
  25 #endif
  26
  27
  28 #define PTGREYAPI __declspec(dllexport)
  29 #include "PTGrey.h"
  30
  31 PTGREYAPI const char* GetErrorMessage();
  32
  33 HANDLE PTGreyProcessorThread;
  34
  35 DigiclopsContext digiclops              = NULL;
  36 TriclopsContext tcontext                = NULL;
  37 #if ( DIGICLOPS_VERSION > 200 )
  38 PgrGuiContext   cameraControlContext    = NULL;
  39 #endif
  40
  41 PGClient *PGC;
  42 PGServer *PGS;
  43 char *PGreyData, *PGData;
  44 int SmootherStart;
  45 TriclopsColorImage t24Image;
  46 TriclopsImage t8Image;
  47 TriclopsInput TriclopsIn[4];
  48 TriclopsError tcError;
  49 DigiclopsError e;
  50 TriclopsImage16 tImage16;
  51 TriclopsInput* TriclopsBuffer = 0;
  52 int PTFrameCount = 0, PTPrevFrameCount = 0, PTFreeFrameCount = 0, PTFrameNumber;
  53 clock_t* tickCount;
  54 int PointCount = 0;
  55 static int RawResolution = 0, StereoResolution = 0;
  56
  57 #define _PT_MAX_THREADS 10
  58 int threadIds[_PT_MAX_THREADS] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
  59 int threadRefCount[_PT_MAX_THREADS] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
  60 int activeThreadCount = 0;
  61
  62 HANDLE PTGreyDataMutex = 0;
  63 HANDLE PTGreyFrameMutex = 0;
  64
  65 static char PTGreyErrorString[9128];
  66
  67 static IplImage* temp;
  68 static IplImage* temp8;
  69
  70 const char* config_filename = 0;
  71
  72
  73 void InitPGClient();
  74 void copyTriclopsInput(TriclopsInput tInput, TriclopsInput* tOutput);
  75 void WriteDisparityFile(TriclopsImage16* depthImage16, const char* filename);
  76 void WritePointFile(TriclopsImage16 *image, char *filename);
  77 void WriteColorPointFile(TriclopsImage16 *image, TriclopsColorImage *colorImage, char *filename);
  78 BOOL PTRectifyInputDisparity();
  79
  80 PTGREYAPI const char* PTGreyGetErrorMessage()
  81 {
  82     return PTGreyErrorString;
  83 }
  84 PTGREYAPI void ClearErrorMessage()
  85 {
  86     PTGreyErrorString[0] = 0;
  87 }
  88
  89 static void SetErrorMessage(const char* str, ...)
  90 {
  91     va_list valist;
  92     va_start(valist, str);
  93     vsprintf(PTGreyErrorString, str, valist);
  94 }
  95
  96 DWORD __stdcall CameraProcessor(void*);
  97
  98 /*BOOL APIENTRY DllMain( HANDLE hModule,
  99                        DWORD  ul_reason_for_call,
 100                        LPVOID lpReserved
 101                                          )
 102 {
 103     return TRUE;
 104 }*/
 105
 106 void SetParameters () {
 107         if(triclopsSetDisparity(tcontext, PGC->MinDisparity, PGC->MaxDisparity))
 108         SetErrorMessage("triclopsSetDisparity setting error.");
 109         if (triclopsSetTextureValidation(tcontext, PGC->TextureValidation != 0))
 110         SetErrorMessage("triclopsSetTextureValidation setting error.");
 111         if(triclopsSetTextureValidationThreshold(tcontext, PGC->TextureValidationThreshold))
 112         SetErrorMessage("triclopsSetTextureValidationThreshold setting error. \n");
 113         if(triclopsSetTextureValidationMapping(tcontext, PGC->TextureValidationMapping))
 114         SetErrorMessage("triclopsSetTextureValidationMapping setting error. \n");
 115
 116         if(triclopsSetUniquenessValidation(tcontext, PGC->UniquenessValidation != 0))
 117         SetErrorMessage("triclopsSetUniquenessValidation setting error. \n");
 118         if(triclopsSetUniquenessValidationThreshold(tcontext, PGC->UniquenessValidationThreshold))
 119         SetErrorMessage("triclopsSetUniquenessValidationThreshold setting error. \n");
 120         if(triclopsSetUniquenessValidationMapping(tcontext, PGC->UniquenessValidationMapping))
 121         SetErrorMessage("triclopsSetUniquenessValidationMapping setting error. \n");
 122
 123         if(triclopsSetStereoMask(tcontext, PGC->StereoMask))
 124         SetErrorMessage("triclopsSetStereoMask setting error. \n");
 125         if(triclopsSetEdgeMask(tcontext, PGC->EdgeMask))
 126         SetErrorMessage("triclopsSetEdgeMask setting error. \n");
 127
 128         if(triclopsSetEdgeCorrelation(tcontext, PGC->EdgeCorrelation != 0))
 129         SetErrorMessage("triclopsSetEdgeCorrelation setting error. \n");
 130         if (triclopsSetSubpixelInterpolation(tcontext, PGC->SubPixelInterpolation != 0))
 131         SetErrorMessage("triclopsSetSubpixelInterpolation setting error. \n");
 132         if(triclopsSetStrictSubpixelValidation(tcontext, PGC->StrictSubPixelValidation != 0))
 133         SetErrorMessage("triclopsSetStrictSubpixelValidation setting error. \n");
 134
 135         if(triclopsSetRectify(tcontext, PGC->Rectify != 0))
 136         SetErrorMessage("triclopsSetRectify setting error. \n");
 137         if(triclopsSetLowpass( tcontext, PGC->LowPass != 0))
 138         SetErrorMessage("triclopsSetLowpass setting error. \n");
 139         if(triclopsSetDoStereo( tcontext, PGC->DoStereo != 0))
 140         SetErrorMessage("triclopsSetDoStereo setting error. \n");
 141         if(triclopsSetDebug(tcontext, PGC->PGDebug != 0))
 142         SetErrorMessage("triclopsSetDebug( setting error. \n");
 143 }
 144
 145 PTGREYAPI BOOL PTGreyInitCamera()
 146 {
 147     HANDLE hFileMappingObject;
 148     DigiclopsError digiclopsError;
 149     TriclopsError triclopsError;
 150
 151     // Clear error string
 152     ClearErrorMessage();
 153
 154         // allocate the Point Grey camera shared memory.
 155         hFileMappingObject = CreateFileMapping(HANDLE(-1), 0, PAGE_READWRITE, 0, 0xa000000, "PointGreySharedMemory");
 156     int e = GetLastError();
 157         PGreyData = (char*) MapViewOfFile(hFileMappingObject, FILE_MAP_ALL_ACCESS, 0, 0, 0);
 158         PGC = (PGClient *) PGreyData;                                           // map server control structure in our shared buffer.
 159         PGS = (PGServer *) ((int)PGreyData + sizeof(PGClient));         // map client control structure in our shared buffer.
 160
 161     PGS->BackGroundAvailable = 0;               //
 162         PGS->Initializing = true;
 163         PGData = (char *) ((int)PGreyData + sizeof(PGClient) + sizeof(PGServer) + (32 - sizeof(PGClient) % 32));           // aligned buffer
 164         PGS->DigiclopsBase = (char *) PGreyData;                        // allow vb code to translate our buffer addresses
 165
 166         // create the Digiclops context
 167     if(digiclopsError = digiclopsCreateContext(&digiclops))
 168     {
 169         SetErrorMessage ("Error - digiclopsCreateContext: %s", digiclopsErrorToString(digiclopsError));
 170         return FALSE;
 171     }
 172
 173 #if ( DIGICLOPS_VERSION < 200 )
 174     // connect to the camera - note this assumes that the Digiclops is device 0
 175     // on the bus
 176     if(digiclopsError = digiclopsInitialize(digiclops, 0))
 177     {
 178         SetErrorMessage ("Error - digiclopsInitialize: %s", digiclopsErrorToString(digiclopsError));
 179         return FALSE;
 180     }
 181 #else
 182     // connect to the camera -
 183     // this call pops up a dialog that allows the user to select the
 184     // Digiclops from a list on the bus
 185     PgrGuiContext pgrGuiContext;
 186     PgrGuiError pgrGuiError = pgrguiCreateContext( &pgrGuiContext,
 187                                                    digiclops,
 188                                                    DIGICLOPS_INITIALIZE_DIALOG );
 189     if ( pgrGuiError != PGRGUI_ok )
 190     {
 191         SetErrorMessage ("Error - PGR Gui Create context: unknown error" );
 192         pgrguiDestroyContext( pgrGuiContext );
 193         return FALSE;
 194     }
 195     bool bResult;
 196     pgrguiShowModal( pgrGuiContext, &bResult );
 197     if ( !bResult )
 198     {
 199         SetErrorMessage ("Error - Select Digiclops: cancelled by user" );
 200         pgrguiDestroyContext( pgrGuiContext );
 201         return FALSE;
 202     }
 203     pgrguiDestroyContext( pgrGuiContext );
 204 #endif
 205
 206     /* Load the configuration file for the PointGrey camera */
 207 #if ( DIGICLOPS_VERSION < 200 )
 208     if(config_filename == 0 ||
 209                 (triclopsError = triclopsGetDefaultContextFromFile(&tcontext, const_cast<char*>(config_filename))))
 210     {
 211         SetErrorMessage ("Error - triclopsGetDefaultContextFromFile: %s", triclopsErrorToString(triclopsError));
 212         return FALSE;
 213     }
 214 #else
 215     // if the config file name is null, try getting the configuration from the
 216     // camera
 217     if( config_filename == 0 )
 218     {
 219         if ( digiclopsGetTriclopsContextFromCamera( digiclops, &tcontext) )
 220         {
 221             SetErrorMessage ("Error - digiclopsGetTriclopsContextFromCamera: " );
 222             return FALSE;
 223         }
 224     }
 225     // try to read the configuration from a file
 226     else
 227     {
 228         if ( (triclopsError = triclopsGetDefaultContextFromFile(&tcontext, const_cast<char*>(config_filename))))
 229         {
 230             SetErrorMessage ("Error - triclopsGetDefaultContextFromFile: %s", triclopsErrorToString(triclopsError));
 231             return FALSE;
 232         }
 233     }
 234 #endif
 235
 236         if(digiclopsError = digiclopsStart(digiclops))
 237     {
 238         SetErrorMessage ("Error - digiclopsStart: %s", digiclopsErrorToString(digiclopsError));
 239         return FALSE;
 240     }
 241
 242 #if ( DIGICLOPS_VERSION < 200 )
 243         // this call is obsolete for v2.0+ as the system automatically detects
 244         // the camera type
 245         if(digiclopsError = digiclopsSetCameraType(digiclops, DIGICLOPS_COLOR))
 246     {
 247         SetErrorMessage ("Error - digiclopsSetCameraType: %s", digiclopsErrorToString(digiclopsError));
 248         return FALSE;
 249     }
 250 #endif
 251
 252         if(digiclopsError = digiclopsSetImageTypes(digiclops, ALL_IMAGES))
 253     {
 254         SetErrorMessage ("Error - digiclopsSetImageTypes: %s", digiclopsErrorToString(digiclopsError));
 255         return FALSE;
 256     }
 257
 258     // Initialize settings...
 259     InitPGClient();
 260
 261         // Reallocate everything based on new rows and cols (if they changed).
 262     PGS->OutputBuffers[0] = PGData;
 263     for(int i = 1; i < PG_IMAGE_MAX; i++)
 264     {
 265         PGS->OutputBuffers[i] = PGS->OutputBuffers[i - 1] + PGC->MaxCols*PGC->MaxRows*PTGreyGetPixelSize(i - 1);
 266     }
 267         for(i = 0; i < PG_IMAGE_MAX; i++)
 268         {
 269                 PGS->DataBuffers[i] = new char[PGC->MaxCols*PGC->MaxRows*PTGreyGetPixelSize(i)];
 270         }
 271
 272         if (PGC->MaxCols*PGC->MaxRows*31 >= PGC->Total_Memory)
 273     {
 274         SetErrorMessage("Insufficient buffer allocated.  Len = 0x%x\n", PGC->ncols*PGC->nrows*42 ); Sleep (4000); return 1;
 275     };
 276         PGS->FrameCount = 0;
 277
 278         PGS->Initializing = true;
 279         PGC->NewParameters = 0;         // reset for the next time.
 280
 281     memset(PGS->NewData, 0, PG_IMAGE_MAX*sizeof(int));
 282
 283 #if ( DIGICLOPS_VERSION < 200 )
 284         if (PGC->nrows == 640) // Only 2 choices for raw input format.
 285     {
 286                 if(digiclopsError = digiclopsSetImageResolution(digiclops, (DigiclopsImageResolution) DIGICLOPS_640x480))
 287         {
 288             SetErrorMessage ("Error - digiclopsSetImageResolution: %s", digiclopsErrorToString(digiclopsError));
 289             return FALSE;
 290         }
 291         }
 292     else
 293     {
 294                 if(digiclopsError = digiclopsSetImageResolution(digiclops, (DigiclopsImageResolution) DIGICLOPS_320x240))
 295         {
 296             SetErrorMessage ("Error - digiclopsSetImageResolution: %s", digiclopsError);
 297             return FALSE;
 298         }
 299         }
 300 #else
 301     if (PGC->nrows > 320) // Only 2 choices for raw input format.
 302     {
 303         if(digiclopsError = digiclopsSetImageResolution(digiclops, DIGICLOPS_FULL))
 304         {
 305             SetErrorMessage ("Error - digiclopsSetImageResolution: %s", digiclopsErrorToString(digiclopsError));
 306             return FALSE;
 307         }
 308     }
 309     else
 310     {
 311         if(digiclopsError = digiclopsSetImageResolution(digiclops, DIGICLOPS_HALF))
 312         {
 313             SetErrorMessage ("Error - digiclopsSetImageResolution: %s", digiclopsError);
 314             return FALSE;
 315         }
 316     }
 317 #endif
 318
 319         if(triclopsError = triclopsSetResolution(tcontext, PGC->DRows, PGC->DCols))
 320     {
 321         SetErrorMessage ("Error - triclopsSetResolution: %s", triclopsErrorToString(triclopsError));
 322         return FALSE;
 323     }
 324
 325         PGS->Initializing = false;
 326
 327         SetParameters ();
 328
 329         if(triclopsError = triclopsSetImageBuffer(tcontext, (unsigned char *) PGS->DataBuffers[PT_EDGE_IMAGE],
 330         TriImg_EDGE, TriCam_REFERENCE))
 331     {
 332         SetErrorMessage ("Error - triclopsSetImageBuffer: %s", triclopsErrorToString(triclopsError));
 333         return FALSE;
 334     }
 335
 336         if(triclopsSetResolutionAndPrepare(tcontext, PGC->DRows, PGC->DCols, PGC->nrows, PGC->ncols))
 337     {
 338         SetErrorMessage ("Error - triclopsSetResolutionAndPrepare: %s", triclopsErrorToString(triclopsError));   // for 16-bit disparity buffer
 339     }
 340
 341
 342     triclopsSetSurfaceValidationSize(tcontext, 1000);
 343     triclopsSetSurfaceValidationMapping(tcontext, 0xff);
 344     triclopsSetSurfaceValidation(tcontext, 1);
 345
 346     temp = cvCreateImage(cvSize(PGC->DCols, PGC->DRows), IPL_DEPTH_16U, 1);
 347     temp8 = cvCreateImage(cvSize(PGC->DCols, PGC->DRows), IPL_DEPTH_8U, 1);
 348     iplSet(temp8, PGC->MinDisparity);
 349
 350     // must supply threadId for people running Win95/98
 351     DWORD threadId;
 352     PTGreyProcessorThread = CreateThread(0, 0, LPTHREAD_START_ROUTINE(CameraProcessor), 0, 0, &threadId);
 353     if ( PTGreyProcessorThread == 0 )
 354     {
 355         DWORD errorId = GetLastError();
 356         SetErrorMessage( "Fatal error : CreateThread failed : Error = %d", errorId );
 357         return FALSE;
 358     }
 359     PTGreyDataMutex = CreateMutex(0, FALSE, "PTGreyDataMutex");
 360
 361     return TRUE;
 362 }
 363
 364 static void _LoadRawImage (int ImageIndex)
 365 {
 366     assert(ImageIndex >= 0 && ImageIndex < 4);
 367
 368     DigiclopsError digiclopsError;
 369     TriclopsInput& TCIn = TriclopsIn[ImageIndex];
 370
 371     int ImageType = PG_DIGICLOPS_IMAGE(ImageIndex);
 372
 373     if(digiclopsError = digiclopsExtractTriclopsInput(digiclops, ImageType, &TCIn))
 374     {
 375         SetErrorMessage("Error - digiclopsExtractTriclopsInput: %s", digiclopsErrorToString(digiclopsError));
 376     }
 377
 378         memcpy (PGS->DataBuffers[ImageIndex], (unsigned char *) TCIn.u.rgb32BitPacked.data, TCIn.ncols*TCIn.nrows*4);      // 4 bytes per pixel
 379 }
 380
 381 static void LoadRawImage (int ImageIndex) // used to copy raw images from triclops area to shared memory.
 382 {
 383         if(!PGC->ImageList[ImageIndex])
 384     {
 385         return;
 386     }
 387
 388     _LoadRawImage(ImageIndex);
 389 }
 390
 391 static void LoadComputedImage(int ImageIndex)           // used to get Rectified or Edge images.
 392 {
 393     DigiclopsError digiclopsError;
 394     TriclopsError triclopsError;
 395         if(!PGC->ImageList[ImageIndex])
 396     {
 397         return;
 398     }
 399
 400     if(ImageIndex == PT_EDGE_IMAGE)
 401     {
 402         if (triclopsError = triclopsGetImage(tcontext, TriImg_EDGE, TriCam_REFERENCE, &t8Image))
 403         {
 404             SetErrorMessage("Error - triclopsGetImage: %s", triclopsErrorToString(triclopsError));
 405         }
 406         memcpy (PGS->DataBuffers[ImageIndex], (unsigned char *) t8Image.data, PGC->DCols*PGC->DRows);
 407     }
 408     else if(ImageIndex == PT_RECTIFIED_IMAGE)
 409     {
 410         PTGreyImageType raw = PT_RIGHT_RAW_IMAGE;
 411         TriclopsInput& TCIn = TriclopsIn[raw];
 412         if(!PGC->ImageList[raw])
 413         {
 414             // Since this view has not been requested, the digiclopsExtractTriclops has
 415             // not been called for it
 416                 if(digiclopsError = digiclopsExtractTriclopsInput(
 417                     digiclops, PG_DIGICLOPS_IMAGE(ImageIndex), &TCIn))
 418                 SetErrorMessage("Error - digiclopsExtractTriclopsInput: %s", digiclopsErrorToString(digiclopsError));
 419         }
 420         if(triclopsError = triclopsPreprocess(tcontext, &TCIn))
 421             SetErrorMessage("Error - triclopsPreprocess: %s", triclopsErrorToString(triclopsError));
 422 #if ( DIGICLOPS_VERSION < 200 )
 423         triclopsRectifyColorImage (tcontext, &TCIn, &t24Image);
 424 #else
 425         triclopsRectifyColorImage (tcontext, TriCam_REFERENCE, &TCIn, &t24Image);
 426 #endif
 427         for (int i = 0, j = 0; i < t24Image.nrows * t24Image.ncols; i++, j += 4)
 428         {
 429                 PGS->DataBuffers[ImageIndex][j + 2] = t24Image.red[i];
 430                 PGS->DataBuffers[ImageIndex][j + 1] = t24Image.green[i];
 431                 PGS->DataBuffers[ImageIndex][j + 0] = t24Image.blue[i];
 432         }
 433     }
 434     else
 435     {
 436         assert(0);
 437     }
 438 }
 439
 440 DWORD __stdcall CameraProcessor(void*)
 441 {
 442         HANDLE hThread;
 443         hThread = GetCurrentThread();
 444 //      int e = SetThreadAffinityMask(hThread, 1);      // just the first processor please.
 445         CloseHandle (hThread);
 446
 447     // Initialize PGS
 448     PGS->BackGroundAvailable = 0;
 449
 450     while( 1 )
 451     {
 452         DigiclopsError digiclopsError;
 453         TriclopsError triclopsError;
 454
 455                 if (PGC->NewParameters)
 456         {               // New settings are present for the camera.
 457                         SetParameters ();
 458                 }
 459
 460         if(PGC->PGTerminate)
 461             return 0;   // Requested termination.
 462
 463                 if(digiclopsError = digiclopsGrabImage(digiclops))
 464         {
 465             SetErrorMessage("Error - digiclopsGrabImage: %s", digiclopsErrorToString(digiclopsError));
 466             return -1;
 467         }
 468
 469                 LoadRawImage(PT_TOP_RAW_IMAGE);         // Look for any raw images.
 470                 LoadRawImage(PT_LEFT_RAW_IMAGE);
 471                 LoadRawImage(PT_RIGHT_RAW_IMAGE);
 472
 473         if(PTFreeFrameCount)
 474         {
 475             // Require memory deallocation:
 476             for(int i = 0; i < PTFreeFrameCount*4; i++)
 477             {
 478                 switch(TriclopsBuffer[i].inputType)
 479                 {
 480                 case TriInp_RGB:
 481                     free(TriclopsBuffer[i].u.rgb.red);
 482                     free(TriclopsBuffer[i].u.rgb.green);
 483                     free(TriclopsBuffer[i].u.rgb.blue);
 484                     break;
 485                 case TriInp_RGB_32BIT_PACKED:
 486                     free(TriclopsBuffer[i].u.rgb32BitPacked.data);
 487                     break;
 488                 }
 489             }
 490
 491             delete TriclopsBuffer;
 492             TriclopsBuffer = 0;
 493             PTFreeFrameCount = 0;
 494             PTPrevFrameCount = PTFrameCount; // Initialize pre-frame-count here if frame count = 0
 495         }
 496
 497         if(PTFrameCount != 0)
 498         {
 499             // Loading raw images
 500             if(!PGC->ImageList[0])
 501                 if(digiclopsError = digiclopsExtractTriclopsInput(digiclops, TOP_IMAGE, &TriclopsIn[0]))
 502                     SetErrorMessage("Error - digiclopsExtractTriclopsInput: %s",
 503                         digiclopsErrorToString(digiclopsError));
 504             if(!PGC->ImageList[1])
 505                 if(digiclopsError = digiclopsExtractTriclopsInput(digiclops, TOP_IMAGE, &TriclopsIn[1]))
 506                     SetErrorMessage("Error - digiclopsExtractTriclopsInput: %s",
 507                         digiclopsErrorToString(digiclopsError));
 508             if(!PGC->ImageList[2])
 509                 if(digiclopsError = digiclopsExtractTriclopsInput(digiclops, TOP_IMAGE, &TriclopsIn[2]))
 510                     SetErrorMessage("Error - digiclopsExtractTriclopsInput: %s",
 511                         digiclopsErrorToString(digiclopsError));
 512                         if(digiclopsError = digiclopsExtractTriclopsInput(digiclops, STEREO_IMAGE, &TriclopsIn[3]))
 513                 SetErrorMessage("Error - digiclopsExtractTriclopsInput: %s", digiclopsErrorToString(digiclopsError));
 514
 515             if(PTFrameNumber == 0)
 516             {
 517                 // This is the first time, let's allocate the memory:
 518                 TriclopsBuffer = new TriclopsInput[PTFrameCount*4];
 519                 memset(TriclopsBuffer, 0, PTFrameCount*4*sizeof(TriclopsInput));
 520             }
 521
 522             // Saving images
 523             for(int i = 0; i < 4; i++)
 524             {
 525                 copyTriclopsInput(TriclopsIn[i], &TriclopsBuffer[4*PTFrameNumber + i]);
 526             }
 527             PTFrameNumber++;
 528             if(PTFrameNumber == PTFrameCount)
 529             {
 530                 // Stop saving frames...
 531                 PTPrevFrameCount = PTFrameCount;
 532                 PTFrameNumber = 0;
 533                 PTFrameCount = 0;
 534             }
 535         }
 536
 537                 if (PGC->ComputedView != 0 )
 538         {
 539             TriclopsInput& TCIn = TriclopsIn[3]; // For stereo image
 540                         // If any of the computed views have been requested, then fill it in.
 541
 542             if(!TriclopsBuffer)
 543             {
 544                 // The stereo image has not been extracted yet...
 545                             if(digiclopsError = digiclopsExtractTriclopsInput(digiclops, STEREO_IMAGE, &TCIn))
 546                     SetErrorMessage("Error - digiclopsExtractTriclopsInput: %s", digiclopsErrorToString(digiclopsError));
 547             }
 548                         if(triclopsError = triclopsPreprocess(tcontext, &TCIn))
 549                 SetErrorMessage("Error - triclopsPreprocess: %s", triclopsErrorToString(triclopsError));
 550                         if(triclopsError = triclopsStereo(tcontext))
 551                 SetErrorMessage("Error - triclopsStereo: %s", triclopsErrorToString(triclopsError));
 552
 553                         // The disparity, point cloud and background views are interrelated.  All need the image16 buffer.
 554                         if(triclopsError = triclopsGetImage16(tcontext, TriImg16_DISPARITY, TriCam_REFERENCE, &tImage16))
 555                 SetErrorMessage("Error - triclopsGetImage16: %s", triclopsErrorToString(triclopsError));
 556
 557                         if (PGC->ImageList[PT_DISPARITY_IMAGE] != 0 )
 558             {   // Disparity image
 559 /*
 560                                 for (int i=0 ;i<PGC->DRows*PGC->DCols; i++)
 561                 {
 562                                         int HighByte = tImage16.data[i];
 563                                         PGS->DataBuffers[PT_DISPARITY_IMAGE][i] = (tImage16.data[i] < 0xff00) ? (char) (HighByte >> 8) : (char) PGC->MinDisparity;
 564                                 }
 565 */
 566
 567                                 IplImage* temp2 = cvCreateImageHeader(cvSize(PGC->DCols, PGC->DRows), IPL_DEPTH_8U, 1);
 568                                 cvSetImageData(temp2, (void*)PGS->DataBuffers[PT_DISPARITY_IMAGE], PGC->DCols);
 569
 570                                 IplImage* temp1 = cvCreateImageHeader(cvSize(PGC->DCols, PGC->DRows), IPL_DEPTH_16U, 1);
 571                                 cvSetImageData(temp1, tImage16.data, tImage16.rowinc);
 572                                 IplImage* mask = iplCreateImageHeader(
 573                                         1, 0, IPL_DEPTH_1U, "GRAY", "GRAY",
 574                                         IPL_DATA_ORDER_PIXEL, IPL_ORIGIN_TL,
 575                                         IPL_ALIGN_DWORD, PGC->DCols, PGC->DRows, NULL, NULL,
 576                                         NULL, NULL);
 577                                 iplAllocateImage(mask, 0, 0);
 578
 579                                 iplCopy(temp1, temp);
 580                                 iplRShiftS(temp, temp, 8);
 581                                 iplConvert(temp, temp2);
 582                                 iplThreshold(temp, mask, 0x0ff0);
 583                                 cvThreshold(temp2, temp2, 0xf0, 0, CV_THRESH_TOZERO_INV);
 584                                 temp8->maskROI = mask;
 585                                 temp2->maskROI = mask;
 586                                 iplOr(temp8, temp2, temp2);
 587
 588                                 temp->maskROI = 0;
 589                                 iplDeallocate(mask, IPL_IMAGE_ALL);
 590                                 cvReleaseImageHeader(&temp1);
 591                                 cvReleaseImageHeader(&temp2);
 592
 593                         }
 594
 595                         if (PGC->ImageList[PT_DISPARITY_OBJ_IMAGE] != 0 )
 596             {   // Disparity image
 597                 memcpy(PGS->DataBuffers[PT_DISPARITY_OBJ_IMAGE],
 598                     PGS->DataBuffers[PT_DISPARITY_IMAGE],
 599                     PGC->DRows*PGC->DCols*PTGreyGetPixelSize(PT_DISPARITY_IMAGE));
 600                         }
 601
 602             // Get the edge image
 603                         LoadComputedImage(PT_EDGE_IMAGE);
 604
 605             // If they have requested a color rectified image or rectified image, provide that as well.
 606             LoadComputedImage(PT_RECTIFIED_IMAGE);
 607
 608             if (PGC->ImageList[PT_BACKGROUND_IMAGE] != 0)
 609             {
 610                                 memcpy (PGS->DataBuffers[PT_BACKGROUND_IMAGE], (unsigned char *) tImage16.data, PGC->DRows*PGC->DCols*2);
 611                                 PGS->BackGroundAvailable = 1;   // Unavailable until we are done averaging.
 612                 PGC->ImageList[PT_BACKGROUND_IMAGE] = 0;
 613                         }
 614
 615             PointCount = 0;
 616             if (PGC->ImageList[PT_DISPARITY_OBJ_IMAGE] && PGS->BackGroundAvailable)
 617 // calculate object's disparity
 618             {
 619                                 for (int i=0; i<PGC->DRows*PGC->DCols; i++)
 620                 {
 621                     unsigned char* bkg = (unsigned char*)PGS->DataBuffers[PT_BACKGROUND_IMAGE];
 622                                         PGS->DataBuffers[PT_DISPARITY_OBJ_IMAGE][i] = (char) PGC->MinDisparity;
 623                                         if (tImage16.data[i] < 0xff00)
 624                     {
 625                         // if the point is valid
 626                                                 if ((tImage16.data[i] >> 8) > PGC->DisparityThreshold &&
 627                             (abs(tImage16.data[i] - bkg[i]) >> 8) > PGC->DifferenceThreshold)
 628                         {
 629                                                         PGS->DataBuffers[PT_DISPARITY_OBJ_IMAGE][i] = (tImage16.data[i] >> 8);
 630                                                 }
 631                                         }
 632                                 }
 633
 634 //    disparity image rectification procedure
 635                                 if(PGC->ImageList[/*PT_RIGHT_RAW_IMAGE*/PT_RECTIFIED_IMAGE] !=0 && PGC->RectifiedDisparity != 0)
 636                                 {
 637 //                                      PTRectifyInputDisparity();
 638                                 }
 639
 640                 if(PGC->ImageList[PT_POINTCLOUD_IMAGE])
 641                 {
 642                                 int Cnt = 0;
 643                                 xyzTuple* OGLCloud = (xyzTuple *) PGS->DataBuffers[PT_POINTCLOUD_IMAGE];
 644                                 for (i=0; i<PGC->DRows*PGC->DCols; i++)
 645                     {
 646                                         if (PGS->DataBuffers[PT_DISPARITY_OBJ_IMAGE][i] != PGC->MinDisparity)
 647                         {
 648                                                 triclopsRCD16ToXYZ      ( tcontext, i/PGC->DCols, i%PGC->DCols, tImage16.data[i], &OGLCloud[Cnt].x, &OGLCloud[Cnt].y, &OGLCloud[Cnt].z);
 649                                                 if (OGLCloud[Cnt].z > 0 && OGLCloud[Cnt].z < PGC->ZCapValue)
 650                             {
 651                                 Cnt++;
 652                                                 }
 653                             else
 654                             { // make the cleaned up image even cleaner.
 655 //                                                      PGS->DataBuffers[PT_DISPARITY_IMAGE][i] = (char) PGC->MinDisparity;
 656                                 PGS->DataBuffers[PT_DISPARITY_OBJ_IMAGE][i] = (char) PGC->MinDisparity;
 657                                                 }
 658                                         }
 659                                 }
 660                     PointCount = Cnt;
 661                 }
 662                         }
 663
 664 //                      WriteDisparityFile(&tImage16, "e:\\users\\vic\\cvl\\matlab\\data\\disparity.bmp");
 665
 666
 667             PGS->ticks = clock();
 668                         PGS->FrameCount++;
 669                 } else {
 670                         PGS->BackGroundAvailable = 0;   // need to recapture background.
 671                         Sleep (1);
 672                 }
 673
 674         PTGreyFreezeData(INFINITE);
 675                 for(int i = 0; i < PG_IMAGE_MAX; i++)
 676                 {
 677                         if(PGC->ImageList[i])
 678                                 memcpy(PGS->OutputBuffers[i], PGS->DataBuffers[i],
 679                                         PTGreyGetWidth(i)*PTGreyGetHeight(i)*PTGreyGetPixelSize(i));
 680                 }
 681         memset(PGS->NewData, 255, PG_IMAGE_MAX*sizeof(int));
 682         PTGreyUnFreezeData();
 683         }
 684
 685     return 0;
 686 }
 687
 688 PTGREYAPI long PTGreyGetDataSize(int dataType)
 689 {
 690         switch(dataType)
 691         {
 692         case PT_TOP_RAW_IMAGE:
 693         case PT_LEFT_RAW_IMAGE:
 694         case PT_RIGHT_RAW_IMAGE:
 695         case PT_OBJ_IMAGE:
 696                 return PGC->nrows*PGC->ncols*3;
 697
 698         case PT_RECTIFIED_IMAGE:
 699         case PT_DISPARITY_IMAGE:
 700         case PT_DISPARITY_OBJ_IMAGE:
 701         case PT_EDGE_IMAGE:
 702                 return PGC->DRows*PGC->DCols;
 703
 704         case PT_BACKGROUND_IMAGE:
 705                 return PGC->DRows*PGC->DCols*2;
 706
 707         case PT_POINTCLOUD_IMAGE:
 708                 return PGC->DCols*PGC->DRows*12;
 709
 710         default:
 711                 assert(0);
 712                 return -1;
 713         }
 714 }
 715
 716 PTGREYAPI BOOL PTGreyGetData(int dataType, void* pData)
 717 {
 718         int _size = PTGreyGetDataSize(dataType);
 719     int width = PTGreyGetWidth(dataType);
 720     int height = PTGreyGetHeight(dataType);
 721     if(_size <= 0)
 722     {
 723         SetErrorMessage("Data type %d not supported.", dataType);
 724         return FALSE;
 725     }
 726
 727     if(!PGC->ImageList[dataType])
 728     {
 729         SetErrorMessage("Data type %d is not selected. Use PTGreySelectDataType.", dataType);
 730         return FALSE;
 731     }
 732
 733         WaitForSingleObject(PTGreyDataMutex, INFINITE);
 734
 735     int i;
 736     switch(dataType)
 737     {
 738     case PT_TOP_RAW_IMAGE:
 739     case PT_LEFT_RAW_IMAGE:
 740     case PT_RIGHT_RAW_IMAGE:
 741         case PT_RECTIFIED_IMAGE:
 742         case PT_OBJ_IMAGE:
 743         for(i = 0; i < width*height; i++)
 744         {
 745             ((char*)pData)[3*i + 2] = PGS->OutputBuffers[dataType][4*i + 2];
 746             ((char*)pData)[3*i + 1] = PGS->OutputBuffers[dataType][4*i + 1];
 747             ((char*)pData)[3*i] = PGS->OutputBuffers[dataType][4*i];
 748         }
 749         break;
 750
 751     case PT_DISPARITY_IMAGE:
 752     case PT_DISPARITY_OBJ_IMAGE:
 753     case PT_EDGE_IMAGE:
 754         memcpy(pData, PGS->OutputBuffers[dataType], _size);
 755         break;
 756
 757     default:
 758         assert(0);
 759         return FALSE;
 760     }
 761
 762     PGS->NewData[dataType] = 0;
 763
 764     ReleaseMutex(PTGreyDataMutex);
 765
 766     return TRUE;
 767 }
 768
 769 PTGREYAPI BOOL PTGreyFreezeData(int milliSec)
 770 {
 771     int wait = WaitForSingleObject(PTGreyDataMutex, milliSec);
 772     if(wait == WAIT_OBJECT_0)
 773     {
 774         return TRUE;
 775     }
 776     else
 777     {
 778         return FALSE;
 779     }
 780 }
 781
 782 PTGREYAPI void PTGreyUnFreezeData()
 783 {
 784     ReleaseMutex(PTGreyDataMutex);
 785 }
 786
 787 PTGREYAPI const char* PTGreyGetDataPointer(int dataType)
 788 {
 789     if(dataType < 0 || dataType > PG_IMAGE_MAX)
 790         return 0;
 791
 792     PGS->NewData[dataType] = 0;
 793     return PGS->OutputBuffers[dataType];
 794 }
 795
 796 PTGREYAPI int PTGreyGetWidth(int dataType)
 797 {
 798     switch(dataType)
 799     {
 800     case PT_TOP_RAW_IMAGE:
 801     case PT_RIGHT_RAW_IMAGE:
 802     case PT_LEFT_RAW_IMAGE:
 803         case PT_OBJ_IMAGE:
 804         return PGC->ncols;
 805
 806         case PT_RECTIFIED_IMAGE:
 807     case PT_DISPARITY_IMAGE:
 808     case PT_DISPARITY_OBJ_IMAGE:
 809     case PT_EDGE_IMAGE:
 810     case PT_POINTCLOUD_IMAGE:
 811         return PGC->DCols;
 812
 813     default:
 814         assert(0);
 815         return -1;
 816     }
 817 }
 818
 819 PTGREYAPI int PTGreyGetHeight(int dataType)
 820 {
 821     switch(dataType)
 822     {
 823     case PT_TOP_RAW_IMAGE:
 824     case PT_RIGHT_RAW_IMAGE:
 825     case PT_LEFT_RAW_IMAGE:
 826         case PT_OBJ_IMAGE:
 827         return PGC->nrows;
 828
 829         case PT_RECTIFIED_IMAGE:
 830     case PT_DISPARITY_IMAGE:
 831     case PT_DISPARITY_OBJ_IMAGE:
 832     case PT_EDGE_IMAGE:
 833     case PT_POINTCLOUD_IMAGE:
 834         return PGC->DRows;
 835
 836     default:
 837         assert(0);
 838         return -1;
 839     }
 840 }
 841
 842 PTGREYAPI void PTGreySelectDataType(int dataType, int option)
 843 {
 844     PTGreyFreezeData(INFINITE);
 845     if(dataType < 0 || dataType > PG_IMAGE_MAX)
 846     {
 847         return;
 848     }
 849
 850     PGC->ImageList[dataType] = option;
 851
 852     PGC->ComputedView = 0;
 853     for(int i = PTGreyImageType(3); i < PG_IMAGE_MAX; i++)
 854     {
 855         PGC->ComputedView = PGC->ComputedView | PGC->ImageList[i];
 856     }
 857
 858     PTGreyUnFreezeData();
 859 }
 860
 861 PTGREYAPI int PTGreyIsNewData(int dataType)
 862 {
 863     if(dataType < 0 || dataType >= PG_IMAGE_MAX)
 864         return FALSE;
 865     return PGS->NewData[dataType];
 866 }
 867
 868 PTGREYAPI long PTGreyGetPixelSize(int dataType)
 869 {
 870     switch(dataType)
 871     {
 872     case PT_TOP_RAW_IMAGE:
 873     case PT_LEFT_RAW_IMAGE:
 874     case PT_RIGHT_RAW_IMAGE:
 875         case PT_RECTIFIED_IMAGE:
 876         case PT_OBJ_IMAGE:
 877         return 4;
 878
 879     case PT_EDGE_IMAGE:
 880     case PT_DISPARITY_IMAGE:
 881     case PT_DISPARITY_OBJ_IMAGE:
 882         return 1;
 883
 884     case PT_BACKGROUND_IMAGE:
 885         return 2;
 886
 887     case PT_POINTCLOUD_IMAGE:
 888         return 12;
 889
 890     default:
 891         assert(0);
 892         return -1;
 893     }
 894 }
 895
 896 PTGREYAPI BOOL PTGreyGetProperty(const char* name, void* buffer)
 897 {
 898     if(strcmp(name, "MinDisparity") == 0)
 899     {
 900         *(int*)buffer = PGC->MinDisparity;
 901     }
 902     else if(strcmp(name, "MaxDisparity") == 0)
 903     {
 904         *(int*)buffer = PGC->MaxDisparity;
 905     }
 906     else if(strcmp(name, "StereoResolution") == 0)
 907     {
 908         *(int*)buffer = PGC->DCols == 640 ? 2 : PGC->DCols == 320 ? 1 : 0;
 909     }
 910     else if(strcmp(name, "RawResolution") == 0)
 911     {
 912         *(int*)buffer = PGC->ncols == 640 ? 1 : 0;
 913     }
 914     else if(strcmp(name, "DifferenceThreshold") == 0)
 915     {
 916         *(float*)buffer = (float)PGC->DifferenceThreshold;
 917     }
 918     else if(strcmp(name, "DisparityThreshold") == 0)
 919     {
 920         *(float*)buffer = (float)PGC->DisparityThreshold;
 921     }
 922     else if(strcmp(name, "DepthThreshold") == 0)
 923     {
 924         *(float*)buffer = PGC->ZCapValue;
 925     }
 926     else if(strcmp(name, "CameraBrightness") == 0)
 927     {
 928         long value;
 929         bool isauto;
 930 #if ( DIGICLOPS_VERSION < 200 )
 931         digiclopsGetVideoProperty(digiclops, DIGICLOPS_BRIGHTNESS, &value, &isauto);
 932 #else
 933         digiclopsGetCameraProperty(digiclops, DIGICLOPS_GAIN, &value, &isauto);
 934 #endif
 935         assert(value >= 0);
 936         *(int*)buffer = isauto ? -value : value;
 937     }
 938     else if(strcmp(name, "CameraIris") == 0)
 939     {
 940         long value;
 941         bool isauto;
 942 #if ( DIGICLOPS_VERSION < 200 )
 943         digiclopsGetVideoProperty(digiclops, DIGICLOPS_IRIS, &value, &isauto);
 944 #else
 945         digiclopsGetCameraProperty(digiclops, DIGICLOPS_SHUTTER, &value, &isauto);
 946 #endif
 947         assert(value >= 0);
 948         *(int*)buffer = isauto ? -value : -value;
 949     }
 950     else if(strcmp(name, "CameraExposure") == 0)
 951     {
 952         long value;
 953         bool isauto;
 954 #if ( DIGICLOPS_VERSION < 200 )
 955         digiclopsGetVideoProperty(digiclops, DIGICLOPS_EXPOSURE, &value, &isauto);
 956 #else
 957         digiclopsGetCameraProperty(digiclops, DIGICLOPS_AUTO_EXPOSURE, &value, &isauto);
 958 #endif
 959         assert(value >= 0);
 960         *(int*)buffer = isauto ? -value : -value;
 961     }
 962     else if(strcmp(name, "") == 0)
 963     {
 964         *(int*)buffer = PGC->RectifiedDisparity;
 965     }
 966     else
 967     {
 968         return FALSE;
 969     }
 970
 971     return TRUE;
 972 }
 973
 974 PTGREYAPI BOOL PTGreySetProperty(const char* name, void* buffer)
 975 {
 976     BOOL ret = TRUE;
 977     PTGreyFreezeData(INFINITE);
 978     if(strcmp(name, "MinDisparity") == 0)
 979     {
 980         PGC->MinDisparity = *(int*)buffer;
 981     }
 982     else if(strcmp(name, "MaxDisparity") == 0)
 983     {
 984         PGC->MaxDisparity = *(int*)buffer;
 985     }
 986     else if(strcmp(name, "StereoResolution") == 0)
 987     {
 988         StereoResolution = *(int*)buffer;
 989     }
 990     else if(strcmp(name, "RawResolution") == 0)
 991     {
 992         RawResolution = *(int*)buffer;
 993     }
 994     else if(strcmp(name, "DifferenceThreshold") == 0)
 995     {
 996         PGC->DifferenceThreshold = (int)*(float*)buffer;
 997     }
 998     else if(strcmp(name, "DisparityThreshold") == 0)
 999     {
1000         PGC->DisparityThreshold = (int)*(float*)buffer;
1001     }
1002     else if(strcmp(name, "DepthThreshold") == 0)
1003     {
1004         PGC->ZCapValue = *(float*)buffer;
1005     }
1006     else if(strcmp(name, "CameraBrightness") == 0)
1007     {
1008         int input = *(int*)buffer;
1009         long value = abs(input);
1010         bool isauto = !!(input & 0x80000000);
1011 #if ( DIGICLOPS_VERSION < 200 )
1012         digiclopsSetVideoPropertyAuto(digiclops, DIGICLOPS_BRIGHTNESS, isauto);
1013         digiclopsSetVideoProperty(digiclops, DIGICLOPS_BRIGHTNESS, value);
1014 #else
1015         digiclopsSetCameraProperty(digiclops, DIGICLOPS_GAIN, value, isauto);
1016 #endif
1017     }
1018     else if(strcmp(name, "CameraIris") == 0)
1019     {
1020         int input = *(int*)buffer;
1021         long value = abs(input);
1022         bool isauto = !!(input & 0x80000000);
1023 #if ( DIGICLOPS_VERSION < 200 )
1024         digiclopsSetVideoPropertyAuto(digiclops, DIGICLOPS_IRIS, isauto);
1025         digiclopsSetVideoProperty(digiclops, DIGICLOPS_IRIS, value);
1026 #else
1027         digiclopsSetCameraProperty(digiclops, DIGICLOPS_SHUTTER, value, isauto);
1028 #endif
1029     }
1030     else if(strcmp(name, "CameraExposure") == 0)
1031     {
1032         int input = *(int*)buffer;
1033         long value = abs(input);
1034         bool isauto = !!(input & 0x80000000);
1035 #if ( DIGICLOPS_VERSION < 200 )
1036         digiclopsSetVideoPropertyAuto(digiclops, DIGICLOPS_EXPOSURE, isauto);
1037         digiclopsSetVideoProperty(digiclops, DIGICLOPS_EXPOSURE, value);
1038 #else
1039         digiclopsSetCameraProperty(digiclops, DIGICLOPS_AUTO_EXPOSURE, value, isauto);
1040 #endif
1041     }
1042     else if(strcmp(name, "") == 0)
1043     {
1044         PGC->RectifiedDisparity = *(int*)buffer;
1045     }
1046
1047     PTGreyUnFreezeData();
1048     return ret;
1049 }
1050
1051 void InitPGClient()
1052 {
1053     PGC->Total_Memory = 0xa000000;
1054     PGC->MaxCols = 640;
1055     PGC->MaxRows = 480;
1056     switch(RawResolution)
1057     {
1058     case 0:
1059         PGC->ncols = 320;
1060         PGC->nrows = 240;
1061         break;
1062     case 1:
1063         PGC->ncols = 640;
1064         PGC->nrows = 480;
1065         break;
1066     default:
1067         assert(0);
1068         break;
1069     }
1070
1071     switch(StereoResolution)
1072     {
1073     case 0:
1074         PGC->DCols = 160;
1075         PGC->DRows = 120;
1076         break;
1077     case 1:
1078         PGC->DCols = 320;
1079         PGC->DRows = 240;
1080         break;
1081     case 2:
1082         PGC->DCols = 640;
1083         PGC->DRows = 480;
1084         break;
1085     default:
1086         assert(0);
1087         break;
1088     }
1089
1090     PGC->MinDisparity = 2;
1091     PGC->MaxDisparity = 80;
1092
1093     PGC->SubPixelInterpolation = 1;
1094     PGC->StrictSubPixelValidation = 1;
1095
1096     PGC->StereoMask = 15;
1097     PGC->Rectify = 1;
1098     PGC->LowPass = 1;
1099     PGC->DoStereo = 1;
1100     PGC->EdgeMask = 11;
1101     PGC->EdgeCorrelation = 1;
1102     PGC->PGDebug = 0;
1103
1104     PGC->UniquenessValidation = 0;
1105     PGC->UniquenessValidationThreshold = 0.8f;
1106     PGC->UniquenessValidationMapping = 129;
1107
1108     PGC->TextureValidation = 0;
1109     PGC->TextureValidationThreshold = 110;
1110     PGC->TextureValidationMapping = 130;
1111
1112     PGC->ZCapValue = 1;
1113     PGC->DifferenceThreshold = 10;
1114     PGC->DisparityThreshold = 10;
1115     PGC->ZBucketMin = 50;
1116
1117         PGC->RectifiedDisparity = 1; // rectification disparity option
1118 }
1119
1120 PTGREYAPI void PTGreyShowPropertyPage()
1121 {
1122 #if (DIGICLOPS_VERSION < 200 )
1123     digiclopsDisplayPropertyPages(digiclops);
1124 #else
1125     PTGreyShowWhiteBalance();
1126 #endif
1127
1128 }
1129
1130 PTGREYAPI void PTGreyShowWhiteBalance()
1131 {
1132 #if (DIGICLOPS_VERSION < 200 )
1133     digiclopsDisplayWhiteBalancePropPages(digiclops);
1134 #else
1135     PgrGuiError pgrGuiError;
1136     if ( cameraControlContext == NULL )
1137     {
1138         pgrGuiError = pgrguiCreateContext( &cameraControlContext,
1139                                            digiclops,
1140                                            DIGICLOPS_CAMERA_SETTINGS_DIALOG );
1141         if ( pgrGuiError != PGRGUI_ok )
1142         {
1143             SetErrorMessage( "Create Camera Control Context failed : %d",
1144                              pgrGuiError );
1145             pgrguiDestroyContext( cameraControlContext );
1146             cameraControlContext = NULL;
1147             return;
1148         }
1149     }
1150
1151     // check window state
1152     BOOL bShowing;
1153     pgrguiGetWindowState( cameraControlContext, &bShowing );
1154     if ( bShowing )
1155     {
1156         // already showing, so lets pop it down
1157         pgrguiHideModeless( cameraControlContext );
1158     }
1159     else
1160     {
1161         pgrguiShowModeless( cameraControlContext, NULL );
1162     }
1163 #endif
1164 }
1165
1166 PTGREYAPI void PTGreySetTriclopsInputBuffer(int frames_count)
1167 {
1168     PTGreyFreezeData(INFINITE);
1169     PTFrameCount = frames_count;
1170     PTFreeFrameCount = PTPrevFrameCount;
1171     PTFrameNumber = 0; /* Start grabbing frames */
1172     PTGreyUnFreezeData();
1173 }
1174
1175 PTGREYAPI int PTGreyGetFrameNumber()
1176 {
1177     return PTFrameCount ? PTFrameNumber : -1;
1178 }
1179
1180 void copyTriclopsInput(TriclopsInput tInput, TriclopsInput* tOutput)
1181 {
1182     memcpy(tOutput, &tInput, sizeof(TriclopsInput));
1183     if(tInput.inputType == TriInp_RGB_32BIT_PACKED)
1184     {
1185         int size = tInput.nrows*tInput.ncols*4;
1186         tOutput->u.rgb32BitPacked.data = malloc(size);
1187         memcpy(tOutput->u.rgb32BitPacked.data, tInput.u.rgb32BitPacked.data, size);
1188     }
1189     else
1190     {
1191         int size = tInput.nrows*tInput.ncols;
1192         tOutput->u.rgb.red = malloc(size);
1193         tOutput->u.rgb.green = malloc(size);
1194         tOutput->u.rgb.blue = malloc(size);
1195         memcpy(tOutput->u.rgb.red, tInput.u.rgb.red, size);
1196         memcpy(tOutput->u.rgb.green, tInput.u.rgb.green, size);
1197         memcpy(tOutput->u.rgb.blue, tInput.u.rgb.blue, size);
1198     }
1199 }
1200
1201 PTGREYAPI void* PTGreyGetTriclopsBuffer()
1202 {
1203     return TriclopsBuffer;
1204 }
1205
1206 PTGREYAPI void PTGreyProcessFrames(void* _buffer, int grabCount, const char* dirName)
1207 {
1208     TriclopsInput* buffer = (TriclopsInput*)_buffer;
1209         // write the data to a file
1210         for (int i = 0; i < grabCount; i++)
1211         {
1212             char filename[_MAX_PATH];
1213 //        TriclopsInput triclopsTopInput = buffer[4*i];
1214 //              TriclopsInput triclopsLeftInput = buffer[4*i + 1];
1215             TriclopsInput triclopsRightInput = buffer[4*i + 2];
1216             TriclopsInput triclopsStereoInput = buffer[4*i+3];
1217
1218             // preprocessing the images
1219             triclopsPreprocess( tcontext, &triclopsStereoInput ) ;
1220
1221             // stereo processing
1222             triclopsStereo( tcontext ) ;
1223
1224             // rectify right color image
1225             sprintf(filename,"%sright_color_rect_%05d.ppm",dirName,i);
1226             printf("writing  image %s\n",filename);
1227             TriclopsColorImage colorImage;
1228 #if ( DIGICLOPS_VERSION < 200 )
1229             triclopsRectifyColorImage( tcontext, &triclopsRightInput, &colorImage ) ;
1230 #else
1231             triclopsRectifyColorImage( tcontext, TriCam_REFERENCE, &triclopsRightInput, &colorImage ) ;
1232 #endif
1233             triclopsSaveColorImage(&colorImage, filename);
1234
1235
1236             // retrieve the interpolated depth image from the tcontext
1237             TriclopsImage16 depthImage16;
1238             triclopsGetImage16( tcontext, TriImg16_DISPARITY, TriCam_REFERENCE, &depthImage16 ) ;
1239
1240         if (PGS->BackGroundAvailable)
1241         {
1242                         for(int i = 0; i < PGC->DRows*PGC->DCols; i++)
1243             {
1244                                 if (depthImage16.data[i] < 0xff00)
1245                 {
1246                     unsigned char* bkg = (unsigned char*)PGS->DataBuffers[PT_BACKGROUND_IMAGE];
1247                     // if the point is valid
1248                                         if ((depthImage16.data[i] >> 8) < PGC->DisparityThreshold ||
1249                         (abs(tImage16.data[i] - bkg[i]) >> 8) < PGC->DifferenceThreshold)
1250                     {
1251                         // Make the pixel invalid...
1252                                                 depthImage16.data[i] = 0xff00;
1253                                         }
1254                                 }
1255                         }
1256         }
1257
1258 //              sprintf(filename, "%sdisparity_%05d.bmp", dirName, i);
1259 //              WriteDisparityFile(&depthImage16, filename);
1260
1261                 sprintf(filename,"%spoints_%05d.txt",dirName,i);
1262                 printf("writing  point file %s\n",filename);
1263                 WritePointFile(&depthImage16,filename);
1264
1265                 sprintf(filename,"%scolor_points_%05d.txt",dirName,i);
1266                 printf("writing  point file %s\n",filename);
1267                 WriteColorPointFile(&depthImage16,&colorImage,filename);
1268         }
1269 }
1270
1271 void WriteDisparityFile(TriclopsImage16* depthImage16, const char* filename)
1272 {
1273         int w = depthImage16->ncols;
1274         int h = depthImage16->nrows;
1275         // Prepare IplImage header
1276         IplImage* image = cvCreateImage(cvSize(w, h), IPL_DEPTH_8U, 1);
1277         for(int j = 0; j < h; j++)
1278         {
1279                 for(int i = 0; i < w; i++)
1280                 {
1281                         int index = j*w + i;
1282                         int disparity = depthImage16->data[j*w + i];
1283                         disparity = disparity < 0xff00 ? disparity >> 8 : PGC->MinDisparity;
1284                         image->imageData[j*image->widthStep + i] = char(disparity);
1285                 }
1286         }
1287
1288         save_iplimage(filename, image);
1289 }
1290
1291
1292 void WritePointFile(TriclopsImage16 *image, char *filename)
1293 {
1294     int                 i,j;
1295         int                                     disparity;
1296     float               x, y, z;
1297         FILE *                          filestream;
1298
1299         filestream = fopen(filename,"w");
1300         FILE* f = fopen("disparity.txt", "w");
1301         assert(filestream);
1302
1303         int pointCount = PGC->nrows*PGC->ncols;
1304         // fprintf(filestream,"%d\n",pointCount);
1305
1306    // determine the number of pixels spacing per row
1307         int pixelinc    = image->rowinc/2;
1308         for ( i = 0; i < image->nrows; i++ )
1309         {
1310                 unsigned short * row = image->data + i * pixelinc;
1311                 for ( j = 0; j < image->ncols; j++ )
1312                 {
1313                         disparity = row[j];
1314
1315                         // filter invalid points
1316                         if ( disparity < 0xFF00 )
1317                         {
1318                                 triclopsRCD16ToXYZ( tcontext, j , i, disparity, &x, &y, &z );
1319                                 if (z > 0 && z < PGC->ZCapValue) fprintf(filestream, "%f %f %f %d %d\n",x, y, z, i, j);
1320 //                              fprintf(f, "%d ", char(disparity >> 8));
1321                         }
1322                         else
1323                                 fprintf(f, "%d ", char(PGC->MinDisparity));
1324                 }
1325                 fprintf(f, "\n");
1326         }
1327
1328
1329         fclose(filestream);
1330         fclose(f);
1331 }
1332
1333 void WriteColorPointFile(TriclopsImage16 *image, TriclopsColorImage *colorImage, char *filename)
1334 {
1335     int                 i,j;
1336         int                                     disparity;
1337     float               x, y, z;
1338         int                                     r, g, b;
1339         FILE *                          filestream;
1340
1341         filestream = fopen(filename,"w");
1342         assert(filestream);
1343
1344         int pointCount = PGC->nrows*PGC->ncols;
1345         // fprintf(filestream,"%d\n",pointCount);
1346
1347         // determine the number of pixels spacing per row
1348         int pixelIndex = 0;
1349         int pixelinc    = image->rowinc/2;
1350         for ( i = 0; i < image->nrows; i++ )
1351         {
1352                 unsigned short * row = image->data + i * pixelinc;
1353                 for ( j = 0; j < image->ncols; j++ )
1354                 {
1355                         disparity = row[j];
1356
1357                         // filter invalid points
1358                         if ( disparity < 0xFF00 )
1359                         {
1360                                 triclopsRCD16ToXYZ( tcontext, i , j, disparity, &x, &y, &z );
1361                                 if (z > 0 && z < PGC->ZCapValue) {
1362                                         r = (int) colorImage->red[pixelIndex];
1363                                         g = (int) colorImage->green[pixelIndex];
1364                                         b = (int) colorImage->blue[pixelIndex];
1365                                         fprintf( filestream, "%d %d %f %f %f %d %d %d\n", x, y, z, r, g, b );
1366                                 }
1367
1368                         }
1369
1370                         // go to the next pixel
1371                         pixelIndex++;
1372                 }
1373         }
1374
1375
1376         fclose(filestream);
1377 }
1378
1379 PTGREYAPI int PTGreyGetPointCount()
1380 {
1381     return PointCount;
1382 }
1383
1384 PTGREYAPI void* PTGreyGetTriclopsImage16()
1385 {
1386     return &tImage16;
1387 }
1388
1389 PTGREYAPI __int64 PTGreyGetTime()
1390 {
1391     return __int64(TriclopsIn[3].timeStamp.sec)*1000 + __int64(TriclopsIn[3].timeStamp.u_sec)/1000;
1392 }
1393
1394 BOOL PTRectifyInputDisparity()
1395 {
1396
1397         IplImage *inputImage, *image_hsv, *img_p[2], *img_v, *image_disp,  *contour_mask;
1398 //    IplImage *resizeImage;
1399         CvSeq *contour, *cont_max;
1400         CvPoint *big_t;
1401         double parameter[2];
1402
1403 //      unsigned char *clean;
1404         int i,j;
1405         int Ncols, Nrows;
1406
1407
1408     CvMemStorage* temp_storage = cvCreateMemStorage( 65536 );
1409     int header_size = sizeof(CvContour);
1410
1411     Ncols = PGC->DCols;
1412         Nrows = PGC->DRows;
1413 //      Ncols = PGC->ncols;
1414 //      Nrows = PGC->nrows;
1415
1416         inputImage = cvCreateImage(cvSize(Ncols, Nrows), IPL_DEPTH_8U, 3);
1417
1418 //      resizeImage = cvCreateImage(cvSize(PGC->DCols, PGC->DRows), IPL_DEPTH_8U, 3);
1419
1420         char* data = inputImage->imageData;
1421         int step = inputImage->widthStep;
1422
1423         for (i = 0; i < PGC->DRows; i++)
1424         {
1425                 for (j = 0; j < PGC->DCols; j++)
1426                 {
1427                         int index = i*step + 3*j;
1428                         int outind = i*PGC->DCols + j;
1429
1430                         data[index] = t24Image.red[outind];
1431                         data[index + 1] = t24Image.green[outind];
1432                         data[index + 2] = t24Image.blue[outind];
1433                 }
1434         }
1435
1436
1437         image_hsv = iplCreateImageHeader(3, 0, IPL_DEPTH_8U, "HSV", "HSV", IPL_DATA_ORDER_PIXEL, IPL_ORIGIN_TL, IPL_ALIGN_QWORD,
1438                                                                          PGC->DCols, PGC->DRows, NULL, NULL, NULL, NULL);
1439         printf("\n Create image_hsv header !!!");
1440         iplAllocateImage(image_hsv, 0, 0 ); // allocate image data
1441
1442         inputImage->roi = 0;
1443 //      resizeImage->roi = 0;
1444
1445 //      m_outputImage = cvCreateImage(cvSize(m_ncols, m_nrows), IPL_DEPTH_8U, 3);
1446 //    m_outputImage->roi = 0;
1447
1448 //      iplResize(inputImage, resizeImage, Ncols, PGC->DCols, Nrows, PGC->DRows, IPL_INTER_NN);
1449
1450         iplRGB2HSV(inputImage, image_hsv);
1451 //      iplRGB2HSV(resizeImage, image_hsv);
1452
1453         img_p[0] = cvCreateImage(cvSize(PGC->DCols, PGC->DRows), IPL_DEPTH_8U, 1);
1454         img_p[1] = cvCreateImage(cvSize(PGC->DCols, PGC->DRows), IPL_DEPTH_8U,1);
1455         img_v    = cvCreateImage(cvSize(PGC->DCols, PGC->DRows), IPL_DEPTH_8U,1);
1456
1457 //      printf("\n Create img_p !!!");
1458
1459         cvCvtPixToPlane(image_hsv, img_p[0], img_p[1], img_v, NULL);
1460
1461
1462         parameter[0] = (double)2;  /* size */
1463         parameter[1] = (double)10; /* minDisp */
1464         cvAdaptiveThreshold( img_p[1], img_v, (double)255, CV_STDDEV_ADAPTIVE_THRESH, CV_THRESH_BINARY, parameter);
1465
1466 //      iplThreshold(img_p[1], img_p[1], 110);
1467         cvFindContours (/*img_p[1]*/img_v, temp_storage, &contour, header_size,
1468                     CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE);
1469
1470 //      float parameter =1.;
1471 //    cvApproxPoly (contour, header_size, temp_storage, &contour_a, (CvPolyApproxMethod)0, parameter );
1472
1473 // find the contour  with maximum disparity
1474     double max_disparity = 0;
1475         cont_max = NULL;
1476         contour_mask = cvCreateImage(cvSize(PGC->DCols, PGC->DRows), IPL_DEPTH_8U, 1);
1477
1478         image_disp = cvCreateImageHeader(cvSize(PGC->DCols, PGC->DRows), IPL_DEPTH_8U, 1);
1479
1480         cvSetImageData( image_disp, (char*) PGS->DataBuffers[PT_DISPARITY_OBJ_IMAGE], PGC->DCols );
1481
1482 //      image_disp1 = cvCreateImage(cvSize(PGC->DCols, PGC->DRows), IPL_DEPTH_8U, 1);
1483
1484 //      iplResize(image_disp, image_disp1, PGC->DCols, PGC->DCols, PGC->DRows, PGC->DRows, IPL_INTER_NN);
1485
1486
1487
1488     while(contour!=NULL)
1489     {
1490         if( contour->total > 30)
1491         {
1492             iplSet(contour_mask, 0/*PGC->MinDisparity*/);
1493                         big_t = (CvPoint*)malloc(contour->total * sizeof(CvPoint));
1494
1495                     cvCvtSeqToArray(contour, big_t );
1496                         cvFillPoly (contour_mask, &big_t, &contour->total, 255, 1 /*PGC->MaxDisparity*/);
1497 //                      m_outputImage->roi->coi = 1;
1498
1499                         iplAnd(image_disp, contour_mask, contour_mask);
1500                         double mean = cvMean(contour_mask);
1501
1502                         if(mean > max_disparity)
1503             {
1504                 max_disparity = mean;
1505                                 cont_max = contour;
1506             }
1507                         free(big_t);
1508         }
1509         contour = contour->h_next;
1510     }
1511
1512         iplSet(contour_mask, 0);
1513         big_t = (CvPoint*)malloc(cont_max->total * sizeof(CvPoint));
1514
1515 //      printf("\n cont_max !!! min_disp = %d max_disparity = %f cont = %d", PGC->MinDisparity, max_disparity, cont_max->total);
1516
1517         cvCvtSeqToArray(cont_max, big_t );
1518         cvFillPoly (contour_mask, &big_t, &cont_max->total, 255, 1);
1519
1520     iplSubtractS(image_disp, image_disp,PGC->MinDisparity, false);
1521         iplAnd(image_disp, contour_mask, image_disp);
1522     iplAddS(image_disp, image_disp, PGC->MinDisparity);
1523
1524         free(big_t);
1525
1526 //      iplResize(image_disp1, image_disp,  PGC->DCols, PGC->DCols, PGC->DRows, PGC->DRows, IPL_INTER_NN);
1527 //      printf("\n Resize !!!");
1528
1529         cvReleaseImage(&img_p[0]);
1530         cvReleaseImage(&img_p[1]);
1531         cvReleaseImage(&img_v);
1532         iplDeallocateImage(image_hsv);
1533         cvReleaseImageHeader(&image_disp);
1534 //      cvReleaseImage(&image_disp1);
1535         cvReleaseImage(&contour_mask);
1536
1537         cvReleaseMemStorage( &temp_storage );
1538
1539     return TRUE;
1540
1541 }
1542
1543 PTGREYAPI void PTGreyExitCamera(int millisec)
1544 {
1545         cvReleaseImage(&temp);
1546
1547 //      for(int i = 0; i < PG_IMAGE_MAX; i++)
1548 //      {
1549 //              delete PGS->DataBuffers[i];
1550 //      }
1551
1552         PGC->PGTerminate = 1;
1553         WaitForSingleObject(PTGreyProcessorThread, millisec);
1554 }
1555
1556 PTGREYAPI void* PTGreyGetTriclopsContext()
1557 {
1558         return tcontext;
1559 }
1560
1561 PTGREYAPI void PTGreySetConfigFileName(const char* config)
1562 {
1563         if(config)
1564         {
1565                 config_filename = strdup(config);
1566         }
1567 }