Работа в D'Fusion Computer Vision, создание сценария отслеживания объекта

#include "pxcsession.h"
#include "pxccapture.h"
#include "pxcsmartptr.h"
#include "util_render.h"
#include "util_capture.h"
#include "util_cmdline.h"

#include "pxcdcvtracker.h" // заголовочный файл находиться в директории PCSDK\contrib\Total Immersion\include

#include "dcvRender.h"

#include <string>
using namespace std;

const pxcCHAR* trackerPath = L"../tracking/tracker.xml"; // путь к файлу сценария

bool computeModuleDirectory(void* iModule, pxcCHAR* oPath, DWORD pathLen)
{
  if (!::GetModuleFileNameW((HMODULE)iModule,oPath,pathLen)) {
    return false;
  }

  pxcCHAR* tmp = wcsrchr(oPath,L'\\');
  if (tmp==0) {
    return false;
  }
  *(tmp+1) = L'\0';
  return true;
}

int wmain(int argc, WCHAR* argv[]) 
{   
    // Сессия
    PXCSmartPtr<PXCSession> session;
    pxcStatus sts=PXCSession_Create(&session);
  if (sts<PXC_STATUS_NO_ERROR) {
    wprintf(L"Невозможно создать сессию\n");
    return 0;
  }

  session->LoadImplFromFile(TEXT("dfusion4PCSDK.dll"));
  {
    PXCSmartPtr<PXCDCVTracker>  tracker;
    PXCDCVTracker::ProfileInfo  pinfo;
    PXCSmartPtr<dcvRender>      renderer;
  
    session->CreateImpl(PXCDCVTracker::CUID,(void**)&tracker);
    if (!tracker)
      return 3;
    UtilCmdLine cmdl(session);
    if (!cmdl.Parse(L"-sdname-nframes",argc,argv)) return 3;

    UtilCapture capture(session);
    if (cmdl.m_sdname) capture.SetFilter(cmdl.m_sdname); /*L"Integrated Camera"*/
    //capture.SetFilter(L"DepthSense Device 325"); 

    tracker->QueryProfile(&pinfo);
    
    sts=capture.LocateStreams(&pinfo.inputs);
    if (sts<PXC_STATUS_NO_ERROR) {
      wprintf(L"Девайс для отслеживания не определен\n");
      return 0;
    }

    {
      PXCCapture::VideoStream * stream = capture.QueryVideoStream(0,0);
      PXCCapture::VideoStream::ProfileInfo vsinfo;
      stream->QueryProfile(&vsinfo);
      pinfo.width = vsinfo.imageInfo.width;
      pinfo.height= vsinfo.imageInfo.height;

      pxcCHAR path[MAX_PATH];
      computeModuleDirectory(0, (wchar_t*)path, MAX_PATH);
      wstring file = path;
      file += trackerPath;
      pxcStatus st = tracker->SetProfile(file.c_str(), &pinfo);
      if (st<PXC_STATUS_NO_ERROR) {
        wprintf(L"Не удалось загрузить сценарий\n");
        return 0;
      }
    }

    // Создание рендера
    renderer=new dcvRender(L"D'Fusion CV Detection Sample");
    if (!renderer) {
      wprintf(L"Не удалось создать рендер\n");
      return 0;
    }

    renderer->addData(tracker);


    for (int fnum=0;fnum<cmdl.m_nframes;fnum++) 
    {
      PXCSmartArray<PXCImage> images;
      PXCSmartSP sp1, sp2;

      /* Выполнение чтения и обработки кадров */
      sts=capture.ReadStreamAsync(images, &sp1);
      if (sts<PXC_STATUS_NO_ERROR) break;
      sts=tracker->ProcessImageAsync(images, &sp2);
      if (sts<PXC_STATUS_NO_ERROR) break;
      

      if(sp2) 
        sp2->Synchronize();

      if (!renderer->RenderFrame(images[0])) 
        break;
    }
  }
  
  session->UnloadImplFromFile(TEXT("dfusion4PCSDK.dll"));

    return(0);
}

#include "dcvRender.h"
#include <tchar.h>


#define TEXT_HEIGHT 16
double focaleX = 900;
double focaleY = 900;

dcvRender::dcvRender(pxcCHAR *title):UtilRender(title), m_tracker(0) {
}

void _proj3to2(double position[3], int& posx, int& posy, int cx=0, int cy=0)
{
  posx = cx+(int)(position[0]/position[2]*focaleX);
  posy = cy+(int)(position[1]/position[2]*focaleY);
}

void dcvRender::DrawMore(HDC hdc, double sx, double sy) {
  RECT Wrect;

  HGDIOBJ hbrushOld; 
  HBRUSH hbrush; 

    GetClientRect(m_hWnd, &Wrect);
  int dx = (int) Wrect.right/2;
  int dy = (int) Wrect.bottom/2;

  PXCDCVTracker::TrackingStatus status = m_tracker->GetTargetStatus(0);
  pxcI32 index = m_tracker->GetTrackedKeyFrameIndex(0);
  if ((status == PXCDCVTracker::STATUS_TRACKING) || (index != -1))
  {
    int targetTracked = m_tracker->GetTrackedKeyFrameIndex(0);
    const pxcCHAR* label = m_tracker->GetTargetName(0);
    double position[3];
    m_tracker->GetTrackedPosition(0, position);
    PXCDCVTracker::ProfileInfo  pinfo;
    m_tracker->QueryProfile(&pinfo);
    focaleX = pinfo.inputs.devCaps[2].value;
    focaleY = pinfo.inputs.devCaps[3].value;

    int x0, y0;
    _proj3to2(position, x0, y0, dx, dy);
        int rx=20*50/position[2];
        int ry=20*50/position[2];

    hbrush = CreateSolidBrush(RGB(255, 255, 0));
    hbrushOld = (HBRUSH)SelectObject(hdc, hbrush);

    Ellipse(hdc,x0-rx,y0-ry,x0+rx,y0+ry);
    SelectObject(hdc, hbrushOld);

    TextOut(hdc, x0, y0, label, _tcslen(label));

    hbrush = CreateSolidBrush(RGB(0, 0, 0));
    hbrushOld = (HBRUSH)SelectObject(hdc, hbrush);

    Ellipse(hdc,x0-rx*0.5,y0-ry*0.5,x0-rx*0.5+20,y0-ry*0.5+20);
    SelectObject(hdc, hbrushOld);

    hbrush = CreateSolidBrush(RGB(0, 0, 0));
    hbrushOld = (HBRUSH)SelectObject(hdc, hbrush);

    Ellipse(hdc,x0-rx*0.5+rx*0.7,y0-ry*0.5,x0-rx*0.5+rx*0.7+20,y0-ry*0.5+20);
    SelectObject(hdc, hbrushOld);

  
//    RECT rect={ (LONG)(x0-rx), (LONG)(y0-ry), (LONG)(x0+rx), (LONG)(y0+ry) };
//    DrawEdge(hdc, &rect, EDGE_RAISED, BF_TOP|BF_RIGHT|BF_LEFT|BF_BOTTOM);


    double orientation[4];
    m_tracker->GetTrackedOrientation(0, orientation);
    printf("Target tracked : %d, position {%f:3, %f:3, %f:3}, 
orientation {%f:3, %f:3, %f:3, %f:3}\n", 
targetTracked, position[0], position[1], position[2], 
      orientation[0],orientation[1],orientation[2],orientation[3]);
  }


}