gistfile1.cpp

#include <boost/thread/thread.hpp>
#include <boost/make_shared.hpp>
#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>
#include <pcl/filters/conditional_removal.h>

typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;

const int NUM_CAPTURES = 6;
const int WIDTH = 500, HEIGHT = 500;
const float ANGLE_STEP = 2 * M_PI / NUM_CAPTURES;

boost::mutex cld_mutex;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;

PointCloud::ConstPtr g_cloud;

float angle = 0;

void cloud_callback (const PointCloud::ConstPtr& cloud)
{
  cld_mutex.lock();

  g_cloud = cloud;

  cld_mutex.unlock();
}

PointCloud::ConstPtr filterCloudByDepth(const PointCloud::ConstPtr c_in) {
 // build the condition
  pcl::ConditionAnd<pcl::PointXYZRGB>::Ptr range_cond (new pcl::ConditionAnd<pcl::PointXYZRGB>());

  range_cond->addComparison (
    pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(
      new pcl::FieldComparison<pcl::PointXYZRGB>("z", pcl::ComparisonOps::LT, 1)
    )
  );

  // build the filter
  pcl::ConditionalRemoval<pcl::PointXYZRGB> conditional_remove(range_cond);
  conditional_remove.setInputCloud(c_in);

  // apply filter
  PointCloud c_out;
  conditional_remove.filter(c_out);
  return c_out.makeShared();
}

PointCloud::ConstPtr translateCloud(const PointCloud::ConstPtr c_in)
{
  Eigen::Affine3f transform;
  transform = Eigen::Translation3f(0,0,-0.7);

  PointCloud c_out;
  pcl::transformPointCloud<pcl::PointXYZRGB>(*c_in, c_out, transform);

  return c_out.makeShared();
}

PointCloud::ConstPtr rotateCloud(const PointCloud::ConstPtr c_in)
{
  Eigen::Affine3f transform;
  transform = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitY());

  PointCloud c_out;
  pcl::transformPointCloud<pcl::PointXYZRGB>(*c_in, c_out, transform);

  return c_out.makeShared();
}

std::string currentCloudID()
{
  int b = floor(angle / ANGLE_STEP + 0.5);
  int id = b % NUM_CAPTURES;

  id += (id < 0) ? NUM_CAPTURES : 0;

  std::stringstream cloud_name_stream;
  cloud_name_stream << "cloud" << id;

  return cloud_name_stream.str();
}

void addCloud(PointCloud::ConstPtr cloud,
   pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> handler,
   std::string name)
{
  if (!viewer->updatePointCloud(cloud, handler, name))
	{
	  viewer->addPointCloud(cloud, handler, name);
    if(name == "cloud0")
    {
	    viewer->resetCameraViewpoint(name);
    }
	}
}

void capture()
{
  if (g_cloud && cld_mutex.try_lock ()) 
  {
    PointCloud::ConstPtr cloud = rotateCloud(translateCloud(filterCloudByDepth(g_cloud)));
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> handler(cloud);

    addCloud(cloud, handler, currentCloudID());
      
    cld_mutex.unlock ();
  }
}

void keyboard_callback (const pcl::visualization::KeyboardEvent& event)
{
  if(event.keyDown())
  {
    switch(event.getKeyCode())
    {
      case 32: // SPACE
        capture();
        break;
      case 45: // -
        angle -= ANGLE_STEP;
        break;
      case 61: // +
        angle += ANGLE_STEP;
        break;
    }
  }
}

void initialize(pcl::Grabber* grabber) {
  grabber->start();
  if (g_cloud && cld_mutex.try_lock ())
  {
    viewer->spinOnce ();
	  viewer->getRenderWindow ()->SetSize (WIDTH, HEIGHT);
	  viewer->getRenderWindow ()->SetPosition (g_cloud->width, 0);
	  cld_mutex.unlock ();
  }
}

int main (int argc, char** argv)
{
  pcl::Grabber* grabber = new pcl::OpenNIGrabber ();

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

  boost::function<void(const PointCloud::ConstPtr&)> callback = boost::bind(&cloud_callback, _1);
  boost::signals2::connection c1 = grabber->registerCallback(callback);

  initialize(grabber);

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

  grabber->stop();
}