pointcloud_to_mesh_alg.cpp

Go to the documentation of this file.

#include "pointcloud_to_mesh_alg.h"

PointCloudToMeshAlgorithm::PointCloudToMeshAlgorithm(void):
down_res_(0.01f),radius_(0.025f), mu_(1.5f), num_neigh_(100),
max_surf_angle_(M_PI/4), min_ang_(M_PI/18),
max_ang_(2*M_PI/3), norm_consistency_(false)
{
}

PointCloudToMeshAlgorithm::~PointCloudToMeshAlgorithm(void)
{
}

void PointCloudToMeshAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}

// PointCloudToMeshAlgorithm Public API
bool PointCloudToMeshAlgorithm::compute_mesh(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
        pcl::PointCloud<pcl::PointXYZ>::Ptr input_pcl (new pcl::PointCloud<pcl::PointXYZ>); 
        pcl::fromROSMsg(*msg, *input_pcl);
        std::vector<int> index;
        pcl::removeNaNFromPointCloud (*input_pcl, *input_pcl, index);
        return compute_mesh(input_pcl);
}

bool PointCloudToMeshAlgorithm::compute_mesh(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud){
 
//    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ> ());
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud(cloud);
    sor.setLeafSize(down_res_, down_res_, down_res_);
    sor.filter(*cloud);

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

    // Set typical values for the parameters
    gp3.setMu (mu_);
    gp3.setMaximumNearestNeighbors (num_neigh_); // 100
    gp3.setMaximumSurfaceAngle(max_surf_angle_); // 45 degrees
    gp3.setMinimumAngle(min_ang_); // 10 degrees
    gp3.setMaximumAngle(max_ang_); // 120 degrees
    gp3.setNormalConsistency(norm_consistency_);

    // 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 ("mesh.vtk", triangles);
    return true;
}

void PointCloudToMeshAlgorithm::set_down_res(double down_res){
    down_res_ = down_res;
}

void PointCloudToMeshAlgorithm::set_radius(double radius){
    radius_ = radius;
}

void PointCloudToMeshAlgorithm::set_mu(double mu){
    mu_ = mu;
}

void PointCloudToMeshAlgorithm::set_num_of_neigh(unsigned int num_neigh){
    num_neigh_ = num_neigh;
}

void PointCloudToMeshAlgorithm::set_max_surface_angle(double max_surf_angle){
    max_surf_angle_ = max_surf_angle;
}

void PointCloudToMeshAlgorithm::set_min_angle(double min_ang){
    min_ang_ = min_ang;
}

void PointCloudToMeshAlgorithm::set_max_angle(double max_ang){
    max_ang_ = max_ang;
}

void PointCloudToMeshAlgorithm::set_normal_consistency(bool norm_consistency){
    norm_consistency_ = norm_consistency;
}