int SensorRealSense::initialize() {
std::cout << "SensorRealSense::initialize()" << std::endl;
sense_manager = PXCSenseManager::CreateInstance();
if (!sense_manager) {
wprintf_s(L"Unable to create the PXCSenseManager\n");
return false;
}
sense_manager->EnableStream(PXCCapture::STREAM_TYPE_COLOR, D_width/2, D_height/2, 60);
sense_manager->EnableStream(PXCCapture::STREAM_TYPE_DEPTH, D_width, D_height, 60);
sense_manager->Init();
PXCSession *session = PXCSession::CreateInstance();
PXCSession::ImplDesc desc, desc1;
memset(&desc, 0, sizeof(desc));
desc.group = PXCSession::IMPL_GROUP_SENSOR;
desc.subgroup = PXCSession::IMPL_SUBGROUP_VIDEO_CAPTURE;
if (session->QueryImpl(&desc, 0, &desc1) < PXC_STATUS_NO_ERROR) return false;
PXCCapture * capture;
pxcStatus status = session->CreateImpl<PXCCapture>(&desc1, &capture);
if(status != PXC_STATUS_NO_ERROR){
QMessageBox::critical(NULL,"FATAL ERROR", "Intel RealSense device not plugged?\n(CreateImpl<PXCCapture> failed)");
exit(0);
}
PXCCapture::Device* device;
device = capture->CreateDevice(0);
projection = device->CreateProjection();
this->initialized = true;
return true;
}
void foo1()
{
PXCSession *session;
PXCSession_Create(&session);
// session is a PXCSession instance
PXCSession::ImplDesc desc1;
memset(&desc1,0,sizeof(desc1));
desc1.group=PXCSession::IMPL_GROUP_SENSOR;
desc1.subgroup=PXCSession::IMPL_SUBGROUP_AUDIO_CAPTURE;
vector<std::wstring> deviceNames;
for (int m=0;;m++) {
PXCSession::ImplDesc desc2;
if (session->QueryImpl(&desc1,m,&desc2)<PXC_STATUS_NO_ERROR) break;
PXCSmartPtr<PXCCapture> capture;
if (session->CreateImpl<PXCCapture>(&desc2,&capture)<PXC_STATUS_NO_ERROR) continue;
for (int d=0;;d++) {
PXCCapture::DeviceInfo dinfo;
if (capture->QueryDevice(d,&dinfo)<PXC_STATUS_NO_ERROR) break;
std::wstring dname(dinfo.name);
deviceNames.push_back(dname);
}
}
// display devices and require a selection
int deviceNum = deviceNames.size();
int selectedDeviceId = 0;
wprintf_s(L"Device list:\n");
for (int i = 0; i < deviceNum; i++)
{
wprintf_s(L"Device[%d]: %s\n",i,deviceNames[i].c_str());
}
if (deviceNum > 0)
{
while (true)
{
wprintf_s(L"Please select a device from [0]~[%d]\n", deviceNum-1);
cin >> selectedDeviceId;
if (selectedDeviceId > -1 && selectedDeviceId < deviceNum)
{
wprintf_s(L"You selected device: [%d]%s\n", selectedDeviceId, deviceNames[selectedDeviceId].c_str());
break;
}
}
}
else
{
std::vector<unsigned int> RealSenseSensor::connectedSensors() {
std::vector<unsigned int> sensors;
PXCSession* session = PXCSession::CreateInstance();
PXCSession::ImplDesc desc1 = { };
desc1.group = PXCSession::IMPL_GROUP_SENSOR;
desc1.subgroup = PXCSession::IMPL_SUBGROUP_VIDEO_CAPTURE;
for (int m = 0; ; m++) {
PXCSession::ImplDesc desc2;
if (session->QueryImpl(&desc1, m, &desc2) < PXC_STATUS_NO_ERROR) {
break;
}
std::cout << "Module (" << m << "): " << desc2.friendlyName << std::endl;
PXCCapture* capture = 0;
if (session->CreateImpl<PXCCapture>(&desc2, &capture) < PXC_STATUS_NO_ERROR) {
continue;
}
for (int d = 0; ; d++) {
PXCCapture::DeviceInfo dinfo;
if (capture->QueryDeviceInfo(d, &dinfo) < PXC_STATUS_NO_ERROR) {
break;
}
std::cout << "Device (" << d << "): " << dinfo.name << std::endl;
if (dinfo.model != PXCCapture::DEVICE_MODEL_F200 && dinfo.model != PXCCapture::DEVICE_MODEL_R200) {
continue;
}
std::string serialStr = toNarrow(dinfo.serial);
std::cout << "Found device type: " << std::hex << static_cast<int>(dinfo.model) << " with serial: " << serialStr << std::endl;
unsigned int serial = 0;
if (tryToNumber(serialStr, serial)) {
sensors.push_back(serial);
} else {
std::cout << "Serial number " << serialStr << " could not be parsed." << std::endl;
}
}
capture->Release();
}
session->Release();
return sensors;
}
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;
}
