Dissertation

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>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/io/vtk_io.h>

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

const int NUM_CAPTURES = 6;
const int WIDTH = 1600, HEIGHT = 800;
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>());

  pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr zCondition(new pcl::FieldComparison<pcl::PointXYZRGB>("z", pcl::ComparisonOps::LT, 1));
  pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr xCondition1(new pcl::FieldComparison<pcl::PointXYZRGB>("x", pcl::ComparisonOps::LT, 0.2));
  pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr xCondition2(new pcl::FieldComparison<pcl::PointXYZRGB>("x", pcl::ComparisonOps::GT, -0.2));

  range_cond->addComparison(zCondition);
  range_cond->addComparison(xCondition1);
  range_cond->addComparison(xCondition2);

  // 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 transformCloud(const PointCloud::ConstPtr c_in, Eigen::Affine3f transform)
{
  PointCloud c_out;
  pcl::transformPointCloud<pcl::PointXYZRGB>(*c_in, c_out, transform);
  return c_out.makeShared();
}

PointCloud::ConstPtr normalizeAlongXAxis(const PointCloud::ConstPtr c_in)
{
  PointCloud c_in_points = *c_in.get();

  float x = 0;

  for(int i=0; i < c_in_points.points.size(); i++)
  {
    x += c_in_points.points[i].x;
  }

  x /= c_in_points.points.size();

  Eigen::Affine3f transform;
  transform = Eigen::Translation3f(-x, 0, 0);

  return transformCloud(c_in, transform);
}

PointCloud::ConstPtr yTranslateCloud(const PointCloud::ConstPtr c_in)
{
  
  PointCloud c_in_points = *c_in.get();
  float miny = 0;
  for(int i=0; i < c_in_points.points.size(); i++)
  {
    pcl::PointXYZRGB point = c_in_points.points[i];
    miny = std::min(point.y, miny);
  }

  Eigen::Affine3f transform;
  transform = Eigen::Translation3f(0, 0, miny - 0.423);

  return transformCloud(c_in, transform);
}

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

  return transformCloud(c_in, transform);
}

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 = currentCloudID();
  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(yTranslateCloud(normalizeAlongXAxis(filterCloudByDepth(g_cloud))));
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> handler(cloud);

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


void createMesh() {

  pcl::PointCloud<pcl::PointXYZRGB> cloud_xyzrgb;

  if (g_cloud && cld_mutex.try_lock ()) 
  {
    cloud_xyzrgb = *g_cloud.get(); 
    cld_mutex.unlock ();
  }

  pcl::PointCloud<pcl::PointXYZ> cloud_xyz;
  cloud_xyz.points.resize(cloud_xyzrgb.size());

  for (size_t i = 0; i < cloud_xyz.points.size(); i++) {
      cloud_xyz.points[i].x = cloud_xyzrgb.points[i].x;
      cloud_xyz.points[i].y = cloud_xyzrgb.points[i].y;
      cloud_xyz.points[i].z = cloud_xyzrgb.points[i].z;
  }
  pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud = cloud_xyz.makeShared();

  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud);
  n.setInputCloud (cloud);
  n.setSearchMethod (tree);
  n.setKSearch (20);
  n.compute (*normals);
  //* normals should not contain the point normals + surface curvatures

  // Concatenate the XYZ and normal fields*
  pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
  pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
  //* cloud_with_normals = cloud + normals

  // Create search tree*
  pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
  tree2->setInputCloud (cloud_with_normals);

  // Initialize objects
  pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
  pcl::PolygonMesh triangles;

  // Set the maximum distance between connected points (maximum edge length)
  gp3.setSearchRadius (0.0005);

  // Set typical values for the parameters
  gp3.setMu (2.5);
  gp3.setMaximumNearestNeighbors (300);
  gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
  gp3.setMinimumAngle(M_PI/18); // 10 degrees
  gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
  gp3.setNormalConsistency(true);

  // Get result
  gp3.setInputCloud (cloud_with_normals);
  gp3.setSearchMethod (tree2);
  gp3.reconstruct (triangles);

  pcl::io::saveVTKFile("mesh.vtk", triangles);
}

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;
      case 13: // ENTER
        //createMesh();
        break;
      case 37: //(left arrow)
        break;
      case 38: //(up arrow)
        break;
      case 39: //(right arrow)
        break;
      case 40: //(down arrow) 
        break;
    }
  }
}

void initialize(pcl::Grabber* grabber) {
  grabber->start();

	viewer->getRenderWindow()->SetSize(WIDTH, HEIGHT);
  viewer->addLine(pcl::PointXYZ(0,0,0), pcl::PointXYZ(1, 0, 0), 255, 0, 0, "xaxis");
  viewer->addLine(pcl::PointXYZ(0,0,0), pcl::PointXYZ(0,-1, 0), 0, 255, 0, "yaxis");
  viewer->addLine(pcl::PointXYZ(0,0,0), pcl::PointXYZ(0, 0, 1), 0, 0, 255, "zaxis");
  viewer->spinOnce();

}

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();
}