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;
}
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);
// Estimate
TicToc tt;
tt.tic ();
print_highlight (stderr, "Computing ");
NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (xyz);
// ne.setSearchMethod (pcl::search::KdTree<pcl::PointXYZ>::Ptr (new pcl::search::KdTree<pcl::PointXYZ>));
ne.setKSearch (k);
ne.setRadiusSearch (radius);
PointCloud<Normal> normals;
ne.compute (normals);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); 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, 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);
}
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);
}
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);
}
}
/* ---[ */
int
main (int argc, char** argv)
{
// Load two standard PCD files from disk
if (argc < 3)
{
std::cerr << "No test files given. Please download `sac_plane_test.pcd` and 'cturtle.pcd' and pass them path to the test." << std::endl;
return (-1);
}
// Load in the point clouds
io::loadPCDFile (argv[1], *cloud_walls);
io::loadPCDFile (argv[2], *cloud_turtle);
// Compute the normals for each cloud, and then clean them up of any NaN values
NormalEstimation<PointXYZ,PointNormal> ne;
ne.setInputCloud (cloud_walls);
ne.setRadiusSearch (0.05);
ne.compute (*cloud_walls_normals);
copyPointCloud (*cloud_walls, *cloud_walls_normals);
std::vector<int> aux_indices;
removeNaNFromPointCloud (*cloud_walls_normals, *cloud_walls_normals, aux_indices);
removeNaNNormalsFromPointCloud (*cloud_walls_normals, *cloud_walls_normals, aux_indices);
ne = NormalEstimation<PointXYZ, PointNormal> ();
ne.setInputCloud (cloud_turtle);
ne.setKSearch (5);
ne.compute (*cloud_turtle_normals);
copyPointCloud (*cloud_turtle, *cloud_turtle_normals);
removeNaNFromPointCloud (*cloud_turtle_normals, *cloud_turtle_normals, aux_indices);
removeNaNNormalsFromPointCloud (*cloud_turtle_normals, *cloud_turtle_normals, aux_indices);
testing::InitGoogleTest (&argc, argv);
return (RUN_ALL_TESTS ());
}
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);
}
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);
}
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, 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, 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, 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);
}