TEST (PCL, CVFHEstimationMilk)
{
// Estimate normals first
NormalEstimation<PointXYZ, Normal> n;
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
n.setInputCloud (cloud_milk);
n.setSearchMethod (tree);
n.setRadiusSearch (leaf_size_ * 4); //2cm to estimate normals
n.compute (*normals);
CVFHEstimation<PointXYZ, Normal, VFHSignature308> cvfh;
cvfh.setInputCloud (cloud_milk);
cvfh.setInputNormals (normals);
cvfh.setSearchMethod (tree_milk);
cvfh.setClusterTolerance (leaf_size_ * 3);
cvfh.setEPSAngleThreshold (0.13f);
cvfh.setCurvatureThreshold (0.025f);
cvfh.setNormalizeBins (false);
cvfh.setRadiusNormals (leaf_size_ * 4);
// Object
PointCloud<VFHSignature308>::Ptr vfhs (new PointCloud<VFHSignature308> ());
// estimate
cvfh.compute (*vfhs);
EXPECT_EQ (static_cast<int>(vfhs->points.size ()), 2);
}
/* ---[ */
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 ());
}
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, 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);
}
void
estimateNormals (const typename PointCloud<PointT>::ConstPtr &input, PointCloud<Normal> &normals)
{
if (input->isOrganized ())
{
IntegralImageNormalEstimation<PointT, Normal> ne;
// Set the parameters for normal estimation
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setMaxDepthChangeFactor (0.02f);
ne.setNormalSmoothingSize (20.0f);
// Estimate normals in the cloud
ne.setInputCloud (input);
ne.compute (normals);
// Save the distance map for the plane comparator
float *map=ne.getDistanceMap ();// This will be deallocated with the IntegralImageNormalEstimation object...
distance_map_.assign(map, map+input->size() ); //...so we must copy the data out
plane_comparator_->setDistanceMap(distance_map_.data());
}
else
{
NormalEstimation<PointT, Normal> ne;
ne.setInputCloud (input);
ne.setRadiusSearch (0.02f);
ne.setSearchMethod (search_);
ne.compute (normals);
}
}
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);
}
// Subsample cloud for faster matching and processing, while filling in normals.
void PointcloudProc::reduceCloud(const PointCloud<PointXYZRGB>& input, PointCloud<PointXYZRGBNormal>& output) const
{
PointCloud<PointXYZRGB> cloud_nan_filtered, cloud_box_filtered, cloud_voxel_reduced;
PointCloud<Normal> normals;
PointCloud<PointXYZRGBNormal> cloud_normals;
std::vector<int> indices;
// Filter out nans.
removeNaNFromPointCloud(input, cloud_nan_filtered, indices);
indices.clear();
// Filter out everything outside a [200x200x200] box.
Eigen::Vector4f min_pt(-100, -100, -100, -100);
Eigen::Vector4f max_pt(100, 100, 100, 100);
getPointsInBox(cloud_nan_filtered, min_pt, max_pt, indices);
ExtractIndices<PointXYZRGB> boxfilter;
boxfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_nan_filtered));
boxfilter.setIndices (boost::make_shared<vector<int> > (indices));
boxfilter.filter(cloud_box_filtered);
// Reduce pointcloud
VoxelGrid<PointXYZRGB> voxelfilter;
voxelfilter.setInputCloud (boost::make_shared<const PointCloud<PointXYZRGB> > (cloud_box_filtered));
voxelfilter.setLeafSize (0.05, 0.05, 0.05);
// voxelfilter.setLeafSize (0.1, 0.1, 0.1);
voxelfilter.filter (cloud_voxel_reduced);
// Compute normals
NormalEstimation<PointXYZRGB, Normal> normalest;
normalest.setViewPoint(0, 0, 0);
normalest.setSearchMethod (boost::make_shared<search::KdTree<PointXYZRGB> > ());
//normalest.setKSearch (10);
normalest.setRadiusSearch (0.25);
// normalest.setRadiusSearch (0.4);
normalest.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_voxel_reduced));
normalest.compute(normals);
pcl::concatenateFields (cloud_voxel_reduced, normals, cloud_normals);
// Filter based on curvature
PassThrough<PointXYZRGBNormal> normalfilter;
normalfilter.setFilterFieldName("curvature");
// normalfilter.setFilterLimits(0.0, 0.2);
normalfilter.setFilterLimits(0.0, 0.2);
normalfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGBNormal> >(cloud_normals));
normalfilter.filter(output);
}
void create_normals (typename PointCloud<PointT>::Ptr cloud,
typename PointCloud<NormalT>::Ptr normals,
float normal_radius=0.03)
{
NormalEstimation<PointT, NormalT> nest;
cout << "[PFHTransformationStrategy::create_normals] Input cloud "
<< cloud->points.size() << " points" << endl;
nest.setInputCloud(cloud);
nest.setSearchMethod (typename search::KdTree<PointT>::Ptr
(new search::KdTree<PointT>));
nest.setRadiusSearch(normal_radius);
nest.compute(*normals);
};
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);
}
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);
}
/* ---[ */
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 ());
}
/*! @brief runs the whole processing pipeline for FPFH features
*
* @note At the moment the evaluation results will be printed to console.
*
* @param[in] in the labeled input point cloud
* @param[out] ref_out the reference point cloud after the preprocessing steps
* @param[out] fpfh_out the labeled point cloud after the classifing process
*/
void processFPFH(const PointCloud<PointXYZRGB>::Ptr in,
PointCloud<PointXYZRGB>::Ptr ref_out,
PointCloud<PointXYZRGB>::Ptr fpfh_out)
{
PointCloud<Normal>::Ptr n(new PointCloud<Normal>());
PointCloud<FPFHSignature33>::Ptr fpfh(new PointCloud<FPFHSignature33>());
// Passthrough filtering (needs to be done to remove NaNs)
cout << "FPFH: Pass (with " << in->points.size() << " points)" << endl;
PassThrough<PointXYZRGB> pass;
pass.setInputCloud(in);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.0f, pass_depth_);
pass.filter(*ref_out);
// Optional voxelgrid filtering
if (fpfh_vox_enable_)
{
cout << "FPFH: Voxel (with " << ref_out->points.size() << " points)" << endl;
VoxelGrid<PointXYZRGB> vox;
vox.setInputCloud(ref_out);
vox.setLeafSize(fpfh_vox_, fpfh_vox_, fpfh_vox_);
vox.filter(*ref_out);
}
#ifdef PCL_VERSION_COMPARE //fuerte
pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>());
#else //electric
pcl::KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ());
#endif
//KdTree<PointXYZRGB>::Ptr tree(new KdTreeFLANN<PointXYZRGB>());
tree->setInputCloud(ref_out);
// Optional surface smoothing
if(fpfh_mls_enable_)
{
cout << "FPFH: MLS (with " << ref_out->points.size() << " points)" << endl;
#ifdef PCL_VERSION_COMPARE
std::cerr << "MLS has changed completely in PCL 1.7! Requires redesign of entire program" << std::endl;
exit(0);
#else
MovingLeastSquares<PointXYZRGB, Normal> mls;
mls.setInputCloud(ref_out);
mls.setOutputNormals(n);
mls.setPolynomialFit(true);
mls.setPolynomialOrder(2);
mls.setSearchMethod(tree);
mls.setSearchRadius(fpfh_rn_);
mls.reconstruct(*ref_out);
#endif
cout << "FPFH: flip normals (with " << ref_out->points.size() << " points)" << endl;
for (size_t i = 0; i < ref_out->points.size(); ++i)
{
flipNormalTowardsViewpoint(ref_out->points[i], 0.0f, 0.0f, 0.0f,
n->points[i].normal[0],
n->points[i].normal[1],
n->points[i].normal[2]);
}
}
else
{
cout << "FPFH: Normals (with " << ref_out->points.size() << " points)" << endl;
NormalEstimation<PointXYZRGB, Normal> norm;
norm.setInputCloud(ref_out);
norm.setSearchMethod(tree);
norm.setRadiusSearch(fpfh_rn_);
norm.compute(*n);
}
// FPFH estimation
#ifdef PCL_VERSION_COMPARE //fuerte
tree.reset(new pcl::search::KdTree<PointXYZRGB>());
#else //electric
tree.reset(new KdTreeFLANN<PointXYZRGB> ());
#endif
tree->setInputCloud(ref_out);
cout << "FPFH: estimation (with " << ref_out->points.size() << " points)" << endl;
FPFHEstimation<PointXYZRGB, Normal, FPFHSignature33> fpfhE;
fpfhE.setInputCloud(ref_out);
fpfhE.setInputNormals(n);
fpfhE.setSearchMethod(tree);
fpfhE.setRadiusSearch(fpfh_rf_);
fpfhE.compute(*fpfh);
cout << "FPFH: classification " << endl;
*fpfh_out = *ref_out;
CvSVM svm;
svm.load(fpfh_svm_model_.c_str());
cv::Mat fpfh_histo(1, 33, CV_32FC1);
int exp_rgb, pre_rgb, predict;
cob_3d_mapping_common::LabelResults stats(fl2label(fpfh_rn_),fl2label(fpfh_rf_),fpfh_mls_enable_);
for (size_t idx = 0; idx < ref_out->points.size(); idx++)
{
exp_rgb = *reinterpret_cast<int*>(&ref_out->points[idx].rgb); // expected label
memcpy(fpfh_histo.ptr<float>(0), fpfh->points[idx].histogram, sizeof(fpfh->points[idx].histogram));
predict = (int)svm.predict(fpfh_histo);
//cout << predict << endl;
switch(predict)
{
case SVM_PLANE:
pre_rgb = LBL_PLANE;
if (exp_rgb != LBL_PLANE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_PLANE]++;
break;
case SVM_EDGE:
pre_rgb = LBL_EDGE;
if (exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGE]++;
if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGECORNER]++;
break;
case SVM_COR:
pre_rgb = LBL_COR;
if (exp_rgb != LBL_COR && exp_rgb != LBL_UNDEF) stats.fp[EVAL_COR]++;
if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGECORNER]++;
break;
case SVM_SPH:
pre_rgb = LBL_SPH;
if (exp_rgb != LBL_SPH && exp_rgb != LBL_UNDEF) stats.fp[EVAL_SPH]++;
if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CURVED]++;
break;
case SVM_CYL:
pre_rgb = LBL_CYL;
if (exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CYL]++;
if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CURVED]++;
break;
default:
pre_rgb = LBL_UNDEF;
break;
}
switch(exp_rgb)
{
case LBL_PLANE:
if (pre_rgb != exp_rgb) stats.fn[EVAL_PLANE]++;
stats.exp[EVAL_PLANE]++;
break;
case LBL_EDGE:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_EDGE]++;
if (pre_rgb != LBL_COR) stats.fn[EVAL_EDGECORNER]++;
}
stats.exp[EVAL_EDGE]++;
stats.exp[EVAL_EDGECORNER]++;
break;
case LBL_COR:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_COR]++;
if (pre_rgb != LBL_EDGE) stats.fn[EVAL_EDGECORNER]++;
}
stats.exp[EVAL_COR]++;
stats.exp[EVAL_EDGECORNER]++;
break;
case LBL_SPH:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_SPH]++;
if (pre_rgb != LBL_CYL) stats.fn[EVAL_CURVED]++;
}
stats.exp[EVAL_SPH]++;
stats.exp[EVAL_CURVED]++;
break;
case LBL_CYL:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_CYL]++;
if (pre_rgb != LBL_SPH) stats.fn[EVAL_CURVED]++;
}
stats.exp[EVAL_CYL]++;
stats.exp[EVAL_CURVED]++;
break;
default:
stats.undef++;
break;
}
fpfh_out->points[idx].rgb = *reinterpret_cast<float*>(&pre_rgb);
}
cout << "FPFH:\n" << stats << endl << endl;
}
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);
}
void processRSD(const PointCloud<PointXYZRGB>::Ptr in,
PointCloud<PointXYZRGB>::Ptr ref_out,
PointCloud<PointXYZRGB>::Ptr rsd_out)
{
PointCloud<Normal>::Ptr n(new PointCloud<Normal>());
PointCloud<PrincipalRadiiRSD>::Ptr rsd(new PointCloud<PrincipalRadiiRSD>());
// passthrough filtering (needed to remove NaNs)
cout << "RSD: Pass (with " << in->points.size() << " points)" << endl;
PassThrough<PointXYZRGB> pass;
pass.setInputCloud(in);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.0f, pass_depth_);
pass.filter(*ref_out);
// optional voxelgrid filtering
if (rsd_vox_enable_)
{
cout << "RSD: Voxel (with " << ref_out->points.size() << " points)" << endl;
VoxelGrid<PointXYZRGB> vox;
vox.setInputCloud(ref_out);
vox.setLeafSize(rsd_vox_, rsd_vox_, rsd_vox_);
vox.filter(*ref_out);
}
#ifdef PCL_VERSION_COMPARE //fuerte
pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>());
#else //electric
KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ());
#endif
tree->setInputCloud(ref_out);
// optional surface smoothing
if(rsd_mls_enable_)
{
cout << "RSD: MLS (with " << ref_out->points.size() << " points)" << endl;
#ifdef PCL_VERSION_COMPARE
std::cerr << "MLS has changed completely in PCL 1.7! Requires redesign of entire program" << std::endl;
exit(0);
#else
MovingLeastSquares<PointXYZRGB, Normal> mls;
mls.setInputCloud(ref_out);
mls.setOutputNormals(n);
mls.setPolynomialFit(true);
mls.setPolynomialOrder(2);
mls.setSearchMethod(tree);
mls.setSearchRadius(rsd_rn_);
mls.reconstruct(*ref_out);
#endif
cout << "RSD: flip normals (with " << ref_out->points.size() << " points)" << endl;
for (size_t i = 0; i < ref_out->points.size(); ++i)
{
flipNormalTowardsViewpoint(ref_out->points[i], 0.0f, 0.0f, 0.0f,
n->points[i].normal[0],
n->points[i].normal[1],
n->points[i].normal[2]);
}
}
else
{
cout << "RSD: Normals (with " << ref_out->points.size() << " points)" << endl;
NormalEstimation<PointXYZRGB, Normal> norm;
norm.setInputCloud(ref_out);
norm.setSearchMethod(tree);
norm.setRadiusSearch(rsd_rn_);
norm.compute(*n);
}
tree->setInputCloud(ref_out);
// RSD estimation
cout << "RSD: estimation (with " << ref_out->points.size() << " points)" << endl;
RSDEstimation<PointXYZRGB, Normal, PrincipalRadiiRSD> rsdE;
rsdE.setInputCloud(ref_out);
rsdE.setInputNormals(n);
rsdE.setSearchMethod(tree);
rsdE.setPlaneRadius(r_limit_);
rsdE.setRadiusSearch(rsd_rf_);
rsdE.compute(*rsd);
cout << "RSD: classification " << endl;
*rsd_out = *ref_out;
// apply RSD rules for classification
int exp_rgb, pre_rgb;
float r_max,r_min;
cob_3d_mapping_common::LabelResults stats(fl2label(rsd_rn_),fl2label(rsd_rf_),rsd_mls_enable_);
for (size_t idx = 0; idx < ref_out->points.size(); idx++)
{
exp_rgb = *reinterpret_cast<int*>(&ref_out->points[idx].rgb); // expected label
r_max = rsd->points[idx].r_max;
r_min = rsd->points[idx].r_min;
if ( r_min > r_high )
{
pre_rgb = LBL_PLANE;
if (exp_rgb != LBL_PLANE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_PLANE]++;
}
else if (r_min < r_low)
{
if (r_max < r_r2 * r_min)
{
pre_rgb = LBL_COR;
if (exp_rgb != LBL_COR && exp_rgb != LBL_UNDEF) stats.fp[EVAL_COR]++;
}
else
{
pre_rgb = LBL_EDGE;
if (exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGE]++;
}
// special case: combined class for corner and edge
if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF)
stats.fp[EVAL_EDGECORNER]++;
}
else
{
if (r_max < r_r1 * r_min)
{
pre_rgb = LBL_SPH;
if (exp_rgb != LBL_SPH && exp_rgb != LBL_UNDEF) stats.fp[EVAL_SPH]++;
}
else
{
pre_rgb = LBL_CYL;
if (exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CYL]++;
}
// special case: combined class for sphere and cylinder
if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF)
stats.fp[EVAL_CURVED]++;
}
switch(exp_rgb)
{
case LBL_PLANE:
if (pre_rgb != exp_rgb) stats.fn[EVAL_PLANE]++;
stats.exp[EVAL_PLANE]++;
break;
case LBL_EDGE:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_EDGE]++;
if (pre_rgb != LBL_COR) stats.fn[EVAL_EDGECORNER]++;
}
stats.exp[EVAL_EDGE]++;
stats.exp[EVAL_EDGECORNER]++;
break;
case LBL_COR:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_COR]++;
if (pre_rgb != LBL_EDGE) stats.fn[EVAL_EDGECORNER]++;
}
stats.exp[EVAL_COR]++;
stats.exp[EVAL_EDGECORNER]++;
break;
case LBL_SPH:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_SPH]++;
if (pre_rgb != LBL_CYL) stats.fn[EVAL_CURVED]++;
}
stats.exp[EVAL_SPH]++;
stats.exp[EVAL_CURVED]++;
break;
case LBL_CYL:
if (pre_rgb != exp_rgb)
{
stats.fn[EVAL_CYL]++;
if (pre_rgb != LBL_SPH) stats.fn[EVAL_CURVED]++;
}
stats.exp[EVAL_CYL]++;
stats.exp[EVAL_CURVED]++;
break;
default:
stats.undef++;
break;
}
rsd_out->points[idx].rgb = *reinterpret_cast<float*>(&pre_rgb);
}
cout << "RSD:\n" << stats << endl << endl;
}
int main(int argc, char** argv)
{
readOptions(argc, argv);
boost::timer t;
// 3D point clouds
PointCloud<PointXYZRGB>::Ptr p(new PointCloud<PointXYZRGB>);
PointCloud<Normal>::Ptr n(new PointCloud<Normal>);
PointCloud<PrincipalCurvatures>::Ptr pc(new PointCloud<PrincipalCurvatures>);
PointCloud<PointLabel>::Ptr l(new PointCloud<PointLabel>);
PCDReader r;
if (r.read(file_in_, *p) == -1) return(0);
vector<PointIndices> indices;
ifstream fs;
string line;
fs.open(file_cluster_.c_str());
if (fs.is_open())
{
while(fs.good())
{
getline (fs,line);
istringstream iss(line);
PointIndices pi;
do
{
int temp;
iss >> temp;
pi.indices.push_back(temp);
} while(iss);
if(pi.indices.size()>0)
indices.push_back(pi);
}
}
std::cout << "indices file read successfully" << std::endl;
// --- Normal Estimation ---
if (en_one_)
{
t.restart();
cob_3d_features::OrganizedNormalEstimation<PointXYZRGB, Normal, PointLabel>one;
one.setInputCloud(p);
one.setOutputLabels(l);
//one.setPixelSearchRadius(pns_n_,points_,circle_); //radius,pixel,circle
one.setPixelSearchRadius(8,2,2);
one.setSkipDistantPointThreshold(12);
one.compute(*n);
cout << t.elapsed() << "s\t for Organized Normal Estimation" << endl;
}
else
{
t.restart();
#ifdef PCL_VERSION_COMPARE //fuerte
pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>());
#else //electric
pcl::KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ());
#endif
NormalEstimation<PointXYZRGB, Normal> ne;
ne.setRadiusSearch(rn_);
ne.setSearchMethod(tree);
ne.setInputCloud(p);
ne.compute(*n);
cout << t.elapsed() << "s\t for normal estimation" << endl;
}
cob_3d_features::OrganizedCurvatureEstimationOMP<PointXYZRGB, Normal, PointLabel, PrincipalCurvatures> oce;
oce.setInputCloud(p);
oce.setInputNormals(n);
oce.setPixelSearchRadius(8,2,2);
oce.setSkipDistantPointThreshold(12);
//KdTreeFLANN<PointXYZRGB>::Ptr tree(new KdTreeFLANN<PointXYZRGB>);
//ne.setRadiusSearch(rn_);
//ne.setSearchMethod(tree);
if (indices.size() == 0)
{
oce.setOutputLabels(l);
oce.compute(*pc);
cob_3d_features::CurvatureClassifier<PrincipalCurvatures, PointLabel>cc;
cc.setInputCloud(pc);
cc.classify(*l);
}
else
{
for(unsigned int i=0; i<indices.size(); i++)
{
std::cout << "cluster " << i << " has " << indices[i].indices.size() << " points" << std::endl;
if(i==3)
{
/*for(unsigned int j=0; j<indices[i].indices.size(); j++)
std::cout << indices[i].indices[j] << ",";
std::cout << std::endl;*/
//cout << i << ": " << indices[i].indices.front() << "!" << endl;
t.restart();
boost::shared_ptr<PointIndices> ind_ptr = boost::make_shared<PointIndices>(indices[i]);
std::cout << ind_ptr->indices.size() << std::endl;
oce.setIndices(ind_ptr);
oce.setOutputLabels(l);
oce.compute(*pc);
cout << t.elapsed() << "s\t for principal curvature estimation" << endl;
cob_3d_features::CurvatureClassifier<PrincipalCurvatures, PointLabel>cc;
cc.setInputCloud(pc);
cc.setIndices(ind_ptr);
cc.classify(*l);
}
}
}
// colorize edges of 3d point cloud
for (size_t i = 0; i < l->points.size(); i++)
{
//std::cout << l->points[i].label << std::endl;
if (l->points[i].label == I_UNDEF)
{
p->points[i].r = 0;
p->points[i].g = 0;
p->points[i].b = 0;
}
else if (l->points[i].label == I_NAN)
{
p->points[i].r = 0;
p->points[i].g = 255;
p->points[i].b = 0;
}
else if (l->points[i].label == I_EDGE)
{
p->points[i].r = 0;
p->points[i].g = 0;
p->points[i].b = 255;
}
else if (l->points[i].label == I_BORDER)
{
p->points[i].r = 255;
p->points[i].g = 0;
p->points[i].b = 0;
}
else if (l->points[i].label == I_PLANE)
{
p->points[i].r = 255;
p->points[i].g = 255;
p->points[i].b = 0;
}
else if (l->points[i].label == I_CYL)
{
p->points[i].r = 255;
p->points[i].g = 0;
p->points[i].b = 255;
}
else
{
p->points[i].r = 255;
p->points[i].g = 255;
p->points[i].b = 255;
}
}
visualization::PCLVisualizer v;
v.setBackgroundColor(0,127,127);
ColorHdlRGB col_hdl(p);
v.addPointCloud<PointXYZRGB>(p,col_hdl, "segmented");
while(!v.wasStopped())
{
v.spinOnce(100);
usleep(100000);
}
return(0);
}
TEST (PCL, GSHOTWithRTransNoised)
{
PointCloud<PointNormal>::Ptr cloud_nr (new PointCloud<PointNormal> ());
PointCloud<PointNormal>::Ptr cloud_rot (new PointCloud<PointNormal> ());
PointCloud<PointNormal>::Ptr cloud_trans (new PointCloud<PointNormal> ());
PointCloud<PointNormal>::Ptr cloud_rot_trans (new PointCloud<PointNormal> ());
PointCloud<PointXYZ>::Ptr cloud_noise (new PointCloud<PointXYZ> ());
Eigen::Affine3f rot = Eigen::Affine3f::Identity ();
float rot_x = static_cast <float> (rand ()) / static_cast <float> (RAND_MAX);
float rot_y = static_cast <float> (rand ()) / static_cast <float> (RAND_MAX);
float rot_z = static_cast <float> (rand ()) / static_cast <float> (RAND_MAX);
rot.prerotate (Eigen::AngleAxisf (rot_x * M_PI, Eigen::Vector3f::UnitX ()));
rot.prerotate (Eigen::AngleAxisf (rot_y * M_PI, Eigen::Vector3f::UnitY ()));
rot.prerotate (Eigen::AngleAxisf (rot_z * M_PI, Eigen::Vector3f::UnitZ ()));
//std::cout << "rot = (" << (rot_x * M_PI) << ", " << (rot_y * M_PI) << ", " << (rot_z * M_PI) << ")" << std::endl;
Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
float HI = 5.0f;
float LO = -HI;
float trans_x = LO + static_cast<float> (rand ()) / (static_cast<float> (RAND_MAX / (HI - LO)));
float trans_y = LO + static_cast<float> (rand ()) / (static_cast<float> (RAND_MAX / (HI - LO)));
float trans_z = LO + static_cast<float> (rand ()) / (static_cast<float> (RAND_MAX / (HI - LO)));
//std::cout << "trans = (" << trans_x << ", " << trans_y << ", " << trans_z << ")" << std::endl;
trans.translate (Eigen::Vector3f (trans_x, trans_y, trans_z));
// Estimate normals first
float mr = 0.002;
NormalEstimation<PointXYZ, pcl::Normal> n;
PointCloud<Normal>::Ptr normals1 (new PointCloud<Normal> ());
n.setViewPoint (0.0, 0.0, 1.0);
n.setInputCloud (cloud.makeShared ());
n.setRadiusSearch (20 * mr);
n.compute (*normals1);
pcl::concatenateFields<PointXYZ, Normal, PointNormal> (cloud, *normals1, *cloud_nr);
pcl::transformPointCloudWithNormals<PointNormal, float> (*cloud_nr, *cloud_rot, rot);
pcl::transformPointCloudWithNormals<PointNormal, float> (*cloud_nr, *cloud_trans, trans);
pcl::transformPointCloudWithNormals<PointNormal, float> (*cloud_rot, *cloud_rot_trans, trans);
add_gaussian_noise (cloud.makeShared (), cloud_noise, 0.005);
PointCloud<Normal>::Ptr normals_noise (new PointCloud<Normal> ());
n.setInputCloud (cloud_noise);
n.compute (*normals_noise);
PointCloud<Normal>::Ptr normals2 (new PointCloud<Normal> ());
n.setInputCloud (cloud2.makeShared ());
n.compute (*normals2);
PointCloud<Normal>::Ptr normals3 (new PointCloud<Normal> ());
n.setInputCloud (cloud3.makeShared ());
n.compute (*normals3);
// Objects
// Descriptors for ground truth (using SHOT)
PointCloud<SHOT352>::Ptr desc01 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc02 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc03 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc04 (new PointCloud<SHOT352> ());
// Descriptors for test GSHOT
PointCloud<SHOT352>::Ptr desc1 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc2 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc3 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc4 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc5 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc6 (new PointCloud<SHOT352> ());
PointCloud<SHOT352>::Ptr desc7 (new PointCloud<SHOT352> ());
// SHOT352 (global)
GSHOTEstimation<PointNormal, PointNormal, SHOT352> gshot1;
gshot1.setInputNormals (cloud_nr);
gshot1.setInputCloud (cloud_nr);
gshot1.compute (*desc1);
// Eigen::Vector4f center_desc1 = gshot.getCentralPoint ();
gshot1.setInputNormals (cloud_rot);
gshot1.setInputCloud (cloud_rot);
gshot1.compute (*desc2);
// Eigen::Vector4f center_desc2 = gshot.getCentralPoint ();
gshot1.setInputNormals (cloud_trans);
gshot1.setInputCloud (cloud_trans);
gshot1.compute (*desc3);
// Eigen::Vector4f center_desc3 = gshot.getCentralPoint ();
gshot1.setInputNormals (cloud_rot_trans);
gshot1.setInputCloud (cloud_rot_trans);
gshot1.compute (*desc4);
// Eigen::Vector4f center_desc4 = gshot.getCentralPoint ();
GSHOTEstimation<PointXYZ, Normal, SHOT352> gshot2;
gshot2.setInputNormals (normals1);
gshot2.setInputCloud (cloud_noise);
gshot2.compute (*desc5);
gshot2.setInputNormals (normals2);
gshot2.setInputCloud (cloud2.makeShared ());
gshot2.compute (*desc6);
gshot2.setInputNormals (normals3);
gshot2.setInputCloud (cloud3.makeShared ());
gshot2.compute (*desc7);
// Eigen::Vector3f distance_desc = (center_desc3.head<3> () - center_desc1.head<3> ());
// std::cout << "dist of desc0 and desc3 -> (" << distance_desc[0] << ", " << distance_desc[1] << ", " << distance_desc[2] << ")\n";
// SHOT352 (local)
GSHOTEstimation<PointNormal, PointNormal, SHOT352> shot;
shot.setInputNormals (cloud_nr);
shot.setInputCloud (ground_truth.makeShared());
shot.setSearchSurface (cloud_nr);
shot.setRadiusSearch (radius_local_shot);
shot.compute (*desc01);
shot.setInputNormals (cloud_rot);
shot.setInputCloud (ground_truth.makeShared());
shot.setSearchSurface (cloud_rot);
shot.setRadiusSearch (radius_local_shot);
shot.compute (*desc02);
shot.setInputNormals (cloud_trans);
shot.setInputCloud (ground_truth.makeShared());
shot.setSearchSurface (cloud_trans);
shot.setRadiusSearch (radius_local_shot);
shot.compute (*desc03);
shot.setInputNormals (cloud_rot_trans);
shot.setInputCloud (ground_truth.makeShared());
shot.setSearchSurface (cloud_rot_trans);
shot.setRadiusSearch (radius_local_shot);
shot.compute (*desc04);
// CHECK GSHOT
checkDesc(*desc01, *desc1);
checkDesc(*desc02, *desc2);
checkDesc(*desc03, *desc3);
checkDesc(*desc04, *desc4);
std::vector<float> d0, d1, d2, d3, d4, d5, d6;
for(int i = 0; i < 352; ++i)
{
d0.push_back(desc1->points[0].descriptor[i]);
d1.push_back(desc2->points[0].descriptor[i]);
d2.push_back(desc3->points[0].descriptor[i]);
d3.push_back(desc4->points[0].descriptor[i]);
d4.push_back(desc5->points[0].descriptor[i]);
d5.push_back(desc6->points[0].descriptor[i]);
d6.push_back(desc7->points[0].descriptor[i]);
}
float dist_0 = pcl::selectNorm< std::vector<float> > (d0, d0, 352, pcl::HIK) ;
float dist_1 = pcl::selectNorm< std::vector<float> > (d0, d1, 352, pcl::HIK) ;
float dist_2 = pcl::selectNorm< std::vector<float> > (d0, d2, 352, pcl::HIK) ;
float dist_3 = pcl::selectNorm< std::vector<float> > (d0, d3, 352, pcl::HIK) ;
float dist_4 = pcl::selectNorm< std::vector<float> > (d0, d4, 352, pcl::HIK) ;
float dist_5 = pcl::selectNorm< std::vector<float> > (d0, d5, 352, pcl::HIK) ;
float dist_6 = pcl::selectNorm< std::vector<float> > (d0, d6, 352, pcl::HIK) ;
std::cout << ">> Itself[HIK]: " << dist_0 << std::endl
<< ">> Rotation[HIK]: " << dist_1 << std::endl
<< ">> Translate[HIK]: " << dist_2 << std::endl
<< ">> Rot+Trans[HIK] " << dist_3 << std::endl
<< ">> GaussNoise[HIK]: " << dist_4 << std::endl
<< ">> bun03[HIK]: " << dist_5 << std::endl
<< ">> milk[HIK]: " << dist_6 << std::endl;
float high_barrier = dist_0 * 0.999f;
float noise_barrier = dist_0 * 0.75f;
float cut_barrier = dist_0 * 0.20f;
float low_barrier = dist_0 * 0.02f;
EXPECT_GT (dist_1, high_barrier);
EXPECT_GT (dist_2, high_barrier);
//EXPECT_GT (dist_3, high_barrier);
EXPECT_GT (dist_4, noise_barrier);
EXPECT_GT (dist_5, cut_barrier);
EXPECT_LT (dist_6, low_barrier);
}
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);
}