gistfile1.cpp

#define MEASURE_FUNCTION_TIME
#include <boost/thread/thread.hpp>
#include <boost/make_shared.hpp>
#include <pcl/common/time.h> //fps calculations
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/point_cloud_handlers.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/image_viewer.h>
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
#include <pcl/common/transforms.h>

boost::mutex cld_mutex;

boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr g_cloud;

const float ANGLE_STEP = 0.125*M_PI;
float angle = 0;

struct EventHelper
{
  void cloud_cb (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud)
  {
    cld_mutex.lock ();

    g_cloud = cloud;

    cld_mutex.unlock ();
  }
};

void keyboard_callback (const pcl::visualization::KeyboardEvent& event)
{
  if(event.keyDown())
  {
    if(event.getKeyCode() == 32) // space
    {
      if (g_cloud && cld_mutex.try_lock ()) 
      {
        Eigen::Affine3f transform;
        transform = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitY());
      
        pcl::PointCloud<pcl::PointXYZRGB> c_out;
    
        pcl::transformPointCloud<pcl::PointXYZRGB>(*g_cloud, c_out, transform);

        pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr new_cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB>> (c_out);
        pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> handler(new_cloud);
      
        std::stringstream cloud_name_stream;
        int id = int(angle / ANGLE_STEP) % int(2*M_PI / ANGLE_STEP);
        cloud_name_stream << "cloud" << id;
        std::string cloud_name = cloud_name_stream.str();

	      if (!viewer->updatePointCloud(new_cloud, handler, cloud_name))
	      {
	        viewer->addPointCloud(new_cloud, handler, cloud_name);
          if(id == 0)
          {
	          viewer->resetCameraViewpoint(cloud_name);
          }
	      }
      
        cld_mutex.unlock ();
      }
    }
    else if(event.getKeyCode() == 45) // -
    {
      angle -= ANGLE_STEP;
    }
    else if(event.getKeyCode() == 61) // +
    {
      angle += ANGLE_STEP;
    }
  }
}

void initialize() {
  if (g_cloud && cld_mutex.try_lock ())
  {
    viewer->spinOnce ();
	  viewer->getRenderWindow ()->SetSize (g_cloud->width, g_cloud->height);
	  viewer->getRenderWindow ()->SetPosition (g_cloud->width, 0);
	  cld_mutex.unlock ();
  }
}

int main (int argc, char** argv)
{
  EventHelper event_helper;
  std::string device_id = "";

  pcl::console::parse_argument(argc, argv, "-dev", device_id);
  pcl::Grabber* grabber = new pcl::OpenNIGrabber (device_id);

  viewer.reset(new pcl::visualization::PCLVisualizer (argc, argv, "OpenNI Viewer"));
  viewer->registerKeyboardCallback(&keyboard_callback);

  boost::function<void(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr&)> f = boost::bind(&EventHelper::cloud_cb, &event_helper, _1);
  boost::signals2::connection c1 = grabber->registerCallback(f);

  grabber->start ();
  initialize();

  while (!viewer->wasStopped ())
  {
		viewer->spinOnce();
		boost::this_thread::sleep (boost::posix_time::microseconds(200));
  }

  grabber->stop ();
}