/*----------------------------------------------------------------------------*/
void pcl::SoftKineticDevice::configureDepthNode()
{
g_dnode.newSampleReceivedEvent().connect(newDepthSampleCallback);
DepthNode::Configuration config = g_dnode.getConfiguration();
config.frameFormat = FRAME_FORMAT_QVGA;
config.framerate = 25;
config.mode = DepthNode::CAMERA_MODE_CLOSE_MODE;
config.saturation = true;
g_dnode.setEnableVertices(true);
g_dnode.setEnableVerticesFloatingPoint(true);
g_dnode.setEnableDepthMapFloatingPoint(true);
try
{
context_.requestControl(g_dnode,0);
g_dnode.setConfiguration(config);
}
catch (ArgumentException& e)
{
printf("Argument Exception: %s\n",e.what());
}
catch (UnauthorizedAccessException& e)
{
printf("Unauthorized Access Exception: %s\n",e.what());
}
catch (IOException& e)
{
printf("IO Exception: %s\n",e.what());
}
catch (InvalidOperationException& e)
{
printf("Invalid Operation Exception: %s\n",e.what());
}
catch (ConfigurationException& e)
{
printf("Configuration Exception: %s\n",e.what());
}
catch (StreamingException& e)
{
printf("Streaming Exception: %s\n",e.what());
}
catch (TimeoutException&)
{
printf("TimeoutException\n");
}
}
/*----------------------------------------------------------------------------*/
void SoftKineticCamera::configureDepthNode()
{
m_dnode.newSampleReceivedEvent().connect(&onNewDepthSample);
DepthNode::Configuration config = m_dnode.getConfiguration();
config.frameFormat = FRAME_FORMAT_QVGA;
config.framerate = 25;
config.mode = DepthNode::CAMERA_MODE_CLOSE_MODE;
config.saturation = true;
m_dnode.setEnableDepthMap(true);
try
{
m_context.requestControl(m_dnode,0);
m_dnode.setEnableConfidenceMap(true);
m_dnode.setConfidenceThreshold(120);
m_dnode.setConfiguration(config);
}
catch (ArgumentException& e)
{
printf("Argument Exception: %s\n",e.what());
}
catch (UnauthorizedAccessException& e)
{
printf("Unauthorized Access Exception: %s\n",e.what());
}
catch (IOException& e)
{
printf("IO Exception: %s\n",e.what());
}
catch (InvalidOperationException& e)
{
printf("Invalid Operation Exception: %s\n",e.what());
}
catch (ConfigurationException& e)
{
printf("Configuration Exception: %s\n",e.what());
}
catch (StreamingException& e)
{
printf("Streaming Exception: %s\n",e.what());
}
catch (TimeoutException&)
{
printf("TimeoutException\n");
}
}
bool SoftKineticCamera::Init() {
m_context = Context::create("localhost");
m_context.deviceAddedEvent().connect(&SoftKineticCamera::onDeviceConnected);
m_context.deviceRemovedEvent().connect(&SoftKineticCamera::onDeviceDisconnected);
// Get the list of currently connected devices
vector<Device> devices = m_context.getDevices();
if (devices.size()==0)
{
return false;
}
//Assume that there is only one device
m_bDeviceFound=true;
m_device=devices[0];
m_device.nodeAddedEvent().connect(&SoftKineticCamera::onNodeConnected);
m_device.nodeRemovedEvent().connect(&SoftKineticCamera::onNodeDisconnected);
vector<Node> nodes = m_device.getNodes();
for (int n = 0; n < (int)nodes.size();n++) {
configureNode(nodes[n]);
}
m_context.startNodes();
//Allocate memory to depth and color frames.
int32_t colorWidth,colorHeight;
int32_t depthWidth,depthHeight;
FrameFormat_toResolution(m_cnode.getConfiguration().frameFormat,&colorWidth,&colorHeight);
FrameFormat_toResolution(m_dnode.getConfiguration().frameFormat,&depthWidth,&depthHeight);
sizeOfDepthMap=depthWidth*depthHeight*sizeof(CAMERA_DEPTH_TYPE);
m_depthMap=new CAMERA_DEPTH_TYPE[sizeOfDepthMap];
sizeOfRGBMap=colorWidth*colorHeight*sizeof(CAMERA_RGB_TYPE);
m_rgbMap=new uint8_t[sizeOfRGBMap];
this->colorWidth=colorWidth;
this->colorHeight=colorHeight;
cutilSafeCall(cudaMalloc((void**)&m_cudaRGBMap, sizeOfRGBMap));
cutilSafeCall(cudaMalloc((void**)&m_cudaDepthMap, sizeOfDepthMap));
tthread::thread eventThread(&runEvents,0);
eventThread.detach();
SetParams(SOFTKINETIC_FOH_RADIANS, SOFTKINETIC_FOV_RADIANS,depthWidth,depthHeight);
m_params.m_z_offset = 0.f;
m_params.m_min_depth = SOFTKINETIC_MIN_DEPTH_MM;
m_params.m_max_depth = SOFTKINETIC_MAX_DEPTH_MM;
m_params.k1 = SOFTKINETIC_K1;
m_params.k2 = SOFTKINETIC_K2;
m_params.p1 = SOFTKINETIC_P1;
m_params.p2 = SOFTKINETIC_P2;
m_params.depthNullValue = SOFTKINETIC_DEPTH_NULL_VALUE;
CameraAbstract::Init();
return true;
}