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