25 #include "VideoSource_OpenNI.hh"
27 #include <Base/Image/ImageIO.hh>
33 int VideoSource_OpenNI::numInstances_ = 0;
34 int VideoSource_OpenNI::numDevices_ = 0;
35 xn::Context VideoSource_OpenNI::context_;
36 std::vector<VideoSource_OpenNI::ONIDeviceNode> VideoSource_OpenNI::devices_;
38 xn::NodeInfoList VideoSource_OpenNI::device_nodes_;
39 xn::NodeInfoList VideoSource_OpenNI::depth_nodes_;
40 xn::NodeInfoList VideoSource_OpenNI::image_nodes_;
41 xn::NodeInfoList VideoSource_OpenNI::ir_nodes_;
43 pthread_mutex_t *VideoSource_OpenNI::exclusive_mutex = NULL;
45 VideoSource_OpenNI::VideoSource_OpenNI() {
53 if (numDevices_ == 0) {
56 if (numInstances_ == 0) {
57 exclusive_mutex =
new pthread_mutex_t;
58 pthread_mutex_init(exclusive_mutex, NULL);
63 contextInited_ =
false;
64 switchModeEveryFrame_ =
false;
67 bool VideoSource_OpenNI::CheckError_(XnStatus rc, std::string what) {
68 if (rc != XN_STATUS_OK)
70 BIASERR(
"OpenNI Fail: " << what <<
" : " << xnGetStatusString(rc));
77 VideoSource_OpenNI::~VideoSource_OpenNI() {
79 if (numInstances_ == 0) {
82 if (numInstances_ == 0) {
89 int VideoSource_OpenNI::OpenDevice() {
93 int VideoSource_OpenNI::OpenDevice(
int device) {
95 int ret = SetVideoModeColor();
97 return StartDepthStream();
103 int VideoSource_OpenNI::CloseDevice() {
108 int VideoSource_OpenNI::PreGrab() {
109 cout <<
"OpenNI: pregrab device" << endl;
113 int VideoSource_OpenNI::PostGrab() {
114 cout <<
"OpenNI: postgrab device " << endl;
116 if (colorMode_ ==
false) {
117 rc = irGenerator_.StopGenerating();
118 if (CheckError_(rc,
"stop generating IR"))
return 1;
120 rc = colorGenerator_.StopGenerating();
121 if (CheckError_(rc,
"stop generating color"))
return 1;
123 if (depthGenerator_.IsValid()) {
124 rc = depthGenerator_.StopGenerating();
125 if (CheckError_(rc,
"stop generating depth"))
return 1;
133 BIASWARN(
"OpenNI Error: image already inited");
136 Image.
Init(640,480,1);
143 BIASWARN(
"OpenNI Error: image already inited");
146 Image.
Init(640,480,1,ImageBase::ST_float);
153 if (deviceId_ == 0) {
154 pthread_mutex_lock(exclusive_mutex);
155 context_.WaitAndUpdateAll();
156 pthread_mutex_unlock(exclusive_mutex);
164 if (depthGenerator_.IsValid())
165 depthGenerator_.GetMetaData(depthMD_);
167 colorGenerator_.GetMetaData(colorMD_);
170 irGenerator_.GetMetaData(irMD_);
172 const XnIRPixel *pix = irMD_.Data();
173 for (
int x=0;x<Width_*Height_;x++) {
174 ida[x] = (
unsigned char) (pix[x] / 2);
177 if (switchModeEveryFrame_) {
178 if (colorMode_ ==
true) {
187 if (!depthGenerator_.IsValid()) {
190 const XnDepthPixel *data = depthMD_.Data();
196 memset(ida,0,3*
sizeof(
float));
197 int size = Width_*Height_-3;
198 for (
int x=0;x<size;x++) {
199 ida[x+3] = data[x] * lut[x];
203 case DepthEuclidean: {
204 memset(ida,0,3*
sizeof(
float));
205 int size = Width_*Height_-3;
206 for (
int x=0;x<size;x++) {
213 BIASERR(
"OpenNI capture mode not supported");
219 int VideoSource_OpenNI::CreateDepthLookupTable(
double px,
double py,
double fx,
double fy) {
220 pthread_mutex_lock(exclusive_mutex);
221 CreateLookupTable_(px,py,fx,fy);
222 pthread_mutex_unlock(exclusive_mutex);
226 int VideoSource_OpenNI::StartDepthStream() {
227 pthread_mutex_lock(exclusive_mutex);
228 if (depthGenerator_.IsValid()) {
229 pthread_mutex_unlock(exclusive_mutex);
234 rc = context_.CreateProductionTree (devices_[deviceId_].depthInfo);
235 CheckError_(rc,
"create depth generator");
237 rc = devices_[deviceId_].depthInfo.GetInstance(depthGenerator_);
238 CheckError_(rc,
"create depth generator instance");
240 XnMapOutputMode depth_mode;
242 depth_mode.nXRes = XN_VGA_X_RES;
243 depth_mode.nYRes = XN_VGA_Y_RES;
244 depth_mode.nFPS = 30;
246 rc = depthGenerator_.SetMapOutputMode(depth_mode);
247 if (CheckError_(rc,
"set depth mode"))
return 1;
249 rc = depthGenerator_.StartGenerating();
250 if (CheckError_(rc,
"starting depth cam"))
return 1;
252 pthread_mutex_unlock(exclusive_mutex);
256 int VideoSource_OpenNI::StopDepthStream() {
257 pthread_mutex_lock(exclusive_mutex);
258 if (depthGenerator_.IsValid() && depthGenerator_.IsGenerating()) {
259 depthGenerator_.StopGenerating();
260 depthGenerator_.Release();
262 pthread_mutex_unlock(exclusive_mutex);
266 int VideoSource_OpenNI::ToggleDepthStream() {
267 if (depthGenerator_.IsValid()) {
268 return StopDepthStream();
270 return StartDepthStream();
274 int VideoSource_OpenNI::SetVideoModeColor() {
275 pthread_mutex_lock(exclusive_mutex);
276 if (colorMode_ ==
false) {
277 if (irGenerator_.IsValid() && irGenerator_.IsGenerating()) {
278 irGenerator_.StopGenerating();
279 irGenerator_.Release();
282 pthread_mutex_unlock(exclusive_mutex);
286 rc = context_.CreateProductionTree (devices_[deviceId_].imageInfo);
287 CheckError_(rc,
"create image generator");
289 rc = devices_[deviceId_].imageInfo.GetInstance(colorGenerator_);
290 CheckError_(rc,
"create image generator instance");
292 XnMapOutputMode frame_mode;
295 frame_mode.nXRes = XN_VGA_X_RES;
296 frame_mode.nYRes = XN_VGA_Y_RES;
297 frame_mode.nFPS = 30;
299 rc = colorGenerator_.SetMapOutputMode(frame_mode);
300 if (CheckError_(rc,
"set color mode"))
return 1;
302 rc = colorGenerator_.SetIntProperty(
"InputFormat", 6);
303 if (CheckError_(rc,
"set bayer uncompressed"))
return 1;
304 rc = colorGenerator_.SetPixelFormat(XN_PIXEL_FORMAT_GRAYSCALE_8_BIT);
305 if (CheckError_(rc,
"disable internal debayer"))
return 1;
308 rc = colorGenerator_.StartGenerating();
309 if (CheckError_(rc,
"starting color cam"))
return 1;
310 pthread_mutex_unlock(exclusive_mutex);
314 int VideoSource_OpenNI::SetVideoModeIR() {
315 pthread_mutex_lock(exclusive_mutex);
316 if (colorMode_ ==
true) {
317 if (colorGenerator_.IsValid() && colorGenerator_.IsGenerating()) {
318 colorGenerator_.StopGenerating();
319 colorGenerator_.Release();
322 pthread_mutex_unlock(exclusive_mutex);
327 rc = context_.CreateProductionTree (devices_[deviceId_].irInfo);
328 CheckError_(rc,
"create ir generator");
330 rc = devices_[deviceId_].irInfo.GetInstance(irGenerator_);
331 CheckError_(rc,
"create IR generator instance");
333 XnMapOutputMode frame_mode;
336 frame_mode.nXRes = XN_VGA_X_RES;
337 frame_mode.nYRes = XN_VGA_Y_RES;
338 frame_mode.nFPS = 30;
340 rc = irGenerator_.SetMapOutputMode(frame_mode);
341 if (CheckError_(rc,
"set ir mode"))
return 1;
345 rc = irGenerator_.StartGenerating();
346 if (CheckError_(rc,
"starting ir cam"))
return 1;
347 pthread_mutex_unlock(exclusive_mutex);
356 int VideoSource_OpenNI::SetSwitchVideoModeAfterGrab() {
357 switchModeEveryFrame_ = !switchModeEveryFrame_;
361 int VideoSource_OpenNI::SetTilt(
double degrees) {
362 BIASERR(
"not yet implemented");
366 int VideoSource_OpenNI::SetLed(
int mode) {
367 BIASERR(
"not yet implemented");
371 int VideoSource_OpenNI::GetAccelerometer(
double &tilt,
double &x,
double &y,
double &z) {
372 BIASERR(
"not yet implemented");
387 void VideoSource_OpenNI::CreateLookupTable_(
double px,
double py,
double fx,
double fy) {
388 if (depthLookupTable_ == NULL) {
394 for(
unsigned y=0;y<480;y++){
395 for(
unsigned x=0;x<640;x++){
396 float alpha = atan(
float((x-px)/fx));
397 float beta = atan(
float((py-y)/ sqrt( pow((x-px)*(fy/fx),2) + pow(fy,2))));
398 ida[y][x] = 1.0f / (cos(beta) * cos(alpha));
403 void VideoSource_OpenNI::DeleteLookupTable_() {
404 if (depthLookupTable_ != NULL) {
410 void VideoSource_OpenNI::ScanBus() {
412 rc = context_.
Init();
413 CheckError_(rc,
"init context");
416 rc = context_.EnumerateProductionTrees (XN_NODE_TYPE_DEVICE, NULL, device_nodes_);
417 CheckError_(rc,
"enumerate devices");
419 vector<xn::NodeInfo> device_info;
420 for (xn::NodeInfoList::Iterator nodeIt = device_nodes_.Begin (); nodeIt != device_nodes_.End (); ++nodeIt)
422 device_info.push_back (*nodeIt);
426 rc = context_.EnumerateProductionTrees (XN_NODE_TYPE_DEPTH, NULL, depth_nodes_, NULL);
427 CheckError_(rc,
"enumerate depth generators");
429 vector<xn::NodeInfo> depth_info;
430 for (xn::NodeInfoList::Iterator nodeIt = depth_nodes_.Begin (); nodeIt != depth_nodes_.End (); ++nodeIt)
432 depth_info.push_back (*nodeIt);
436 rc = context_.EnumerateProductionTrees (XN_NODE_TYPE_IMAGE, NULL, image_nodes_, NULL);
437 CheckError_(rc,
"enumerate image generators");
439 vector<xn::NodeInfo> image_info;
440 for (xn::NodeInfoList::Iterator nodeIt = image_nodes_.Begin (); nodeIt != image_nodes_.End (); ++nodeIt)
442 image_info.push_back (*nodeIt);
446 rc = context_.EnumerateProductionTrees (XN_NODE_TYPE_IR, NULL, ir_nodes_, NULL);
447 CheckError_(rc,
"enumerate ir generators");
449 vector<xn::NodeInfo> ir_info;
450 for (xn::NodeInfoList::Iterator nodeIt = ir_nodes_.Begin (); nodeIt != ir_nodes_.End (); ++nodeIt)
452 ir_info.push_back (*nodeIt);
455 for (
unsigned int i=0;i<device_info.size();i++) {
456 devices_.push_back( ONIDeviceNode( depth_info[i], image_info[i], ir_info[i], depth_info[i]));
459 numDevices_ = devices_.size();
460 cout <<
"OpenNI: There are " << numDevices_ <<
" devices connected. " << endl;
int BIASVideoSource_EXPORT ScanBus(std::stringstream &ofs)
support function to get the number of cameras and IDs of all cameras on one bus
bool IsEmpty() const
check if ImageData_ points to allocated image buffer or not
void CopyIn_NoInit(void *data)
Take some data and fill it into the Image.
BIAS::Image< float > * depthLookupTable_
void SetColorModel(EColorModel Model)
The image template class for specific storage types.
void Init(unsigned int Width, unsigned int Height, unsigned int channels=1, enum EStorageType storageType=ST_unsignedchar, const bool interleaved=true)
calls Init from ImageBase storageType is ignored, just dummy argument
const StorageType * GetImageData() const
overloaded GetImageData() from ImageBase
void Init(unsigned int width, unsigned int height, unsigned int nChannels=1, enum EStorageType storageType=ST_unsignedchar, const bool interleaved=true)
Initialize image size and channels.
Checks for VideoSource capabilities.
This is the base class for images in BIAS.
const StorageType ** GetImageDataArray() const
overloaded GetImageDataArray() from ImageBase