TEST (PCL, SHOTGlobalReferenceFrame)
{
// Estimate normals first
double mr = 0.002;
NormalEstimation<PointXYZ, Normal> n;
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
// set parameters
n.setInputCloud (cloud.makeShared ());
boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
n.setIndices (indicesptr);
n.setSearchMethod (tree);
n.setRadiusSearch (20 * mr);
n.compute (*normals);
EXPECT_NEAR (normals->points[103].normal_x, 0.36683175, 1e-4);
EXPECT_NEAR (normals->points[103].normal_y, -0.44696972, 1e-4);
EXPECT_NEAR (normals->points[103].normal_z, -0.81587529, 1e-4);
EXPECT_NEAR (normals->points[200].normal_x, -0.71414840, 1e-4);
EXPECT_NEAR (normals->points[200].normal_y, -0.06002361, 1e-4);
EXPECT_NEAR (normals->points[200].normal_z, -0.69741613, 1e-4);
EXPECT_NEAR (normals->points[140].normal_x, -0.45109111, 1e-4);
EXPECT_NEAR (normals->points[140].normal_y, -0.19499126, 1e-4);
EXPECT_NEAR (normals->points[140].normal_z, -0.87091631, 1e-4);
boost::shared_ptr<vector<int> > test_indices (new vector<int> (0));
for (size_t i = 0; i < cloud.size (); i+=3)
test_indices->push_back (static_cast<int> (i));
testGSHOTGlobalReferenceFrame<GSHOTEstimation<PointXYZ, Normal, SHOT352>, PointXYZ, Normal, SHOT352> (cloud.makeShared (), normals, test_indices);
}
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);
}
/* ---[ */
int
main (int argc, char** argv)
{
if (argc < 2)
{
std::cerr << "No test file given. Please download `sac_plane_test.pcd` and pass its path to the test." << std::endl;
return (-1);
}
// Load a standard PCD file from disk
sensor_msgs::PointCloud2 cloud_blob;
if (loadPCDFile (argv[1], cloud_blob) < 0)
{
std::cerr << "Failed to read test file. Please download `sac_plane_test.pcd` and pass its path to the test." << std::endl;
return (-1);
}
fromROSMsg (cloud_blob, *cloud_);
indices_.resize (cloud_->points.size ());
for (size_t i = 0; i < indices_.size (); ++i) { indices_[i] = int (i); }
// Estimate surface normals
NormalEstimation<PointXYZ, Normal> n;
search::Search<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ>);
tree->setInputCloud (cloud_);
n.setInputCloud (cloud_);
boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices_));
n.setIndices (indicesptr);
n.setSearchMethod (tree);
n.setRadiusSearch (0.02); // Use 2cm radius to estimate the normals
n.compute (*normals_);
testing::InitGoogleTest (&argc, argv);
return (RUN_ALL_TESTS ());
}
TEST (PCL, VFHEstimation)
{
// Estimate normals first
NormalEstimation<PointXYZ, Normal> n;
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
// set parameters
n.setInputCloud (cloud.makeShared ());
boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
n.setIndices (indicesptr);
n.setSearchMethod (tree);
n.setKSearch (10); // Use 10 nearest neighbors to estimate the normals
// estimate
n.compute (*normals);
VFHEstimation<PointXYZ, Normal, VFHSignature308> vfh;
vfh.setInputNormals (normals);
// PointCloud<PointNormal> cloud_normals;
// concatenateFields (cloud, normals, cloud_normals);
// savePCDFile ("bun0_n.pcd", cloud_normals);
// Object
PointCloud<VFHSignature308>::Ptr vfhs (new PointCloud<VFHSignature308> ());
// set parameters
vfh.setInputCloud (cloud.makeShared ());
vfh.setIndices (indicesptr);
vfh.setSearchMethod (tree);
// estimate
vfh.compute (*vfhs);
EXPECT_EQ (int (vfhs->points.size ()), 1);
//for (size_t d = 0; d < 308; ++d)
// std::cerr << vfhs.points[0].histogram[d] << std::endl;
}
TEST (PCL, GSHOTRadius)
{
float radius = radius_local_shot / 4.0f;
// Estimate normals first
double mr = 0.002;
NormalEstimation<PointXYZ, Normal> n;
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
// set parameters
n.setInputCloud (cloud.makeShared ());
boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
n.setIndices (indicesptr);
n.setSearchMethod (tree);
n.setRadiusSearch (20 * mr);
n.compute (*normals);
// Objects
PointCloud<SHOT352>::Ptr gshots352 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr shots352 (new PointCloud<SHOT352> ());
// SHOT352 (local)
SHOTEstimation<PointXYZ, Normal, SHOT352> shot352;
shot352.setInputNormals (normals);
shot352.setRadiusSearch (radius);
shot352.setInputCloud (cloud_for_lrf.makeShared ());
boost::shared_ptr<vector<int> > indices_local_shot_ptr (new vector<int> (indices_local_shot));
shot352.setIndices (indices_local_shot_ptr);
shot352.setSearchSurface (cloud.makeShared());
shot352.compute (*shots352);
// SHOT352 (global)
GSHOTEstimation<PointXYZ, Normal, SHOT352> gshot352;
gshot352.setInputNormals (normals);
// set parameters
gshot352.setInputCloud (cloud.makeShared ());
gshot352.setIndices (indicesptr);
gshot352.setSearchMethod (tree);
gshot352.setRadiusSearch (radius);
EXPECT_EQ (gshot352.getRadiusSearch (), shot352.getRadiusSearch ());
// estimate
gshot352.compute (*gshots352);
checkDescNear (*gshots352, *shots352, 1E-7);
}
TEST (PCL, PrincipalCurvaturesEstimation)
{
float pcx, pcy, pcz, pc1, pc2;
// Estimate normals first
NormalEstimation<PointXYZ, Normal> n;
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
// set parameters
n.setInputCloud (cloud.makeShared ());
boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
n.setIndices (indicesptr);
n.setSearchMethod (tree);
n.setKSearch (10); // Use 10 nearest neighbors to estimate the normals
// estimate
n.compute (*normals);
PrincipalCurvaturesEstimation<PointXYZ, Normal, PrincipalCurvatures> pc;
pc.setInputNormals (normals);
EXPECT_EQ (pc.getInputNormals (), normals);
// computePointPrincipalCurvatures (indices)
pc.computePointPrincipalCurvatures (*normals, 0, indices, pcx, pcy, pcz, pc1, pc2);
EXPECT_NEAR (fabs (pcx), 0.98509, 1e-4);
EXPECT_NEAR (fabs (pcy), 0.10714, 1e-4);
EXPECT_NEAR (fabs (pcz), 0.13462, 1e-4);
EXPECT_NEAR (pc1, 0.23997423052787781, 1e-4);
EXPECT_NEAR (pc2, 0.19400238990783691, 1e-4);
pc.computePointPrincipalCurvatures (*normals, 2, indices, pcx, pcy, pcz, pc1, pc2);
EXPECT_NEAR (pcx, 0.98079, 1e-4);
EXPECT_NEAR (pcy, -0.04019, 1e-4);
EXPECT_NEAR (pcz, 0.19086, 1e-4);
EXPECT_NEAR (pc1, 0.27207490801811218, 1e-4);
EXPECT_NEAR (pc2, 0.19464978575706482, 1e-4);
int indices_size = static_cast<int> (indices.size ());
pc.computePointPrincipalCurvatures (*normals, indices_size - 3, indices, pcx, pcy, pcz, pc1, pc2);
EXPECT_NEAR (pcx, 0.86725, 1e-4);
EXPECT_NEAR (pcy, -0.37599, 1e-4);
EXPECT_NEAR (pcz, 0.32635, 1e-4);
EXPECT_NEAR (pc1, 0.25900053977966309, 1e-4);
EXPECT_NEAR (pc2, 0.17906945943832397, 1e-4);
pc.computePointPrincipalCurvatures (*normals, indices_size - 1, indices, pcx, pcy, pcz, pc1, pc2);
EXPECT_NEAR (pcx, 0.86725, 1e-4);
EXPECT_NEAR (pcy, -0.375851, 1e-3);
EXPECT_NEAR (pcz, 0.32636, 1e-4);
EXPECT_NEAR (pc1, 0.2590005099773407, 1e-4);
EXPECT_NEAR (pc2, 0.17906956374645233, 1e-4);
// Object
PointCloud<PrincipalCurvatures>::Ptr pcs (new PointCloud<PrincipalCurvatures> ());
// set parameters
pc.setInputCloud (cloud.makeShared ());
pc.setIndices (indicesptr);
pc.setSearchMethod (tree);
pc.setKSearch (indices_size);
// estimate
pc.compute (*pcs);
EXPECT_EQ (pcs->points.size (), indices.size ());
// Adjust for small numerical inconsitencies (due to nn_indices not being sorted)
EXPECT_NEAR (fabs (pcs->points[0].principal_curvature[0]), 0.98509, 1e-4);
EXPECT_NEAR (fabs (pcs->points[0].principal_curvature[1]), 0.10713, 1e-4);
EXPECT_NEAR (fabs (pcs->points[0].principal_curvature[2]), 0.13462, 1e-4);
EXPECT_NEAR (fabs (pcs->points[0].pc1), 0.23997458815574646, 1e-4);
EXPECT_NEAR (fabs (pcs->points[0].pc2), 0.19400238990783691, 1e-4);
EXPECT_NEAR (pcs->points[2].principal_curvature[0], 0.98079, 1e-4);
EXPECT_NEAR (pcs->points[2].principal_curvature[1], -0.04019, 1e-4);
EXPECT_NEAR (pcs->points[2].principal_curvature[2], 0.19086, 1e-4);
EXPECT_NEAR (pcs->points[2].pc1, 0.27207502722740173, 1e-4);
EXPECT_NEAR (pcs->points[2].pc2, 0.1946497857570648, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 3].principal_curvature[0], 0.86725, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 3].principal_curvature[1], -0.37599, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 3].principal_curvature[2], 0.32636, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 3].pc1, 0.2590007483959198, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 3].pc2, 0.17906941473484039, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 1].principal_curvature[0], 0.86725, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 1].principal_curvature[1], -0.375851, 1e-3);
EXPECT_NEAR (pcs->points[indices.size () - 1].principal_curvature[2], 0.32636, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 1].pc1, 0.25900065898895264, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 1].pc2, 0.17906941473484039, 1e-4);
}
TEST (PCL, SpinImageEstimation)
{
// Estimate normals first
double mr = 0.002;
NormalEstimation<PointXYZ, Normal> n;
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
// set parameters
n.setInputCloud (cloud.makeShared ());
boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
n.setIndices (indicesptr);
n.setSearchMethod (tree);
n.setRadiusSearch (20 * mr);
n.compute (*normals);
EXPECT_NEAR (normals->points[103].normal_x, 0.36683175, 1e-4);
EXPECT_NEAR (normals->points[103].normal_y, -0.44696972, 1e-4);
EXPECT_NEAR (normals->points[103].normal_z, -0.81587529, 1e-4);
EXPECT_NEAR (normals->points[200].normal_x, -0.71414840, 1e-4);
EXPECT_NEAR (normals->points[200].normal_y, -0.06002361, 1e-4);
EXPECT_NEAR (normals->points[200].normal_z, -0.69741613, 1e-4);
EXPECT_NEAR (normals->points[140].normal_x, -0.45109111, 1e-4);
EXPECT_NEAR (normals->points[140].normal_y, -0.19499126, 1e-4);
EXPECT_NEAR (normals->points[140].normal_z, -0.87091631, 1e-4);
typedef Histogram<153> SpinImage;
SpinImageEstimation<PointXYZ, Normal, SpinImage> spin_est(8, 0.5, 16);
// set parameters
//spin_est.setInputWithNormals (cloud.makeShared (), normals);
spin_est.setInputCloud (cloud.makeShared ());
spin_est.setInputNormals (normals);
spin_est.setIndices (indicesptr);
spin_est.setSearchMethod (tree);
spin_est.setRadiusSearch (40*mr);
// Object
PointCloud<SpinImage>::Ptr spin_images (new PointCloud<SpinImage> ());
// radial SI
spin_est.setRadialStructure();
// estimate
spin_est.compute (*spin_images);
EXPECT_EQ (spin_images->points.size (), indices.size ());
EXPECT_NEAR (spin_images->points[100].histogram[0], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[12], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[24], 0.00233226, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[36], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[48], 8.48662e-005, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[60], 0.0266387, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[72], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[84], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[96], 0.0414662, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[108], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[120], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[132], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[144], 0.0128513, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[0], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[12], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[24], 0.00932424, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[36], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[48], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[60], 0.0145733, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[72], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[84], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[96], 0.00034457, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[108], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[120], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[132], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[144], 0.0121195, 1e-4);
// radial SI, angular spin-images
spin_est.setAngularDomain ();
// estimate
spin_est.compute (*spin_images);
EXPECT_EQ (spin_images->points.size (), indices.size ());
EXPECT_NEAR (spin_images->points[100].histogram[0], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[12], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[24], 0.132139, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[36], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[48], 0.908814, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[60], 0.63875, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[72], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[84], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[96], 0.550392, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[108], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[120], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[132], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[144], 0.257136, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[0], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[12], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[24], 0.230605, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[36], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[48], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[60], 0.764872, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[72], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[84], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[96], 1.02824, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[108], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[120], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[132], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[144], 0.293567, 1e-4);
// rectangular SI
spin_est.setRadialStructure (false);
spin_est.setAngularDomain (false);
// estimate
spin_est.compute (*spin_images);
EXPECT_EQ (spin_images->points.size (), indices.size ());
EXPECT_NEAR (spin_images->points[100].histogram[0], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[12], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[24], 0.000889345, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[36], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[48], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[60], 0.0489534, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[72], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[84], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[96], 0.0747141, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[108], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[120], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[132], 0.0173423, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[144], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[0], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[12], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[24], 0.0267132, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[36], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[48], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[60], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[72], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[84], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[96], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[108], 0.0209709, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[120], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[132], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[144], 0.029372, 1e-4);
// rectangular SI, angular spin-images
spin_est.setAngularDomain ();
// estimate
spin_est.compute (*spin_images);
EXPECT_EQ (spin_images->points.size (), indices.size ());
EXPECT_NEAR (spin_images->points[100].histogram[0], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[12], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[24], 0.132139, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[36], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[48], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[60], 0.38800787925720215, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[72], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[84], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[96], 0.468881, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[108], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[120], 0, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[132], 0.67901438474655151, 1e-4);
EXPECT_NEAR (spin_images->points[100].histogram[144], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[0], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[12], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[24], 0.143845, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[36], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[48], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[60], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[72], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[84], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[96], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[108], 0.706084, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[120], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[132], 0, 1e-4);
EXPECT_NEAR (spin_images->points[300].histogram[144], 0.272542, 1e-4);
}
TEST (PCL, SpinImageEstimationEigen)
{
// Estimate normals first
double mr = 0.002;
NormalEstimation<PointXYZ, Normal> n;
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
// set parameters
n.setInputCloud (cloud.makeShared ());
boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
n.setIndices (indicesptr);
n.setSearchMethod (tree);
n.setRadiusSearch (20 * mr);
n.compute (*normals);
EXPECT_NEAR (normals->points[103].normal_x, 0.36683175, 1e-4);
EXPECT_NEAR (normals->points[103].normal_y, -0.44696972, 1e-4);
EXPECT_NEAR (normals->points[103].normal_z, -0.81587529, 1e-4);
EXPECT_NEAR (normals->points[200].normal_x, -0.71414840, 1e-4);
EXPECT_NEAR (normals->points[200].normal_y, -0.06002361, 1e-4);
EXPECT_NEAR (normals->points[200].normal_z, -0.69741613, 1e-4);
EXPECT_NEAR (normals->points[140].normal_x, -0.45109111, 1e-4);
EXPECT_NEAR (normals->points[140].normal_y, -0.19499126, 1e-4);
EXPECT_NEAR (normals->points[140].normal_z, -0.87091631, 1e-4);
SpinImageEstimation<PointXYZ, Normal, Eigen::MatrixXf> spin_est (8, 0.5, 16);
// set parameters
//spin_est.setInputWithNormals (cloud.makeShared (), normals);
spin_est.setInputCloud (cloud.makeShared ());
spin_est.setInputNormals (normals);
spin_est.setIndices (indicesptr);
spin_est.setSearchMethod (tree);
spin_est.setRadiusSearch (40*mr);
// Object
PointCloud<Eigen::MatrixXf>::Ptr spin_images (new PointCloud<Eigen::MatrixXf>);
// radial SI
spin_est.setRadialStructure ();
// estimate
spin_est.computeEigen (*spin_images);
EXPECT_EQ (spin_images->points.rows (), indices.size ());
EXPECT_NEAR (spin_images->points (100, 0), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 12), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 24), 0.00233226, 1e-4);
EXPECT_NEAR (spin_images->points (100, 36), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 48), 8.48662e-005, 1e-4);
EXPECT_NEAR (spin_images->points (100, 60), 0.0266387, 1e-4);
EXPECT_NEAR (spin_images->points (100, 72), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 84), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 96), 0.0414662, 1e-4);
EXPECT_NEAR (spin_images->points (100, 108), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 120), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 132), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 144), 0.0128513, 1e-4);
EXPECT_NEAR (spin_images->points (300, 0), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 12), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 24), 0.00932424, 1e-4);
EXPECT_NEAR (spin_images->points (300, 36), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 48), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 60), 0.0145733, 1e-4);
EXPECT_NEAR (spin_images->points (300, 72), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 84), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 96), 0.00034457, 1e-4);
EXPECT_NEAR (spin_images->points (300, 108), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 120), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 132), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 144), 0.0121195, 1e-4);
// radial SI, angular spin-images
spin_est.setAngularDomain ();
// estimate
spin_est.computeEigen (*spin_images);
EXPECT_EQ (spin_images->points.rows (), indices.size ());
EXPECT_NEAR (spin_images->points (100, 0), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 12), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 24), 0.13213, 1e-4);
EXPECT_NEAR (spin_images->points (100, 36), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 48), 0.908804, 1.1e-4);
EXPECT_NEAR (spin_images->points (100, 60), 0.63875, 1e-4);
EXPECT_NEAR (spin_images->points (100, 72), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 84), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 96), 0.550392, 1e-4);
EXPECT_NEAR (spin_images->points (100, 108), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 120), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 132), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 144), 0.25713, 1e-4);
EXPECT_NEAR (spin_images->points (300, 0), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 12), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 24), 0.230605, 1e-4);
EXPECT_NEAR (spin_images->points (300, 36), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 48), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 60), 0.764872, 1e-4);
EXPECT_NEAR (spin_images->points (300, 72), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 84), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 96), 1.02824, 1e-4);
EXPECT_NEAR (spin_images->points (300, 108), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 120), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 132), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 144), 0.293567, 1e-4);
// rectangular SI
spin_est.setRadialStructure (false);
spin_est.setAngularDomain (false);
// estimate
spin_est.computeEigen (*spin_images);
EXPECT_EQ (spin_images->points.rows (), indices.size ());
EXPECT_NEAR (spin_images->points (100, 0), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 12), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 24), 0.000889345, 1e-4);
EXPECT_NEAR (spin_images->points (100, 36), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 48), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 60), 0.0489534, 1e-4);
EXPECT_NEAR (spin_images->points (100, 72), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 84), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 96), 0.0747141, 1e-4);
EXPECT_NEAR (spin_images->points (100, 108), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 120), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 132), 0.0173423, 1e-4);
EXPECT_NEAR (spin_images->points (100, 144), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 0), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 12), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 24), 0.0267132, 1e-4);
EXPECT_NEAR (spin_images->points (300, 36), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 48), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 60), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 72), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 84), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 96), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 108), 0.0209709, 1e-4);
EXPECT_NEAR (spin_images->points (300, 120), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 132), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 144), 0.029372, 1e-4);
// rectangular SI, angular spin-images
spin_est.setAngularDomain ();
// estimate
spin_est.computeEigen (*spin_images);
EXPECT_EQ (spin_images->points.rows (), indices.size ());
EXPECT_NEAR (spin_images->points (100, 0), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 12), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 24), 0.132126, 1e-4);
EXPECT_NEAR (spin_images->points (100, 36), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 48), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 60), 0.388011, 1e-4);
EXPECT_NEAR (spin_images->points (100, 72), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 84), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 96), 0.468881, 1e-4);
EXPECT_NEAR (spin_images->points (100, 108), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 120), 0, 1e-4);
EXPECT_NEAR (spin_images->points (100, 132), 0.678995, 1e-4);
EXPECT_NEAR (spin_images->points (100, 144), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 0), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 12), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 24), 0.143845, 1e-4);
EXPECT_NEAR (spin_images->points (300, 36), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 48), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 60), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 72), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 84), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 96), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 108), 0.706084, 1e-4);
EXPECT_NEAR (spin_images->points (300, 120), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 132), 0, 1e-4);
EXPECT_NEAR (spin_images->points (300, 144), 0.272542, 1e-4);
}
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 ());
}
TEST (PCL, GSHOTShapeEstimation)
{
// Estimate normals first
double mr = 0.002;
NormalEstimation<PointXYZ, Normal> n;
boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
n.setInputCloud (cloud.makeShared ());
n.setIndices (indicesptr);
n.setSearchMethod (tree);
n.setRadiusSearch (20 * mr);
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
n.compute (*normals);
EXPECT_NEAR (normals->points[103].normal_x, 0.36683175, 1e-4);
EXPECT_NEAR (normals->points[103].normal_y, -0.44696972, 1e-4);
EXPECT_NEAR (normals->points[103].normal_z, -0.81587529, 1e-4);
EXPECT_NEAR (normals->points[200].normal_x, -0.71414840, 1e-4);
EXPECT_NEAR (normals->points[200].normal_y, -0.06002361, 1e-4);
EXPECT_NEAR (normals->points[200].normal_z, -0.69741613, 1e-4);
EXPECT_NEAR (normals->points[140].normal_x, -0.45109111, 1e-4);
EXPECT_NEAR (normals->points[140].normal_y, -0.19499126, 1e-4);
EXPECT_NEAR (normals->points[140].normal_z, -0.87091631, 1e-4);
// Objects
PointCloud<SHOT352>::Ptr gshots352 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr shots352 (new PointCloud<SHOT352> ());
// SHOT352 (local)
SHOTEstimation<PointXYZ, Normal, SHOT352> shot352;
shot352.setInputNormals (normals);
shot352.setRadiusSearch (radius_local_shot);
shot352.setInputCloud (cloud_for_lrf.makeShared ());
boost::shared_ptr<vector<int> > indices_local_shot_ptr (new vector<int> (indices_local_shot));
shot352.setIndices (indices_local_shot_ptr);
shot352.setSearchSurface (cloud.makeShared());
shot352.compute (*shots352);
EXPECT_NEAR (shots352->points[0].descriptor[9 ], 0.0f, 1E-4);
EXPECT_NEAR (shots352->points[0].descriptor[10], 0.0f, 1E-4);
EXPECT_NEAR (shots352->points[0].descriptor[11], 0.317935f, 1E-4);
EXPECT_NEAR (shots352->points[0].descriptor[19], 0.0f, 1E-4);
EXPECT_NEAR (shots352->points[0].descriptor[20], 0.0f, 1E-4);
EXPECT_NEAR (shots352->points[0].descriptor[21], 0.0f, 1E-4);
EXPECT_NEAR (shots352->points[0].descriptor[42], 0.0f, 1E-4);
EXPECT_NEAR (shots352->points[0].descriptor[53], 0.0f, 1E-4);
EXPECT_NEAR (shots352->points[0].descriptor[54], 0.0f, 1E-4);
EXPECT_NEAR (shots352->points[0].descriptor[55], 0.089004f, 1E-4);
// SHOT352 (global)
GSHOTEstimation<PointXYZ, Normal, SHOT352> gshot352;
gshot352.setSearchMethod (tree);
gshot352.setInputNormals (normals);
EXPECT_EQ (gshot352.getInputNormals (), normals);
// set parameters
gshot352.setInputCloud (cloud.makeShared ());
gshot352.setIndices (indicesptr);
// estimate
int gshot_size = 1;
gshot352.compute (*gshots352);
EXPECT_EQ (gshots352->points.size (), gshot_size);
checkDescNear (*gshots352, *shots352, 1E-7);
}
TEST (PCL, 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, FPFHEstimationOpenMP)
{
// Estimate normals first
NormalEstimation<PointXYZ, Normal> n;
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
// set parameters
n.setInputCloud (cloud.makeShared ());
boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
n.setIndices (indicesptr);
n.setSearchMethod (tree);
n.setKSearch (10); // Use 10 nearest neighbors to estimate the normals
// estimate
n.compute (*normals);
FPFHEstimationOMP<PointXYZ, Normal, FPFHSignature33> fpfh (4); // instantiate 4 threads
fpfh.setInputNormals (normals);
// Object
PointCloud<FPFHSignature33>::Ptr fpfhs (new PointCloud<FPFHSignature33> ());
// set parameters
fpfh.setInputCloud (cloud.makeShared ());
fpfh.setNrSubdivisions (11, 11, 11);
fpfh.setIndices (indicesptr);
fpfh.setSearchMethod (tree);
fpfh.setKSearch (static_cast<int> (indices.size ()));
// estimate
fpfh.compute (*fpfhs);
EXPECT_EQ (fpfhs->points.size (), indices.size ());
EXPECT_NEAR (fpfhs->points[0].histogram[0], 1.58591, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[1], 1.68365, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[2], 6.71 , 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[3], 23.073, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[4], 33.3828, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[5], 20.4002, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[6], 7.31067, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[7], 1.02635, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[8], 0.48591, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[9], 1.47069, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[10], 2.87061, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[11], 1.78321, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[12], 4.30795, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[13], 7.05514, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[14], 9.37615, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[15], 17.963 , 1e-3);
//EXPECT_NEAR (fpfhs->points[0].histogram[16], 18.2801, 1e-3);
//EXPECT_NEAR (fpfhs->points[0].histogram[17], 14.2766, 1e-3);
//EXPECT_NEAR (fpfhs->points[0].histogram[18], 10.8542, 1e-3);
//EXPECT_NEAR (fpfhs->points[0].histogram[19], 6.07925, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[20], 5.28991, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[21], 4.73438, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[22], 0.56984, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[23], 3.29826, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[24], 5.28156, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[25], 5.26939, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[26], 3.13191, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[27], 1.74453, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[28], 9.41971, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[29], 21.5894, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[30], 24.6302, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[31], 17.7764, 1e-3);
EXPECT_NEAR (fpfhs->points[0].histogram[32], 7.28878, 1e-3);
// Test results when setIndices and/or setSearchSurface are used
boost::shared_ptr<vector<int> > test_indices (new vector<int> (0));
for (size_t i = 0; i < cloud.size (); i+=3)
test_indices->push_back (static_cast<int> (i));
testIndicesAndSearchSurface<FPFHEstimationOMP<PointXYZ, Normal, FPFHSignature33>, PointXYZ, Normal, FPFHSignature33>
(cloud.makeShared (), normals, test_indices, 33);
}
TEST (PCL, 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, 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);
}