int main (int argc, char** argv)
{
	// Load input file into a PointCloud<T> with an appropriate type
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCLPointCloud2 cloud_blob;
	pcl::io::loadPCDFile ("12_inch_block_downsampled.pcd", cloud_blob);
	pcl::fromPCLPointCloud2 (cloud_blob, *cloud);
	//* the data should be available in cloud

	// Normal estimation*
	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.025);

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

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

	// Additional vertex information
	std::vector<int> parts = gp3.getPartIDs();
	std::vector<int> states = gp3.getPointStates();

	for(int i = 0; i < parts.size(); i++){
		std::cout<<parts[i]<<", "<<states[i]<<std::endl;
	}

	// Finish
	return (0);
}
Esempio n. 2
0
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormalsSmoothed(
		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
		float smoothingSearchRadius,
		bool smoothingPolynomialFit,
		float voxelSize)
{
	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
	tree->setInputCloud (cloud);

	// Init object (second point type is for the normals, even if unused)
	pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> mls;

	mls.setComputeNormals (true);

	// Set parameters
	mls.setInputCloud (cloud);
	mls.setPolynomialFit (smoothingPolynomialFit);
	mls.setSearchMethod (tree);
	mls.setSearchRadius (smoothingSearchRadius);
	if(voxelSize > 0.0f)
	{
		mls.setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal>::VOXEL_GRID_DILATION);
		mls.setDilationVoxelSize(voxelSize);
	}

	// Reconstruct
	mls.process (*cloud_with_normals);

	return cloud_with_normals;
}
Esempio n. 3
0
pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
                const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
                int normalKSearch)
{
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud (cloud);

    // Normal estimation*
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    n.setInputCloud(cloud);
    n.setSearchMethod(tree);
    n.setKSearch(normalKSearch);
    n.compute(*normals);
    //* normals should not contain the point normals + surface curvatures
    
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals_filtered(new pcl::PointCloud<pcl::PointNormal>);
    cloud_with_normals_filtered->reserve(cloud_with_normals->size());
    for (int i = 0; i < cloud_with_normals->size(); ++i) {
        const pcl::PointNormal& p = cloud_with_normals->at(i);
        // Filter out nan
        if (p.normal_x != p.normal_x || p.normal_y != p.normal_y || p.normal_z != p.normal_z)
            continue;
        
        cloud_with_normals_filtered->push_back(p);
    }

    // Concatenate the XYZ and normal fields*
    pcl::concatenateFields(*cloud, *normals, *cloud_with_normals_filtered);
    //* cloud_with_normals = cloud + normals*/

    return cloud_with_normals;
}
int
main (int, char** argv)
{
  std::string filename = argv[1];
  std::cout << "Reading " << filename << std::endl;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file");
    return (-1);
  }

  std::cout << "Loaded " << cloud->points.size () << " points." << std::endl;

  // Compute the normals
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
  normal_estimation.setInputCloud (cloud);

  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  normal_estimation.setSearchMethod (tree);

  pcl::PointCloud<pcl::Normal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::Normal>);

  normal_estimation.setRadiusSearch (0.03);

  normal_estimation.compute (*cloud_with_normals);

  // Setup the feature computation

  pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> pfh_estimation;
  // Provide the original point cloud (without normals)
  pfh_estimation.setInputCloud (cloud);
  // Provide the point cloud with normals
  pfh_estimation.setInputNormals (cloud_with_normals);

  // pfh_estimation.setInputWithNormals (cloud, cloud_with_normals); PFHEstimation does not have this function
  // Use the same KdTree from the normal estimation
  pfh_estimation.setSearchMethod (tree);

  pcl::PointCloud<pcl::PFHSignature125>::Ptr pfh_features (new pcl::PointCloud<pcl::PFHSignature125>);

  pfh_estimation.setRadiusSearch (0.2);

  // Actually compute the spin images
  pfh_estimation.compute (*pfh_features);

  std::cout << "output points.size (): " << pfh_features->points.size () << std::endl;

  // Display and retrieve the shape context descriptor vector for the 0th point.
  pcl::PFHSignature125 descriptor = pfh_features->points[0];
  std::cout << descriptor << std::endl;

  return 0;
}
int
main (int argc, char** argv)
{
  std::string filename = argv[1];
  std::cout << "Reading " << filename << std::endl;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file");
    return (-1);
  }

  std::cout << "Loaded " << cloud->points.size () << " points." << std::endl;

  // Compute the normals
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
  normal_estimation.setInputCloud (cloud);

  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  normal_estimation.setSearchMethod (tree);

  pcl::PointCloud<pcl::Normal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::Normal>);

  normal_estimation.setRadiusSearch (0.03);

  normal_estimation.compute (*cloud_with_normals);

  // Setup the principal curvatures computation
  pcl::PrincipalCurvaturesEstimation<pcl::PointXYZ, pcl::Normal, pcl::PrincipalCurvatures> principal_curvatures_estimation;

  // Provide the original point cloud (without normals)
  principal_curvatures_estimation.setInputCloud (cloud);

  // Provide the point cloud with normals
  principal_curvatures_estimation.setInputNormals (cloud_with_normals);

  // Use the same KdTree from the normal estimation
  principal_curvatures_estimation.setSearchMethod (tree);
  principal_curvatures_estimation.setRadiusSearch (1.0);

  // Actually compute the principal curvatures
  pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr principal_curvatures (new pcl::PointCloud<pcl::PrincipalCurvatures> ());
  principal_curvatures_estimation.compute (*principal_curvatures);

  std::cout << "output points.size (): " << principal_curvatures->points.size () << std::endl;

  // Display and retrieve the shape context descriptor vector for the 0th point.
  pcl::PrincipalCurvatures descriptor = principal_curvatures->points[0];
  std::cout << descriptor << std::endl;

  return 0;
}
Esempio n. 6
0
void greedy_proj () {
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2 ());
  pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
  pcl::io::loadPCDFile ("input.pcd", *cloud_blob);
  pcl::fromPCLPointCloud2 (*cloud_blob, *cloud);

  cloud_blob = cloud_filtered;

  // Normal estimation
  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);

  // 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);

  // 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 (1);

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

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

  // Additional vertex information
  std::vector<int> parts = gp3.getPartIDs();
  std::vector<int> states = gp3.getPointStates();

  pcl::io::saveVTKFile("output.vtk", triangles);
  pcl::io::savePolygonFileSTL("output.stl", triangles);
}
void Get_ToTal_FPFH(const char *path_pcd,float normal_search_radius, float fpfh_search_radius, float a[33])
{
	  std::string fileName = path_pcd;
	//  std::cout << "Reading " << fileName << std::endl;

	  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

	  if (pcl::io::loadPCDFile<pcl::PointXYZ> (fileName, *cloud) == -1) // load the file
	  {
	    PCL_ERROR ("Couldn't read file");
	    exit(0);
	  }

	//  std::cout << "Loaded " << cloud->points.size () << " points." << std::endl;
	  // Compute the normals
	  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
	  normal_estimation.setInputCloud (cloud);

	  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
	  normal_estimation.setSearchMethod (tree);

	  pcl::PointCloud<pcl::Normal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::Normal>);

	  normal_estimation.setRadiusSearch (0.05);

	  normal_estimation.compute (*cloud_with_normals);

	  // Setup the feature computation
	  pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_estimation;
	  // Provide the original point cloud (without normals)
	  fpfh_estimation.setInputCloud (cloud);
	  // Provide the point cloud with normals
	  fpfh_estimation.setInputNormals (cloud_with_normals);
	  // fpfhEstimation.setInputWithNormals(cloud, cloudWithNormals); PFHEstimation does not have this function
	  // Use the same KdTree from the normal estimation
	  fpfh_estimation.setSearchMethod (tree);

	  pcl::PointCloud<pcl::FPFHSignature33>::Ptr pfh_features (new pcl::PointCloud<pcl::FPFHSignature33>);

	  fpfh_estimation.setRadiusSearch (0.25);

	  // Actually compute the spin images
	  fpfh_estimation.compute (*pfh_features);

	//  std::cout << "output points.size (): " << pfh_features->points.size () << std::endl;
	// float a[33] = {0.0f};
	  // Display and retrieve the shape context descriptor vector for the 0th point.
	  for (std::size_t i = 0; i < pfh_features->points.size(); i++)
	    {
	      pcl::FPFHSignature33 descriptor = pfh_features->points[i];
	      for (int j = 0; j < 33; j++)
	    	  a[j] = descriptor.histogram[j];
	    }
}
Esempio n. 8
0
pcl::PolygonMesh RScloud::triangulate_cloud() {

    pcl::PointCloud<pointT>::Ptr cloudfiltered(new pcl::PointCloud<pointT>);
    pcl::StatisticalOutlierRemoval<pointT> sor;
    sor.setInputCloud (pointcloud);
    sor.setMeanK (50);
    sor.setStddevMulThresh (2.5);
    sor.filter (*cloudfiltered);
    // Normal estimation*
    pcl::NormalEstimation<pointT, pointTnormal> n;
    pcl::PointCloud<pointTnormal>::Ptr normals (new pcl::PointCloud<pointTnormal>);
    pcl::search::KdTree<pointT>::Ptr tree (new pcl::search::KdTree<pointT>);
    tree->setInputCloud (cloudfiltered);
    n.setInputCloud (cloudfiltered);
    n.setSearchMethod (tree);
    n.setKSearch (50);
    n.compute (*normals);
    // Concatenate the XYZ and normal fields*
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
    pcl::concatenateFields (*cloudfiltered, *normals, *cloud_with_normals);
    pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
    tree2->setInputCloud (cloud_with_normals);
    // Initialize objects
    pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> gp3;
    pcl::PolygonMesh triangles;
    // Set the maximum distance between connected points (maximum edge length)
    gp3.setSearchRadius (0.025);
    // Set typical values for the parameters
    gp3.setMu (2.5);
    gp3.setMaximumNearestNeighbors (100);
    gp3.setMaximumSurfaceAngle(M_PI/4);
    gp3.setMinimumAngle(M_PI/18); // 10 degrees
    gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
    gp3.setNormalConsistency(false);
    gp3.setConsistentVertexOrdering(true);
    // Get result
    gp3.setInputCloud (cloud_with_normals);
    gp3.setSearchMethod (tree2);
    gp3.reconstruct (triangles);
    // Additional vertex information
    std::vector<int> parts = gp3.getPartIDs();
    std::vector<int> states = gp3.getPointStates();

    return triangles;

}
Esempio n. 9
0
void FSModel::convertPointCloudToSurfaceMesh2()
{
    // Load input file into a PointCloud<T> with an appropriate type
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

    cloud->points.resize(pointCloud->size());
    for (size_t i = 0; i < pointCloud->points.size(); i++) {
        cloud->points[i].x = pointCloud->points[i].x;
        cloud->points[i].y = pointCloud->points[i].y;
        cloud->points[i].z = pointCloud->points[i].z;
    }

    // Normal estimation*
    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.setRadiusSearch(15.0);
    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);


    pcl::Poisson<pcl::PointNormal> poisson;
    poisson.setDepth(9);
    poisson.setDegree(2);
    poisson.setSamplesPerNode(1);
    poisson.setScale(1.25);
    poisson.setIsoDivide(8);
    poisson.setConfidence(0);
    poisson.setManifold(0);
    poisson.setOutputPolygons(0);
    poisson.setSolverDivide(8);

    poisson.setInputCloud(cloud_with_normals);

    poisson.reconstruct (triangles);

}
Esempio n. 10
0
pcl::PolygonMesh createMesh( pcl::PointCloud<PointType>::Ptr cloud )
{
    pcl::console::print_info( "createMesh" );

    // 法線を取得する
    pcl::PointCloud<NormalType>::Ptr cloud_with_normals(
                                    new pcl::PointCloud<NormalType> );
    pcl::MovingLeastSquares<PointType, NormalType> mls;

    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(
                                    new pcl::search::KdTree<PointType> );

    mls.setComputeNormals( true );
    mls.setInputCloud( cloud );
    mls.setPolynomialFit( true );
    mls.setSearchMethod( tree );
    mls.setSearchRadius( 0.03 );
    mls.process( *cloud_with_normals );

    pcl::search::KdTree<NormalType>::Ptr tree2(
                                new pcl::search::KdTree<NormalType> );

    // メッシュ化
    pcl::GreedyProjectionTriangulation<NormalType> gp3;

    gp3.setSearchRadius( 0.025 );
    gp3.setMu( 2.5 );
    gp3.setMaximumNearestNeighbors( 100 );

    gp3.setMaximumSurfaceAngle( M_PI / 4 );
    gp3.setMinimumAngle( M_PI / 18 );
    gp3.setMaximumAngle( 2 * M_PI / 3 );
    gp3.setNormalConsistency( false );

    // 結果を取得
    pcl::PolygonMesh triangles;
    gp3.setInputCloud( cloud_with_normals );
    gp3.setSearchMethod( tree2 );
    gp3.reconstruct( triangles );

    return triangles;
}
Esempio n. 11
0
pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
		int normalKSearch)
{
	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud (cloud);

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

	// Concatenate the XYZ and normal fields*
	pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
	//* cloud_with_normals = cloud + normals*/

	return cloud_with_normals;
}
Esempio n. 12
0
//--------------------------------------------------------------
void ofApp::setup(){
	ofSetLogLevel(OF_LOG_VERBOSE);
	
	glEnable(GL_DEPTH_TEST);
	
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
	
	mesh.load(ofToDataPath("out.ply"));
	cloud = ofxPCL::toPCL<ofxPCL::PointXYZCloud>(mesh);
	
	textures.resize(2);
	textures.at(0).loadImage(ofToDataPath("tex0.jpg"));
	textures.at(1).loadImage(ofToDataPath("tex1.jpg"));
	
	pcl::SacModel model_type = pcl::SACMODEL_PLANE;
	float distance_threshold = 30;
	int min_points_limit = 10;
	int max_segment_count = 30;
	
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
	
	pcl::SACSegmentation<pcl::PointXYZ> seg;
	seg.setOptimizeCoefficients(false);
	
	seg.setModelType(model_type);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setDistanceThreshold(distance_threshold);
	seg.setMaxIterations(500);
	
	pcl::PointCloud<pcl::PointXYZ>::Ptr temp(new pcl::PointCloud<pcl::PointXYZ>(*cloud));
	const size_t original_szie = temp->points.size();
	
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	
	int segment_count = 0;
	while (temp->size() > original_szie * 0.3)
	{
		if (segment_count > max_segment_count) break;
		segment_count++;
		
		seg.setInputCloud(temp);
		seg.segment(*inliers, *coefficients);
		
		if (inliers->indices.size() < min_points_limit)
			break;
		
		pcl::PointCloud<pcl::PointXYZ>::Ptr filterd_point_cloud(new pcl::PointCloud<pcl::PointXYZ>);
		
		extract.setInputCloud(temp);
		extract.setIndices(inliers);
		extract.setNegative(false);
		extract.filter(*filterd_point_cloud);
		
		if (filterd_point_cloud->points.size() > 0)
		{
			clouds.push_back(filterd_point_cloud);
		}
		
		extract.setNegative(true);
		extract.filter(*temp);
		
		ofMesh m;
		pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
		
		ofxPCL::normalEstimation(filterd_point_cloud, cloud_with_normals);
		
		m = ofxPCL::triangulate(cloud_with_normals, 100);
		m.clearColors();
		jointMesh.addVertices(m.getVertices());
		jointMesh.addColors(m.getColors());
		
		ofVec3f center = m.getCentroid();
		ofVec3f planeNormal(coefficients->values[0], coefficients->values[1], coefficients->values[2]);
		
		ofVec3f tangent, bitangent;
		ofVec3f arb(0, 1, 0);
		tangent = arb.cross(planeNormal).normalize();
		bitangent = planeNormal.cross(tangent).normalize();
		
		for(int j = 0; j < m.getNumVertices(); j++) {
			float x = m.getVertex(j).dot(tangent) * 0.001;
			x = x - (long)x;
			if(x < 0) x += 1;
			float y = m.getVertex(j).dot(bitangent) * 0.001;
			y = y - (long)y;
			if(y < 0) y += 1;
			m.addTexCoord(ofVec2f(x, y));
		}
		meshes.push_back(m);
	}
	
	ofxPCL::convert(temp, meshResidual);
	
	ofLogVerbose() << clouds.size() << " meshes extracted";
	
	center = jointMesh.getCentroid();
}
Esempio n. 13
0
int main (int argc, char** argv)
{
    if(argc<2){ 
	  std::cout << "need ply/pcd file. " << std::endl;
          return -1;
	}
    // 确定文件格式
    char tmpStr[100];
    strcpy(tmpStr,argv[1]);
    char* pext = strrchr(tmpStr, '.');
    std::string extply("ply");
    std::string extpcd("pcd");
    if(pext){
        *pext='\0';
        pext++;
    }
    std::string ext(pext);
    //如果不支持文件格式,退出程序
    if (!((ext == extply)||(ext == extpcd))){
        std::cout << "文件格式不支持!" << std::endl;
        std::cout << "支持文件格式:*.pcd和*.ply!" << std::endl;
        return(-1);
    }

    //根据文件格式选择输入方式
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>) ; //创建点云对象指针,用于存储输入
    if (ext == extply){
        if (pcl::io::loadPLYFile(argv[1] , *cloud) == -1){
            PCL_ERROR("Could not read ply file!\n") ;
            return -1;
        }
    }
    else{
        if (pcl::io::loadPCDFile(argv[1] , *cloud) == -1){
            PCL_ERROR("Could not read pcd file!\n") ;
            return -1;
        }
    }

  // 估计法向量 x,y,x + 法向量 + 曲率
  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);//20个邻居
  n.compute (*normals); //计算法线,结果存储在normals中
  //* normals 不能同时包含点的法向量和表面的曲率

  //将点云和法线放到一起
  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


  //创建搜索树
  pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
  tree2->setInputCloud (cloud_with_normals);

  //初始化 移动立方体算法 MarchingCubes对象,并设置参数
    pcl::MarchingCubes<pcl::PointNormal> *mc;
    mc = new pcl::MarchingCubesHoppe<pcl::PointNormal> ();
    /*
  if (hoppe_or_rbf == 0)
    mc = new pcl::MarchingCubesHoppe<pcl::PointNormal> ();
  else
  {
    mc = new pcl::MarchingCubesRBF<pcl::PointNormal> ();
    (reinterpret_cast<pcl::MarchingCubesRBF<pcl::PointNormal>*> (mc))->setOffSurfaceDisplacement (off_surface_displacement);
  }
    */

    //创建多变形网格,用于存储结果
  pcl::PolygonMesh mesh;

  //设置MarchingCubes对象的参数
  mc->setIsoLevel (0.0f);
  mc->setGridResolution (50, 50, 50);
  mc->setPercentageExtendGrid (0.0f);

  //设置搜索方法
  mc->setInputCloud (cloud_with_normals);

  //执行重构,结果保存在mesh中
  mc->reconstruct (mesh);

  //保存网格图
  pcl::io::savePLYFile("result2.ply", mesh);

  // 显示结果图
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->setBackgroundColor (0, 0, 0); //设置背景
  viewer->addPolygonMesh(mesh,"my"); //设置显示的网格
  //viewer->addCoordinateSystem (1.0); //设置坐标系
  viewer->initCameraParameters ();
  while (!viewer->wasStopped ()){
    viewer->spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }

  return (0);
}
Esempio n. 14
0
PointViewSet PoissonFilter::run(PointViewPtr input)
{
    PointViewPtr output = input->makeNew();
    PointViewSet viewSet;
    viewSet.insert(output);

    bool logOutput = log()->getLevel() > LogLevel::Debug1;
    if (logOutput)
        log()->floatPrecision(8);

    log()->get(LogLevel::Debug2) << "Process PoissonFilter..." << std::endl;

    BOX3D buffer_bounds;
    input->calculateBounds(buffer_bounds);

    // convert PointView to PointNormal
    typedef pcl::PointCloud<pcl::PointXYZ> Cloud;
    Cloud::Ptr cloud(new Cloud);
    pclsupport::PDALtoPCD(input, *cloud, buffer_bounds);

    pclsupport::setLogLevel(log()->getLevel());

    // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;
    pcl::search::KdTree<pcl::PointNormal>::Ptr tree2;

    // Create search tree
    tree.reset(new pcl::search::KdTree<pcl::PointXYZ> (false));
    tree->setInputCloud(cloud);

    // Normal estimation
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal> ());
    n.setInputCloud(cloud);
    n.setSearchMethod(tree);
    n.setKSearch(20);
    n.compute(*normals);

    // Concatenate XYZ and normal information
    pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);

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

    // initial setup
    pcl::Poisson<pcl::PointNormal> p;
    p.setInputCloud(cloud_with_normals);
    p.setSearchMethod(tree2);
    p.setDepth(m_depth);
    p.setPointWeight(m_point_weight);

    // create PointCloud for results
    pcl::PolygonMesh grid;
    p.reconstruct(grid);

    Cloud::Ptr cloud_f(new Cloud);
    pcl::fromPCLPointCloud2(grid.cloud, *cloud_f);

    if (cloud_f->points.empty())
    {
        log()->get(LogLevel::Debug2) << "Filtered cloud has no points!" << std::endl;
        return viewSet;
    }

    pclsupport::PCDtoPDAL(*cloud_f, output, buffer_bounds);

    log()->get(LogLevel::Debug2) << cloud->points.size() << " before, " <<
                                 cloud_f->points.size() << " after" << std::endl;
    log()->get(LogLevel::Debug2) << output->size() << std::endl;

    return viewSet;
}
Esempio n. 15
0
int main(int argc, char** argv)
{
	if(argc <= 2)
	{
		std::cerr << "You must supply a pcd filename and an output filename" << std::endl;

		return 1;
	}

	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
	pcl::io::loadPCDFile<pcl::PointNormal>(std::string(argv[1]), *cloud_with_normals);

	std::cout << cloud_with_normals->points.size() << " points in " << std::string(argv[1]) << std::endl;

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

	pcl::Poisson<pcl::PointNormal> poisson;
	pcl::PolygonMesh triangles;

	poisson.setDepth(10);
	
	poisson.setInputCloud(cloud_with_normals);
	poisson.setSearchMethod(tree);
	poisson.reconstruct(triangles);

	std::cout << triangles.polygons.size() << " polygons in reconstructed mesh" << std::endl;

	pcl::PointCloud<pcl::PointXYZ>::Ptr mesh_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::fromROSMsg(triangles.cloud, *mesh_cloud);

	FILE* mesh_file = fopen(argv[2], "w");

	if(!mesh_file)
	{
		std::cerr << "ERROR: Could not open file " << std::string(argv[2]) << std::endl;

		return 2;
	}

	for(pcl::PointCloud<pcl::PointXYZ>::const_iterator point = mesh_cloud->points.begin();
		point != mesh_cloud->points.end();
		point++)
	{
		fprintf(mesh_file, "v %f %f %f\n",
				point->x,
				point->y,
				point->z);
	}

	fprintf(mesh_file, "\n");

	for(std::vector<pcl::Vertices>::const_iterator polygon = triangles.polygons.begin();
		polygon != triangles.polygons.end();
		polygon++)
	{
		fprintf(mesh_file, "f %d %d %d\n",
				polygon->vertices[0]+1,
				polygon->vertices[1]+1,
				polygon->vertices[2]+1);
	}

	fclose(mesh_file);

}
int
main (int argc, char** argv)
{
  // Data containers used
  pcl::PointCloud<PointTypeIO>::Ptr cloud_in (new pcl::PointCloud<PointTypeIO>), cloud_out (new pcl::PointCloud<PointTypeIO>);
  pcl::PointCloud<PointTypeFull>::Ptr cloud_with_normals (new pcl::PointCloud<PointTypeFull>);
  pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters), small_clusters (new pcl::IndicesClusters), large_clusters (new pcl::IndicesClusters);
  pcl::search::KdTree<PointTypeIO>::Ptr search_tree (new pcl::search::KdTree<PointTypeIO>);
  pcl::console::TicToc tt;

  // Load the input point cloud
  std::cerr << "Loading...\n", tt.tic ();
  pcl::io::loadPCDFile (argv[1], *cloud_in);
  std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud_in->points.size () << " points\n";

  // Downsample the cloud using a Voxel Grid class
  std::cerr << "Downsampling...\n", tt.tic ();
  pcl::VoxelGrid<PointTypeIO> vg;
  vg.setInputCloud (cloud_in);
  vg.setLeafSize (80.0, 80.0, 80.0);
  vg.setDownsampleAllData (true);
  vg.filter (*cloud_out);
  std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud_out->points.size () << " points\n";

  // Set up a Normal Estimation class and merge data in cloud_with_normals
  std::cerr << "Computing normals...\n", tt.tic ();
  pcl::copyPointCloud (*cloud_out, *cloud_with_normals);
  pcl::NormalEstimation<PointTypeIO, PointTypeFull> ne;
  ne.setInputCloud (cloud_out);
  ne.setSearchMethod (search_tree);
  ne.setRadiusSearch (300.0);
  ne.compute (*cloud_with_normals);
  std::cerr << ">> Done: " << tt.toc () << " ms\n";

  // Set up a Conditional Euclidean Clustering class
  std::cerr << "Segmenting to clusters...\n", tt.tic ();
  pcl::ConditionalEuclideanClustering<PointTypeFull> cec (true);
  cec.setInputCloud (cloud_with_normals);
  cec.setConditionFunction (&customRegionGrowing);
  cec.setClusterTolerance (500.0);
  cec.setMinClusterSize (cloud_with_normals->points.size () / 1000);
  cec.setMaxClusterSize (cloud_with_normals->points.size () / 5);
  cec.segment (*clusters);
  cec.getRemovedClusters (small_clusters, large_clusters);
  std::cerr << ">> Done: " << tt.toc () << " ms\n";

  // Using the intensity channel for lazy visualization of the output
  for (int i = 0; i < small_clusters->size (); ++i)
    for (int j = 0; j < (*small_clusters)[i].indices.size (); ++j)
      cloud_out->points[(*small_clusters)[i].indices[j]].intensity = -2.0;
  for (int i = 0; i < large_clusters->size (); ++i)
    for (int j = 0; j < (*large_clusters)[i].indices.size (); ++j)
      cloud_out->points[(*large_clusters)[i].indices[j]].intensity = +10.0;
  for (int i = 0; i < clusters->size (); ++i)
  {
    int label = rand () % 8;
    for (int j = 0; j < (*clusters)[i].indices.size (); ++j)
      cloud_out->points[(*clusters)[i].indices[j]].intensity = label;
  }

  // Save the output point cloud
  std::cerr << "Saving...\n", tt.tic ();
  pcl::io::savePCDFile ("output.pcd", *cloud_out);
  std::cerr << ">> Done: " << tt.toc () << " ms\n";

  return (0);
}
Esempio n. 17
0
	/**
	 * Finalizing the calculation while feeding the cloud with the
	 * normals information.
	 */
	PointCloud<PointNormal>::Ptr NormalCalculator::mergeCloudWithNormals()
	{
		PointCloud<PointNormal>::Ptr cloud_with_normals (new PointCloud<PointNormal>);
		concatenateFields(*_cloud, *_normals, *cloud_with_normals);
		return cloud_with_normals;
	}
Esempio n. 18
0
void FSModel::convertPointCloudToSurfaceMesh()
{
    // Load input file into a PointCloud<T> with an appropriate type
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    //sensor_msgs::PointCloud2 cloud_blob;
    //pcl::io::loadPCDFile ("bearHigh.pcd", cloud_blob);
    //pcl::fromROSMsg (cloud_blob, *cloud);
    //* the data should be available in cloud

    cloud->points.resize(pointCloud->size());
    for (size_t i = 0; i < pointCloud->points.size(); i++) {
        cloud->points[i].x = pointCloud->points[i].x;
        cloud->points[i].y = pointCloud->points[i].y;
        cloud->points[i].z = pointCloud->points[i].z;
    }

    // Normal estimation*
    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.setRadiusSearch(15.0);
    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;

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

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

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

    // Additional vertex information
    std::vector<int> parts = gp3.getPartIDs();
    std::vector<int> states = gp3.getPointStates();

    pcl::io::savePLYFile("mesh.ply", triangles);
}
int main(int argc, char** argv)
{
	if(argc <= 2)
	{
		std::cerr << "You must supply a pcd filename and output filename" << std::endl;

		return 1;
	}

	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
	pcl::io::loadPCDFile<pcl::PointNormal>(std::string(argv[1]), *cloud_with_normals);

	std::cout << cloud_with_normals->points.size() << " points in " << std::string(argv[1]) << std::endl;

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

	pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
	pcl::PolygonMesh triangles;

	gp3.setSearchRadius (0.025);

	gp3.setMu(2.5);
	gp3.setMaximumNearestNeighbors(100);
	gp3.setMaximumSurfaceAngle(M_PI/4);
	gp3.setMinimumAngle(M_PI/18);
	gp3.setMaximumAngle(2*M_PI/3);
	gp3.setNormalConsistency(false);

	gp3.setInputCloud(cloud_with_normals);
	gp3.setSearchMethod(tree);
	gp3.reconstruct(triangles);

	std::cout << triangles.polygons.size() << " polygons in reconstructed mesh" << std::endl;

	pcl::PointCloud<pcl::PointXYZ>::Ptr mesh_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::fromROSMsg(triangles.cloud, *mesh_cloud);

	FILE* mesh_file = fopen(argv[2], "w");

	if(!mesh_file)
	{
		std::cerr << "ERROR: Could not open file " << std::string(argv[2]) << std::endl;

		return 2;
	}

	for(pcl::PointCloud<pcl::PointXYZ>::const_iterator point = mesh_cloud->points.begin();
		point != mesh_cloud->points.end();
		point++)
	{
		fprintf(mesh_file, "v %f %f %f\n",
				point->x,
				point->y,
				point->z);
	}

	fprintf(mesh_file, "\n");
	
	for(std::vector<pcl::Vertices>::const_iterator polygon = triangles.polygons.begin();
		polygon != triangles.polygons.end();
		polygon++)
	{
		fprintf(mesh_file, "f %d %d %d\n",
				polygon->vertices[0]+1,
				polygon->vertices[1]+1,
				polygon->vertices[2]+1);
	}

	fclose(mesh_file);
}
Esempio n. 20
0
int main (int argc, char** argv)
{

	/* DOWNSAMPLING ********************************************************************************************************************/
	std::ofstream output_file("properties.txt");
	std::ofstream curvature("curvature.txt");
	std::ofstream normals_text("normals.txt");

	/*

	std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height 
		<< " data points (" << pcl::getFieldsList (*cloud) << ")." << std::endl;

	// Create the filtering object
	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	sor.setInputCloud (cloud);
	sor.setLeafSize (0.01f, 0.01f, 0.01f);
	sor.filter (*cloud_filtered);

	output_file << "Number of filtered points" << std::endl;
	output_file << cloud_filtered->width * cloud_filtered->height<<std::endl;

	std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height 
		<< " data points (" << pcl::getFieldsList (*cloud_filtered) << ")." << std::endl;


	pcl::PointCloud<pcl::PointXYZ>::Ptr descriptor (new pcl::PointCloud<pcl::PointXYZ>);

	descriptor->width = 5000 ;
	descriptor->height = 1 ;
	descriptor->points.resize (descriptor->width * descriptor->height) ;
	std::cerr << descriptor->points.size() << std::endl ;

	pcl::PCDWriter writer;
	writer.write ("out.pcd", *cloud_filtered, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);

	*/
		
	/***********************************************************************************************************************************/

	/* CALCULATING VOLUME AND SURFACE AREA ********************************************************************************************************************/		

	/*pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
	  pcl::ConcaveHull<pcl::PointXYZ> chull;
	  chull.setInputCloud(test);

	  chull.reconstruct(*cloud_hull);*/

	/*for (size_t i=0; i < test->points.size (); i++)
	  {
	  std::cout<< test->points[i].x <<std::endl;
	  std::cout<< test->points[i].y <<std::endl;
	  std::cout<< test->points[i].z <<std::endl;
	  }*/
	//std::cout<<data.size()<<std::endl;

	// Load input file into a PointCloud<T> with an appropriate type
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCLPointCloud2 cloud_blob;
	pcl::io::loadPCDFile ("mini_soccer_ball_downsampled.pcd", cloud_blob);
	pcl::fromPCLPointCloud2 (cloud_blob, *cloud);		
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::fromPCLPointCloud2 (cloud_blob, *cloud1);
		//* the data should be available in cloud

	// Normal estimation*
	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.025);

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


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

	// Additional vertex information
	std::vector<int> parts = gp3.getPartIDs();
	std::vector<int> states = gp3.getPointStates();	

	pcl::PolygonMesh::Ptr mesh(&triangles);
	pcl::PointCloud<pcl::PointXYZ>::Ptr triangle_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::fromPCLPointCloud2(mesh->cloud, *triangle_cloud);	
/*	for(int i = 0; i < 2; i++){
		std::cout<<triangles.polygons[i]<<std::endl;
	}
*/	std::cout<<"first vertice "<<triangles.polygons[0].vertices[0] << std::endl; 	
	
	//std::cout<<"Prolly not gonna work "<<triangle_cloud->points[triangles.polygons[0].vertices[0]] << std::endl; 	


	//pcl::fromPCLPointCloud2(triangles.cloud, triangle_cloud); 
	std::cout << "size of points " << triangle_cloud->points.size() << std::endl ;
	
	std::cout<<triangle_cloud->points[0]<<std::endl;
	for(unsigned i = 0; i < triangle_cloud->points.size(); i++){
		std::cout << triangles.polgyons[i].getVector3fMap() <<"test"<< std::endl;
	} 	
	//std::cout<<"surface: "<<calculateAreaPolygon(triangles, triangle_cloud)<<std::endl;

	/******************************************************************************************************************************************/	

	/* CALCULATING CURVATURE AND NORMALS ********************************************************************************************************************/

	// Create the normal estimation class, and pass the input dataset to it
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;	

	ne.setInputCloud (cloud);

	// Create an empty kdtree representation, and pass it to the normal estimation object.
	// Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree3 (new pcl::search::KdTree<pcl::PointXYZ> ());

	ne.setSearchMethod (tree3);

	// Output datasets
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

	// Use all neighbors in a sphere of radius 3cm
	ne.setRadiusSearch (0.03);

	// Compute the features
	ne.compute (*cloud_normals);

	output_file << "size of points " << cloud->points.size() << std::endl ;

	output_file << "size of the normals " << cloud_normals->points.size() << std::endl ; 	

	//pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);

	/************************************************************************************************************************************/

	/* PARSING DATA ********************************************************************************************************************/

	int k=0 ;
	float dist ;
	float square_of_dist ;
	float x1,y1,z1,x2,y2,z2 ;
	float nu[3], nv[3], pv_pu[3], pu_pv[3] ;
	float highest = triangle_cloud->points[0].z;
	float lowest = triangle_cloud->points[0].z;


	for ( int i = 0; i < cloud_normals->points.size() ; i++)
	{
		output_file<<i<<": triangulated "<<triangle_cloud->points[i].x<<", "<<triangle_cloud->points[i].y<<", "<<triangle_cloud->points[i].z<<std::endl;
		output_file<<i<<": normal"<<cloud1->points[i].x<<", "<<cloud1->points[i].y<<", "<<cloud1->points[i].z<<std::endl;
		if(triangle_cloud->points[i].z > highest)
		{
			highest = triangle_cloud->points[i].z;
		}
		if(triangle_cloud->points[i].z < lowest)
		{
			lowest = triangle_cloud->points[i].z;
		}
		normals_text <<i+1 <<": "<<" x-normal-> "<<cloud_normals->points[i].normal_x<<" y-normal-> "<<cloud_normals->points[i].normal_y<<" z-normal-> "<<cloud_normals->points[i].normal_z<<std::endl;
		curvature <<i+1 <<": curvature: "<<cloud_normals->points[i].curvature<<std::endl;
		
		float x, y, z, dist, nx, ny, nz, ndist;
		
		/*

		if(i != cloud_normals->points.size()-1){
			x = cloud->points[i+1].x - cloud->points[i].x;
			y = cloud->points[i+1].y - cloud->points[i].y;
			z = cloud->points[i+1].z - cloud->points[i].z;
			dist = sqrt(pow(x, 2)+pow(y, 2) + pow(z, 2));
			output_file << i+1 <<" -> "<< i+2 << " distance normal: " << dist <<std::endl;
			
			nx = triangle_cloud[i+1].indices[0] - triangle_cloud.points[i].x;
			ny = triangle_cloud.points[i+1].y - triangle_cloud.points[i].y;
			nz = triangle_cloud.points[i+1].z - triangle_cloud.points[i].z;
			ndist = sqrt(pow(nx, 2)+pow(ny, 2) + pow(nz, 2));
			output_file << i+1 <<" -> "<< i+2 << " distance triangulated: " << ndist <<std::endl;
		}

		*/	
		/*
		   pcl::PointXYZRGB point;
		   point.x = cloud_filtered_converted->points[i].x;
		   point.y = cloud_filtered_converted->points[i].y;
		   point.z = cloud_filtered_converted->points[i].z;
		   point.r = 0;
		   point.g = 100;
		   point.b = 200;
		   point_cloud_ptr->points.push_back (point);
		 */
	}
	output_file << "highest point: "<< highest<<std::endl;
	output_file << "lowest point: "<< lowest<<std::endl;	
	//pcl::PointCloud<pcl::PointXYZ>::Ptr test (new pcl::PointCloud<pcl::PointXYZ>);
	//float surface_area = calculateAreaPolygon(test);
	//std::cout<< surface_area<<std::endl;

	/*

	   descriptor->width = k ;
	   descriptor->height = 1 ;
	   descriptor->points.resize (descriptor->width * descriptor->height) ;
	   std::cerr << descriptor->points.size() << std::endl ;
	   float voxelSize = 0.01f ;  // how to find appropriate voxel resolution
	   pcl::octree::OctreePointCloud<pcl::PointXYZ> octree (voxelSize);
	   octree.setInputCloud(descriptor) ;
	   ctree.defineBoundingBox(0.0,0.0,0.0,3.14,3.14,3.14) ;   //octree.defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ)
	   octree.addPointsFromInputCloud ();   // octree created for block

	   int k_ball=0 ;
	   float dist_ball ;
	   float square_of_dist_ball ;
	   double X,Y,Z ;
	   bool occupied ;
	   highest = cloud_ball->points[0].z;

	   for ( int i = 0; i < cloud_normals_ball->points.size() ; i++)
	   {
	   if(cloud->points[i].z > highest){
	   highest = cloud_ball->points[i].z;
	   }
	   for (int j = i+1 ; (j < cloud_normals_ball->points.size()) ; j++)     
	   {
	   x1 = cloud_ball->points[i].x ;
	   y1 = cloud_ball->points[i].y ;
	   z1 = cloud_ball->points[i].z ;
	   nu[0] = cloud_normals_ball->points[i].normal_x ;
	   nu[1] = cloud_normals_ball->points[i].normal_y ;
	   nu[2] = cloud_normals_ball->points[i].normal_z ;
	   x2 = cloud_ball->points[j].x ;
	   y2 = cloud_ball->points[j].y ;
	   z2 = cloud_ball->points[j].z ;
	   nv[0] = cloud_normals_ball->points[j].normal_x ;
	   nv[1] = cloud_normals_ball->points[j].normal_y ;
	   nv[2] = cloud_normals_ball->points[j].normal_z ;
	   square_of_dist = ((x2-x1)*(x2-x1)) + ((y2-y1)*(y2-y1)) + ((z2-z1)*(z2-z1)) ;
	   dist = sqrt(square_of_dist) ;
	//std::cerr << dist ;
	pv_pu[0] = x2-x1 ;
	pv_pu[1] = y2-y1 ;
	pv_pu[2] = z2-z1 ;
	pu_pv[0] = x1-x2 ;
	pu_pv[1] = y1-y2 ;
	pu_pv[2] = z1-z2 ;
	if ((dist > 0.0099) && (dist < 0.0101))
	{
	X = angle_between_vectors (nu, nv) ;
	Y  = angle_between_vectors (nu, pv_pu) ;
	Z = angle_between_vectors (nv, pu_pv) ;
	// output_file << descriptor->points[k].x << "\t" << descriptor->points[k].y << "\t" << descriptor->points[k].z  ;
	// output_file << "\n";	
	//k_ball = k_ball + 1 ;
	occupied = octree.isVoxelOccupiedAtPoint (X,Y,Z) ;
	if (occupied == 1)
	{
	//k_ball = k_ball + 1 ;
	std::cerr << "Objects Matched" << "\t" << k_ball << std::endl ;
	return(0); 
	}

	}

	}

	}	

	 */

	/***********************************************************************************************************************************/


	/*

	   points.open("secondItemPoints.txt");
	   myfile<<"Second point \n";
	   myfile<<"second highest "<<highest;
	   for(int i = 0; i < cloud_normals->points.size(); i++){
	   points<<cloud->points[i].x<<", "<<cloud->points[i].y<<", "<<cloud->points[i].z<<"\n";
	   if(cloud->points[i].z >= highest - (highest/100)){
	   myfile<<cloud->points[i].x<<", "<<cloud->points[i].y<<", "<<cloud->points[i].z<<"\n";
	   }
	   }
	   points.close();
	   myfile.close();

	   std::cerr << "Objects Not Matched" << "\t" << k_ball << std::endl ;

	 */

	//output_file <<"Volume: "<<volume <<std::endl;
	//output_file <<"Surface Area: "<<surface_area <<std::endl;

	return (0);
}