/* ---[ */ int main (int argc, char** argv) { if (argc < 2) { std::cerr << "No test file given. Please download `sac_plane_test.pcd` and pass its path to the test." << std::endl; return (-1); } // Load a standard PCD file from disk sensor_msgs::PointCloud2 cloud_blob; if (loadPCDFile (argv[1], cloud_blob) < 0) { std::cerr << "Failed to read test file. Please download `sac_plane_test.pcd` and pass its path to the test." << std::endl; return (-1); } fromROSMsg (cloud_blob, *cloud_); indices_.resize (cloud_->points.size ()); for (size_t i = 0; i < indices_.size (); ++i) { indices_[i] = int (i); } // Estimate surface normals NormalEstimation<PointXYZ, Normal> n; search::Search<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ>); tree->setInputCloud (cloud_); n.setInputCloud (cloud_); boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices_)); n.setIndices (indicesptr); n.setSearchMethod (tree); n.setRadiusSearch (0.02); // Use 2cm radius to estimate the normals n.compute (*normals_); testing::InitGoogleTest (&argc, argv); return (RUN_ALL_TESTS ()); }
void estimateNormals (const typename PointCloud<PointT>::ConstPtr &input, PointCloud<Normal> &normals) { if (input->isOrganized ()) { IntegralImageNormalEstimation<PointT, Normal> ne; // Set the parameters for normal estimation ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); ne.setMaxDepthChangeFactor (0.02f); ne.setNormalSmoothingSize (20.0f); // Estimate normals in the cloud ne.setInputCloud (input); ne.compute (normals); // Save the distance map for the plane comparator float *map=ne.getDistanceMap ();// This will be deallocated with the IntegralImageNormalEstimation object... distance_map_.assign(map, map+input->size() ); //...so we must copy the data out plane_comparator_->setDistanceMap(distance_map_.data()); } else { NormalEstimation<PointT, Normal> ne; ne.setInputCloud (input); ne.setRadiusSearch (0.02f); ne.setSearchMethod (search_); ne.compute (normals); } }
TEST (PCL, VFHEstimation) { // Estimate normals first NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); // set parameters n.setInputCloud (cloud.makeShared ()); boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices)); n.setIndices (indicesptr); n.setSearchMethod (tree); n.setKSearch (10); // Use 10 nearest neighbors to estimate the normals // estimate n.compute (*normals); VFHEstimation<PointXYZ, Normal, VFHSignature308> vfh; vfh.setInputNormals (normals); // PointCloud<PointNormal> cloud_normals; // concatenateFields (cloud, normals, cloud_normals); // savePCDFile ("bun0_n.pcd", cloud_normals); // Object PointCloud<VFHSignature308>::Ptr vfhs (new PointCloud<VFHSignature308> ()); // set parameters vfh.setInputCloud (cloud.makeShared ()); vfh.setIndices (indicesptr); vfh.setSearchMethod (tree); // estimate vfh.compute (*vfhs); EXPECT_EQ (int (vfhs->points.size ()), 1); //for (size_t d = 0; d < 308; ++d) // std::cerr << vfhs.points[0].histogram[d] << std::endl; }
TEST (PCL, CVFHEstimationMilk) { // Estimate normals first NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); n.setInputCloud (cloud_milk); n.setSearchMethod (tree); n.setRadiusSearch (leaf_size_ * 4); //2cm to estimate normals n.compute (*normals); CVFHEstimation<PointXYZ, Normal, VFHSignature308> cvfh; cvfh.setInputCloud (cloud_milk); cvfh.setInputNormals (normals); cvfh.setSearchMethod (tree_milk); cvfh.setClusterTolerance (leaf_size_ * 3); cvfh.setEPSAngleThreshold (0.13f); cvfh.setCurvatureThreshold (0.025f); cvfh.setNormalizeBins (false); cvfh.setRadiusNormals (leaf_size_ * 4); // Object PointCloud<VFHSignature308>::Ptr vfhs (new PointCloud<VFHSignature308> ()); // estimate cvfh.compute (*vfhs); EXPECT_EQ (static_cast<int>(vfhs->points.size ()), 2); }
TEST (PCL, CVFHEstimation) { // Estimate normals first NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); // set parameters n.setInputCloud (cloud.makeShared ()); boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices)); n.setIndices (indicesptr); n.setSearchMethod (tree); n.setKSearch (10); // Use 10 nearest neighbors to estimate the normals // estimate n.compute (*normals); CVFHEstimation<PointXYZ, Normal, VFHSignature308> cvfh; cvfh.setInputNormals (normals); // Object PointCloud<VFHSignature308>::Ptr vfhs (new PointCloud<VFHSignature308> ()); // set parameters cvfh.setInputCloud (cloud.makeShared ()); cvfh.setIndices (indicesptr); cvfh.setSearchMethod (tree); // estimate cvfh.compute (*vfhs); EXPECT_EQ (static_cast<int>(vfhs->points.size ()), 1); }
TEST (PCL, SHOTGlobalReferenceFrame) { // Estimate normals first double mr = 0.002; NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); // set parameters n.setInputCloud (cloud.makeShared ()); boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices)); n.setIndices (indicesptr); n.setSearchMethod (tree); n.setRadiusSearch (20 * mr); n.compute (*normals); EXPECT_NEAR (normals->points[103].normal_x, 0.36683175, 1e-4); EXPECT_NEAR (normals->points[103].normal_y, -0.44696972, 1e-4); EXPECT_NEAR (normals->points[103].normal_z, -0.81587529, 1e-4); EXPECT_NEAR (normals->points[200].normal_x, -0.71414840, 1e-4); EXPECT_NEAR (normals->points[200].normal_y, -0.06002361, 1e-4); EXPECT_NEAR (normals->points[200].normal_z, -0.69741613, 1e-4); EXPECT_NEAR (normals->points[140].normal_x, -0.45109111, 1e-4); EXPECT_NEAR (normals->points[140].normal_y, -0.19499126, 1e-4); EXPECT_NEAR (normals->points[140].normal_z, -0.87091631, 1e-4); boost::shared_ptr<vector<int> > test_indices (new vector<int> (0)); for (size_t i = 0; i < cloud.size (); i+=3) test_indices->push_back (static_cast<int> (i)); testGSHOTGlobalReferenceFrame<GSHOTEstimation<PointXYZ, Normal, SHOT352>, PointXYZ, Normal, SHOT352> (cloud.makeShared (), normals, test_indices); }
void SurfaceTriangulation::perform(int ksearch) { PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>); PointCloud<PointNormal>::Ptr cloud_with_normals (new PointCloud<PointNormal>); search::KdTree<PointXYZ>::Ptr tree; search::KdTree<PointNormal>::Ptr tree2; cloud->reserve(myPoints.size()); for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) { if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y) && !boost::math::isnan(it->z)) cloud->push_back(PointXYZ(it->x, it->y, it->z)); } // Create search tree tree.reset (new search::KdTree<PointXYZ> (false)); tree->setInputCloud (cloud); // Normal estimation NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); n.setInputCloud (cloud); //n.setIndices (indices[B); n.setSearchMethod (tree); n.setKSearch (ksearch); n.compute (*normals); // Concatenate XYZ and normal information pcl::concatenateFields (*cloud, *normals, *cloud_with_normals); // Create search tree tree2.reset (new search::KdTree<PointNormal>); tree2->setInputCloud (cloud_with_normals); // Init objects GreedyProjectionTriangulation<PointNormal> gp3; // Set parameters gp3.setInputCloud (cloud_with_normals); gp3.setSearchMethod (tree2); gp3.setSearchRadius (searchRadius); gp3.setMu (mu); 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); gp3.setConsistentVertexOrdering(true); // Reconstruct PolygonMesh mesh; gp3.reconstruct (mesh); MeshConversion::convert(mesh, myMesh); // Additional vertex information //std::vector<int> parts = gp3.getPartIDs(); //std::vector<int> states = gp3.getPointStates(); }
void computeFeatureViaNormals (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output, int argc, char** argv, bool set_search_flag = true) { int n_k = default_n_k; int f_k = default_f_k; double n_radius = default_n_radius; double f_radius = default_f_radius; parse_argument (argc, argv, "-n_k", n_k); parse_argument (argc, argv, "-n_radius", n_radius); parse_argument (argc, argv, "-f_k", f_k); parse_argument (argc, argv, "-f_radius", f_radius); // Convert data to PointCloud<PointIn> typename PointCloud<PointIn>::Ptr xyz (new PointCloud<PointIn>); fromPCLPointCloud2 (*input, *xyz); // Estimate TicToc tt; tt.tic (); print_highlight (stderr, "Computing "); NormalEstimation<PointIn, NormalT> ne; ne.setInputCloud (xyz); ne.setSearchMethod (typename pcl::search::KdTree<PointIn>::Ptr (new pcl::search::KdTree<PointIn>)); ne.setKSearch (n_k); ne.setRadiusSearch (n_radius); typename PointCloud<NormalT>::Ptr normals = typename PointCloud<NormalT>::Ptr (new PointCloud<NormalT>); ne.compute (*normals); FeatureAlgorithm feature_est; feature_est.setInputCloud (xyz); feature_est.setInputNormals (normals); feature_est.setSearchMethod (typename pcl::search::KdTree<PointIn>::Ptr (new pcl::search::KdTree<PointIn>)); PointCloud<PointOut> output_features; if (set_search_flag) { feature_est.setKSearch (f_k); feature_est.setRadiusSearch (f_radius); } feature_est.compute (output_features); print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n"); // Convert data back toPCLPointCloud2 (output_features, output); }
void FeatSegment::segment(const PointCloud<PointXYZRGB >::Ptr cloud, PointCloud<PointXYZRGB >::Ptr outcloud) { // PARAM - Lots of them int min_pts_per_cluster = 10; int max_pts_per_cluster = INT_MAX; //3000000; assert(max_pts_per_cluster > 3000000); // Avoid overflow int number_neighbours = 50; // PARAM float radius = 0.025; // 0.025 PARAM float angle = 0.52; // PARAM // Required KdTrees pcl::search::KdTree<PointXYZRGB >::Ptr NormalsTree(new pcl::search::KdTree<PointXYZRGB >); pcl::search::KdTree<PointXYZRGB >::Ptr ClustersTree(new pcl::search::KdTree<PointXYZRGB >); PointCloud<Normal >::Ptr CloudNormals(new PointCloud<Normal >); NormalEstimation<PointXYZRGB, Normal > NormalsFinder; std::vector<PointIndices > clusters; ClustersTree->setInputCloud(cloud); // Set normal estimation parameters NormalsFinder.setKSearch(number_neighbours); NormalsFinder.setSearchMethod(NormalsTree); NormalsFinder.setInputCloud(cloud); NormalsFinder.compute(*CloudNormals); extractEuclideanClusters(cloud, CloudNormals, ClustersTree, radius, clusters, angle, min_pts_per_cluster, max_pts_per_cluster); fprintf(stderr, "Number of clusters found matching the given constraints: %d.", (int)clusters.size ()); std::ofstream FileStr2; FileStr2.open("live_segments.txt", ios::app); // NOTE: Don't add the ios:trunc flag here! // Copy to clusters to segments for (size_t i = 0; i < clusters.size (); ++i) { if(FileStr2.is_open()) { for(int j = 0; j < clusters[i].indices.size(); ++j) { if(j == clusters[i].indices.size() - 1) { FileStr2 << clusters[i].indices.at(j) << endl; continue; // Also break; } FileStr2 << clusters[i].indices.at(j) << " "; } FeatPointCloud * Segment = new FeatPointCloud(m_SceneCloud, clusters[i].indices, m_ConfigFName); m_Segments.push_back(Segment); } else cout << "Failed to open file.\n"; } FileStr2.close(); }
void PoissonReconstruction::perform(int ksearch) { PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>); PointCloud<PointNormal>::Ptr cloud_with_normals (new PointCloud<PointNormal>); search::KdTree<PointXYZ>::Ptr tree; search::KdTree<PointNormal>::Ptr tree2; cloud->reserve(myPoints.size()); for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) { if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y) && !boost::math::isnan(it->z)) cloud->push_back(PointXYZ(it->x, it->y, it->z)); } // Create search tree tree.reset (new search::KdTree<PointXYZ> (false)); tree->setInputCloud (cloud); // Normal estimation NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); n.setInputCloud (cloud); //n.setIndices (indices[B); n.setSearchMethod (tree); n.setKSearch (ksearch); n.compute (*normals); // Concatenate XYZ and normal information pcl::concatenateFields (*cloud, *normals, *cloud_with_normals); // Create search tree tree2.reset (new search::KdTree<PointNormal>); tree2->setInputCloud (cloud_with_normals); // Init objects Poisson<PointNormal> poisson; // Set parameters poisson.setInputCloud (cloud_with_normals); poisson.setSearchMethod (tree2); if (depth >= 1) poisson.setDepth(depth); if (solverDivide >= 1) poisson.setSolverDivide(solverDivide); if (samplesPerNode >= 1.0f) poisson.setSamplesPerNode(samplesPerNode); // Reconstruct PolygonMesh mesh; poisson.reconstruct (mesh); MeshConversion::convert(mesh, myMesh); }
/** estimate the normals of a point cloud */ static PointCloud<Normal>::Ptr compute_pcn(PointCloud<PointXYZ>::ConstPtr in, float vx, float vy, float vz) { PointCloud<Normal>::Ptr pcn (new PointCloud<Normal>()); NormalEstimation<PointXYZ, Normal> ne; search::KdTree<PointXYZ>::Ptr kdt (new search::KdTree<PointXYZ>()); ne.setInputCloud(in); ne.setSearchMethod(kdt); ne.setKSearch(20); ne.setViewPoint(vx, vy, vz); ne.compute(*pcn); return pcn; }
void Reen::MarchingCubesRBF::perform(int ksearch) { PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>); PointCloud<PointNormal>::Ptr cloud_with_normals (new PointCloud<PointNormal>); search::KdTree<PointXYZ>::Ptr tree; search::KdTree<PointNormal>::Ptr tree2; cloud->reserve(myPoints.size()); for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) { if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y) && !boost::math::isnan(it->z)) cloud->push_back(PointXYZ(it->x, it->y, it->z)); } // Create search tree tree.reset (new search::KdTree<PointXYZ> (false)); tree->setInputCloud (cloud); // Normal estimation NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); n.setInputCloud (cloud); //n.setIndices (indices[B); n.setSearchMethod (tree); n.setKSearch (ksearch); n.compute (*normals); // Concatenate XYZ and normal information pcl::concatenateFields (*cloud, *normals, *cloud_with_normals); // Create search tree tree2.reset (new search::KdTree<PointNormal>); tree2->setInputCloud (cloud_with_normals); // Init objects pcl::MarchingCubesRBF<PointNormal> rbf; // Set parameters rbf.setIsoLevel (0); rbf.setGridResolution (60, 60, 60); rbf.setPercentageExtendGrid (0.1f); rbf.setOffSurfaceDisplacement (0.02f); rbf.setInputCloud (cloud_with_normals); rbf.setSearchMethod (tree2); // Reconstruct PolygonMesh mesh; rbf.reconstruct (mesh); MeshConversion::convert(mesh, myMesh); }
void GridReconstruction::perform(int ksearch) { PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>); PointCloud<PointNormal>::Ptr cloud_with_normals (new PointCloud<PointNormal>); search::KdTree<PointXYZ>::Ptr tree; search::KdTree<PointNormal>::Ptr tree2; cloud->reserve(myPoints.size()); for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) { if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y) && !boost::math::isnan(it->z)) cloud->push_back(PointXYZ(it->x, it->y, it->z)); } // Create search tree tree.reset (new search::KdTree<PointXYZ> (false)); tree->setInputCloud (cloud); // Normal estimation NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); n.setInputCloud (cloud); //n.setIndices (indices[B); n.setSearchMethod (tree); n.setKSearch (ksearch); n.compute (*normals); // Concatenate XYZ and normal information pcl::concatenateFields (*cloud, *normals, *cloud_with_normals); // Create search tree tree2.reset (new search::KdTree<PointNormal>); tree2->setInputCloud (cloud_with_normals); // Init objects GridProjection<PointNormal> grid; // Set parameters grid.setResolution(0.005); grid.setPaddingSize(3); grid.setNearestNeighborNum(100); grid.setMaxBinarySearchLevel(10); grid.setInputCloud (cloud_with_normals); grid.setSearchMethod (tree2); // Reconstruct PolygonMesh mesh; grid.reconstruct (mesh); MeshConversion::convert(mesh, myMesh); }
// Subsample cloud for faster matching and processing, while filling in normals. void PointcloudProc::reduceCloud(const PointCloud<PointXYZRGB>& input, PointCloud<PointXYZRGBNormal>& output) const { PointCloud<PointXYZRGB> cloud_nan_filtered, cloud_box_filtered, cloud_voxel_reduced; PointCloud<Normal> normals; PointCloud<PointXYZRGBNormal> cloud_normals; std::vector<int> indices; // Filter out nans. removeNaNFromPointCloud(input, cloud_nan_filtered, indices); indices.clear(); // Filter out everything outside a [200x200x200] box. Eigen::Vector4f min_pt(-100, -100, -100, -100); Eigen::Vector4f max_pt(100, 100, 100, 100); getPointsInBox(cloud_nan_filtered, min_pt, max_pt, indices); ExtractIndices<PointXYZRGB> boxfilter; boxfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_nan_filtered)); boxfilter.setIndices (boost::make_shared<vector<int> > (indices)); boxfilter.filter(cloud_box_filtered); // Reduce pointcloud VoxelGrid<PointXYZRGB> voxelfilter; voxelfilter.setInputCloud (boost::make_shared<const PointCloud<PointXYZRGB> > (cloud_box_filtered)); voxelfilter.setLeafSize (0.05, 0.05, 0.05); // voxelfilter.setLeafSize (0.1, 0.1, 0.1); voxelfilter.filter (cloud_voxel_reduced); // Compute normals NormalEstimation<PointXYZRGB, Normal> normalest; normalest.setViewPoint(0, 0, 0); normalest.setSearchMethod (boost::make_shared<search::KdTree<PointXYZRGB> > ()); //normalest.setKSearch (10); normalest.setRadiusSearch (0.25); // normalest.setRadiusSearch (0.4); normalest.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_voxel_reduced)); normalest.compute(normals); pcl::concatenateFields (cloud_voxel_reduced, normals, cloud_normals); // Filter based on curvature PassThrough<PointXYZRGBNormal> normalfilter; normalfilter.setFilterFieldName("curvature"); // normalfilter.setFilterLimits(0.0, 0.2); normalfilter.setFilterLimits(0.0, 0.2); normalfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGBNormal> >(cloud_normals)); normalfilter.filter(output); }
void create_normals (typename PointCloud<PointT>::Ptr cloud, typename PointCloud<NormalT>::Ptr normals, float normal_radius=0.03) { NormalEstimation<PointT, NormalT> nest; cout << "[PFHTransformationStrategy::create_normals] Input cloud " << cloud->points.size() << " points" << endl; nest.setInputCloud(cloud); nest.setSearchMethod (typename search::KdTree<PointT>::Ptr (new search::KdTree<PointT>)); nest.setRadiusSearch(normal_radius); nest.compute(*normals); };
TEST (PCL, GSHOTRadius) { float radius = radius_local_shot / 4.0f; // Estimate normals first double mr = 0.002; NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); // set parameters n.setInputCloud (cloud.makeShared ()); boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices)); n.setIndices (indicesptr); n.setSearchMethod (tree); n.setRadiusSearch (20 * mr); n.compute (*normals); // Objects PointCloud<SHOT352>::Ptr gshots352 (new PointCloud<SHOT352> ()); PointCloud<SHOT352>::Ptr shots352 (new PointCloud<SHOT352> ()); // SHOT352 (local) SHOTEstimation<PointXYZ, Normal, SHOT352> shot352; shot352.setInputNormals (normals); shot352.setRadiusSearch (radius); shot352.setInputCloud (cloud_for_lrf.makeShared ()); boost::shared_ptr<vector<int> > indices_local_shot_ptr (new vector<int> (indices_local_shot)); shot352.setIndices (indices_local_shot_ptr); shot352.setSearchSurface (cloud.makeShared()); shot352.compute (*shots352); // SHOT352 (global) GSHOTEstimation<PointXYZ, Normal, SHOT352> gshot352; gshot352.setInputNormals (normals); // set parameters gshot352.setInputCloud (cloud.makeShared ()); gshot352.setIndices (indicesptr); gshot352.setSearchMethod (tree); gshot352.setRadiusSearch (radius); EXPECT_EQ (gshot352.getRadiusSearch (), shot352.getRadiusSearch ()); // estimate gshot352.compute (*gshots352); checkDescNear (*gshots352, *shots352, 1E-7); }
void getCylClusters (boost::shared_ptr<PointCloud<T> > sceneCloud, vector<boost::shared_ptr<PointCloud<T> > > &outVector) { typedef typename pcl::search::KdTree<T>::Ptr my_KdTreePtr; typedef typename pcl::PointCloud<T>::Ptr my_PointCloudPtr; NormalEstimation<T, Normal> ne; SACSegmentationFromNormals<T, Normal> seg; my_KdTreePtr tree (new pcl::search::KdTree<T> ()); PointCloud<Normal>::Ptr cloud_normals (new PointCloud<pcl::Normal>); ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients); PointIndices::Ptr inliers_cylinder (new pcl::PointIndices); ExtractIndices<T> extract; // Estimate point normals ne.setSearchMethod (tree); ne.setInputCloud (sceneCloud); ne.setKSearch (50); ne.compute (*cloud_normals); seg.setOptimizeCoefficients (true); seg.setModelType (SACMODEL_CYLINDER); seg.setMethodType (SAC_RANSAC); seg.setNormalDistanceWeight (0.1); seg.setMaxIterations (10000); seg.setDistanceThreshold (0.05); seg.setRadiusLimits (0, 0.1); seg.setInputCloud (sceneCloud); seg.setInputNormals (cloud_normals); // Obtain the cylinder inliers and coefficients seg.segment (*inliers_cylinder, *coefficients_cylinder); cout << " Number of inliers vs total number of points " << inliers_cylinder->indices.size() << " vs " << sceneCloud->size() << endl; // Write the cylinder inliers to disk extract.setInputCloud (sceneCloud); extract.setIndices (inliers_cylinder); extract.setNegative (false); my_PointCloudPtr cloud_cylinder (new PointCloud<T> ()); extract.filter (*cloud_cylinder); outVector.push_back(cloud_cylinder); }
void compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output, int k, double radius) { // Convert data to PointCloud<T> PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>); fromROSMsg (*input, *xyz); TicToc tt; tt.tic (); PointCloud<Normal> normals; // Try our luck with organized integral image based normal estimation if (xyz->isOrganized ()) { IntegralImageNormalEstimation<PointXYZ, Normal> ne; ne.setInputCloud (xyz); ne.setNormalEstimationMethod (IntegralImageNormalEstimation<PointXYZ, Normal>::COVARIANCE_MATRIX); ne.setNormalSmoothingSize (float (radius)); ne.setDepthDependentSmoothing (true); ne.compute (normals); } else { NormalEstimation<PointXYZ, Normal> ne; ne.setInputCloud (xyz); ne.setSearchMethod (search::KdTree<PointXYZ>::Ptr (new search::KdTree<PointXYZ>)); ne.setKSearch (k); ne.setRadiusSearch (radius); ne.compute (normals); } print_highlight ("Computed normals in "); print_value ("%g", tt.toc ()); print_info (" ms for "); print_value ("%d", normals.width * normals.height); print_info (" points.\n"); // Convert data back sensor_msgs::PointCloud2 output_normals; toROSMsg (normals, output_normals); concatenateFields (*input, output_normals, output); }
TEST (PCL, NormalEstimation) { tree.reset (new search::KdTree<PointXYZ> (false)); n.setSearchMethod (tree); n.setKSearch (10); n.setInputCloud (cloud.makeShared ()); PointCloud<Normal> output; n.compute (output); EXPECT_EQ (output.points.size (), cloud.points.size ()); EXPECT_EQ (output.width, cloud.width); EXPECT_EQ (output.height, cloud.height); for (size_t i = 0; i < cloud.points.size (); ++i) { EXPECT_NEAR (fabs (output.points[i].normal_x), 0, 1e-2); EXPECT_NEAR (fabs (output.points[i].normal_y), 0, 1e-2); EXPECT_NEAR (fabs (output.points[i].normal_z), 1.0, 1e-2); } }
TEST (PCL, GSHOTShapeEstimation) { // Estimate normals first double mr = 0.002; NormalEstimation<PointXYZ, Normal> n; boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices)); n.setInputCloud (cloud.makeShared ()); n.setIndices (indicesptr); n.setSearchMethod (tree); n.setRadiusSearch (20 * mr); PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); n.compute (*normals); EXPECT_NEAR (normals->points[103].normal_x, 0.36683175, 1e-4); EXPECT_NEAR (normals->points[103].normal_y, -0.44696972, 1e-4); EXPECT_NEAR (normals->points[103].normal_z, -0.81587529, 1e-4); EXPECT_NEAR (normals->points[200].normal_x, -0.71414840, 1e-4); EXPECT_NEAR (normals->points[200].normal_y, -0.06002361, 1e-4); EXPECT_NEAR (normals->points[200].normal_z, -0.69741613, 1e-4); EXPECT_NEAR (normals->points[140].normal_x, -0.45109111, 1e-4); EXPECT_NEAR (normals->points[140].normal_y, -0.19499126, 1e-4); EXPECT_NEAR (normals->points[140].normal_z, -0.87091631, 1e-4); // Objects PointCloud<SHOT352>::Ptr gshots352 (new PointCloud<SHOT352> ()); PointCloud<SHOT352>::Ptr shots352 (new PointCloud<SHOT352> ()); // SHOT352 (local) SHOTEstimation<PointXYZ, Normal, SHOT352> shot352; shot352.setInputNormals (normals); shot352.setRadiusSearch (radius_local_shot); shot352.setInputCloud (cloud_for_lrf.makeShared ()); boost::shared_ptr<vector<int> > indices_local_shot_ptr (new vector<int> (indices_local_shot)); shot352.setIndices (indices_local_shot_ptr); shot352.setSearchSurface (cloud.makeShared()); shot352.compute (*shots352); EXPECT_NEAR (shots352->points[0].descriptor[9 ], 0.0f, 1E-4); EXPECT_NEAR (shots352->points[0].descriptor[10], 0.0f, 1E-4); EXPECT_NEAR (shots352->points[0].descriptor[11], 0.317935f, 1E-4); EXPECT_NEAR (shots352->points[0].descriptor[19], 0.0f, 1E-4); EXPECT_NEAR (shots352->points[0].descriptor[20], 0.0f, 1E-4); EXPECT_NEAR (shots352->points[0].descriptor[21], 0.0f, 1E-4); EXPECT_NEAR (shots352->points[0].descriptor[42], 0.0f, 1E-4); EXPECT_NEAR (shots352->points[0].descriptor[53], 0.0f, 1E-4); EXPECT_NEAR (shots352->points[0].descriptor[54], 0.0f, 1E-4); EXPECT_NEAR (shots352->points[0].descriptor[55], 0.089004f, 1E-4); // SHOT352 (global) GSHOTEstimation<PointXYZ, Normal, SHOT352> gshot352; gshot352.setSearchMethod (tree); gshot352.setInputNormals (normals); EXPECT_EQ (gshot352.getInputNormals (), normals); // set parameters gshot352.setInputCloud (cloud.makeShared ()); gshot352.setIndices (indicesptr); // estimate int gshot_size = 1; gshot352.compute (*gshots352); EXPECT_EQ (gshots352->points.size (), gshot_size); checkDescNear (*gshots352, *shots352, 1E-7); }
TEST (PCL, PrincipalCurvaturesEstimation) { float pcx, pcy, pcz, pc1, pc2; // Estimate normals first NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); // set parameters n.setInputCloud (cloud.makeShared ()); boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices)); n.setIndices (indicesptr); n.setSearchMethod (tree); n.setKSearch (10); // Use 10 nearest neighbors to estimate the normals // estimate n.compute (*normals); PrincipalCurvaturesEstimation<PointXYZ, Normal, PrincipalCurvatures> pc; pc.setInputNormals (normals); EXPECT_EQ (pc.getInputNormals (), normals); // computePointPrincipalCurvatures (indices) pc.computePointPrincipalCurvatures (*normals, 0, indices, pcx, pcy, pcz, pc1, pc2); EXPECT_NEAR (fabs (pcx), 0.98509, 1e-4); EXPECT_NEAR (fabs (pcy), 0.10714, 1e-4); EXPECT_NEAR (fabs (pcz), 0.13462, 1e-4); EXPECT_NEAR (pc1, 0.23997423052787781, 1e-4); EXPECT_NEAR (pc2, 0.19400238990783691, 1e-4); pc.computePointPrincipalCurvatures (*normals, 2, indices, pcx, pcy, pcz, pc1, pc2); EXPECT_NEAR (pcx, 0.98079, 1e-4); EXPECT_NEAR (pcy, -0.04019, 1e-4); EXPECT_NEAR (pcz, 0.19086, 1e-4); EXPECT_NEAR (pc1, 0.27207490801811218, 1e-4); EXPECT_NEAR (pc2, 0.19464978575706482, 1e-4); int indices_size = static_cast<int> (indices.size ()); pc.computePointPrincipalCurvatures (*normals, indices_size - 3, indices, pcx, pcy, pcz, pc1, pc2); EXPECT_NEAR (pcx, 0.86725, 1e-4); EXPECT_NEAR (pcy, -0.37599, 1e-4); EXPECT_NEAR (pcz, 0.32635, 1e-4); EXPECT_NEAR (pc1, 0.25900053977966309, 1e-4); EXPECT_NEAR (pc2, 0.17906945943832397, 1e-4); pc.computePointPrincipalCurvatures (*normals, indices_size - 1, indices, pcx, pcy, pcz, pc1, pc2); EXPECT_NEAR (pcx, 0.86725, 1e-4); EXPECT_NEAR (pcy, -0.375851, 1e-3); EXPECT_NEAR (pcz, 0.32636, 1e-4); EXPECT_NEAR (pc1, 0.2590005099773407, 1e-4); EXPECT_NEAR (pc2, 0.17906956374645233, 1e-4); // Object PointCloud<PrincipalCurvatures>::Ptr pcs (new PointCloud<PrincipalCurvatures> ()); // set parameters pc.setInputCloud (cloud.makeShared ()); pc.setIndices (indicesptr); pc.setSearchMethod (tree); pc.setKSearch (indices_size); // estimate pc.compute (*pcs); EXPECT_EQ (pcs->points.size (), indices.size ()); // Adjust for small numerical inconsitencies (due to nn_indices not being sorted) EXPECT_NEAR (fabs (pcs->points[0].principal_curvature[0]), 0.98509, 1e-4); EXPECT_NEAR (fabs (pcs->points[0].principal_curvature[1]), 0.10713, 1e-4); EXPECT_NEAR (fabs (pcs->points[0].principal_curvature[2]), 0.13462, 1e-4); EXPECT_NEAR (fabs (pcs->points[0].pc1), 0.23997458815574646, 1e-4); EXPECT_NEAR (fabs (pcs->points[0].pc2), 0.19400238990783691, 1e-4); EXPECT_NEAR (pcs->points[2].principal_curvature[0], 0.98079, 1e-4); EXPECT_NEAR (pcs->points[2].principal_curvature[1], -0.04019, 1e-4); EXPECT_NEAR (pcs->points[2].principal_curvature[2], 0.19086, 1e-4); EXPECT_NEAR (pcs->points[2].pc1, 0.27207502722740173, 1e-4); EXPECT_NEAR (pcs->points[2].pc2, 0.1946497857570648, 1e-4); EXPECT_NEAR (pcs->points[indices.size () - 3].principal_curvature[0], 0.86725, 1e-4); EXPECT_NEAR (pcs->points[indices.size () - 3].principal_curvature[1], -0.37599, 1e-4); EXPECT_NEAR (pcs->points[indices.size () - 3].principal_curvature[2], 0.32636, 1e-4); EXPECT_NEAR (pcs->points[indices.size () - 3].pc1, 0.2590007483959198, 1e-4); EXPECT_NEAR (pcs->points[indices.size () - 3].pc2, 0.17906941473484039, 1e-4); EXPECT_NEAR (pcs->points[indices.size () - 1].principal_curvature[0], 0.86725, 1e-4); EXPECT_NEAR (pcs->points[indices.size () - 1].principal_curvature[1], -0.375851, 1e-3); EXPECT_NEAR (pcs->points[indices.size () - 1].principal_curvature[2], 0.32636, 1e-4); EXPECT_NEAR (pcs->points[indices.size () - 1].pc1, 0.25900065898895264, 1e-4); EXPECT_NEAR (pcs->points[indices.size () - 1].pc2, 0.17906941473484039, 1e-4); }
TEST (PCL, SpinImageEstimation) { // Estimate normals first double mr = 0.002; NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); // set parameters n.setInputCloud (cloud.makeShared ()); boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices)); n.setIndices (indicesptr); n.setSearchMethod (tree); n.setRadiusSearch (20 * mr); n.compute (*normals); EXPECT_NEAR (normals->points[103].normal_x, 0.36683175, 1e-4); EXPECT_NEAR (normals->points[103].normal_y, -0.44696972, 1e-4); EXPECT_NEAR (normals->points[103].normal_z, -0.81587529, 1e-4); EXPECT_NEAR (normals->points[200].normal_x, -0.71414840, 1e-4); EXPECT_NEAR (normals->points[200].normal_y, -0.06002361, 1e-4); EXPECT_NEAR (normals->points[200].normal_z, -0.69741613, 1e-4); EXPECT_NEAR (normals->points[140].normal_x, -0.45109111, 1e-4); EXPECT_NEAR (normals->points[140].normal_y, -0.19499126, 1e-4); EXPECT_NEAR (normals->points[140].normal_z, -0.87091631, 1e-4); typedef Histogram<153> SpinImage; SpinImageEstimation<PointXYZ, Normal, SpinImage> spin_est(8, 0.5, 16); // set parameters //spin_est.setInputWithNormals (cloud.makeShared (), normals); spin_est.setInputCloud (cloud.makeShared ()); spin_est.setInputNormals (normals); spin_est.setIndices (indicesptr); spin_est.setSearchMethod (tree); spin_est.setRadiusSearch (40*mr); // Object PointCloud<SpinImage>::Ptr spin_images (new PointCloud<SpinImage> ()); // radial SI spin_est.setRadialStructure(); // estimate spin_est.compute (*spin_images); EXPECT_EQ (spin_images->points.size (), indices.size ()); EXPECT_NEAR (spin_images->points[100].histogram[0], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[12], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[24], 0.00233226, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[36], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[48], 8.48662e-005, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[60], 0.0266387, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[72], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[84], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[96], 0.0414662, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[108], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[120], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[132], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[144], 0.0128513, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[0], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[12], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[24], 0.00932424, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[36], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[48], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[60], 0.0145733, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[72], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[84], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[96], 0.00034457, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[108], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[120], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[132], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[144], 0.0121195, 1e-4); // radial SI, angular spin-images spin_est.setAngularDomain (); // estimate spin_est.compute (*spin_images); EXPECT_EQ (spin_images->points.size (), indices.size ()); EXPECT_NEAR (spin_images->points[100].histogram[0], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[12], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[24], 0.132139, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[36], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[48], 0.908814, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[60], 0.63875, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[72], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[84], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[96], 0.550392, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[108], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[120], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[132], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[144], 0.257136, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[0], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[12], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[24], 0.230605, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[36], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[48], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[60], 0.764872, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[72], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[84], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[96], 1.02824, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[108], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[120], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[132], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[144], 0.293567, 1e-4); // rectangular SI spin_est.setRadialStructure (false); spin_est.setAngularDomain (false); // estimate spin_est.compute (*spin_images); EXPECT_EQ (spin_images->points.size (), indices.size ()); EXPECT_NEAR (spin_images->points[100].histogram[0], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[12], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[24], 0.000889345, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[36], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[48], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[60], 0.0489534, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[72], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[84], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[96], 0.0747141, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[108], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[120], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[132], 0.0173423, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[144], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[0], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[12], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[24], 0.0267132, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[36], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[48], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[60], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[72], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[84], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[96], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[108], 0.0209709, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[120], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[132], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[144], 0.029372, 1e-4); // rectangular SI, angular spin-images spin_est.setAngularDomain (); // estimate spin_est.compute (*spin_images); EXPECT_EQ (spin_images->points.size (), indices.size ()); EXPECT_NEAR (spin_images->points[100].histogram[0], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[12], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[24], 0.132139, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[36], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[48], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[60], 0.38800787925720215, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[72], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[84], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[96], 0.468881, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[108], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[120], 0, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[132], 0.67901438474655151, 1e-4); EXPECT_NEAR (spin_images->points[100].histogram[144], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[0], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[12], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[24], 0.143845, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[36], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[48], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[60], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[72], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[84], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[96], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[108], 0.706084, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[120], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[132], 0, 1e-4); EXPECT_NEAR (spin_images->points[300].histogram[144], 0.272542, 1e-4); }
TEST (PCL, SpinImageEstimationEigen) { // Estimate normals first double mr = 0.002; NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); // set parameters n.setInputCloud (cloud.makeShared ()); boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices)); n.setIndices (indicesptr); n.setSearchMethod (tree); n.setRadiusSearch (20 * mr); n.compute (*normals); EXPECT_NEAR (normals->points[103].normal_x, 0.36683175, 1e-4); EXPECT_NEAR (normals->points[103].normal_y, -0.44696972, 1e-4); EXPECT_NEAR (normals->points[103].normal_z, -0.81587529, 1e-4); EXPECT_NEAR (normals->points[200].normal_x, -0.71414840, 1e-4); EXPECT_NEAR (normals->points[200].normal_y, -0.06002361, 1e-4); EXPECT_NEAR (normals->points[200].normal_z, -0.69741613, 1e-4); EXPECT_NEAR (normals->points[140].normal_x, -0.45109111, 1e-4); EXPECT_NEAR (normals->points[140].normal_y, -0.19499126, 1e-4); EXPECT_NEAR (normals->points[140].normal_z, -0.87091631, 1e-4); SpinImageEstimation<PointXYZ, Normal, Eigen::MatrixXf> spin_est (8, 0.5, 16); // set parameters //spin_est.setInputWithNormals (cloud.makeShared (), normals); spin_est.setInputCloud (cloud.makeShared ()); spin_est.setInputNormals (normals); spin_est.setIndices (indicesptr); spin_est.setSearchMethod (tree); spin_est.setRadiusSearch (40*mr); // Object PointCloud<Eigen::MatrixXf>::Ptr spin_images (new PointCloud<Eigen::MatrixXf>); // radial SI spin_est.setRadialStructure (); // estimate spin_est.computeEigen (*spin_images); EXPECT_EQ (spin_images->points.rows (), indices.size ()); EXPECT_NEAR (spin_images->points (100, 0), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 12), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 24), 0.00233226, 1e-4); EXPECT_NEAR (spin_images->points (100, 36), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 48), 8.48662e-005, 1e-4); EXPECT_NEAR (spin_images->points (100, 60), 0.0266387, 1e-4); EXPECT_NEAR (spin_images->points (100, 72), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 84), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 96), 0.0414662, 1e-4); EXPECT_NEAR (spin_images->points (100, 108), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 120), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 132), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 144), 0.0128513, 1e-4); EXPECT_NEAR (spin_images->points (300, 0), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 12), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 24), 0.00932424, 1e-4); EXPECT_NEAR (spin_images->points (300, 36), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 48), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 60), 0.0145733, 1e-4); EXPECT_NEAR (spin_images->points (300, 72), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 84), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 96), 0.00034457, 1e-4); EXPECT_NEAR (spin_images->points (300, 108), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 120), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 132), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 144), 0.0121195, 1e-4); // radial SI, angular spin-images spin_est.setAngularDomain (); // estimate spin_est.computeEigen (*spin_images); EXPECT_EQ (spin_images->points.rows (), indices.size ()); EXPECT_NEAR (spin_images->points (100, 0), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 12), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 24), 0.13213, 1e-4); EXPECT_NEAR (spin_images->points (100, 36), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 48), 0.908804, 1.1e-4); EXPECT_NEAR (spin_images->points (100, 60), 0.63875, 1e-4); EXPECT_NEAR (spin_images->points (100, 72), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 84), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 96), 0.550392, 1e-4); EXPECT_NEAR (spin_images->points (100, 108), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 120), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 132), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 144), 0.25713, 1e-4); EXPECT_NEAR (spin_images->points (300, 0), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 12), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 24), 0.230605, 1e-4); EXPECT_NEAR (spin_images->points (300, 36), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 48), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 60), 0.764872, 1e-4); EXPECT_NEAR (spin_images->points (300, 72), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 84), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 96), 1.02824, 1e-4); EXPECT_NEAR (spin_images->points (300, 108), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 120), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 132), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 144), 0.293567, 1e-4); // rectangular SI spin_est.setRadialStructure (false); spin_est.setAngularDomain (false); // estimate spin_est.computeEigen (*spin_images); EXPECT_EQ (spin_images->points.rows (), indices.size ()); EXPECT_NEAR (spin_images->points (100, 0), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 12), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 24), 0.000889345, 1e-4); EXPECT_NEAR (spin_images->points (100, 36), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 48), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 60), 0.0489534, 1e-4); EXPECT_NEAR (spin_images->points (100, 72), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 84), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 96), 0.0747141, 1e-4); EXPECT_NEAR (spin_images->points (100, 108), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 120), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 132), 0.0173423, 1e-4); EXPECT_NEAR (spin_images->points (100, 144), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 0), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 12), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 24), 0.0267132, 1e-4); EXPECT_NEAR (spin_images->points (300, 36), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 48), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 60), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 72), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 84), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 96), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 108), 0.0209709, 1e-4); EXPECT_NEAR (spin_images->points (300, 120), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 132), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 144), 0.029372, 1e-4); // rectangular SI, angular spin-images spin_est.setAngularDomain (); // estimate spin_est.computeEigen (*spin_images); EXPECT_EQ (spin_images->points.rows (), indices.size ()); EXPECT_NEAR (spin_images->points (100, 0), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 12), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 24), 0.132126, 1e-4); EXPECT_NEAR (spin_images->points (100, 36), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 48), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 60), 0.388011, 1e-4); EXPECT_NEAR (spin_images->points (100, 72), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 84), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 96), 0.468881, 1e-4); EXPECT_NEAR (spin_images->points (100, 108), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 120), 0, 1e-4); EXPECT_NEAR (spin_images->points (100, 132), 0.678995, 1e-4); EXPECT_NEAR (spin_images->points (100, 144), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 0), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 12), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 24), 0.143845, 1e-4); EXPECT_NEAR (spin_images->points (300, 36), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 48), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 60), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 72), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 84), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 96), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 108), 0.706084, 1e-4); EXPECT_NEAR (spin_images->points (300, 120), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 132), 0, 1e-4); EXPECT_NEAR (spin_images->points (300, 144), 0.272542, 1e-4); }
void segment(PointCloud<PointXYZRGB> cloud) { CloudT object_cloud = get_object_points(cloud); objects_pub.publish(object_cloud); // Estimate point normals // PointCloud<Normal> cloud_normals; // KdTreeANN<PointXYZRGB>::Ptr tree = boost::make_shared<KdTreeANN<PointXYZRGB> >(); // NormalEstimation<PointXYZRGB, Normal> ne; // ne.setSearchMethod(tree); // ne.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(object_cloud)); // ne.setKSearch(50); // ne.compute(cloud_normals); // Extract clusters of points (i.e., objects) EuclideanClusterExtraction<PointXYZRGB> clustering; KdTreeANN<PointXYZRGB>::Ptr cluster_tree = boost::make_shared<KdTreeANN<PointXYZRGB> >(); vector<PointIndices> clusters; clustering.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(object_cloud)); clustering.setClusterTolerance(0.05); // ? clustering.setMinClusterSize(300); clustering.setSearchMethod(cluster_tree); // Not sure if this should be ANN, or FLANN, or if it matters clustering.extract(clusters); // cout << (cloud_normals.height * cloud_normals.width) << " " << (object_cloud.height * object_cloud.width) << endl; cout << "FOUND " << clusters.size() << " OBJECTS" << endl; for (uint i = 0; i < clusters.size(); i++) { cout << "CLUSTER " << i << " has " << clusters[i].indices.size() << " points" << endl; PointCloud<PointXYZRGB> cluster_points; ExtractIndices<PointXYZRGB> extract_cluster; extract_cluster.setInputCloud(boost::make_shared<PointCloud<PointXYZRGB> >(object_cloud)); extract_cluster.setIndices(boost::make_shared<PointIndices>(clusters[i])); extract_cluster.filter(cluster_points); cluster_pub.publish(cluster_points); // Estimate point normals PointCloud<Normal> cluster_normals; KdTreeANN<PointT>::Ptr tree = boost::make_shared<KdTreeANN<PointT> >(); // Estimate point normals NormalEstimation<PointT, Normal> ne; ne.setSearchMethod(tree); ne.setInputCloud(boost::make_shared<pcl::PointCloud<PointT> >(cluster_points)); ne.setKSearch(10); ne.compute(cluster_normals); // NormalEstimation<PointT, Normal> ne; // ne.setSearchMethod(tree); // ne.setInputCloud(boost::make_shared<CloudT>(cluster_points)); // ne.setKSearch(50); // ne.compute(cluster_normals); // cout << cluster_points.height * cluster_points.width << " " << cluster_normals.height * cluster_normals.width // << endl; // Obtain the plane inliers and coefficients ModelCoefficients coefficients_plane; PointIndices inliers_plane; SACSegmentationFromNormals<PointT, Normal> seg_plane; seg_plane.setOptimizeCoefficients(true); seg_plane.setMethodType(SAC_RANSAC); seg_plane.setModelType(SACMODEL_NORMAL_PLANE); seg_plane.setDistanceThreshold(0.05); seg_plane.setInputCloud(boost::make_shared<CloudT>(cluster_points)); seg_plane.setInputNormals(boost::make_shared<PointCloud<Normal> >(cluster_normals)); seg_plane.segment(inliers_plane, coefficients_plane); // cout << "Plane coefficients: " << coefficients_plane << endl; cout << "PLANE INLIERS: " << inliers_plane.indices.size() << endl; // Obtain the Sphere inliers and coefficients ModelCoefficients coefficients_sphere; PointIndices inliers_sphere; SACSegmentation<PointXYZRGB> seg_sphere; seg_sphere.setOptimizeCoefficients(true); seg_sphere.setMethodType(SAC_RANSAC); seg_sphere.setModelType(SACMODEL_SPHERE); seg_sphere.setRadiusLimits(0.01, 0.1); seg_sphere.setDistanceThreshold(0.005); seg_sphere.setInputCloud(boost::make_shared<CloudT>(cluster_points)); seg_sphere.segment(inliers_sphere, coefficients_sphere); // cout << "Sphere coefficients: " << coefficients_sphere << endl; cout << "SPHERE INLIERS: " << inliers_sphere.indices.size() << endl; ModelCoefficients coefficients_cylinder; PointIndices inliers_cylinder; SACSegmentationFromNormals<PointT, Normal> seg_cylinder; seg_cylinder.setOptimizeCoefficients(true); seg_cylinder.setModelType(SACMODEL_CYLINDER); seg_cylinder.setMethodType(SAC_RANSAC); seg_cylinder.setNormalDistanceWeight(0.1); seg_cylinder.setMaxIterations(1000); seg_cylinder.setDistanceThreshold(0.05); seg_cylinder.setRadiusLimits(0.01, 0.1); seg_cylinder.setInputCloud(boost::make_shared<CloudT>(cluster_points)); seg_cylinder.setInputNormals(boost::make_shared<PointCloud<Normal> >(cluster_normals)); seg_cylinder.segment(inliers_cylinder, coefficients_cylinder); cout << "CYLINDER INLIERS: " << inliers_cylinder.indices.size() << endl; cout << "Cylinder coefficients: " << coefficients_cylinder << endl; cout << endl; if (!inliers_sphere.indices.empty()) { visualization_msgs::Marker marker; marker.header.frame_id = coefficients_sphere.header.frame_id; marker.header.stamp = ros::Time::now(); marker.ns = "basic_shapes" + boost::lexical_cast<string>(i); marker.id = 0; marker.type = visualization_msgs::Marker::SPHERE; marker.action = visualization_msgs::Marker::ADD; marker.pose.position.x = coefficients_sphere.values[0]; marker.pose.position.y = coefficients_sphere.values[1]; marker.pose.position.z = coefficients_sphere.values[2]; marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; marker.pose.orientation.w = 1.0; marker.scale.x = coefficients_sphere.values[3] * 2; marker.scale.y = coefficients_sphere.values[3] * 2; marker.scale.z = coefficients_sphere.values[3] * 2; marker.color.r = 0.0f; marker.color.g = 1.0f; marker.color.b = 0.0f; marker.color.a = 1.0; marker.lifetime = ros::Duration(); marker_pub.publish(marker); } if (!inliers_cylinder.indices.empty()) { // TODO: The cylinder coefficients are not as straightforward, need to do some // math to pull out the location and angle (from axis orientation and point on axis) // visualization_msgs::Marker marker; // marker.header.frame_id = coefficients_sphere.header.frame_id; // marker.header.stamp = ros::Time::now(); // marker.ns = "basic_shapes" + boost::lexical_cast<string>(i); // marker.id = 0; // marker.type = visualization_msgs::Marker::SPHERE; // marker.action = visualization_msgs::Marker::ADD; // marker.pose.position.x = coefficients_sphere.values[0]; // marker.pose.position.y = coefficients_sphere.values[1]; // marker.pose.position.z = coefficients_sphere.values[2]; // marker.pose.orientation.x = 0.0; // marker.pose.orientation.y = 0.0; // marker.pose.orientation.z = 0.0; // marker.pose.orientation.w = 1.0; // marker.scale.x = coefficients_sphere.values[3] * 2; // marker.scale.y = coefficients_sphere.values[3] * 2; // marker.scale.z = coefficients_sphere.values[3] * 2; // marker.color.r = 0.0f; // marker.color.g = 1.0f; // marker.color.b = 0.0f; // marker.color.a = 1.0; // marker.lifetime = ros::Duration(); // marker_pub.publish(marker); } } }
TEST (PCL, PFHEstimation) { float f1, f2, f3, f4; // Estimate normals first NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); // set parameters n.setInputCloud (cloud.makeShared ()); boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices)); n.setIndices (indicesptr); n.setSearchMethod (tree); n.setKSearch (10); // Use 10 nearest neighbors to estimate the normals // estimate n.compute (*normals); PFHEstimation<PointXYZ, Normal, PFHSignature125> pfh; pfh.setInputNormals (normals); EXPECT_EQ (pfh.getInputNormals (), normals); // computePairFeatures pfh.computePairFeatures (cloud, *normals, 0, 12, f1, f2, f3, f4); EXPECT_NEAR (f1, -0.072575, 1e-4); EXPECT_NEAR (f2, -0.040221, 1e-4); EXPECT_NEAR (f3, 0.068133, 1e-4); EXPECT_NEAR (f4, 0.006130, 1e-4); // computePointPFHSignature int nr_subdiv = 3; Eigen::VectorXf pfh_histogram (nr_subdiv * nr_subdiv * nr_subdiv); pfh.computePointPFHSignature (cloud, *normals, indices, nr_subdiv, pfh_histogram); EXPECT_NEAR (pfh_histogram[0], 0.932506, 1e-2); EXPECT_NEAR (pfh_histogram[1], 2.32429 , 1e-2); EXPECT_NEAR (pfh_histogram[2], 0.357477, 1e-2); EXPECT_NEAR (pfh_histogram[3], 0.848541, 1e-2); EXPECT_NEAR (pfh_histogram[4], 3.65565 , 2e-2); // larger error w.r.t. considering all point pairs (feature bins=0,1,1 where 1 is middle, so angle of 0) EXPECT_NEAR (pfh_histogram[5], 0.178104, 1e-2); EXPECT_NEAR (pfh_histogram[6], 1.45284 , 1e-2); EXPECT_NEAR (pfh_histogram[7], 3.60666 , 1e-2); EXPECT_NEAR (pfh_histogram[8], 0.298959, 1e-2); EXPECT_NEAR (pfh_histogram[9], 0.295143, 1e-2); EXPECT_NEAR (pfh_histogram[10], 2.13474 , 1e-2); EXPECT_NEAR (pfh_histogram[11], 0.41218 , 1e-2); EXPECT_NEAR (pfh_histogram[12], 0.165382, 1e-2); EXPECT_NEAR (pfh_histogram[13], 8.97407 , 1e-2); EXPECT_NEAR (pfh_histogram[14], 0.306592, 1e-2); EXPECT_NEAR (pfh_histogram[15], 0.455432, 1e-2); EXPECT_NEAR (pfh_histogram[16], 4.5977 , 1e-2); EXPECT_NEAR (pfh_histogram[17], 0.393097, 1e-2); EXPECT_NEAR (pfh_histogram[18], 7.54668 , 1e-2); EXPECT_NEAR (pfh_histogram[19], 6.78336 , 1e-2); EXPECT_NEAR (pfh_histogram[20], 1.63858 , 1e-2); EXPECT_NEAR (pfh_histogram[21], 9.93842 , 1e-2); EXPECT_NEAR (pfh_histogram[22], 18.4947 , 2e-2); // larger error w.r.t. considering all point pairs (feature bins=2,1,1 where 1 is middle, so angle of 0) EXPECT_NEAR (pfh_histogram[23], 1.96553 , 1e-4); EXPECT_NEAR (pfh_histogram[24], 8.04793 , 1e-4); EXPECT_NEAR (pfh_histogram[25], 11.2793 , 1e-4); EXPECT_NEAR (pfh_histogram[26], 2.91714 , 1e-4); // Sum of values should be 100 EXPECT_NEAR (pfh_histogram.sum (), 100.0, 1e-2); //std::cerr << pfh_histogram << std::endl; // Object PointCloud<PFHSignature125>::Ptr pfhs (new PointCloud<PFHSignature125> ()); // set parameters pfh.setInputCloud (cloud.makeShared ()); pfh.setIndices (indicesptr); pfh.setSearchMethod (tree); pfh.setKSearch (static_cast<int> (indices.size ())); // estimate pfh.compute (*pfhs); EXPECT_EQ (pfhs->points.size (), indices.size ()); for (size_t i = 0; i < pfhs->points.size (); ++i) { EXPECT_NEAR (pfhs->points[i].histogram[0], 0.156477 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[1], 0.539396 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[2], 0.410907 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[3], 0.184465 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[4], 0.115767 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[5], 0.0572475 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[6], 0.206092 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[7], 0.339667 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[8], 0.265883 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[9], 0.0038165 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[10], 0.103046 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[11], 0.214997 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[12], 0.398186 , 3e-2); // larger error w.r.t. considering all point pairs (feature bins=0,2,2 where 2 is middle, so angle of 0) EXPECT_NEAR (pfhs->points[i].histogram[13], 0.298959 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[14], 0.00127217, 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[15], 0.11704 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[16], 0.255706 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[17], 0.356205 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[18], 0.265883 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[19], 0.00127217, 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[20], 0.148844 , 1e-4); //EXPECT_NEAR (pfhs->points[i].histogram[21], 0.721316 , 1e-3); //EXPECT_NEAR (pfhs->points[i].histogram[22], 0.438899 , 1e-2); EXPECT_NEAR (pfhs->points[i].histogram[23], 0.22263 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[24], 0.0216269 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[25], 0.223902 , 1e-4); EXPECT_NEAR (pfhs->points[i].histogram[26], 0.07633 , 1e-4); } //Eigen::Map<Eigen::VectorXf> h (&(pfhs->points[0].histogram[0]), 125); //std::cerr << h.head<27> () << std::endl; // Test results when setIndices and/or setSearchSurface are used boost::shared_ptr<vector<int> > test_indices (new vector<int> (0)); for (size_t i = 0; i < cloud.size (); i+=3) test_indices->push_back (static_cast<int> (i)); testIndicesAndSearchSurface<PFHEstimation<PointXYZ, Normal, PFHSignature125>, PointXYZ, Normal, PFHSignature125> (cloud.makeShared (), normals, test_indices, 125); }
TEST (PCL, FPFHEstimation) { // Estimate normals first NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); // set parameters n.setInputCloud (cloud.makeShared ()); boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices)); n.setIndices (indicesptr); n.setSearchMethod (tree); n.setKSearch (10); // Use 10 nearest neighbors to estimate the normals // estimate n.compute (*normals); FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fpfh; fpfh.setInputNormals (normals); EXPECT_EQ (fpfh.getInputNormals (), normals); // computePointSPFHSignature int nr_subdiv = 11; // use the same number of bins for all three angular features Eigen::MatrixXf hist_f1 (indices.size (), nr_subdiv), hist_f2 (indices.size (), nr_subdiv), hist_f3 (indices.size (), nr_subdiv); hist_f1.setZero (); hist_f2.setZero (); hist_f3.setZero (); for (int i = 0; i < static_cast<int> (indices.size ()); ++i) fpfh.computePointSPFHSignature (cloud, *normals, i, i, indices, hist_f1, hist_f2, hist_f3); EXPECT_NEAR (hist_f1 (0, 0), 0.757576, 1e-4); EXPECT_NEAR (hist_f1 (0, 1), 0.757576, 1e-4); EXPECT_NEAR (hist_f1 (0, 2), 4.54545, 1e-4); EXPECT_NEAR (hist_f1 (0, 3), 19.697, 1e-4); EXPECT_NEAR (hist_f1 (0, 4), 40.6566, 1e-4); EXPECT_NEAR (hist_f1 (0, 5), 21.4647, 1e-4); EXPECT_NEAR (hist_f1 (0, 6), 7.575759, 1e-4); EXPECT_NEAR (hist_f1 (0, 7), 0.000000, 1e-4); EXPECT_NEAR (hist_f1 (0, 8), 0.000000, 1e-4); EXPECT_NEAR (hist_f1 (0, 9), 0.50505, 1e-4); EXPECT_NEAR (hist_f1 (0, 10), 4.0404, 1e-4); EXPECT_NEAR (hist_f2 (0, 0), 0.757576, 1e-4); EXPECT_NEAR (hist_f2 (0, 1), 1.51515, 1e-4); EXPECT_NEAR (hist_f2 (0, 2), 6.31313, 1e-4); EXPECT_NEAR (hist_f2 (0, 3), 9.59596, 1e-4); EXPECT_NEAR (hist_f2 (0, 4), 20.7071, 1e-4); EXPECT_NEAR (hist_f2 (0, 5), 18.9394, 1e-4); EXPECT_NEAR (hist_f2 (0, 6), 15.9091, 1e-4); EXPECT_NEAR (hist_f2 (0, 7), 12.8788, 1e-4); EXPECT_NEAR (hist_f2 (0, 8), 6.56566, 1e-4); EXPECT_NEAR (hist_f2 (0, 9), 4.29293, 1e-4); EXPECT_NEAR (hist_f2 (0, 10), 2.52525, 1e-4); EXPECT_NEAR (hist_f3 (0, 0), 0.000000, 1e-4); EXPECT_NEAR (hist_f3 (0, 1), 5.05051, 1e-4); EXPECT_NEAR (hist_f3 (0, 2), 4.54545, 1e-4); EXPECT_NEAR (hist_f3 (0, 3), 5.05051, 1e-4); EXPECT_NEAR (hist_f3 (0, 4), 1.76768, 1e-4); EXPECT_NEAR (hist_f3 (0, 5), 3.0303, 1e-4); EXPECT_NEAR (hist_f3 (0, 6), 9.09091, 1e-4); EXPECT_NEAR (hist_f3 (0, 7), 31.8182, 1e-4); EXPECT_NEAR (hist_f3 (0, 8), 22.2222, 1e-4); EXPECT_NEAR (hist_f3 (0, 9), 11.8687, 1e-4); EXPECT_NEAR (hist_f3 (0, 10), 5.55556, 1e-4); // weightPointSPFHSignature Eigen::VectorXf fpfh_histogram (nr_subdiv + nr_subdiv + nr_subdiv); fpfh_histogram.setZero (); vector<float> dists (indices.size ()); for (size_t i = 0; i < dists.size (); ++i) dists[i] = static_cast<float> (i); fpfh.weightPointSPFHSignature (hist_f1, hist_f2, hist_f3, indices, dists, fpfh_histogram); EXPECT_NEAR (fpfh_histogram[0], 1.9798 , 1e-2); EXPECT_NEAR (fpfh_histogram[1], 2.86927, 1e-2); EXPECT_NEAR (fpfh_histogram[2], 8.47911, 1e-2); EXPECT_NEAR (fpfh_histogram[3], 22.8784, 1e-2); EXPECT_NEAR (fpfh_histogram[4], 29.8597, 1e-2); EXPECT_NEAR (fpfh_histogram[5], 19.6877, 1e-2); EXPECT_NEAR (fpfh_histogram[6], 7.38611, 1e-2); EXPECT_NEAR (fpfh_histogram[7], 1.44265, 1e-2); EXPECT_NEAR (fpfh_histogram[8], 0.69677, 1e-2); EXPECT_NEAR (fpfh_histogram[9], 1.72609, 1e-2); EXPECT_NEAR (fpfh_histogram[10], 2.99435, 1e-2); EXPECT_NEAR (fpfh_histogram[11], 2.26313, 1e-2); EXPECT_NEAR (fpfh_histogram[12], 5.16573, 1e-2); EXPECT_NEAR (fpfh_histogram[13], 8.3263 , 1e-2); EXPECT_NEAR (fpfh_histogram[14], 9.92427, 1e-2); EXPECT_NEAR (fpfh_histogram[15], 16.8062, 1e-2); EXPECT_NEAR (fpfh_histogram[16], 16.2767, 1e-2); EXPECT_NEAR (fpfh_histogram[17], 12.251 , 1e-2); //EXPECT_NEAR (fpfh_histogram[18], 10.354, 1e-1); //EXPECT_NEAR (fpfh_histogram[19], 6.65578, 1e-2); EXPECT_NEAR (fpfh_histogram[20], 6.1437 , 1e-2); EXPECT_NEAR (fpfh_histogram[21], 5.83341, 1e-2); EXPECT_NEAR (fpfh_histogram[22], 1.08809, 1e-2); EXPECT_NEAR (fpfh_histogram[23], 3.34133, 1e-2); EXPECT_NEAR (fpfh_histogram[24], 5.59236, 1e-2); EXPECT_NEAR (fpfh_histogram[25], 5.6355 , 1e-2); EXPECT_NEAR (fpfh_histogram[26], 3.03257, 1e-2); EXPECT_NEAR (fpfh_histogram[27], 1.37437, 1e-2); EXPECT_NEAR (fpfh_histogram[28], 7.99746, 1e-2); EXPECT_NEAR (fpfh_histogram[29], 18.0343, 1e-2); EXPECT_NEAR (fpfh_histogram[30], 23.691 , 1e-2); EXPECT_NEAR (fpfh_histogram[31], 19.8475, 1e-2); EXPECT_NEAR (fpfh_histogram[32], 10.3655, 1e-2); // Object PointCloud<FPFHSignature33>::Ptr fpfhs (new PointCloud<FPFHSignature33> ()); // set parameters fpfh.setInputCloud (cloud.makeShared ()); fpfh.setNrSubdivisions (11, 11, 11); fpfh.setIndices (indicesptr); fpfh.setSearchMethod (tree); fpfh.setKSearch (static_cast<int> (indices.size ())); // estimate fpfh.compute (*fpfhs); EXPECT_EQ (fpfhs->points.size (), indices.size ()); EXPECT_NEAR (fpfhs->points[0].histogram[0], 1.58591, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[1], 1.68365, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[2], 6.71 , 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[3], 23.0717, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[4], 33.3844, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[5], 20.4002, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[6], 7.31067, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[7], 1.02635, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[8], 0.48591, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[9], 1.47069, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[10], 2.87061, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[11], 1.78321, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[12], 4.30795, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[13], 7.05514, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[14], 9.37615, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[15], 17.963 , 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[16], 18.2801, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[17], 14.2766, 1e-2); //EXPECT_NEAR (fpfhs->points[0].histogram[18], 10.8542, 1e-2); //EXPECT_NEAR (fpfhs->points[0].histogram[19], 6.07925, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[20], 5.28565, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[21], 4.73887, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[22], 0.56984, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[23], 3.29826, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[24], 5.28156, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[25], 5.26939, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[26], 3.13191, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[27], 1.74453, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[28], 9.41971, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[29], 21.5894, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[30], 24.6302, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[31], 17.7764, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[32], 7.28878, 1e-2); // Test results when setIndices and/or setSearchSurface are used boost::shared_ptr<vector<int> > test_indices (new vector<int> (0)); for (size_t i = 0; i < cloud.size (); i+=3) test_indices->push_back (static_cast<int> (i)); testIndicesAndSearchSurface<FPFHEstimation<PointXYZ, Normal, FPFHSignature33>, PointXYZ, Normal, FPFHSignature33> (cloud.makeShared (), normals, test_indices, 33); }
TEST (PCL, NormalEstimation) { Eigen::Vector4f plane_parameters; float curvature; NormalEstimation<PointXYZ, Normal> n; // computePointNormal (indices, Vector) computePointNormal (cloud, indices, plane_parameters, curvature); EXPECT_NEAR (fabs (plane_parameters[0]), 0.035592, 1e-4); EXPECT_NEAR (fabs (plane_parameters[1]), 0.369596, 1e-4); EXPECT_NEAR (fabs (plane_parameters[2]), 0.928511, 1e-4); EXPECT_NEAR (fabs (plane_parameters[3]), 0.0622552, 1e-4); EXPECT_NEAR (curvature, 0.0693136, 1e-4); float nx, ny, nz; // computePointNormal (indices) n.computePointNormal (cloud, indices, nx, ny, nz, curvature); EXPECT_NEAR (fabs (nx), 0.035592, 1e-4); EXPECT_NEAR (fabs (ny), 0.369596, 1e-4); EXPECT_NEAR (fabs (nz), 0.928511, 1e-4); EXPECT_NEAR (curvature, 0.0693136, 1e-4); // computePointNormal (Vector) computePointNormal (cloud, plane_parameters, curvature); EXPECT_NEAR (plane_parameters[0], 0.035592, 1e-4); EXPECT_NEAR (plane_parameters[1], 0.369596, 1e-4); EXPECT_NEAR (plane_parameters[2], 0.928511, 1e-4); EXPECT_NEAR (plane_parameters[3], -0.0622552, 1e-4); EXPECT_NEAR (curvature, 0.0693136, 1e-4); // flipNormalTowardsViewpoint (Vector) flipNormalTowardsViewpoint (cloud.points[0], 0, 0, 0, plane_parameters); EXPECT_NEAR (plane_parameters[0], -0.035592, 1e-4); EXPECT_NEAR (plane_parameters[1], -0.369596, 1e-4); EXPECT_NEAR (plane_parameters[2], -0.928511, 1e-4); EXPECT_NEAR (plane_parameters[3], 0.0799743, 1e-4); // flipNormalTowardsViewpoint flipNormalTowardsViewpoint (cloud.points[0], 0, 0, 0, nx, ny, nz); EXPECT_NEAR (nx, -0.035592, 1e-4); EXPECT_NEAR (ny, -0.369596, 1e-4); EXPECT_NEAR (nz, -0.928511, 1e-4); // Object PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); // set parameters PointCloud<PointXYZ>::Ptr cloudptr = cloud.makeShared (); n.setInputCloud (cloudptr); EXPECT_EQ (n.getInputCloud (), cloudptr); boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices)); n.setIndices (indicesptr); EXPECT_EQ (n.getIndices (), indicesptr); n.setSearchMethod (tree); EXPECT_EQ (n.getSearchMethod (), tree); n.setKSearch (static_cast<int> (indices.size ())); // estimate n.compute (*normals); EXPECT_EQ (normals->points.size (), indices.size ()); for (size_t i = 0; i < normals->points.size (); ++i) { EXPECT_NEAR (normals->points[i].normal[0], -0.035592, 1e-4); EXPECT_NEAR (normals->points[i].normal[1], -0.369596, 1e-4); EXPECT_NEAR (normals->points[i].normal[2], -0.928511, 1e-4); EXPECT_NEAR (normals->points[i].curvature, 0.0693136, 1e-4); } PointCloud<PointXYZ>::Ptr surfaceptr = cloudptr; n.setSearchSurface (surfaceptr); EXPECT_EQ (n.getSearchSurface (), surfaceptr); // Additional test for searchForNeigbhors surfaceptr.reset (new PointCloud<PointXYZ>); *surfaceptr = *cloudptr; surfaceptr->points.resize (640 * 480); surfaceptr->width = 640; surfaceptr->height = 480; EXPECT_EQ (surfaceptr->points.size (), surfaceptr->width * surfaceptr->height); n.setSearchSurface (surfaceptr); tree.reset (); n.setSearchMethod (tree); // estimate n.compute (*normals); EXPECT_EQ (normals->points.size (), indices.size ()); }
/* ---[ */ int main (int argc, char** argv) { if (argc < 2) { std::cerr << "No test file given. Please download `bun0.pcd` and pass its path to the test." << std::endl; return (-1); } // Load file sensor_msgs::PointCloud2 cloud_blob; loadPCDFile (argv[1], cloud_blob); fromROSMsg (cloud_blob, *cloud); // Create search tree tree.reset (new search::KdTree<PointXYZ> (false)); tree->setInputCloud (cloud); // Normal estimation NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); n.setInputCloud (cloud); //n.setIndices (indices[B); 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 search::KdTree<PointNormal>); tree2->setInputCloud (cloud_with_normals); // Process for update cloud if(argc == 3){ sensor_msgs::PointCloud2 cloud_blob1; loadPCDFile (argv[2], cloud_blob1); fromROSMsg (cloud_blob1, *cloud1); // Create search tree tree3.reset (new search::KdTree<PointXYZ> (false)); tree3->setInputCloud (cloud1); // Normal estimation NormalEstimation<PointXYZ, Normal> n1; PointCloud<Normal>::Ptr normals1 (new PointCloud<Normal> ()); n1.setInputCloud (cloud1); n1.setSearchMethod (tree3); n1.setKSearch (20); n1.compute (*normals1); // Concatenate XYZ and normal information pcl::concatenateFields (*cloud1, *normals1, *cloud_with_normals1); // Create search tree tree4.reset (new search::KdTree<PointNormal>); tree4->setInputCloud (cloud_with_normals1); } // Testing testing::InitGoogleTest (&argc, argv); return (RUN_ALL_TESTS ()); }
TEST (PCL, FPFHEstimationOpenMP) { // Estimate normals first NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); // set parameters n.setInputCloud (cloud.makeShared ()); boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices)); n.setIndices (indicesptr); n.setSearchMethod (tree); n.setKSearch (10); // Use 10 nearest neighbors to estimate the normals // estimate n.compute (*normals); FPFHEstimationOMP<PointXYZ, Normal, FPFHSignature33> fpfh (4); // instantiate 4 threads fpfh.setInputNormals (normals); // Object PointCloud<FPFHSignature33>::Ptr fpfhs (new PointCloud<FPFHSignature33> ()); // set parameters fpfh.setInputCloud (cloud.makeShared ()); fpfh.setNrSubdivisions (11, 11, 11); fpfh.setIndices (indicesptr); fpfh.setSearchMethod (tree); fpfh.setKSearch (static_cast<int> (indices.size ())); // estimate fpfh.compute (*fpfhs); EXPECT_EQ (fpfhs->points.size (), indices.size ()); EXPECT_NEAR (fpfhs->points[0].histogram[0], 1.58591, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[1], 1.68365, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[2], 6.71 , 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[3], 23.073, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[4], 33.3828, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[5], 20.4002, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[6], 7.31067, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[7], 1.02635, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[8], 0.48591, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[9], 1.47069, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[10], 2.87061, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[11], 1.78321, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[12], 4.30795, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[13], 7.05514, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[14], 9.37615, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[15], 17.963 , 1e-3); //EXPECT_NEAR (fpfhs->points[0].histogram[16], 18.2801, 1e-3); //EXPECT_NEAR (fpfhs->points[0].histogram[17], 14.2766, 1e-3); //EXPECT_NEAR (fpfhs->points[0].histogram[18], 10.8542, 1e-3); //EXPECT_NEAR (fpfhs->points[0].histogram[19], 6.07925, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[20], 5.28991, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[21], 4.73438, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[22], 0.56984, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[23], 3.29826, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[24], 5.28156, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[25], 5.26939, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[26], 3.13191, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[27], 1.74453, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[28], 9.41971, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[29], 21.5894, 1e-2); EXPECT_NEAR (fpfhs->points[0].histogram[30], 24.6302, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[31], 17.7764, 1e-3); EXPECT_NEAR (fpfhs->points[0].histogram[32], 7.28878, 1e-3); // Test results when setIndices and/or setSearchSurface are used boost::shared_ptr<vector<int> > test_indices (new vector<int> (0)); for (size_t i = 0; i < cloud.size (); i+=3) test_indices->push_back (static_cast<int> (i)); testIndicesAndSearchSurface<FPFHEstimationOMP<PointXYZ, Normal, FPFHSignature33>, PointXYZ, Normal, FPFHSignature33> (cloud.makeShared (), normals, test_indices, 33); }
TEST (PCL, BoundaryEstimation) { Eigen::Vector4f u = Eigen::Vector4f::Zero (); Eigen::Vector4f v = Eigen::Vector4f::Zero (); // Estimate normals first NormalEstimation<PointXYZ, Normal> n; PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ()); // set parameters n.setInputCloud (cloud.makeShared ()); boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices)); n.setIndices (indicesptr); n.setSearchMethod (tree); n.setKSearch (static_cast<int> (indices.size ())); // estimate n.compute (*normals); BoundaryEstimation<PointXYZ, Normal, Boundary> b; b.setInputNormals (normals); EXPECT_EQ (b.getInputNormals (), normals); // getCoordinateSystemOnPlane for (size_t i = 0; i < normals->points.size (); ++i) { b.getCoordinateSystemOnPlane (normals->points[i], u, v); Vector4fMap n4uv = normals->points[i].getNormalVector4fMap (); EXPECT_NEAR (n4uv.dot(u), 0, 1e-4); EXPECT_NEAR (n4uv.dot(v), 0, 1e-4); EXPECT_NEAR (u.dot(v), 0, 1e-4); } // isBoundaryPoint (indices) bool pt = false; pt = b.isBoundaryPoint (cloud, 0, indices, u, v, float (M_PI) / 2.0); EXPECT_EQ (pt, false); pt = b.isBoundaryPoint (cloud, static_cast<int> (indices.size ()) / 3, indices, u, v, float (M_PI) / 2.0); EXPECT_EQ (pt, false); pt = b.isBoundaryPoint (cloud, static_cast<int> (indices.size ()) / 2, indices, u, v, float (M_PI) / 2.0); EXPECT_EQ (pt, false); pt = b.isBoundaryPoint (cloud, static_cast<int> (indices.size ()) - 1, indices, u, v, float (M_PI) / 2.0); EXPECT_EQ (pt, true); // isBoundaryPoint (points) pt = false; pt = b.isBoundaryPoint (cloud, cloud.points[0], indices, u, v, float (M_PI) / 2.0); EXPECT_EQ (pt, false); pt = b.isBoundaryPoint (cloud, cloud.points[indices.size () / 3], indices, u, v, float (M_PI) / 2.0); EXPECT_EQ (pt, false); pt = b.isBoundaryPoint (cloud, cloud.points[indices.size () / 2], indices, u, v, float (M_PI) / 2.0); EXPECT_EQ (pt, false); pt = b.isBoundaryPoint (cloud, cloud.points[indices.size () - 1], indices, u, v, float (M_PI) / 2.0); EXPECT_EQ (pt, true); // Object PointCloud<Boundary>::Ptr bps (new PointCloud<Boundary> ()); // set parameters b.setInputCloud (cloud.makeShared ()); b.setIndices (indicesptr); b.setSearchMethod (tree); b.setKSearch (static_cast<int> (indices.size ())); // estimate b.compute (*bps); EXPECT_EQ (bps->points.size (), indices.size ()); pt = bps->points[0].boundary_point; EXPECT_EQ (pt, false); pt = bps->points[indices.size () / 3].boundary_point; EXPECT_EQ (pt, false); pt = bps->points[indices.size () / 2].boundary_point; EXPECT_EQ (pt, false); pt = bps->points[indices.size () - 1].boundary_point; EXPECT_EQ (pt, true); }