openni_range_image_visualization.cpp

Go to the documentation of this file.

/* \author Bastian Steder */

/* ---[ */
#include <iostream>
using namespace std;
#include <pcl/io/openni_grabber.h>
#include "pcl/range_image/range_image_planar.h"
#include "pcl/common/common_headers.h"
#include "pcl/visualization/range_image_visualizer.h"
#include "pcl/visualization/pcl_visualizer.h"
#include "pcl/console/parse.h"

using namespace pcl;
using namespace pcl::visualization;

std::string device_id = "#1";

float angular_resolution = -1.0f;
//RangeImage::CoordinateFrame coordinate_frame = RangeImage::CAMERA_FRAME;
//float noise_level = 0.03;    // Factor regarding average model radius
//bool set_unseen_to_max_range = true;
//int range_image_source = -1;

boost::mutex depth_image_mutex,
             ir_image_mutex,
             image_mutex;
pcl::PointCloud<pcl::PointXYZ>::ConstPtr point_cloud_ptr;
boost::shared_ptr<openni_wrapper::DepthImage> depth_image_ptr;
boost::shared_ptr<openni_wrapper::IRImage> ir_image_ptr;
boost::shared_ptr<openni_wrapper::Image> image_ptr;

bool received_new_depth_data = false,
     received_new_ir_image   = false,
     received_new_image   = false;
struct EventHelper
{
  //void 
  //cloud_cb (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud)
  //{
    //if (mutex_.try_lock ())
    //{
      //point_cloud_ptr = cloud;
      //received_new_depth_data = true;
      //mutex_.unlock ();
    //}
  //}
  
  void
  depth_image_cb (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image)
  {
    if (depth_image_mutex.try_lock ())
    {
      depth_image_ptr = depth_image;
      //const unsigned short* depth_map = depth_image->getDepthMetaData().Data();
      //int width = depth_image->getWidth (), height = depth_image->getWidth ();
      //float center_x = width/2, center_y = height/2;
      //float focal_length_x = depth_image->getFocalLength(), focal_length_y = focal_length_x;
      //float original_angular_resolution = asinf (0.5f*float(width)/float(focal_length_x)) / (0.5f*float(width));
      //float desired_angular_resolution = angular_resolution;
      //cout << PVARC(width)<<PVARC(height)<<PVARC(center_x)<<PVARC(center_y)
           //<< PVARC(focal_length_x)<<PVARC(focal_length_y)
           //<< PVARAC(original_angular_resolution)<<PVARAC(desired_angular_resolution)
           //<< "\n";
      //received_new_depth_data = depth_image_ptr;
      depth_image_mutex.unlock ();
      received_new_depth_data = true;
    }
  }
  
  void
  ir_image_cb (const boost::shared_ptr<openni_wrapper::IRImage>& ir_image)
  {
    if (ir_image_mutex.try_lock ())
    {
      ir_image_ptr = ir_image;
      ir_image_mutex.unlock ();
      received_new_ir_image = true;
    }
  }
  
  void
  image_cb (const boost::shared_ptr<openni_wrapper::Image>& image)
  {
    if (image_mutex.try_lock ())
    {
      image_ptr = image;
      image_mutex.unlock ();
      received_new_image = true;
    }
  }
};


void
printUsage (const char* progName)
{
  cout << "\n\nUsage: "<<progName<<" [options] [scene.pcd] <model.pcl> [model_2.pcl] ... [model_n.pcl]\n\n"
       << "Options:\n"
       << "-------------------------------------------\n"
       << "-r <float>      angular resolution in degrees (default "<<rad2deg (angular_resolution)<<")\n"
       << "-d <device_id>  set the device id (default \""<<device_id<<"\")\n"
       << "-h           this help\n"
       << "\n\n";
}

int main (int argc, char** argv)
{
  // --------------------------------------
  // -----Parse Command Line Arguments-----
  // --------------------------------------
  if (pcl::console::find_argument (argc, argv, "-h") >= 0)
  {
    printUsage (argv[0]);
    return 0;
  }
  if (pcl::console::parse (argc, argv, "-d", device_id) >= 0)
    cout << "Using device id \""<<device_id<<"\".\n";
  if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
    cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";
  angular_resolution = deg2rad (angular_resolution);
  
  RangeImageVisualizer range_image_widget ("Range Image"),
                       ir_image_widget ("IR Image"),
                       rgb_image_widget ("RGB Image");
  
  PCLVisualizer viewer ("3D Viewer");
  viewer.addCoordinateSystem (1.0f);
  viewer.setBackgroundColor (1, 1, 1);
  
  viewer.initCameraParameters ();
  viewer.camera_.pos[0] = 0.0;
  viewer.camera_.pos[1] = -0.3;
  viewer.camera_.pos[2] = -2.0;
  viewer.camera_.focal[0] = 0.0;
  viewer.camera_.focal[1] = 0.0+viewer.camera_.pos[1];
  viewer.camera_.focal[2] = 1.0;
  viewer.camera_.view[0] = 0.0;
  viewer.camera_.view[1] = -1.0;
  viewer.camera_.view[2] = 0.0;
  viewer.updateCamera();

  openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance ();
  if (driver.getNumberDevices () > 0)
  {
    for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx)
    {
      cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx)
           << ", product: " << driver.getProductName (deviceIdx) << ", connected: "
           << (int) driver.getBus (deviceIdx) << " @ " << (int) driver.getAddress (deviceIdx)
           << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'\n";
    }
  }
  else
  {
    cout << "\nNo devices connected.\n\n";
    return 1;
  }
  
  pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
  EventHelper event_helper;
  //boost::function<void (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&) > f_point_cloud =
    //boost::bind (&EventHelper::cloud_cb, &event_helper, _1);
  //boost::signals2::connection c_point_cloud = interface->registerCallback (f_point_cloud);
  
  boost::function<void (const boost::shared_ptr<openni_wrapper::DepthImage>&) > f_depth_image =
    boost::bind (&EventHelper::depth_image_cb, &event_helper, _1);
  boost::signals2::connection c_depth_image = interface->registerCallback (f_depth_image);
  
  boost::function<void (const boost::shared_ptr<openni_wrapper::IRImage>&) > f_ir_image =
    boost::bind (&EventHelper::ir_image_cb, &event_helper, _1);
  //boost::signals2::connection c_ir_image = interface->registerCallback (f_ir_image);
  
  boost::function<void (const boost::shared_ptr<openni_wrapper::Image>&) > f_image =
    boost::bind (&EventHelper::image_cb, &event_helper, _1);
  boost::signals2::connection c_image = interface->registerCallback (f_image);
  
  cout << "Starting grabber\n";
  interface->start ();
  cout << "Done\n";
  
  boost::shared_ptr<RangeImagePlanar> range_image_planar_ptr (new RangeImagePlanar);
  RangeImagePlanar& range_image_planar = *range_image_planar_ptr;
  
  while (true)
  {
    viewer.spinOnce (10);
    ImageWidgetWX::spinOnce ();  // process GUI events
    usleep (10000);
    
    bool got_new_range_image = false;
    if (received_new_depth_data && depth_image_mutex.try_lock ())
    {
      received_new_depth_data = false;
      
      unsigned long time_stamp = depth_image_ptr->getTimeStamp ();
      int frame_id = depth_image_ptr->getFrameID ();
      const unsigned short* depth_map = depth_image_ptr->getDepthMetaData().Data();
      int width = depth_image_ptr->getWidth (), height = depth_image_ptr->getHeight ();
      float center_x = width/2, center_y = height/2;
      float focal_length_x = depth_image_ptr->getFocalLength(), focal_length_y = focal_length_x;
      float original_angular_resolution = asinf (0.5f*float(width)/float(focal_length_x)) / (0.5f*float(width));
      float desired_angular_resolution = angular_resolution;
      range_image_planar.setDepthImage (depth_map, width, height, center_x, center_y,
                                        focal_length_x, focal_length_y, desired_angular_resolution);
      // TODO handling of shadows / no data etc...
      depth_image_mutex.unlock ();
      got_new_range_image = !range_image_planar.points.empty();

      //cout << "Depth image: " << PVARC(time_stamp)<<PVARC(frame_id)
           //<< PVARC(width)<<PVARC(height)<<PVARC(center_x)<<PVARC(center_y)
           //<< PVARC(focal_length_x)<<PVARC(focal_length_y)
           //<< PVARAC(original_angular_resolution)<<PVARAC(desired_angular_resolution)
           //<< "\n";
      
      depth_image_mutex.unlock ();
    }
    
    if (received_new_ir_image && ir_image_mutex.try_lock ())
    {
      received_new_ir_image = false;
      
      unsigned long time_stamp = ir_image_ptr->getTimeStamp ();
      int frame_id = ir_image_ptr->getFrameID ();
      const unsigned short* ir_map = ir_image_ptr->getMetaData().Data();
      int width = ir_image_ptr->getWidth (), height = ir_image_ptr->getHeight ();
      float center_x = width/2, center_y = height/2;
      //float focal_length_x = ir_image_ptr->getFocalLength(), focal_length_y = focal_length_x;
      
      cout << "IR image: " << PVARC(time_stamp)<<PVARC(frame_id)
           << PVARC(width)<<PVARC(height)<<PVARC(center_x)<<PVARC(center_y)
           //<< PVARC(focal_length_x)<<PVARC(focal_length_y)
           << "\n";
      ir_image_widget.setShortImage(ir_map, width, height, "IR Image", 0, 1000);
      
      ir_image_mutex.unlock ();
    }
    
    if (received_new_image && image_mutex.try_lock ())
    {
      received_new_image = false;
      
      unsigned long time_stamp = image_ptr->getTimeStamp ();
      int frame_id = image_ptr->getFrameID ();
      openni_wrapper::Image::Encoding encoding = image_ptr->getEncoding();
      //if (encoding==openni_wrapper::Image::RGB)
        //cout << "Image encoding is RGB.\n";
      //else if(encoding==openni_wrapper::Image::BAYER_GRBG)
        //cout << "Image encoding is BAYER_GRBG.\n";
      //else if(encoding==openni_wrapper::Image::YUV422)
        //cout << "Image encoding is YUV422.\n";
      const unsigned char* color_map = image_ptr->getMetaData().Data();
      int width = image_ptr->getWidth (), height = image_ptr->getHeight ();
      float center_x = width/2, center_y = height/2;
      //float focal_length_x = image_ptr->getFocalLength(), focal_length_y = focal_length_x;
      

      if (encoding==openni_wrapper::Image::RGB)
        rgb_image_widget.setRGBImage(color_map, width, height);
      else if(encoding==openni_wrapper::Image::BAYER_GRBG)
        rgb_image_widget.setBayerGRBGImage(color_map, width, height);
      
      
      //cout << "RGB image: " << PVARC(time_stamp)<<PVARC(frame_id)
           //<< PVARC(width)<<PVARC(height)<<PVARC(center_x)<<PVARC(center_y)
           //<< "\n";
      
      image_mutex.unlock ();
    }
    
    if (!got_new_range_image)
      continue;
    
    cout << "." << std::flush;
    
    static bool first_range_image = true;
    range_image_widget.setRangeImage (range_image_planar, 0.0f, 10.0f);
    
    PointCloudColorHandlerCustom<pcl::PointWithRange> color_handler_cloud (range_image_planar_ptr, 0, 0, 0);
    if (first_range_image)
    {
      viewer.addPointCloud<pcl::PointWithRange> (range_image_planar_ptr, color_handler_cloud, "range image");
      first_range_image = false;
    }
    else
    {
      viewer.updatePointCloud<pcl::PointWithRange> (range_image_planar_ptr, color_handler_cloud, "range image");
    }
  }
  
  interface->stop ();
}