Go to the documentation of this file.00001 #include "mesh_from_pointcloud_alg.h"
00002
00003 MeshFromPointcloudAlgorithm::MeshFromPointcloudAlgorithm(void)
00004 {
00005 }
00006
00007 MeshFromPointcloudAlgorithm::~MeshFromPointcloudAlgorithm(void)
00008 {
00009 }
00010
00011 void MeshFromPointcloudAlgorithm::config_update(Config& new_cfg, uint32_t level)
00012 {
00013 this->lock();
00014
00015
00016 this->config_=new_cfg;
00017
00018 this->unlock();
00019 }
00020
00021
00022 bool MeshFromPointcloudAlgorithm::compute_mesh(const sensor_msgs::PointCloud2::ConstPtr& msg)
00023 {
00024 pcl::PointCloud<pcl::PointXYZ>::Ptr input_pcl (new pcl::PointCloud<pcl::PointXYZ>);
00025 pcl::fromROSMsg(*msg, *input_pcl);
00026 std::vector<int> index;
00027 pcl::removeNaNFromPointCloud (*input_pcl, *input_pcl, index);
00028 return compute_mesh(input_pcl);
00029 }
00030
00031 bool MeshFromPointcloudAlgorithm::compute_mesh(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud){
00032
00033 pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
00034 pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
00035 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
00036 tree->setInputCloud (cloud);
00037 n.setInputCloud (cloud);
00038 n.setSearchMethod (tree);
00039 n.setKSearch (20);
00040 n.compute (*normals);
00041
00042
00043
00044 pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
00045 pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
00046
00047
00048
00049 pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
00050 tree2->setInputCloud (cloud_with_normals);
00051
00052
00053 pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
00054 pcl::PolygonMesh triangles;
00055
00056
00057 gp3.setSearchRadius (0.025);
00058
00059
00060 gp3.setMu (2.5);
00061 gp3.setMaximumNearestNeighbors (100);
00062 gp3.setMaximumSurfaceAngle(M_PI/4);
00063 gp3.setMinimumAngle(M_PI/18);
00064 gp3.setMaximumAngle(2*M_PI/3);
00065 gp3.setNormalConsistency(false);
00066
00067
00068 gp3.setInputCloud (cloud_with_normals);
00069 gp3.setSearchMethod (tree2);
00070 gp3.reconstruct (triangles);
00071
00072
00073 std::vector<int> parts = gp3.getPartIDs();
00074 std::vector<int> states = gp3.getPointStates();
00075
00076 pcl::io::saveVTKFile ("mesh.vtk", triangles);
00077 return true;
00078 }
00079