/* ---[ */ 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); } if (loadPCDFile<PointXYZ> (argv[1], cloud) < 0) { std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl; return (-1); } indices.resize (cloud.points.size ()); for (int i = 0; i < static_cast<int> (indices.size ()); ++i) indices[i] = i; tree.reset (new search::KdTree<PointXYZ> (false)); tree->setInputCloud (cloud.makeShared ()); testing::InitGoogleTest (&argc, argv); return (RUN_ALL_TESTS ()); }
pcl::IndicesPtr normalFiltering( const typename pcl::PointCloud<PointT>::Ptr & cloud, const pcl::IndicesPtr & indices, float angleMax, const Eigen::Vector4f & normal, float radiusSearch, const Eigen::Vector4f & viewpoint) { typedef typename pcl::search::KdTree<PointT> KdTree; typedef typename KdTree::Ptr KdTreePtr; pcl::NormalEstimation<PointT, pcl::Normal> ne; ne.setInputCloud (cloud); if(indices->size()) { ne.setIndices(indices); } KdTreePtr tree (new KdTree(false)); if(indices->size()) { tree->setInputCloud(cloud, indices); } else { tree->setInputCloud(cloud); } ne.setSearchMethod (tree); pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); ne.setRadiusSearch (radiusSearch); if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0) { ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]); } ne.compute (*cloud_normals); pcl::IndicesPtr output(new std::vector<int>(cloud_normals->size())); int oi = 0; // output iterator Eigen::Vector3f n(normal[0], normal[1], normal[2]); for(unsigned int i=0; i<cloud_normals->size(); ++i) { Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f); float angle = pcl::getAngle3D(normal, v); if(angle < angleMax) { output->at(oi++) = indices->size()!=0?indices->at(i):i; } } output->resize(oi); return output; }
pcl::IndicesPtr radiusFiltering( const typename pcl::PointCloud<PointT>::Ptr & cloud, const pcl::IndicesPtr & indices, float radiusSearch, int minNeighborsInRadius) { typedef typename pcl::search::KdTree<PointT> KdTree; typedef typename KdTree::Ptr KdTreePtr; KdTreePtr tree (new KdTree(false)); if(indices->size()) { pcl::IndicesPtr output(new std::vector<int>(indices->size())); int oi = 0; // output iterator tree->setInputCloud(cloud, indices); for(unsigned int i=0; i<indices->size(); ++i) { std::vector<int> kIndices; std::vector<float> kDistances; int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances); if(k > minNeighborsInRadius) { output->at(oi++) = indices->at(i); } } output->resize(oi); return output; } else { pcl::IndicesPtr output(new std::vector<int>(cloud->size())); int oi = 0; // output iterator tree->setInputCloud(cloud); for(unsigned int i=0; i<cloud->size(); ++i) { std::vector<int> kIndices; std::vector<float> kDistances; int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances); if(k > minNeighborsInRadius) { output->at(oi++) = i; } } output->resize(oi); return output; } }
/* ---[ */ int main (int argc, char** argv) { if (argc < 3) { std::cerr << "No test file given. Please download `bun0.pcd` and `milk.pcd` pass its path to the test." << std::endl; return (-1); } if (loadPCDFile<PointXYZ> (argv[1], cloud) < 0) { std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl; return (-1); } CloudPtr milk_loaded(new PointCloud<PointXYZ>()); if (loadPCDFile<PointXYZ> (argv[2], *milk_loaded) < 0) { std::cerr << "Failed to read test file. Please download `milk.pcd` and pass its path to the test." << std::endl; return (-1); } indices.resize (cloud.points.size ()); for (size_t i = 0; i < indices.size (); ++i) { indices[i] = static_cast<int>(i); } tree.reset (new search::KdTree<PointXYZ> (false)); tree->setInputCloud (cloud.makeShared ()); cloud_milk.reset(new PointCloud<PointXYZ>()); CloudPtr grid; pcl::VoxelGrid < pcl::PointXYZ > grid_; grid_.setInputCloud (milk_loaded); grid_.setLeafSize (leaf_size_, leaf_size_, leaf_size_); grid_.filter (*cloud_milk); tree_milk.reset (new search::KdTree<PointXYZ> (false)); tree_milk->setInputCloud (cloud_milk); testing::InitGoogleTest (&argc, argv); return (RUN_ALL_TESTS ()); }
static void computeNormals(PointCloudPtr pcl_cloud_input, pcl::PointCloud<Normal>::Ptr pcl_cloud_normals, const double search_radius) { PointCloud mls_points; typedef pcl::search::KdTree<PointT> KdTree; typedef typename KdTree::Ptr KdTreePtr; // Create a KD-Tree KdTreePtr tree = boost::make_shared<pcl::search::KdTree<PointT> >(); MovingLeastSquares<PointT, Normal> mls; tree->setInputCloud(pcl_cloud_input); mls.setOutputNormals(pcl_cloud_normals); mls.setInputCloud(pcl_cloud_input); //mls.setPolynomialFit(true); mls.setSearchMethod(tree); mls.setSearchRadius(search_radius); mls.reconstruct(mls_points); }
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); } }
template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output) { // Check if normals have to be computed/saved if (compute_normals_) { normals_.reset (new NormalCloud); // Copy the header normals_->header = input_->header; // Clear the fields in case the method exits before computation normals_->width = normals_->height = 0; normals_->points.clear (); } // Copy the header output.header = input_->header; output.width = output.height = 0; output.points.clear (); if (search_radius_ <= 0 || sqr_gauss_param_ <= 0) { PCL_ERROR ("[pcl::%s::reconstruct] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_); return; } if (!initCompute ()) return; // Initialize the spatial locator if (!tree_) { KdTreePtr tree; if (input_->isOrganized ()) tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ()); else tree.reset (new pcl::search::KdTree<PointInT> (false)); setSearchMethod (tree); } // Send the surface dataset to the spatial locator tree_->setInputCloud (input_, indices_); // Initialize random number generator if necessary switch (upsample_method_) { case (RANDOM_UNIFORM_DENSITY): { boost::mt19937 *rng = new boost::mt19937 (static_cast<unsigned int>(std::time(0))); float tmp = static_cast<float> (search_radius_ / 2.0f); boost::uniform_real<float> *uniform_distrib = new boost::uniform_real<float> (-tmp, tmp); rng_uniform_distribution_ = new boost::variate_generator<boost::mt19937, boost::uniform_real<float> > (*rng, *uniform_distrib); break; } case (VOXEL_GRID_DILATION): { mls_results_.resize (input_->size ()); break; } default: break; } // Perform the actual surface reconstruction performProcessing (output); if (compute_normals_) { normals_->height = 1; normals_->width = static_cast<uint32_t> (normals_->size ()); // TODO!!! MODIFY TO PER-CLOUD COPYING - much faster than per-point for (unsigned int i = 0; i < output.size (); ++i) { typedef typename pcl::traits::fieldList<PointOutT>::type FieldList; pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_x", normals_->points[i].normal_x)); pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_y", normals_->points[i].normal_y)); pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_z", normals_->points[i].normal_z)); pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "curvature", normals_->points[i].curvature)); } } // Set proper widths and heights for the clouds output.height = 1; output.width = static_cast<uint32_t> (output.size ()); deinitCompute (); }
bool fpfh_cb(feature_extractor_fpfh::FPFHCalc::Request &req, feature_extractor_fpfh::FPFHCalc::Response &res) { float leaf_size = .01; pcl::VoxelGrid<sensor_msgs::PointCloud2> sor; sensor_msgs::PointCloud2::Ptr input_cloud(new sensor_msgs::PointCloud2()); sensor_msgs::convertPointCloudToPointCloud2(req.input, *input_cloud); //sensor_msgs::PointCloud2::Ptr input_cloud(&req.input); sensor_msgs::PointCloud2::Ptr cloud_filtered(new sensor_msgs::PointCloud2()); sor.setInputCloud(input_cloud); sor.setLeafSize (leaf_size, leaf_size, leaf_size); sor.filter(*cloud_filtered); ROS_INFO("after filtering: %d points", cloud_filtered->width * cloud_filtered->height); PointCloud<PointXYZ> cloud; fromROSMsg(*cloud_filtered, cloud); std::vector<int> indices; indices.resize (cloud.points.size()); for (size_t i = 0; i < indices.size (); ++i) { indices[i] = i; } // make tree KdTreePtr tree; tree.reset(new KdTreeFLANN<PointXYZ> (false)); tree->setInputCloud(cloud.makeShared()); PointCloud<Normal>::Ptr normals(new PointCloud<Normal>()); boost::shared_ptr< vector<int> > indicesptr(new vector<int> (indices)); NormalEstimation<PointXYZ, Normal> normal_estimator; // set normal estimation parameters normal_estimator.setIndices(indicesptr); normal_estimator.setSearchMethod(tree); normal_estimator.setKSearch(10); // Use 10 nearest neighbors to estimate the normals // estimate ROS_INFO("Calculating normals...\n"); normal_estimator.setInputCloud(cloud.makeShared()); normal_estimator.compute(*normals); // calculate FPFH //FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fpfh; FPFHEstimationOMP<PointXYZ, Normal, FPFHSignature33> fpfh(4); // instantiate 4 threads // object PointCloud<FPFHSignature33>::Ptr fphists (new PointCloud<FPFHSignature33>()); // set parameters int d1, d2, d3; d1 = d2 = d3 = 11; fpfh.setNrSubdivisions(d1, d2, d3); fpfh.setIndices(indicesptr); fpfh.setSearchMethod(tree); fpfh.setKSearch(50); // estimate ROS_INFO("Calculating fpfh...\n"); fpfh.setInputNormals(normals); fpfh.setInputCloud(cloud.makeShared()); fpfh.compute(*fphists); res.hist.dim[0] = d1; res.hist.dim[1] = d2; res.hist.dim[2] = d3; unsigned int total_size = d1+d2+d3; res.hist.histograms.resize(total_size * cloud.points.size()); res.hist.points3d.resize(3*cloud.points.size()); ROS_INFO("copying into message...\n"); for (unsigned int i = 0; i < cloud.points.size(); i++) { for (unsigned int j = 0; j < total_size; j++) { res.hist.histograms[i*total_size + j] = fphists->points[i].histogram[j]; //if (i == 0) //{ // printf(">> %.2f \n", fphists->points[i].histogram[j]); //} //if (i == 4) //{ // printf("X %.2f \n", fphists->points[i].histogram[j]); //} } res.hist.points3d[3*i] = cloud.points[i].x; res.hist.points3d[3*i+1] = cloud.points[i].y; res.hist.points3d[3*i+2] = cloud.points[i].z; //if (i == 0) // printf(">> 0 %.4f %.4f %.4f \n", cloud.points[i].x, cloud.points[i].y, cloud.points[i].z); //if (i == 4) // printf(">> 4 %.4f %.4f %.4f \n", cloud.points[i].x, cloud.points[i].y, cloud.points[i].z); } res.hist.npoints = cloud.points.size(); ROS_INFO("done.\n"); //printf("%d\n", ); // sensor_msgs::PointCloud2 req // new feature_extractor::FPFHist() return true; }
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 < 4) { std::cerr << "No test file given. Please download `bun0.pcd` `bun03.pcd` `milk.pcd` and pass its path to the test." << std::endl; return (-1); } // // Load first cloud and prepare objets to test // if (loadPCDFile<PointXYZ> (argv[1], cloud) < 0) { std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl; return (-1); } cloud_for_lrf = cloud; indices.resize (cloud.size (), 0); indices_for_lrf.resize (cloud.size (), 0); for (size_t i = 0; i < indices.size (); ++i) { indices[i] = static_cast<int> (i); indices_for_lrf[i] = static_cast<int> (i); } tree.reset (new search::KdTree<PointXYZ> ()); tree->setInputCloud (cloud.makeShared ()); tree->setSortedResults (true); Eigen::Vector4f centroid; pcl::compute3DCentroid<PointXYZ, float> (cloud_for_lrf, centroid); Eigen::Vector4f max_pt; pcl::getMaxDistance<PointXYZ> (cloud_for_lrf, centroid, max_pt); radius_local_shot = (max_pt - centroid).norm(); PointXYZ p_centroid; p_centroid.getVector4fMap () = centroid; cloud_for_lrf.push_back (p_centroid); PointNormal p_centroid_nr; p_centroid_nr.getVector4fMap() = centroid; ground_truth.push_back(p_centroid_nr); cloud_for_lrf.height = 1; cloud_for_lrf.width = cloud_for_lrf.size (); indices_for_lrf.push_back (cloud_for_lrf.width - 1); indices_local_shot.resize (1); indices_local_shot[0] = cloud_for_lrf.width - 1; // // Load second cloud and prepare objets to test // if (loadPCDFile<PointXYZ> (argv[2], cloud2) < 0) { std::cerr << "Failed to read test file. Please download `bun03.pcd` and pass its path to the test." << std::endl; return (-1); } indices2.resize (cloud2.size (), 0); for (size_t i = 0; i < indices2.size (); ++i) indices2[i] = static_cast<int> (i); tree2.reset (new search::KdTree<PointXYZ> ()); tree2->setInputCloud (cloud2.makeShared ()); tree2->setSortedResults (true); // // Load third cloud and prepare objets to test // if (loadPCDFile<PointXYZ> (argv[3], cloud3) < 0) { std::cerr << "Failed to read test file. Please download `milk.pcd` and pass its path to the test." << std::endl; return (-1); } indices3.resize (cloud3.size (), 0); for (size_t i = 0; i < indices3.size (); ++i) indices3[i] = static_cast<int> (i); tree3.reset (new search::KdTree<PointXYZ> ()); tree3->setInputCloud (cloud3.makeShared ()); tree3->setSortedResults (true); testing::InitGoogleTest (&argc, argv); return (RUN_ALL_TESTS ()); }
template <typename PointInT, typename PointOutT> void pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output) { // Reset or initialize the collection of indices corresponding_input_indices_.reset (new PointIndices); // Check if normals have to be computed/saved if (compute_normals_) { normals_.reset (new NormalCloud); // Copy the header normals_->header = input_->header; // Clear the fields in case the method exits before computation normals_->width = normals_->height = 0; normals_->points.clear (); } // Copy the header output.header = input_->header; output.width = output.height = 0; output.points.clear (); if (search_radius_ <= 0 || sqr_gauss_param_ <= 0) { PCL_ERROR ("[pcl::%s::process] Invalid search radius (%f) or Gaussian parameter (%f)!\n", getClassName ().c_str (), search_radius_, sqr_gauss_param_); return; } // Check if distinct_cloud_ was set if (upsample_method_ == DISTINCT_CLOUD && !distinct_cloud_) { PCL_ERROR ("[pcl::%s::process] Upsample method was set to DISTINCT_CLOUD, but no distinct cloud was specified.\n", getClassName ().c_str ()); return; } if (!initCompute ()) return; // Initialize the spatial locator if (!tree_) { KdTreePtr tree; if (input_->isOrganized ()) tree.reset (new pcl::search::OrganizedNeighbor<PointInT> ()); else tree.reset (new pcl::search::KdTree<PointInT> (false)); setSearchMethod (tree); } // Send the surface dataset to the spatial locator tree_->setInputCloud (input_); switch (upsample_method_) { // Initialize random number generator if necessary case (RANDOM_UNIFORM_DENSITY): { rng_alg_.seed (static_cast<unsigned> (std::time (0))); float tmp = static_cast<float> (search_radius_ / 2.0f); boost::uniform_real<float> uniform_distrib (-tmp, tmp); rng_uniform_distribution_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > (rng_alg_, uniform_distrib)); break; } case (VOXEL_GRID_DILATION): case (DISTINCT_CLOUD): { if (!cache_mls_results_) PCL_WARN ("The cache mls results is forced when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.\n"); cache_mls_results_ = true; break; } default: break; } if (cache_mls_results_) { mls_results_.resize (input_->size ()); } else { mls_results_.resize (1); // Need to have a reference to a single dummy result. } // Perform the actual surface reconstruction performProcessing (output); if (compute_normals_) { normals_->height = 1; normals_->width = static_cast<uint32_t> (normals_->size ()); for (unsigned int i = 0; i < output.size (); ++i) { typedef typename pcl::traits::fieldList<PointOutT>::type FieldList; pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_x", normals_->points[i].normal_x)); pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_y", normals_->points[i].normal_y)); pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "normal_z", normals_->points[i].normal_z)); pcl::for_each_type<FieldList> (SetIfFieldExists<PointOutT, float> (output.points[i], "curvature", normals_->points[i].curvature)); } } // Set proper widths and heights for the clouds output.height = 1; output.width = static_cast<uint32_t> (output.size ()); deinitCompute (); }