00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <openni_camera/openni_driver.h>
00038 #include <openni_camera/openni_device.h>
00039 #include <openni_camera/openni_image.h>
00040 #include <openni_camera/openni_depth_image.h>
00041 #include <iostream>
00042 #include <string>
00043 #include <map>
00044 #include <XnCppWrapper.h>
00045 #include <opencv2/opencv.hpp>
00046 #include <boost/thread.hpp>
00047 #include <sys/times.h>
00048
00049 using namespace std;
00050 using namespace openni_wrapper;
00051 using namespace cv;
00052 using namespace boost;
00053
00054 class MyOpenNIExample
00055 {
00056 public:
00057
00058 typedef struct ImgContext : public boost::noncopyable
00059 {
00060
00061 ImgContext () : is_new (false)
00062 {
00063 }
00064
00065 ImgContext (const Mat & img) : image (img), is_new (false)
00066 {
00067 }
00068 Mat image;
00069 mutable boost::mutex lock;
00070 bool is_new;
00071 } ImageContext;
00072 public:
00073 MyOpenNIExample (const vector<unsigned>& device_indices);
00074 ~MyOpenNIExample ();
00075 int run ();
00076 private:
00077 void imageCallback (boost::shared_ptr<Image> image, void* cookie);
00078 void depthCallback (boost::shared_ptr<DepthImage> depth, void* cookie);
00079 void writeImages () const;
00080 map<string, ImageContext*> rgb_images_;
00081 map<string, ImageContext*> gray_images_;
00082 map<string, ImageContext*> depth_images_;
00083 vector< boost::shared_ptr<OpenNIDevice> > devices_;
00084 bool running_;
00085 unsigned selected_device_;
00086
00087 double image_timestamp;
00088 double depth_timestamp;
00089 };
00090
00091 MyOpenNIExample::MyOpenNIExample (const vector<unsigned>& device_indices)
00092 : running_ (false)
00093 , selected_device_ (0)
00094 {
00095 OpenNIDriver& driver = OpenNIDriver::getInstance ();
00096
00097 for (vector<unsigned>::const_iterator indexIt = device_indices.begin (); indexIt != device_indices.end (); ++indexIt)
00098 {
00099 if (*indexIt >= driver.getNumberDevices ())
00100 {
00101 cout << "Index out of range." << driver.getNumberDevices () << " devices found." << endl;
00102 exit (1);
00103 }
00104
00105 boost::shared_ptr<OpenNIDevice> device = driver.getDeviceByIndex (*indexIt);
00106 cout << devices_.size () + 1 << ". device on bus: " << (int)device->getBus () << " @ " << (int)device->getAddress ()
00107 << " with serial number: " << device->getSerialNumber () << " "
00108 << device->getVendorName () << " : " << device->getProductName () << endl;
00109 devices_.push_back (device);
00110
00111 const int width = 640;
00112 const int height = 480;
00113 XnMapOutputMode mode;
00114 mode.nXRes = width;
00115 mode.nYRes = height;
00116 mode.nFPS = 30;
00117
00118 if (device->hasImageStream())
00119 {
00120 if (!device->isImageModeSupported (mode))
00121 {
00122 cout << "image stream mode " << mode.nXRes << " x " << mode.nYRes << " @ " << mode.nFPS << " not supported" << endl;
00123 exit (-1);
00124 }
00125 namedWindow (string (device->getConnectionString ()) + "RGB", WINDOW_AUTOSIZE);
00126 namedWindow (string (device->getConnectionString ()) + "Gray", WINDOW_AUTOSIZE);
00127 rgb_images_[device->getConnectionString ()] = new ImageContext (Mat::zeros (height, width, CV_8UC3));
00128 gray_images_[device->getConnectionString ()] = new ImageContext (Mat::zeros (height, width, CV_8UC1));
00129 device->registerImageCallback (&MyOpenNIExample::imageCallback, *this, &(*device));
00130 }
00131 if (device->hasDepthStream())
00132 {
00133 if (!device->isDepthModeSupported (mode))
00134 {
00135 cout << "depth stream mode " << mode.nXRes << " x " << mode.nYRes << " @ " << mode.nFPS << " not supported" << endl;
00136 exit (-1);
00137 }
00138 namedWindow (string (device->getConnectionString ()) + "Depth", WINDOW_AUTOSIZE);
00139 depth_images_[device->getConnectionString ()] = new ImageContext (Mat::zeros (height, width, CV_32FC1));
00140 device->registerDepthCallback (&MyOpenNIExample::depthCallback, *this, &(*device));
00141 }
00142 }
00143
00144 timeval timestamp;
00145 gettimeofday (×tamp, NULL);
00146 image_timestamp = depth_timestamp = timestamp.tv_sec + timestamp.tv_usec * 0.000001;
00147 }
00148
00149 MyOpenNIExample::~MyOpenNIExample ()
00150 {
00151
00152 devices_.clear ();
00153
00154
00155 for (map<string, ImageContext*>::iterator imageIt = rgb_images_.begin (); imageIt != rgb_images_.end (); ++imageIt)
00156 {
00157 delete imageIt->second;
00158 }
00159
00160 for (map<string, ImageContext*>::iterator imageIt = gray_images_.begin (); imageIt != gray_images_.end (); ++imageIt)
00161 {
00162 delete imageIt->second;
00163 }
00164
00165 for (map<string, ImageContext*>::iterator imageIt = depth_images_.begin (); imageIt != depth_images_.end (); ++imageIt)
00166 {
00167 delete imageIt->second;
00168 }
00169 }
00170
00171 void MyOpenNIExample::writeImages () const
00172 {
00173 cout << "write images" << endl;
00174 static unsigned index = 0;
00175 ++index;
00176
00177 cout << "locking rgb images..." << flush;
00178 map<string, ImageContext*>::const_iterator imageIt;
00179 for (imageIt = rgb_images_.begin (); imageIt != rgb_images_.end (); ++imageIt)
00180 imageIt->second->lock.lock ();
00181
00182 cout << "done\nlocking gray images..." << flush;
00183 for (imageIt = gray_images_.begin (); imageIt != gray_images_.end (); ++imageIt)
00184 imageIt->second->lock.lock ();
00185
00186
00187
00188 cout << "locking rgb images..." << flush;
00189 char file_name[255];
00190 for (imageIt = rgb_images_.begin (); imageIt != rgb_images_.end (); ++imageIt)
00191 {
00192 sprintf (file_name, "rgb_%03u.png", index);
00193 imwrite (file_name, imageIt->second->image);
00194 }
00195
00196 for (imageIt = gray_images_.begin (); imageIt != gray_images_.end (); ++imageIt)
00197 {
00198 sprintf (file_name, "gray_%03u.png", index);
00199 imwrite (file_name, imageIt->second->image);
00200 }
00201
00202
00203
00204
00205
00206
00207
00208
00209 for (imageIt = rgb_images_.begin (); imageIt != rgb_images_.end (); ++imageIt)
00210 imageIt->second->lock.unlock ();
00211
00212 for (imageIt = gray_images_.begin (); imageIt != gray_images_.end (); ++imageIt)
00213 imageIt->second->lock.unlock ();
00214
00215
00216 }
00217
00218 void MyOpenNIExample::imageCallback (boost::shared_ptr<Image> image, void* cookie)
00219 {
00220 timeval timestamp;
00221 gettimeofday (×tamp, NULL);
00222
00223 double now = timestamp.tv_sec + timestamp.tv_usec * 0.000001;
00224
00225
00226
00227
00228 image_timestamp = now;
00229 OpenNIDevice* device = reinterpret_cast<OpenNIDevice*>(cookie);
00230 ImageContext* rgb_image_context = rgb_images_[device->getConnectionString ()];
00231 ImageContext* gray_image_context = gray_images_[device->getConnectionString ()];
00232
00233
00234 unique_lock<mutex> rgb_lock (rgb_image_context->lock);
00235 unsigned char* rgb_buffer = (unsigned char*)(rgb_image_context->image.data + (rgb_image_context->image.cols >> 2) * rgb_image_context->image.elemSize () +
00236 (rgb_image_context->image.rows >> 2) * rgb_image_context->image.step);
00237 image->fillRGB (rgb_image_context->image.cols >> 1, rgb_image_context->image.rows >> 1, rgb_buffer, rgb_image_context->image.step);
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259 rgb_image_context->is_new = true;
00260 rgb_lock.unlock ();
00261
00262 unique_lock<mutex> gray_lock (gray_image_context->lock);
00263
00264 unsigned char* gray_buffer = (unsigned char*)(gray_image_context->image.data + (gray_image_context->image.cols >> 2) +
00265 (gray_image_context->image.rows >> 2) * gray_image_context->image.step);
00266 image->fillGrayscale (gray_image_context->image.cols >> 1, gray_image_context->image.rows >> 1, gray_buffer, gray_image_context->image.step);
00267
00268 gray_image_context->is_new = true;
00269 }
00270
00271 void MyOpenNIExample::depthCallback (boost::shared_ptr<DepthImage> depth, void* cookie)
00272 {
00273
00274 timeval timestamp;
00275 gettimeofday (×tamp, NULL);
00276 depth_timestamp = timestamp.tv_sec + timestamp.tv_usec * 0.000001;
00277
00278 OpenNIDevice* device = reinterpret_cast<OpenNIDevice*>(cookie);
00279 ImageContext* depth_image_context = depth_images_[device->getConnectionString ()];
00280
00281
00282 unique_lock<mutex> depth_lock (depth_image_context->lock);
00283 float* buffer = (float*)(depth_image_context->image.data + (depth_image_context->image.cols >> 2) * sizeof(float) +
00284 (depth_image_context->image.rows >> 2) * depth_image_context->image.step );
00285 depth->fillDepthImage (depth_image_context->image.cols >> 1, depth_image_context->image.rows >> 1, buffer, depth_image_context->image.step);
00286
00287 depth_image_context->is_new = true;
00288 }
00289
00290 int MyOpenNIExample::run ()
00291 {
00292 running_ = true;
00293 try
00294 {
00295 while (running_)
00296 {
00297 for (map<string, ImageContext*>::iterator imageIt = rgb_images_.begin (); imageIt != rgb_images_.end (); ++imageIt)
00298 {
00299 if (imageIt->second->is_new && imageIt->second->lock.try_lock ())
00300 {
00301 cv::Mat bgr_image;
00302 cvtColor (imageIt->second->image, bgr_image, CV_RGB2BGR);
00303 imshow (imageIt->first + "RGB", bgr_image);
00304 imageIt->second->is_new = false;
00305 imageIt->second->lock.unlock ();
00306 }
00307 }
00308
00309 for (map<string, ImageContext*>::iterator imageIt = gray_images_.begin (); imageIt != gray_images_.end (); ++imageIt)
00310 {
00311 if (imageIt->second->is_new && imageIt->second->lock.try_lock ())
00312 {
00313 imshow (imageIt->first + "Gray", imageIt->second->image);
00314 imageIt->second->is_new = false;
00315 imageIt->second->lock.unlock ();
00316 }
00317 }
00318
00319 for (map<string, ImageContext*>::iterator imageIt = depth_images_.begin (); imageIt != depth_images_.end (); ++imageIt)
00320 {
00321 if (imageIt->second->is_new && imageIt->second->lock.try_lock ())
00322 {
00323 Mat gray_image;
00324 imageIt->second->image.convertTo (gray_image, CV_8UC1, 25.5);
00325 imshow (imageIt->first + "Depth", gray_image);
00326 imageIt->second->is_new = false;
00327 imageIt->second->lock.unlock ();
00328 }
00329 }
00330
00331 unsigned char key = waitKey (30) & 0xFF;
00332
00333 switch (key)
00334 {
00335 case 27:
00336 case 'q':
00337 case 'Q': running_ = false;
00338 break;
00339
00340 case '1':
00341 selected_device_ = 0;
00342 break;
00343 case '2':
00344 selected_device_ = 1;
00345 break;
00346 case '3':
00347 selected_device_ = 2;
00348 break;
00349
00350 case 'r':
00351 case 'R':
00352 devices_[selected_device_]->setDepthRegistration (!devices_[selected_device_]->isDepthRegistered ());
00353 break;
00354 case 's':
00355 case 'S':
00356 if (devices_[selected_device_]->isSynchronizationSupported ())
00357 devices_[selected_device_]->setSynchronization (!devices_[selected_device_]->isSynchronized ());
00358 break;
00359 case 'c':
00360 case 'C':
00361 if (devices_[selected_device_]->isDepthCropped ())
00362 {
00363 depth_images_[devices_[selected_device_]->getConnectionString ()]->lock.lock ();
00364
00365 depth_images_[devices_[selected_device_]->getConnectionString ()]->image.rows = 480;
00366 depth_images_[devices_[selected_device_]->getConnectionString ()]->image.cols = 640;
00367 depth_images_[devices_[selected_device_]->getConnectionString ()]->lock.unlock ();
00368 devices_[selected_device_]->setDepthCropping (0, 0, 0, 0);
00369 }
00370 else if (devices_[selected_device_]->isDepthCroppingSupported ())
00371 {
00372 depth_images_[devices_[selected_device_]->getConnectionString ()]->lock.lock ();
00373
00374 depth_images_[devices_[selected_device_]->getConnectionString ()]->image.rows = 240;
00375 depth_images_[devices_[selected_device_]->getConnectionString ()]->image.cols = 320;
00376 depth_images_[devices_[selected_device_]->getConnectionString ()]->lock.unlock ();
00377 devices_[selected_device_]->setDepthCropping (100, 100, 320, 240);
00378 }
00379 break;
00380
00381 case 'd':
00382 case 'D':
00383 if (devices_[selected_device_]->isDepthStreamRunning ())
00384 devices_[selected_device_]->stopDepthStream ();
00385 else
00386 devices_[selected_device_]->startDepthStream ();
00387 break;
00388 case 'i':
00389 case 'I':
00390 if (devices_[selected_device_]->isImageStreamRunning ())
00391 devices_[selected_device_]->stopImageStream ();
00392 else
00393 devices_[selected_device_]->startImageStream ();
00394 break;
00395
00396 case 'w':
00397 case 'W':
00398 writeImages ();
00399 break;
00400 }
00401 }
00402 }
00403 catch (const OpenNIException& exception)
00404 {
00405 cout << "exception caught: " << exception.what () << endl;
00406 return (-1);
00407 }
00408 catch (...)
00409 {
00410
00411 cout << "unknown exception caught" << endl;
00412 return (-1);
00413 }
00414 return 0;
00415 }
00416
00417 int main (int argc, char** argv)
00418 {
00419 OpenNIDriver& driver = OpenNIDriver::getInstance ();
00420 if (argc == 1)
00421 {
00422 cout << "Usage: " << argv[0] << " (<device-index>)+" << endl;
00423 if (driver.getNumberDevices () > 0)
00424 {
00425 for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
00426 {
00427 cout << "Device: " << deviceIdx << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx)
00428 << ", connected: " << (int)driver.getBus (deviceIdx) << " @ " << (int)driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << endl;
00429 }
00430 }
00431 else
00432 cout << "No devices connected." << endl;
00433 exit (1);
00434 }
00435
00436 vector <unsigned> device_indices;
00437 for (int argIdx = 1; argIdx < argc; ++argIdx)
00438 {
00439 unsigned deviceIdx = (unsigned)atoi (argv[argIdx]);
00440 if (deviceIdx >= driver.getNumberDevices ())
00441 {
00442 if (driver.getNumberDevices () > 0)
00443 {
00444 cout << "Device index out of range. " << driver.getNumberDevices () << " devices found." << endl;
00445 for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
00446 {
00447 cout << "Device: " << deviceIdx << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: "
00448 << driver.getProductName (deviceIdx) << ", connected: " << (int)driver.getBus (deviceIdx) << " @ "
00449 << (int)driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << endl;
00450 }
00451 }
00452 else
00453 cout << "No devices connected." << endl;
00454 exit (-1);
00455 }
00456 device_indices.push_back ((unsigned)deviceIdx);
00457 }
00458
00459 cout << "<1,2,3...> to select device" << endl;
00460 cout << "<I> to start or stop image stream of selected device" << endl;
00461 cout << "<D> to start or stop depth stream of selected device" << endl;
00462 cout << "<R> to turn on or off registration for selected device" << endl;
00463 cout << "<S> to turn on or off synchronization for selected device" << endl;
00464 cout << "<C> to turn on or off image cropping for selected device" << endl;
00465 cout << "<W> write current images" << endl;
00466 cout << "<Q> to quit application" << endl;
00467 MyOpenNIExample example (device_indices);
00468
00469 return example.run ();
00470 }