test_concave_hull.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2012-, Open Perception, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: test_surface.cpp 6579 2012-07-27 18:57:32Z rusu $
00037  *
00038  */
00039 
00040 #include <gtest/gtest.h>
00041 
00042 #include <pcl/point_types.h>
00043 #include <pcl/io/pcd_io.h>
00044 #include <pcl/io/vtk_io.h>
00045 #include <pcl/features/normal_3d.h>
00046 #include <pcl/surface/concave_hull.h>
00047 #include <pcl/common/common.h>
00048 #include <pcl/sample_consensus/sac_model_plane.h>
00049 #include <pcl/filters/project_inliers.h>
00050 
00051 
00052 using namespace pcl;
00053 using namespace pcl::io;
00054 using namespace std;
00055 
00056 PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
00057 PointCloud<PointNormal>::Ptr cloud_with_normals (new PointCloud<PointNormal>);
00058 search::KdTree<PointXYZ>::Ptr tree;
00059 search::KdTree<PointNormal>::Ptr tree2;
00060 
00061 // add by ktran to test update functions
00062 PointCloud<PointXYZ>::Ptr cloud1 (new PointCloud<PointXYZ>);
00063 PointCloud<PointNormal>::Ptr cloud_with_normals1 (new PointCloud<PointNormal>);
00064 search::KdTree<PointXYZ>::Ptr tree3;
00065 search::KdTree<PointNormal>::Ptr tree4;
00066 
00068 TEST (PCL, ConcaveHull_bunny)
00069 {
00070   //construct dataset
00071   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2D (new pcl::PointCloud<pcl::PointXYZ> (*cloud));
00072   for (size_t i = 0; i < cloud2D->points.size (); i++)
00073     cloud2D->points[i].z = 0;
00074 
00075   pcl::PointCloud<pcl::PointXYZ> alpha_shape;
00076   pcl::PointCloud<pcl::PointXYZ>::Ptr voronoi_centers (new pcl::PointCloud<pcl::PointXYZ>);
00077   std::vector<pcl::Vertices> polygons_alpha;
00078 
00079   pcl::ConcaveHull<pcl::PointXYZ> concave_hull;
00080   concave_hull.setInputCloud (cloud2D);
00081   concave_hull.setAlpha (0.5);
00082   concave_hull.setVoronoiCenters (voronoi_centers);
00083   concave_hull.reconstruct (alpha_shape, polygons_alpha);
00084 
00085   EXPECT_EQ (alpha_shape.points.size (), 21);
00086 
00087   pcl::PointCloud<pcl::PointXYZ> alpha_shape1;
00088   pcl::PointCloud<pcl::PointXYZ>::Ptr voronoi_centers1 (new pcl::PointCloud<pcl::PointXYZ>);
00089   std::vector<pcl::Vertices> polygons_alpha1;
00090 
00091   pcl::ConcaveHull<pcl::PointXYZ> concave_hull1;
00092   concave_hull1.setInputCloud (cloud2D);
00093   concave_hull1.setAlpha (1.5);
00094   concave_hull1.setVoronoiCenters (voronoi_centers1);
00095   concave_hull1.reconstruct (alpha_shape1, polygons_alpha1);
00096 
00097   EXPECT_EQ (alpha_shape1.points.size (), 20);
00098 
00099   pcl::PointCloud<pcl::PointXYZ> alpha_shape2;
00100   pcl::PointCloud<pcl::PointXYZ>::Ptr voronoi_centers2 (new pcl::PointCloud<pcl::PointXYZ>);
00101   std::vector<pcl::Vertices> polygons_alpha2;
00102   pcl::ConcaveHull<pcl::PointXYZ> concave_hull2;
00103   concave_hull2.setInputCloud (cloud2D);
00104   concave_hull2.setAlpha (0.01);
00105   concave_hull2.setVoronoiCenters (voronoi_centers2);
00106   concave_hull2.reconstruct (alpha_shape2, polygons_alpha2);
00107 
00108   EXPECT_EQ (alpha_shape2.points.size (), 81);
00109 
00110   //PolygonMesh concave;
00111   //toPCLPointCloud2 (alpha_shape2, concave.cloud);
00112   //concave.polygons = polygons_alpha2;
00113   //saveVTKFile ("./test/bun0-concave2d.vtk", concave);
00114 }
00115 
00117 TEST (PCL, ConcaveHull_planar_bunny)
00118 {
00119   ConcaveHull<PointXYZ> concave_hull_3d;
00120   concave_hull_3d.setAlpha (0.5);
00121   concave_hull_3d.setInputCloud (cloud);
00122   PointCloud<PointXYZ> hull_3d;
00123   concave_hull_3d.reconstruct (hull_3d);
00124 
00125 #pragma GCC diagnostic push
00126 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
00127   EXPECT_EQ (concave_hull_3d.getDim (), 3);         // test for PCL_DEPRECATED
00128 #pragma GCC diagnostic pop
00129   EXPECT_EQ (concave_hull_3d.getDimension (), 3);
00130 
00131   ModelCoefficients::Ptr plane_coefficients (new ModelCoefficients ());
00132   plane_coefficients->values.resize (4);
00133   plane_coefficients->values[0] = -0.010666f;
00134   plane_coefficients->values[1] = -0.793771f;
00135   plane_coefficients->values[2] = -0.607779f;
00136   plane_coefficients->values[3] = 0.993252f;
00137 
00139   ProjectInliers<PointXYZ> project_inliers_filter;
00140   project_inliers_filter.setInputCloud (cloud);
00141   project_inliers_filter.setModelType (SACMODEL_PLANE);
00142   project_inliers_filter.setModelCoefficients (plane_coefficients);
00143   PointCloud<PointXYZ>::Ptr cloud_projected (new PointCloud<PointXYZ> ());
00144   project_inliers_filter.filter (*cloud_projected);
00145 
00146   ConcaveHull<PointXYZ> concave_hull_2d;
00147   concave_hull_2d.setAlpha (0.5);
00148   concave_hull_2d.setInputCloud (cloud_projected);
00149   PointCloud<PointXYZ> hull_2d;
00150   concave_hull_2d.reconstruct (hull_2d);
00151 
00152   EXPECT_EQ (concave_hull_2d.getDimension (), 2);
00153 }
00154 
00155 
00156 TEST (PCL, ConcaveHull_4points)
00157 {
00158   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_4 (new pcl::PointCloud<pcl::PointXYZ> ());
00159   pcl::PointXYZ p;
00160   p.x = p.y = p.z = 0.f;
00161   cloud_4->push_back (p);
00162 
00163   p.x = 1.f;
00164   p.y = 0.f;
00165   p.z = 0.f;
00166   cloud_4->push_back (p);
00167 
00168   p.x = 0.f;
00169   p.y = 1.f;
00170   p.z = 0.f;
00171   cloud_4->push_back (p);
00172 
00173   p.x = 1.f;
00174   p.y = 1.f;
00175   p.z = 0.f;
00176   cloud_4->push_back (p);
00177 
00178   cloud_4->height = 1;
00179   cloud_4->width = uint32_t (cloud_4->size ());
00180 
00181   ConcaveHull<PointXYZ> concave_hull;
00182   concave_hull.setInputCloud (cloud_4);
00183   concave_hull.setAlpha (10.);
00184   PolygonMesh mesh;
00185   concave_hull.reconstruct (mesh);
00186 
00187   EXPECT_EQ (mesh.polygons.size (), 1);
00188   EXPECT_EQ (mesh.polygons[0].vertices.size (), 4);
00189 
00190   PointCloud<PointXYZ> mesh_cloud;
00191   fromPCLPointCloud2 (mesh.cloud, mesh_cloud);
00192 
00193   EXPECT_NEAR (mesh_cloud[0].x, 1.f, 1e-6);
00194   EXPECT_NEAR (mesh_cloud[0].y, 0.f, 1e-6);
00195   EXPECT_NEAR (mesh_cloud[0].z, 0.f, 1e-6);
00196 
00197   EXPECT_NEAR (mesh_cloud[1].x, 0.f, 1e-6);
00198   EXPECT_NEAR (mesh_cloud[1].y, 0.f, 1e-6);
00199   EXPECT_NEAR (mesh_cloud[1].z, 0.f, 1e-6);
00200 
00201   EXPECT_NEAR (mesh_cloud[2].x, 0.f, 1e-6);
00202   EXPECT_NEAR (mesh_cloud[2].y, 1.f, 1e-6);
00203   EXPECT_NEAR (mesh_cloud[2].z, 0.f, 1e-6);
00204 
00205   EXPECT_NEAR (mesh_cloud[3].x, 1.f, 1e-6);
00206   EXPECT_NEAR (mesh_cloud[3].y, 1.f, 1e-6);
00207   EXPECT_NEAR (mesh_cloud[3].z, 0.f, 1e-6);
00208 }
00209 
00211 TEST (PCL, ConcaveHull_LTable)
00212 {
00213   //construct dataset
00214   pcl::PointCloud<pcl::PointXYZ> cloud_out_ltable;
00215   cloud_out_ltable.points.resize (100);
00216 
00217   int npoints = 0;
00218   for (size_t i = 0; i < 8; i++)
00219   {
00220     for (size_t j = 0; j <= 2; j++)
00221     {
00222       cloud_out_ltable.points[npoints].x = float (i) * 0.5f;
00223       cloud_out_ltable.points[npoints].y = -float (j) * 0.5f;
00224       cloud_out_ltable.points[npoints].z = 0.f;
00225       npoints++;
00226     }
00227   }
00228 
00229   for (size_t i = 0; i <= 2; i++)
00230   {
00231     for(size_t j = 3; j < 8; j++)
00232     {
00233       cloud_out_ltable.points[npoints].x = float (i) * 0.5f;
00234       cloud_out_ltable.points[npoints].y = -float (j) * 0.5f;
00235       cloud_out_ltable.points[npoints].z = 0.f;
00236       npoints++;
00237     }
00238   }
00239 
00240   cloud_out_ltable.points.resize (npoints);
00241 
00242   pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloudptr (new pcl::PointCloud<pcl::PointXYZ> (cloud_out_ltable));
00243 
00244   pcl::PointCloud<pcl::PointXYZ> alpha_shape;
00245   pcl::PointCloud<pcl::PointXYZ>::Ptr voronoi_centers (new pcl::PointCloud<pcl::PointXYZ>);
00246   std::vector<pcl::Vertices> polygons_alpha;
00247 
00248   pcl::ConcaveHull<pcl::PointXYZ> concave_hull;
00249   concave_hull.setInputCloud (cloudptr);
00250   concave_hull.setAlpha (0.5);
00251   concave_hull.setVoronoiCenters (voronoi_centers);
00252   concave_hull.reconstruct (alpha_shape, polygons_alpha);
00253 
00254   EXPECT_EQ (alpha_shape.points.size (), 27);
00255 
00256   pcl::PointCloud<pcl::PointXYZ> alpha_shape1;
00257   pcl::PointCloud<pcl::PointXYZ>::Ptr voronoi_centers1 (new pcl::PointCloud<pcl::PointXYZ>);
00258   std::vector<pcl::Vertices> polygons_alpha1;
00259 
00260   pcl::ConcaveHull<pcl::PointXYZ> concave_hull1;
00261   concave_hull1.setInputCloud (cloudptr);
00262   concave_hull1.setAlpha (1.5);
00263   concave_hull1.setVoronoiCenters (voronoi_centers1);
00264   concave_hull1.reconstruct (alpha_shape1, polygons_alpha1);
00265 
00266   EXPECT_EQ (alpha_shape1.points.size (), 23);
00267 
00268   pcl::PointCloud<pcl::PointXYZ> alpha_shape2;
00269   pcl::PointCloud<pcl::PointXYZ>::Ptr voronoi_centers2 (new pcl::PointCloud<pcl::PointXYZ>);
00270   std::vector<pcl::Vertices> polygons_alpha2;
00271   pcl::ConcaveHull<pcl::PointXYZ> concave_hull2;
00272   concave_hull2.setInputCloud (cloudptr);
00273   concave_hull2.setAlpha (3);
00274   concave_hull2.setVoronoiCenters (voronoi_centers2);
00275   concave_hull2.reconstruct (alpha_shape2, polygons_alpha2);
00276 
00277   EXPECT_EQ (alpha_shape2.points.size (), 19);
00278 }
00279 
00280 /* ---[ */
00281 int
00282 main (int argc, char** argv)
00283 {
00284   if (argc < 2)
00285   {
00286     std::cerr << "No test file given. Please download `bun0.pcd` and pass its path to the test." << std::endl;
00287     return (-1);
00288   }
00289 
00290   // Load file
00291   pcl::PCLPointCloud2 cloud_blob;
00292   loadPCDFile (argv[1], cloud_blob);
00293   fromPCLPointCloud2 (cloud_blob, *cloud);
00294 
00295   // Create search tree
00296   tree.reset (new search::KdTree<PointXYZ> (false));
00297   tree->setInputCloud (cloud);
00298 
00299   // Normal estimation
00300   NormalEstimation<PointXYZ, Normal> n;
00301   PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
00302   n.setInputCloud (cloud);
00303   //n.setIndices (indices[B);
00304   n.setSearchMethod (tree);
00305   n.setKSearch (20);
00306   n.compute (*normals);
00307 
00308   // Concatenate XYZ and normal information
00309   pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
00310       
00311   // Create search tree
00312   tree2.reset (new search::KdTree<PointNormal>);
00313   tree2->setInputCloud (cloud_with_normals);
00314 
00315   // Process for update cloud
00316   if (argc == 3)
00317   {
00318     pcl::PCLPointCloud2 cloud_blob1;
00319     loadPCDFile (argv[2], cloud_blob1);
00320     fromPCLPointCloud2 (cloud_blob1, *cloud1);
00321         // Create search tree
00322     tree3.reset (new search::KdTree<PointXYZ> (false));
00323     tree3->setInputCloud (cloud1);
00324 
00325     // Normal estimation
00326     NormalEstimation<PointXYZ, Normal> n1;
00327     PointCloud<Normal>::Ptr normals1 (new PointCloud<Normal> ());
00328     n1.setInputCloud (cloud1);
00329 
00330     n1.setSearchMethod (tree3);
00331     n1.setKSearch (20);
00332     n1.compute (*normals1);
00333 
00334     // Concatenate XYZ and normal information
00335     pcl::concatenateFields (*cloud1, *normals1, *cloud_with_normals1);
00336     // Create search tree
00337     tree4.reset (new search::KdTree<PointNormal>);
00338     tree4->setInputCloud (cloud_with_normals1);
00339   }
00340 
00341   // Testing
00342   testing::InitGoogleTest (&argc, argv);
00343   return (RUN_ALL_TESTS ());
00344 }
00345 /* ]--- */


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:34:32