Dissertation - Meshing - Loading/Saving - Kinect not required

gistfile1.txt

#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>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/obj_io.h>

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

const int WIDTH = 1600, HEIGHT = 800;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;

boost::mutex cld_mutex;
PointCloud::ConstPtr g_cloud;
pcl::Grabber* grabber;

const int NUM_CAPTURES = 6;
PointCloud::ConstPtr clouds[NUM_CAPTURES];

const float ANGLE_STEP = 2 * M_PI / NUM_CAPTURES;
int currentCloud = 0;

const float X_STEP = 0.005;
const float Z_STEP = 0.005;

float xTranslation = -0.034999996;
float zTranslation = -0.71499950;

const std::string LOAD_DIRECTORY = "E:\\University\\Year 3\\Dissertation\\C++\\PCL\\visualization\\tools\\";
const std::string SAVE_DIRECTORY = "E:\\University\\Year 3\\Dissertation\\Meshes\\";

void cloud_callback (const PointCloud::ConstPtr& cloud)
{
  cld_mutex.lock();
  g_cloud = cloud;
  cld_mutex.unlock();
}

PointCloud::ConstPtr filterCloudByDepth(const PointCloud::ConstPtr c_in) {
  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);

  pcl::ConditionalRemoval<pcl::PointXYZRGB> conditional_remove(range_cond);
  conditional_remove.setInputCloud(c_in);

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

/* pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  icp.setInputCloud(cloud_in);
  icp.setInputTarget(cloud_out);*/

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 xTranslateCloud(const PointCloud::ConstPtr c_in)
{
  Eigen::Affine3f transform;
  transform = Eigen::Translation3f(xTranslation, 0, 0);
  return transformCloud(c_in, transform);
}

PointCloud::ConstPtr zTranslateCloud(const PointCloud::ConstPtr c_in)
{
  Eigen::Affine3f transform;
  transform = Eigen::Translation3f(0, 0, zTranslation);
  return transformCloud(c_in, transform);
}

PointCloud::ConstPtr rotateCloud(const PointCloud::ConstPtr c_in, int cloudID)
{
  float angle = cloudID * ANGLE_STEP;

  Eigen::Affine3f transform;
  transform = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitY());

  return transformCloud(c_in, transform);
}

std::string cloudName(int cloudNum, std::string filetype = "")
{
  std::stringstream cloud_name_stream;
  cloud_name_stream << "cloud" << cloudNum << filetype;

  return cloud_name_stream.str();
}

void updateCloud(PointCloud::ConstPtr cloud, int cloudID)
{
  PointCloud::ConstPtr transformedCloud = rotateCloud(zTranslateCloud(xTranslateCloud(cloud)), cloudID);
  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> handler(transformedCloud);

  std::string name = cloudName(cloudID);

  if (!viewer->updatePointCloud(transformedCloud, handler, name))
  {
    viewer->addPointCloud(transformedCloud, handler, name);
  }
}

void updateClouds()
{
  for(int i=0; i < NUM_CAPTURES; i++)
  {
    if(clouds[i]) {
      updateCloud(clouds[i], i);
    }
  }
}

void capture()
{
  if (g_cloud && cld_mutex.try_lock ())
  {
    PointCloud::ConstPtr c_in = filterCloudByDepth(g_cloud);
    clouds[currentCloud] = c_in;
    
    updateCloud(c_in, currentCloud);
    
    cld_mutex.unlock();
  }
}

void createMesh() {

  pcl::PointCloud<pcl::PointXYZ> cloud_xyz;

  int point_count = 0;

  for(int i=0; i < NUM_CAPTURES; i++) {
    point_count += clouds[i]->points.size();
  }
  
  cloud_xyz.points.resize(point_count);
  point_count = 0;

  for(int i=0; i < NUM_CAPTURES; i++) {
    if(clouds[i]) {

      PointCloud::ConstPtr cloud = rotateCloud(zTranslateCloud(xTranslateCloud(clouds[i])), i);

      for (size_t j = 0; j < cloud->points.size(); j++) {
        pcl::PointXYZRGB point = cloud->points[j];
        if(point.x == point.x) // Gaurd against NaN
        {
          cloud_xyz.points[point_count].x = point.x;
          cloud_xyz.points[point_count].y = point.y;
          cloud_xyz.points[point_count++].z = point.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.2);

  // Set typical values for the parameters
  const int mu = 2;
  const int maxNearestNeighbors = 200;
  const double maxSurfaceAngle = 2 * M_PI / 3;
  const double minAngle = M_PI / 18;
  const double maxAngle = 2 * M_PI / 3;
  const bool normalConsistency = false;

  gp3.setMu (mu);
  gp3.setMaximumNearestNeighbors(maxNearestNeighbors);
  gp3.setMaximumSurfaceAngle(maxSurfaceAngle);
  gp3.setMinimumAngle(minAngle);
  gp3.setMaximumAngle(maxAngle);
  gp3.setNormalConsistency(normalConsistency);

  // Get result
  gp3.setInputCloud (cloud_with_normals);
  gp3.setSearchMethod (tree2);
  gp3.reconstruct (triangles);
  
  /*Create Filename*/
  time_t t = time(0);
  struct tm * date = localtime( & t );

  std::stringstream mesh_name_stream;
  mesh_name_stream << (date->tm_year + 1900)  << '-' 
                   << (date->tm_mon + 1)      << '-'
                   << date->tm_mday           << "-"
                   << date->tm_hour           << "-"
                   << date->tm_min            << "-mesh";

  std::string mesh_name = mesh_name_stream.str();

  ofstream detailsFile(SAVE_DIRECTORY + mesh_name + ".txt");

  detailsFile << "mu = " << mu << std::endl
              << "maximum nearest neighbors = " << maxNearestNeighbors  << endl
              << "maximum surface angle = "     << maxSurfaceAngle      << endl
              << "minimum angle = "             << minAngle             << endl
              << "maximum angle = "             << maxAngle             << endl
              << "normal consistency = "        << normalConsistency    << endl;

  detailsFile.close();

  pcl::io::saveVTKFile(SAVE_DIRECTORY + mesh_name + ".vtk", triangles);
  pcl::io::saveOBJFile(SAVE_DIRECTORY + mesh_name + ".obj", triangles);

  viewer->addPolygonMesh(triangles);
}

void loadPointClouds()
{
  for(int i = 0; i < NUM_CAPTURES; i++) {
    PointCloud::Ptr cloud (new PointCloud);

    std::string filename = cloudName(i, ".pcd");
    int error = pcl::io::loadPCDFile<pcl::PointXYZRGB>(LOAD_DIRECTORY + filename, *cloud);

    if(error != -1)
    {
      xTranslation = 0;
      zTranslation = 0;
      clouds[i] = cloud;
    }
  }
  updateClouds();
}


void savePointClouds()
{
  for(int i = 0; i < NUM_CAPTURES; i++) {
    if(clouds[i]) {
      PointCloud::ConstPtr cloud = zTranslateCloud(xTranslateCloud(clouds[i]));
      pcl::io::savePCDFileASCII (SAVE_DIRECTORY + cloudName(i, ".pcd"), *cloud);
      pcl::io::savePLYFile (SAVE_DIRECTORY + cloudName(i, ".ply"), *cloud);
    }
  }
}

void initializeGrabber() {
  grabber = new pcl::OpenNIGrabber ();
  boost::function<void(const PointCloud::ConstPtr&)> callback = boost::bind(&cloud_callback, _1);
  boost::signals2::connection c1 = grabber->registerCallback(callback);
  grabber->start();
}

void keyboard_callback (const pcl::visualization::KeyboardEvent& event)
{
  if(event.keyDown())
  {
    char keyCode = event.getKeyCode();
    switch(keyCode)
    {
      case 32: // SPACE
        if(!grabber)
          initializeGrabber();
        capture();
        break;
      case 13: // ENTER
        createMesh();
        break;

      case 111: // o
        savePointClouds();
        break;
      case 79: // O
        loadPointClouds();
        break;

      case 45: // -
        currentCloud--;
        break;
      case 61: // +
        currentCloud= currentCloud + 1 % NUM_CAPTURES;
        break;

      case 97:  //a
        xTranslation += X_STEP;
        updateClouds();
        break;
      case 100: //d
        xTranslation -= X_STEP;
        updateClouds();
        break;

      case 119: //w
        zTranslation += Z_STEP;
        updateClouds();
        break;
      case 115: //s
        zTranslation -= Z_STEP;
        updateClouds();
        break;
    }
  }
}

void initializeViewer() {
  viewer.reset(new pcl::visualization::PCLVisualizer ("OpenNI Viewer"));
  viewer->registerKeyboardCallback(&keyboard_callback);
  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)
{
  initializeViewer();

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

  if(grabber);
    grabber->stop();
}