Patched openni2_viewer to support Structure.io (and other OpenNI2 dev…

…ices without RGB sensor)
PointCloudLibrary · Mar 11, 2015 · 7dfc3ca · 7dfc3ca
1 parent d1ea2fd
commit 7dfc3ca
Show file tree

Hide file tree

Showing 3 changed files with 73 additions and 8 deletions.
diff --git a/io/src/openni2/openni2_device.cpp b/io/src/openni2/openni2_device.cpp
@@ -89,7 +89,10 @@ pcl::io::openni2::OpenNI2Device::OpenNI2Device (const std::string& device_URI) :
   // Set default resolution if not reading a file
   if (!openni_device_->isFile ())
   {
-    setColorVideoMode (getDefaultColorMode ());
+    if (openni_device_->hasSensor (openni::SENSOR_COLOR))
+    {
+     setColorVideoMode (getDefaultColorMode ());
+    }
     setDepthVideoMode (getDefaultDepthMode ());
     setIRVideoMode (getDefaultIRMode ());
   }

diff --git a/io/src/openni2_grabber.cpp b/io/src/openni2_grabber.cpp
@@ -219,7 +219,7 @@ pcl::io::OpenNI2Grabber::start ()
   try
   {
     // check if we need to start/stop any stream
-    if (image_required_ && !device_->isColorStreamStarted () )
+    if (image_required_ && !device_->isColorStreamStarted () && device_->hasColorSensor () )
     {
       block_signals ();
       device_->startColorStream ();
@@ -401,8 +401,8 @@ pcl::io::OpenNI2Grabber::startSynchronization ()
 {
   try
   {
-    if (device_->isSynchronizationSupported () && !device_->isSynchronized () && !device_->isFile () &&
-      device_->getColorVideoMode ().frame_rate_ == device_->getDepthVideoMode ().frame_rate_)
+    if (device_->hasColorSensor () && (device_->isSynchronizationSupported () && !device_->isSynchronized () && !device_->isFile () &&
+      device_->getColorVideoMode ().frame_rate_ == device_->getDepthVideoMode ().frame_rate_))
       device_->setSynchronization (true);
   }
   catch (const IOException& exception)

diff --git a/visualization/tools/openni2_viewer.cpp b/visualization/tools/openni2_viewer.cpp
@@ -42,6 +42,7 @@
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/visualization/boost.h>
 #include <pcl/visualization/image_viewer.h>
+#include <pcl/visualization/common/float_image_utils.h>
 #include <pcl/console/print.h>
 #include <pcl/console/parse.h>
 #include <pcl/console/time.h>
@@ -119,8 +120,10 @@ class OpenNI2Viewer
   OpenNI2Viewer (pcl::io::OpenNI2Grabber& grabber)
     : cloud_viewer_ (new pcl::visualization::PCLVisualizer ("PCL OpenNI2 cloud"))
     , image_viewer_ ()
+    , depth_viewer_ ()
     , grabber_ (grabber)
     , rgb_data_ (0), rgb_data_size_ (0)
+    , xyz_data_ (0), xyz_data_size_ (0)
   {
   }
 
@@ -152,6 +155,17 @@ class OpenNI2Viewer
     }
   }
 
+
+  void
+  depth_callback (const boost::shared_ptr<pcl::io::openni2::DepthImage>& depth)
+  {
+    FPS_CALC ("depth callback");
+
+    boost::mutex::scoped_lock lock (depth_mutex_);
+    depth_ = depth;
+
+  }
+
   void
   keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
   {
@@ -189,20 +203,34 @@ class OpenNI2Viewer
     boost::signals2::connection image_connection;
     if (grabber_.providesCallback<void (const boost::shared_ptr<pcl::io::openni2::Image>&)>())
     {
-      image_viewer_.reset (new pcl::visualization::ImageViewer ("PCL OpenNI image"));
+      image_viewer_.reset (new pcl::visualization::ImageViewer ("PCL OpenNI2 image"));
       image_viewer_->registerMouseCallback (&OpenNI2Viewer::mouse_callback, *this);
       image_viewer_->registerKeyboardCallback (&OpenNI2Viewer::keyboard_callback, *this);
       boost::function<void (const boost::shared_ptr<pcl::io::openni2::Image>&) > image_cb = boost::bind (&OpenNI2Viewer::image_callback, this, _1);
       image_connection = grabber_.registerCallback (image_cb);
     }
 
-    bool image_init = false, cloud_init = false;
+    boost::signals2::connection depth_connection;
+    if (grabber_.providesCallback<void (const boost::shared_ptr<pcl::io::openni2::DepthImage>&)>())
+    {
+      depth_viewer_.reset (new pcl::visualization::ImageViewer ("PCL OpenNI2 Depth"));
+      depth_viewer_->registerMouseCallback (&OpenNI2Viewer::mouse_callback, *this);
+      depth_viewer_->registerKeyboardCallback (&OpenNI2Viewer::keyboard_callback, *this);
+      boost::function<void (const boost::shared_ptr<pcl::io::openni2::DepthImage>&) > depth_cb = boost::bind (&OpenNI2Viewer::depth_callback, this, _1);
+      depth_connection = grabber_.registerCallback (depth_cb);
+    }
+
+
+
+
+    bool image_init = false, cloud_init = false, depth_init = false;
 
     grabber_.start ();
 
-    while (!cloud_viewer_->wasStopped () && (image_viewer_ && !image_viewer_->wasStopped ()))
+    while (!cloud_viewer_->wasStopped () && ((image_viewer_ && !image_viewer_->wasStopped ()) || ( depth_viewer_ && !depth_viewer_->wasStopped ())))
     {
       boost::shared_ptr<pcl::io::openni2::Image> image;
+      boost::shared_ptr<pcl::io::openni2::DepthImage> depth;
       CloudConstPtr cloud;
 
       cloud_viewer_->spinOnce ();
@@ -260,32 +288,66 @@ class OpenNI2Viewer
         image_viewer_->spinOnce ();
 
       }
-    }
 
+      if (depth_mutex_.try_lock ())
+      {
+        depth_.swap (depth);
+        depth_mutex_.unlock ();
+      }
+
+      if (depth)
+      {
+        if (!depth_init && cloud && cloud->width != 0)
+        {
+          depth_viewer_->setPosition (cloud->width, 0);
+          depth_viewer_->setSize (cloud->width, cloud->height);
+          depth_init = !depth_init;
+        }
+
+            unsigned char* data = pcl::visualization::FloatImageUtils::getVisualImage (
+                reinterpret_cast<const unsigned short*> (depth->getData ()), 
+                  depth->getWidth (), depth->getHeight (),
+                  std::numeric_limits<unsigned short>::min (), 
+                  std::numeric_limits<unsigned short>::max () / 10, 
+                                                       true);
+
+        depth_viewer_->addRGBImage ( data, depth->getWidth (), depth->getHeight ());
+
+        depth_viewer_->spinOnce ();
+
+      }
+    }
     grabber_.stop ();
 
     cloud_connection.disconnect ();
     image_connection.disconnect ();
+    depth_connection.disconnect ();
     if (rgb_data_)
       delete[] rgb_data_;
   }
 
   boost::shared_ptr<pcl::visualization::PCLVisualizer> cloud_viewer_;
   boost::shared_ptr<pcl::visualization::ImageViewer> image_viewer_;
+  boost::shared_ptr<pcl::visualization::ImageViewer> depth_viewer_;
 
   pcl::io::OpenNI2Grabber& grabber_;
   boost::mutex cloud_mutex_;
   boost::mutex image_mutex_;
+  boost::mutex depth_mutex_;
 
   CloudConstPtr cloud_;
   boost::shared_ptr<pcl::io::openni2::Image> image_;
+  boost::shared_ptr<pcl::io::openni2::DepthImage> depth_;
   unsigned char* rgb_data_;
+  unsigned char* xyz_data_;
   unsigned rgb_data_size_;
+  unsigned xyz_data_size_;
 };
 
 // Create the PCLVisualizer object
 boost::shared_ptr<pcl::visualization::PCLVisualizer> cld;
 boost::shared_ptr<pcl::visualization::ImageViewer> img;
+boost::shared_ptr<pcl::visualization::ImageViewer> dep;
 
 /* ---[ */
 int