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;
}
/* ---[ */
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 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);
}
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;
}
/* ---[ */
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 ());
}