35 #include <pcl/point_types.h>
36 #include <pcl/io/pcd_io.h>
37 #include <pcl/kdtree/kdtree_flann.h>
38 #include <pcl/features/normal_3d.h>
39 #include <pcl/surface/gp3.h>
42 main (
int argc,
char** argv)
45 pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud (
new pcl::PointCloud<pcl::PointXYZ>);
46 sensor_msgs::PointCloud2 cloud_blob;
47 pcl::io::loadPCDFile (
"bun0.pcd", cloud_blob);
48 pcl::fromROSMsg (cloud_blob, *cloud);
52 pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
53 pcl::PointCloud<pcl::Normal>::Ptr normals (
new pcl::PointCloud<pcl::Normal>);
54 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (
new pcl::search::KdTree<pcl::PointXYZ>);
55 tree->setInputCloud (cloud);
56 n.setInputCloud (cloud);
57 n.setSearchMethod (tree);
63 pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (
new pcl::PointCloud<pcl::PointNormal>);
64 pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
68 pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (
new pcl::search::KdTree<pcl::PointNormal>);
69 tree2->setInputCloud (cloud_with_normals);
72 pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
73 pcl::PolygonMesh triangles;
76 gp3.setSearchRadius (0.025);
80 gp3.setMaximumNearestNeighbors (100);
81 gp3.setMaximumSurfaceAngle(M_PI/4);
82 gp3.setMinimumAngle(M_PI/18);
83 gp3.setMaximumAngle(2*M_PI/3);
84 gp3.setNormalConsistency(
false);
87 gp3.setInputCloud (cloud_with_normals);
88 gp3.setSearchMethod (tree2);
89 gp3.reconstruct (triangles);
92 std::vector<int> parts = gp3.getPartIDs();
93 std::vector<int> states = gp3.getPointStates();
int main(int argc, char **argv)