openni_example.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011 2011 Willow Garage, Inc.
5  * Suat Gedikli <gedikli@willowgarage.com>
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
41 #include <iostream>
42 #include <string>
43 #include <map>
44 #include <XnCppWrapper.h>
45 #include <opencv2/opencv.hpp>
46 #include <boost/thread.hpp>
47 #include <sys/times.h>
48 
49 using namespace std;
50 using namespace openni_wrapper;
51 using namespace cv;
52 using namespace boost;
53 
55 {
56 public:
57 
58  typedef struct ImgContext : public boost::noncopyable
59  {
60 
61  ImgContext () : is_new (false)
62  {
63  }
64 
65  ImgContext (const Mat & img) : image (img), is_new (false)
66  {
67  }
68  Mat image;
69  mutable boost::mutex lock;
70  bool is_new;
71  } ImageContext;
72 public:
73  MyOpenNIExample (const vector<unsigned>& device_indices);
74  ~MyOpenNIExample ();
75  int run ();
76 private:
77  void imageCallback (boost::shared_ptr<Image> image, void* cookie);
78  void depthCallback (boost::shared_ptr<DepthImage> depth, void* cookie);
79  void writeImages () const;
80  map<string, ImageContext*> rgb_images_;
81  map<string, ImageContext*> gray_images_;
82  map<string, ImageContext*> depth_images_;
83  vector< boost::shared_ptr<OpenNIDevice> > devices_;
84  bool running_;
85  unsigned selected_device_;
86 
89 };
90 
91 MyOpenNIExample::MyOpenNIExample (const vector<unsigned>& device_indices)
92 : running_ (false)
93 , selected_device_ (0)
94 {
95  OpenNIDriver& driver = OpenNIDriver::getInstance ();
96 
97  for (vector<unsigned>::const_iterator indexIt = device_indices.begin (); indexIt != device_indices.end (); ++indexIt)
98  {
99  if (*indexIt >= driver.getNumberDevices ())
100  {
101  cout << "Index out of range." << driver.getNumberDevices () << " devices found." << endl;
102  exit (1);
103  }
104 
105  boost::shared_ptr<OpenNIDevice> device = driver.getDeviceByIndex (*indexIt);
106  cout << devices_.size () + 1 << ". device on bus: " << (int)device->getBus () << " @ " << (int)device->getAddress ()
107  << " with serial number: " << device->getSerialNumber () << " "
108  << device->getVendorName () << " : " << device->getProductName () << endl;
109  devices_.push_back (device);
110 
111  const int width = 640;
112  const int height = 480;
113  XnMapOutputMode mode;
114  mode.nXRes = width;
115  mode.nYRes = height;
116  mode.nFPS = 30;
117 
118  if (device->hasImageStream())
119  {
120  if (!device->isImageModeSupported (mode))
121  {
122  cout << "image stream mode " << mode.nXRes << " x " << mode.nYRes << " @ " << mode.nFPS << " not supported" << endl;
123  exit (-1);
124  }
125  namedWindow (string (device->getConnectionString ()) + "RGB", WINDOW_AUTOSIZE);
126  namedWindow (string (device->getConnectionString ()) + "Gray", WINDOW_AUTOSIZE);
127  rgb_images_[device->getConnectionString ()] = new ImageContext (Mat::zeros (height, width, CV_8UC3));
128  gray_images_[device->getConnectionString ()] = new ImageContext (Mat::zeros (height, width, CV_8UC1));
129  device->registerImageCallback (&MyOpenNIExample::imageCallback, *this, &(*device));
130  }
131  if (device->hasDepthStream())
132  {
133  if (!device->isDepthModeSupported (mode))
134  {
135  cout << "depth stream mode " << mode.nXRes << " x " << mode.nYRes << " @ " << mode.nFPS << " not supported" << endl;
136  exit (-1);
137  }
138  namedWindow (string (device->getConnectionString ()) + "Depth", WINDOW_AUTOSIZE);
139  depth_images_[device->getConnectionString ()] = new ImageContext (Mat::zeros (height, width, CV_32FC1));
140  device->registerDepthCallback (&MyOpenNIExample::depthCallback, *this, &(*device));
141  }
142  }
143 
144  timeval timestamp;
145  gettimeofday (&timestamp, NULL);
146  image_timestamp = depth_timestamp = timestamp.tv_sec + timestamp.tv_usec * 0.000001;
147 }
148 
150 {
151  // this should call the device destructors, which are blocking until workerthreads return.
152  devices_.clear ();
153 
154  //now its save to free images
155  for (map<string, ImageContext*>::iterator imageIt = rgb_images_.begin (); imageIt != rgb_images_.end (); ++imageIt)
156  {
157  delete imageIt->second;
158  }
159 
160  for (map<string, ImageContext*>::iterator imageIt = gray_images_.begin (); imageIt != gray_images_.end (); ++imageIt)
161  {
162  delete imageIt->second;
163  }
164 
165  for (map<string, ImageContext*>::iterator imageIt = depth_images_.begin (); imageIt != depth_images_.end (); ++imageIt)
166  {
167  delete imageIt->second;
168  }
169 }
170 
172 {
173  cout << "write images" << endl;
174  static unsigned index = 0;
175  ++index;
176 
177  cout << "locking rgb images..." << flush;
178  map<string, ImageContext*>::const_iterator imageIt;
179  for (imageIt = rgb_images_.begin (); imageIt != rgb_images_.end (); ++imageIt)
180  imageIt->second->lock.lock ();
181 
182  cout << "done\nlocking gray images..." << flush;
183  for (imageIt = gray_images_.begin (); imageIt != gray_images_.end (); ++imageIt)
184  imageIt->second->lock.lock ();
185  // for (imageIt = depth_images_.begin (); imageIt != depth_images_.end (); ++imageIt)
186  // imageIt->second->lock.lock ();
187 
188  cout << "locking rgb images..." << flush;
189  char file_name[255];
190  for (imageIt = rgb_images_.begin (); imageIt != rgb_images_.end (); ++imageIt)
191  {
192  sprintf (file_name, "rgb_%03u.png", index);
193  imwrite (file_name, imageIt->second->image);
194  }
195 
196  for (imageIt = gray_images_.begin (); imageIt != gray_images_.end (); ++imageIt)
197  {
198  sprintf (file_name, "gray_%03u.png", index);
199  imwrite (file_name, imageIt->second->image);
200  }
201  // cout << "write depth images" << endl;
202  // for (imageIt = depth_images_.begin (); imageIt != depth_images_.end (); ++imageIt)
203  // {
204  // sprintf (file_name, "%s_depth_%03d.png", imageIt->first, index);
205  // cout << "saving depth image: " << file_name << endl;
206  // imwrite (file_name, imageIt->second->image);
207  // }
208 
209  for (imageIt = rgb_images_.begin (); imageIt != rgb_images_.end (); ++imageIt)
210  imageIt->second->lock.unlock ();
211 
212  for (imageIt = gray_images_.begin (); imageIt != gray_images_.end (); ++imageIt)
213  imageIt->second->lock.unlock ();
214  // for (imageIt = depth_images_.begin (); imageIt != depth_images_.end (); ++imageIt)
215  // imageIt->second->lock.unlock ();
216 }
217 
219 {
220  timeval timestamp;
221  gettimeofday (&timestamp, NULL);
222 
223  double now = timestamp.tv_sec + timestamp.tv_usec * 0.000001;
224 // double diff1 = min (fabs (now - depth_timestamp), fabs (depth_timestamp - image_timestamp));
225 // double diff2 = max (fabs (now - depth_timestamp), fabs (depth_timestamp - image_timestamp));
226  //cout << diff1 * 1000.0 << "\tms vs. " << diff2 * 1000.0 << endl;
227 
228  image_timestamp = now;
229  OpenNIDevice* device = reinterpret_cast<OpenNIDevice*>(cookie);
230  ImageContext* rgb_image_context = rgb_images_[device->getConnectionString ()];
231  ImageContext* gray_image_context = gray_images_[device->getConnectionString ()];
232 
233  // lock image so it does not get drawn
234  unique_lock<mutex> rgb_lock (rgb_image_context->lock);
235  unsigned char* rgb_buffer = (unsigned char*)(rgb_image_context->image.data + (rgb_image_context->image.cols >> 2) * rgb_image_context->image.elemSize () +
236  (rgb_image_context->image.rows >> 2) * rgb_image_context->image.step);
237  image->fillRGB (rgb_image_context->image.cols >> 1, rgb_image_context->image.rows >> 1, rgb_buffer, rgb_image_context->image.step);
238 
239 /*
240  unsigned char* rgb_buffer = (unsigned char*)(rgb_image_context->image.data + (rgb_image_context->image.cols >> 3 ) * 3 * rgb_image_context->image.elemSize () +
241  (rgb_image_context->image.rows >> 3 ) * 3 * rgb_image_context->image.step);
242  image->fillRGB (rgb_image_context->image.cols >> 2, rgb_image_context->image.rows >> 2, rgb_buffer, rgb_image_context->image.step);
243 */
244  //image->fillRGB (rgb_image_context->image.cols, rgb_image_context->image.rows, rgb_image_context->image.data, rgb_image_context->image.step);
245  /*
246  cv::Mat raw (image->getHeight(), image->getWidth(), CV_8UC1);
247  image->fillRaw (raw.data);
248 
249  static int calls = 0;
250  if (++calls % 30)
251  {
252  int index = calls / 30;
253  char filename [1024];
254  sprintf (filename, "image_%03d.png", index);
255  imwrite (filename, raw);
256  }
257  imshow ("raw", raw);
258  */
259  rgb_image_context->is_new = true;
260  rgb_lock.unlock ();
261 
262  unique_lock<mutex> gray_lock (gray_image_context->lock);
263 
264  unsigned char* gray_buffer = (unsigned char*)(gray_image_context->image.data + (gray_image_context->image.cols >> 2) +
265  (gray_image_context->image.rows >> 2) * gray_image_context->image.step);
266  image->fillGrayscale (gray_image_context->image.cols >> 1, gray_image_context->image.rows >> 1, gray_buffer, gray_image_context->image.step);
267  //image->fillGrayscale (gray_image_context->image.cols, gray_image_context->image.rows, gray_image_context->image.data, gray_image_context->image.step);
268  gray_image_context->is_new = true;
269 }
270 
272 {
273 
274  timeval timestamp;
275  gettimeofday (&timestamp, NULL);
276  depth_timestamp = timestamp.tv_sec + timestamp.tv_usec * 0.000001;
277 
278  OpenNIDevice* device = reinterpret_cast<OpenNIDevice*>(cookie);
279  ImageContext* depth_image_context = depth_images_[device->getConnectionString ()];
280 
281  // lock depth image so it does not get drawn
282  unique_lock<mutex> depth_lock (depth_image_context->lock);
283  float* buffer = (float*)(depth_image_context->image.data + (depth_image_context->image.cols >> 2) * sizeof(float) +
284  (depth_image_context->image.rows >> 2) * depth_image_context->image.step );
285  depth->fillDepthImage (depth_image_context->image.cols >> 1, depth_image_context->image.rows >> 1, buffer, depth_image_context->image.step);
286  //depth.fillDepthImage (depth_image_context->image.cols, depth_image_context->image.rows, (float*)depth_image_context->image.data, depth_image_context->image.step);
287  depth_image_context->is_new = true;
288 }
289 
291 {
292  running_ = true;
293  try
294  {
295  while (running_)
296  {
297  for (map<string, ImageContext*>::iterator imageIt = rgb_images_.begin (); imageIt != rgb_images_.end (); ++imageIt)
298  {
299  if (imageIt->second->is_new && imageIt->second->lock.try_lock ())
300  {
301  cv::Mat bgr_image;
302  cvtColor (imageIt->second->image, bgr_image, CV_RGB2BGR);
303  imshow (imageIt->first + "RGB", bgr_image);
304  imageIt->second->is_new = false;
305  imageIt->second->lock.unlock ();
306  }
307  }
308 
309  for (map<string, ImageContext*>::iterator imageIt = gray_images_.begin (); imageIt != gray_images_.end (); ++imageIt)
310  {
311  if (imageIt->second->is_new && imageIt->second->lock.try_lock ())
312  {
313  imshow (imageIt->first + "Gray", imageIt->second->image);
314  imageIt->second->is_new = false;
315  imageIt->second->lock.unlock ();
316  }
317  }
318 
319  for (map<string, ImageContext*>::iterator imageIt = depth_images_.begin (); imageIt != depth_images_.end (); ++imageIt)
320  {
321  if (imageIt->second->is_new && imageIt->second->lock.try_lock ())
322  {// depth image is in range 0-10 meter -> convert to 0-255 values
323  Mat gray_image;
324  imageIt->second->image.convertTo (gray_image, CV_8UC1, 25.5);
325  imshow (imageIt->first + "Depth", gray_image);
326  imageIt->second->is_new = false;
327  imageIt->second->lock.unlock ();
328  }
329  }
330 
331  unsigned char key = waitKey (30) & 0xFF;
332 
333  switch (key)
334  {
335  case 27:
336  case 'q':
337  case 'Q': running_ = false;
338  break;
339 
340  case '1':
341  selected_device_ = 0;
342  break;
343  case '2':
344  selected_device_ = 1;
345  break;
346  case '3':
347  selected_device_ = 2;
348  break;
349 
350  case 'r':
351  case 'R':
352  devices_[selected_device_]->setDepthRegistration (!devices_[selected_device_]->isDepthRegistered ());
353  break;
354  case 's':
355  case 'S':
356  if (devices_[selected_device_]->isSynchronizationSupported ())
357  devices_[selected_device_]->setSynchronization (!devices_[selected_device_]->isSynchronized ());
358  break;
359  case 'c':
360  case 'C':
361  if (devices_[selected_device_]->isDepthCropped ())
362  {
363  depth_images_[devices_[selected_device_]->getConnectionString ()]->lock.lock ();
364  //depth_images_[devices_[selected_device_]->getConnectionString ()]->image.create (480, 640, CV_32FC1);
365  depth_images_[devices_[selected_device_]->getConnectionString ()]->image.rows = 480;
366  depth_images_[devices_[selected_device_]->getConnectionString ()]->image.cols = 640;
367  depth_images_[devices_[selected_device_]->getConnectionString ()]->lock.unlock ();
368  devices_[selected_device_]->setDepthCropping (0, 0, 0, 0);
369  }
370  else if (devices_[selected_device_]->isDepthCroppingSupported ())
371  {
372  depth_images_[devices_[selected_device_]->getConnectionString ()]->lock.lock ();
373  //depth_images_[devices_[selected_device_]->getConnectionString ()]->image.create (300, 400, CV_32FC1);
374  depth_images_[devices_[selected_device_]->getConnectionString ()]->image.rows = 240;
375  depth_images_[devices_[selected_device_]->getConnectionString ()]->image.cols = 320;
376  depth_images_[devices_[selected_device_]->getConnectionString ()]->lock.unlock ();
377  devices_[selected_device_]->setDepthCropping (100, 100, 320, 240);
378  }
379  break;
380 
381  case 'd':
382  case 'D':
383  if (devices_[selected_device_]->isDepthStreamRunning ())
384  devices_[selected_device_]->stopDepthStream ();
385  else
386  devices_[selected_device_]->startDepthStream ();
387  break;
388  case 'i':
389  case 'I':
390  if (devices_[selected_device_]->isImageStreamRunning ())
391  devices_[selected_device_]->stopImageStream ();
392  else
393  devices_[selected_device_]->startImageStream ();
394  break;
395 
396  case 'w':
397  case 'W':
398  writeImages ();
399  break;
400  }
401  }
402  }
403  catch (const OpenNIException& exception)
404  {
405  cout << "exception caught: " << exception.what () << endl;
406  return (-1);
407  }
408  catch (...)
409  {
410 
411  cout << "unknown exception caught" << endl;
412  return (-1);
413  }
414  return 0;
415 }
416 
417 int main (int argc, char** argv)
418 {
419  OpenNIDriver& driver = OpenNIDriver::getInstance ();
420  if (argc == 1)
421  {
422  cout << "Usage: " << argv[0] << " (<device-index>)+" << endl;
423  if (driver.getNumberDevices () > 0)
424  {
425  for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
426  {
427  cout << "Device: " << deviceIdx << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
428  << ", connected: " << (int)driver.getBus (deviceIdx) << " @ " << (int)driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << endl;
429  }
430  }
431  else
432  cout << "No devices connected." << endl;
433  exit (1);
434  }
435 
436  vector <unsigned> device_indices;
437  for (int argIdx = 1; argIdx < argc; ++argIdx)
438  {
439  unsigned deviceIdx = (unsigned)atoi (argv[argIdx]);
440  if (deviceIdx >= driver.getNumberDevices ())
441  {
442  if (driver.getNumberDevices () > 0)
443  {
444  cout << "Device index out of range. " << driver.getNumberDevices () << " devices found." << endl;
445  for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
446  {
447  cout << "Device: " << deviceIdx << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: "
448  << driver.getProductName (deviceIdx) << ", connected: " << (int)driver.getBus (deviceIdx) << " @ "
449  << (int)driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << endl;
450  }
451  }
452  else
453  cout << "No devices connected." << endl;
454  exit (-1);
455  }
456  device_indices.push_back ((unsigned)deviceIdx);
457  }
458 
459  cout << "<1,2,3...> to select device" << endl;
460  cout << "<I> to start or stop image stream of selected device" << endl;
461  cout << "<D> to start or stop depth stream of selected device" << endl;
462  cout << "<R> to turn on or off registration for selected device" << endl;
463  cout << "<S> to turn on or off synchronization for selected device" << endl;
464  cout << "<C> to turn on or off image cropping for selected device" << endl;
465  cout << "<W> write current images" << endl;
466  cout << "<Q> to quit application" << endl;
467  MyOpenNIExample example (device_indices);
468 
469  return example.run ();
470 }
unsigned char getBus(unsigned index) const
const char * getSerialNumber(unsigned index) const
unsigned selected_device_
virtual const char * what() const
const char * getProductName(unsigned index) const
const char * getVendorName(unsigned index) const
map< string, ImageContext * > depth_images_
Driver class implemented as Singleton. This class contains the xn::Context object used by all devices...
Definition: openni_driver.h:58
General exception class.
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
MyOpenNIExample::ImgContext ImageContext
unsigned getNumberDevices() const
vector< boost::shared_ptr< OpenNIDevice > > devices_
MyOpenNIExample(const vector< unsigned > &device_indices)
void depthCallback(boost::shared_ptr< DepthImage > depth, void *cookie)
int main(int argc, char **argv)
boost::shared_ptr< OpenNIDevice > getDeviceByIndex(unsigned index) const
void writeImages() const
map< string, ImageContext * > gray_images_
const char * getConnectionString() const
returns the connectionstring for current device, which has following format vendorID/productID@BusID/...
Class representing an astract device for Primesense or MS Kinect devices.
Definition: openni_device.h:66
void imageCallback(boost::shared_ptr< Image > image, void *cookie)
map< string, ImageContext * > rgb_images_
unsigned char getAddress(unsigned index) const
void run(ClassLoader *loader)


openni_camera
Author(s): Patrick Mihelich, Suat Gedikli, Radu Bogdan Rusu
autogenerated on Wed Jun 5 2019 20:15:22