DWORD WINAPI RealSenseAdaptor::acquireThread(void* param) {
RealSenseAdaptor* adaptor = reinterpret_cast<RealSenseAdaptor*>(param);
// Set the thread priority.
SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_TIME_CRITICAL);
MSG msg;
while (GetMessage(&msg, NULL, 0, 0) > 0) {
switch (msg.message) {
case WM_USER:
{
// Check if a frame needs to be acquired
std::auto_ptr<imaqkit::IAutoCriticalSection> driverSection(imaqkit::createAutoCriticalSection(adaptor->_driverGuard, false));
// Get frame type & dimensions.
imaqkit::frametypes::FRAMETYPE frameType =
adaptor->getFrameType();
int imWidth = adaptor->getMaxWidth();
int imHeight = adaptor->getMaxHeight();
int imBand = adaptor->getNumberOfBands();
int camera_id = adaptor->getDeviceID();
PXCSession *session = PXCSession_Create();
if (!session) return 0;
PXCSenseManager *psm = session->CreateSenseManager();
if (imBand == 3)
{
if (camera_id == 1)
{
PXCCapture::DeviceInfo dinfo = {};
pxcCHAR *myname = L"Intel(R) RealSense(TM) 3D Camera";
psm->QueryCaptureManager()->FilterByDeviceInfo(myname, dinfo.did, dinfo.didx);
psm->EnableStream(PXCCapture::STREAM_TYPE_COLOR, imWidth, imHeight);
}
else
{
PXCCapture::DeviceInfo dinfo = {};
pxcCHAR *myname = L"Intel(R) RealSense(TM) 3D Camera R200";
psm->QueryCaptureManager()->FilterByDeviceInfo(myname, dinfo.did, dinfo.didx);
psm->EnableStream(PXCCapture::STREAM_TYPE_COLOR, imWidth, imHeight, 30);
}
}
else
{
if (camera_id == 2)
{
PXCCapture::DeviceInfo dinfo = {};
pxcCHAR *myname = L"Intel(R) RealSense(TM) 3D Camera";
psm->QueryCaptureManager()->FilterByDeviceInfo(myname, dinfo.did, dinfo.didx);
psm->EnableStream(PXCCapture::STREAM_TYPE_DEPTH, imWidth, imHeight);
}
else
{
PXCCapture::DeviceInfo dinfo = {};
pxcCHAR *myname = L"Intel(R) RealSense(TM) 3D Camera R200";
psm->QueryCaptureManager()->FilterByDeviceInfo(myname, dinfo.did, dinfo.didx);
psm->EnableStream(PXCCapture::STREAM_TYPE_DEPTH, imWidth, imHeight, 30);
psm->EnableStream(PXCCapture::STREAM_TYPE_COLOR, 640, 480, 30);
}
}
UtilRender *renderColor = new UtilRender(L"COLOR STREAM");
UtilRender *renderDepth = new UtilRender(L"DEPTH STREAM");
psm->Init();
PXCImage *colorIm, *depthIm;
while (adaptor->isAcquisitionNotComplete() && adaptor->isAcquisitionActive()) {
driverSection->enter();
if (psm->AcquireFrame(true) < PXC_STATUS_NO_ERROR) break;
// retrieve all available image samples
PXCCapture::Sample *sample = psm->QuerySample();
if (!sample) break;
unsigned char *imBuffer1 = new unsigned char[imWidth * imHeight * imBand];
//unsigned short *imBuffer2 = new unsigned short[imWidth * imHeight];
unsigned char *imBuffer2 = new unsigned char[imWidth * imHeight * 3];
if (imBand == 3)
{
colorIm = sample->color;
if (!colorIm) break;
PXCImage::ImageData cdata;
pxcStatus sts = colorIm->AcquireAccess(PXCImage::ACCESS_READ, PXCImage::PIXEL_FORMAT_RGB32, &cdata);
for (int ix = 0; ix < imHeight; ix++)
{
for (int jx = 0; jx < imWidth; jx++)
{
for (int c = 0; c < imBand; c++)
{
imBuffer1[(ix * imWidth + jx) * imBand + imBand - 1 - c] = *(pxcBYTE*)(cdata.planes[0] + ix * imWidth * 4 + 4 * jx + c);
}
}
}
colorIm->ReleaseAccess(&cdata);
renderColor->RenderFrame(colorIm);
}
else
{
depthIm = sample->depth;
if (!depthIm) break;
PXCImage::ImageData ddata;
pxcStatus sts = depthIm->AcquireAccess(PXCImage::ACCESS_READ, PXCImage::PIXEL_FORMAT_DEPTH, &ddata);
for (int id = 0; id < imHeight; id++)
{
for (int jd = 0; jd < imWidth; jd++)
{
imBuffer2[id * imWidth + jd] = *(pxcU16*)(ddata.planes[0] + id * imWidth * 2 + jd * 2);
}
}
depthIm->ReleaseAccess(&ddata);
renderDepth->RenderFrame(depthIm);
}
if (adaptor->isSendFrame()) {
// Create a frame object.
imaqkit::IAdaptorFrame* frame =
adaptor->getEngine()->makeFrame(frameType,
imWidth,
imHeight);
// Copy data from buffer into frame object.
if (imBand == 3)
{
frame->setImage(imBuffer1,
imWidth,
imHeight,
0, // X Offset from origin
0); // Y Offset from origin
}
else
{
frame->setImage(imBuffer2,
imWidth,
imHeight,
0, // X Offset from origin
0);
}
// Set image's timestamp.
frame->setTime(imaqkit::getCurrentTime());
// Send frame object to engine.
adaptor->getEngine()->receiveFrame(frame);
} // if isSendFrame()
psm->ReleaseFrame();
// Increment the frame count.
adaptor->incrementFrameCount();
// Cleanup. Deallocate imBuffer.
delete[] imBuffer1;
delete[] imBuffer2;
driverSection->leave();
} // while(isAcquisitionNotComplete()
delete renderColor;
delete renderDepth;
psm->Release();
}
break;
} //switch-case WM_USER
}//while message is not WM_QUIT
return 0;
}
/*******************************************************************************
Main entry point. Here we demonstrate, after some initial housekeeping, how to
initialize the camera, start streaming, grab samples, and process them.
*******************************************************************************/
int main(int argc, char *argv[]) {
if (!ProcessCmdArgs(argc, argv)) {
return 1;
}
// Check / create file directories
if (xdmPath != nullptr) {
if (_mkdir(xdmPath) != 0 && errno != EEXIST){
fprintf(stderr, "Error: Invalid XDM path. Error %d\n", errno);
fprintf(stderr, "Terminate? [Y/n]\n");
char choice = _getch();
if (choice != 'n' && choice != 'N') {
return ERROR_FILE;
}
xdmPath = nullptr;
}
else {
// Remove any trailing '\' in the path since we'll add it later.
if (xdmPath[strlen(xdmPath) - 1] == '\\')
xdmPath[strlen(xdmPath) - 1] = '0';
}
}
if (bufPath != nullptr) {
if (_mkdir(bufPath) != 0 && errno != EEXIST){
fprintf(stderr, "Error: Invalid Buffer path. Error %d\n", errno);
fprintf(stderr, "Terminate? [Y/n]\n");
char choice = _getch();
if (choice != 'n' && choice != 'N') {
return ERROR_FILE;
}
bufPath = nullptr;
}
else {
// Remove any trailing '\' in the path since we'll add it later.
if (bufPath[strlen(bufPath) - 1] == '\\')
bufPath[strlen(bufPath) - 1] = '0';
}
}
// Start timer for total test execution
unsigned long long programStart = GetTickCount64();
// Initialize camera and streams
if (verbose) fprintf_s(stdout, "Initializing camera...\n");
// The Sense Manager is the root object for interacting with the camera.
PXCSenseManager *senseManager = nullptr;
senseManager = PXCSenseManager::CreateInstance();
if (!senseManager) {
fprintf_s(stderr, "Unable to create the PXCSenseManager\n");
return ERROR_CAMERA;
}
// When enabling the streams (color and depth), the parameters must match
// the capabilities of the camera. For example, 60fps for color will fail
// on the DS4 / R200.
// Here we're hard-coding the resolution and frame rate
senseManager->EnableStream(PXCCapture::STREAM_TYPE_COLOR, 320, 240, 30);
senseManager->EnableStream(PXCCapture::STREAM_TYPE_DEPTH, 320, 240, 30);
// Initialize the PXCSenseManager
pxcStatus camStatus;
camStatus = senseManager->Init();
if (camStatus != PXC_STATUS_NO_ERROR) {
fprintf_s(stderr, "Unable to initizlize PXCSenseManager\n");
senseManager->Release();
return ERROR_CAMERA;
}
PXCImage *colorImage;
PXCImage *depthImage;
PXCPhoto *xdmPhoto = senseManager->QuerySession()->CreatePhoto();
// These two objects come from the Intel RealSense SDK helper for rendering
// camera data. Any rendering technique could be used or omitted if no
// visual feedback is desired.
UtilRender *renderColor = new UtilRender(L"COLOR STREAM");
UtilRender *renderDepth = new UtilRender(L"DEPTH STREAM");
// Start test
if (verbose) fprintf_s(stdout, "Running...\n");
// This section may be wrapped in additional code to automate
// repetitive tests. Closure provided just for convenience.
{ // Beginning of single test block
unsigned long totalFrames = 0;
unsigned long long streamStart = GetTickCount64();
for (unsigned int i = 0; i < framesPerTest; i++) {
// Passing 'true' to AcquireFrame blocks until all streams are
// ready (depth and color). Passing 'false' will result in
// frames unaligned in time.
camStatus = senseManager->AcquireFrame(true);
if (camStatus < PXC_STATUS_NO_ERROR) {
fprintf_s(stderr, "Error acquiring frame: %f\n", camStatus);
break;
}
// Retrieve all available image samples
PXCCapture::Sample *sample = senseManager->QuerySample();
if (sample == NULL) {
fprintf_s(stderr, "Sample unavailable\n");
}
else {
totalFrames++;
colorImage = sample->color;
depthImage = sample->depth;
// Render the frames (not necessary)
if (!renderColor->RenderFrame(colorImage)) break;
if (!renderDepth->RenderFrame(depthImage)) break;
// Save data if requested
wchar_t filePath[MAXPATH];
char fileName[18];
TimeStampString(fileName);
if (xdmPath != nullptr) { // Save XDM image
swprintf_s(filePath, MAXPATH, L"%S\\%S%S",
xdmPath,
fileName,
xdmExt);
xdmPhoto->ImportFromPreviewSample(sample);
pxcStatus saveStatus = xdmPhoto->SaveXDM(filePath);
if (saveStatus != PXC_STATUS_NO_ERROR)
fprintf_s(stderr, "Error: SaveXDM\n");
}
if (bufPath != NULL) { // Save depth buffer
swprintf_s(filePath, MAXPATH, L"%S\\%S%S",
bufPath, fileName, depBufExt);
// The WriteDepthBuf function has a lot of detail about
// accessing image data and should be examined.
if (!WriteDepthBuf(depthImage, filePath)) {
fprintf(stderr, "Error: WriteDepthBuf\n");
}
}
}
// Unlock the current frame to fetch the next one
senseManager->ReleaseFrame();
}
float frameRate = (float)totalFrames /
((float)(GetTickCount64() - streamStart) / 1000);
if (verbose) fprintf_s(stdout, "Frame Rate: %.2f\n", frameRate);
} // End of single test block
// Wrap up
if(verbose) fprintf_s(stdout, "Finished in %llu seconds.\n",
(GetTickCount64() - programStart) / 1000);
delete renderColor;
delete renderDepth;
xdmPhoto->Release();
if(senseManager != nullptr) senseManager->Release();
return 0;
}
