openni::VideoMode OpenNi2Video::FindOpenNI2Mode(
openni::Device & device,
openni::SensorType sensorType,
int width, int height,
int fps, openni::PixelFormat fmt
) {
// Query supported modes for device
const openni::Array<openni::VideoMode>& modes =
device.getSensorInfo(sensorType)->getSupportedVideoModes();
// Select last listed mode which matches parameters
int best_mode = -1;
for(int i = 0; i < modes.getSize(); i++) {
if( (!width || modes[i].getResolutionX() == width) &&
(!height || modes[i].getResolutionY() == height) &&
(!fps || modes[i].getFps() == fps) &&
(!fmt || modes[i].getPixelFormat() == fmt)
) {
best_mode = i;
}
}
if(best_mode >= 0) {
return modes[best_mode];
}
throw pangolin::VideoException("Video mode not supported");
}
int openStream(openni::Device& device, const char* name, openni::SensorType sensorType, SensorOpenType openType, openni::VideoStream& stream, const openni::SensorInfo** ppSensorInfo, bool* pbIsStreamOn)
{
*ppSensorInfo = device.getSensorInfo(sensorType);
*pbIsStreamOn = false;
if (openType == SENSOR_OFF)
{
return 0;
}
if (*ppSensorInfo == NULL)
{
if (openType == SENSOR_ON)
{
printf("No %s sensor available\n", name);
return -1;
}
else
{
return 0;
}
}
openni::Status nRetVal = stream.create(device, sensorType);
if (nRetVal != openni::STATUS_OK)
{
if (openType == SENSOR_ON)
{
printf("Failed to create %s stream:\n%s\n", openni::OpenNI::getExtendedError(), name);
return -2;
}
else
{
return 0;
}
}
nRetVal = stream.start();
if (nRetVal != openni::STATUS_OK)
{
stream.destroy();
if (openType == SENSOR_ON)
{
printf("Failed to start depth stream:\n%s\n", openni::OpenNI::getExtendedError());
return -3;
}
else
{
return 0;
}
}
*pbIsStreamOn = true;
return 0;
}
void openCommon(openni::Device& device, bool defaultRightColor)
{
XnStatus nRetVal = XN_STATUS_OK;
g_bIsDepthOn = false;
g_bIsColorOn = false;
g_bIsIROn = false;
g_depthSensorInfo = device.getSensorInfo(openni::SENSOR_DEPTH);
g_colorSensorInfo = device.getSensorInfo(openni::SENSOR_COLOR);
g_irSensorInfo = device.getSensorInfo(openni::SENSOR_IR);
if (g_depthSensorInfo != NULL)
{
nRetVal = g_depthStream.create(device, openni::SENSOR_DEPTH);
if (nRetVal != openni::STATUS_OK)
{
printf("Failed to create depth stream:\n%s\n", openni::OpenNI::getExtendedError());
return;
}
nRetVal = g_depthStream.start();
if (nRetVal != openni::STATUS_OK)
{
printf("Failed to start depth stream:\n%s\n", openni::OpenNI::getExtendedError());
g_depthStream.destroy();
return;
}
g_bIsDepthOn = true;
}
if (g_colorSensorInfo != NULL)
{
nRetVal = g_colorStream.create(device, openni::SENSOR_COLOR);
if (nRetVal != openni::STATUS_OK)
{
printf("Failed to create color stream:\n%s\n", openni::OpenNI::getExtendedError());
return;
}
if (defaultRightColor)
{
nRetVal = g_colorStream.start();
if (nRetVal != openni::STATUS_OK)
{
printf("Failed to start color stream:\n%s\n", openni::OpenNI::getExtendedError());
g_colorStream.destroy();
return;
}
g_bIsColorOn = true;
}
}
if (g_irSensorInfo != NULL)
{
nRetVal = g_irStream.create(device, openni::SENSOR_IR);
if (nRetVal != openni::STATUS_OK)
{
printf("Failed to create IR stream:\n%s\n", openni::OpenNI::getExtendedError());
return;
}
if (!g_bIsColorOn)
{
nRetVal = g_irStream.start();
if (nRetVal != openni::STATUS_OK)
{
printf("Failed to start IR stream:\n%s\n", openni::OpenNI::getExtendedError());
g_irStream.destroy();
return;
}
g_bIsIROn = true;
}
}
initConstants();
readFrame();
}