void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, PolygonMesh &output,
int hoppe_or_rbf, float iso_level, int grid_res, float extend_percentage, float off_surface_displacement)
{
PointCloud<PointNormal>::Ptr xyz_cloud (new pcl::PointCloud<PointNormal> ());
fromROSMsg (*input, *xyz_cloud);
MarchingCubes<PointNormal> *mc;
if (hoppe_or_rbf == 0)
mc = new MarchingCubesHoppe<PointNormal> ();
else
{
mc = new MarchingCubesRBF<PointNormal> ();
((MarchingCubesRBF<PointNormal>*) mc)->setOffSurfaceDisplacement (off_surface_displacement);
}
mc->setIsoLevel (iso_level);
mc->setGridResolution (grid_res, grid_res, grid_res);
mc->setPercentageExtendGrid (extend_percentage);
mc->setInputCloud (xyz_cloud);
TicToc tt;
tt.tic ();
print_highlight ("Computing ");
mc->reconstruct (output);
delete mc;
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");
}
void
compute (ConstCloudPtr &input, Cloud &output, int max_window_size, float slope, float max_distance, float initial_distance, float cell_size, float base, bool exponential)
{
// Estimate
TicToc tt;
tt.tic ();
print_highlight (stderr, "Computing ");
std::vector<int> ground;
ProgressiveMorphologicalFilter<PointType> pmf;
pmf.setInputCloud (input);
pmf.setMaxWindowSize (max_window_size);
pmf.setSlope (slope);
pmf.setMaxDistance (max_distance);
pmf.setInitialDistance (initial_distance);
pmf.setCellSize (cell_size);
pmf.setBase (base);
pmf.setExponential (exponential);
pmf.extract (ground);
PointIndicesPtr idx (new PointIndices);
idx->indices = ground;
ExtractIndices<PointType> extract;
extract.setInputCloud (input);
extract.setIndices (idx);
extract.setNegative (false);
extract.filter (output);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
}
void
compute (const CloudT::Ptr &input, std::vector<FeatureT::Ptr> &trained_features,
CloudLT::Ptr &out,
float normal_radius_search,
float fpfh_radius_search,
float feature_threshold)
{
TicToc tt;
tt.tic ();
print_highlight ("Computing ");
UnaryClassifier<PointT> classifier;
classifier.setInputCloud (input);
classifier.setTrainedFeatures (trained_features);
classifier.setNormalRadiusSearch (normal_radius_search);
classifier.setFPFHRadiusSearch (fpfh_radius_search);
classifier.setFeatureThreshold (feature_threshold);
classifier.segment (out);
print_info ("[done, ");
print_value ("%g", tt.toc ());
print_info (" ms : "); print_value ("%d", out->width * out->height);
print_info (" points]\n");
}
void
saveImage (const std::string &filename, const pcl::PCLImage& image)
{
TicToc tt;
tt.tic ();
print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
savePNGFile (filename, image);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", image.width * image.height); print_info (" points]\n");
}
int main(int argc, char *argv[])
{
if (argc < 2)
{
print_help(argv);
return -1;
}
// Parse the command line arguments for .pcd files
vector<int> txt_file_indices;
txt_file_indices = parse_file_extension_argument(argc, argv, ".pcd");
if (txt_file_indices.size () != 1)
{
print_error("Need one input PCD file to continue.\n");
return -1;
}
string inputFileName = string(argv[txt_file_indices[0]]);
size_t ext = inputFileName.find(".pcd");
string outputFileName = inputFileName.substr(0, ext) + string(".vtk");
PointCloud<PointNormal>::Ptr cloud_with_normals(new PointCloud<PointNormal>);
loadPCDFile<PointNormal>(argv[1], *cloud_with_normals);
search::KdTree<PointNormal>::Ptr tree2 (new search::KdTree<PointNormal>);
tree2->setInputCloud (cloud_with_normals);
// Initialize objects
GreedyProjectionTriangulation<PointNormal> gp3;
PolygonMesh triangles;
// Set the maximum distance between connected points (maximum edge length)
gp3.setSearchRadius (0.025);
// Set typical values for the parameters
gp3.setMu (2.5);
gp3.setMaximumNearestNeighbors(500);
gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
gp3.setMinimumAngle(M_PI/18); // 10 degrees
gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
gp3.setNormalConsistency(false);
// Get result of triangulation
gp3.setInputCloud (cloud_with_normals);
gp3.setSearchMethod (tree2);
TicToc tt;
tt.tic();
print_highlight("Computing GreedyProjectionTriangulation ");
gp3.reconstruct (triangles);
print_info("[done, "); print_value("%g", tt.toc()); print_info(" ms]\n");
saveVTKFile (outputFileName, triangles);
print_highlight(string(string("Saving ") + outputFileName + string("\n")).c_str());
return 0;
}
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);
}
template<typename PointInT> void
saveCloud(const std::string &filename, const PointCloud<PointInT> &cloud, bool format)
{
TicToc tt;
tt.tic();
print_highlight("Saving "); print_value("%s ", filename.c_str());
savePCDFile(filename, cloud, format);
print_info("[done, "); print_value("%g", tt.toc()); print_info(" ms : "); print_value("%d", cloud.width * cloud.height); print_info(" points]\n");
}
void
compute (const pcl::PCLPointCloud2::ConstPtr &input,
pcl::PCLPointCloud2 &output,
double search_radius,
bool sqr_gauss_param_set,
double sqr_gauss_param,
int polynomial_order)
{
PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()),
xyz_cloud (new pcl::PointCloud<PointXYZ> ());
fromPCLPointCloud2 (*input, *xyz_cloud_pre);
// Filter the NaNs from the cloud
for (size_t i = 0; i < xyz_cloud_pre->size (); ++i)
if (pcl_isfinite (xyz_cloud_pre->points[i].x))
xyz_cloud->push_back (xyz_cloud_pre->points[i]);
xyz_cloud->header = xyz_cloud_pre->header;
xyz_cloud->height = 1;
xyz_cloud->width = static_cast<uint32_t> (xyz_cloud->size ());
xyz_cloud->is_dense = false;
PointCloud<PointNormal>::Ptr xyz_cloud_smoothed (new PointCloud<PointNormal> ());
MovingLeastSquares<PointXYZ, PointNormal> mls;
mls.setInputCloud (xyz_cloud);
mls.setSearchRadius (search_radius);
if (sqr_gauss_param_set) mls.setSqrGaussParam (sqr_gauss_param);
mls.setPolynomialOrder (polynomial_order);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::SAMPLE_LOCAL_PLANE);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::RANDOM_UNIFORM_DENSITY);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::VOXEL_GRID_DILATION);
mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::NONE);
mls.setPointDensity (60000 * int (search_radius)); // 300 points in a 5 cm radius
mls.setUpsamplingRadius (0.025);
mls.setUpsamplingStepSize (0.015);
mls.setDilationIterations (2);
mls.setDilationVoxelSize (0.01f);
search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> ());
mls.setSearchMethod (tree);
mls.setComputeNormals (true);
PCL_INFO ("Computing smoothed surface and normals with search_radius %f , sqr_gaussian_param %f, polynomial order %d\n",
mls.getSearchRadius(), mls.getSqrGaussParam(), mls.getPolynomialOrder());
TicToc tt;
tt.tic ();
mls.process (*xyz_cloud_smoothed);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", xyz_cloud_smoothed->width * xyz_cloud_smoothed->height); print_info (" points]\n");
toPCLPointCloud2 (*xyz_cloud_smoothed, output);
}
void
saveCloud (const std::string &filename, const PolygonMesh &output)
{
TicToc tt;
tt.tic ();
print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
saveVTKFile (filename, output);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");
}
void
saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &output)
{
TicToc tt;
tt.tic ();
print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
pcl::io::savePCDFile (filename, output, translation, orientation, false);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
}
void
saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &output)
{
TicToc tt;
tt.tic ();
print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
pcl::io::savePCDFile (filename, output, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true); // Save as binary
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
}
void
saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &cloud)
{
TicToc tt;
tt.tic ();
print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
saveVTKFile (filename, cloud);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
}
void
compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output,
Eigen::Matrix4f &tform)
{
TicToc tt;
tt.tic ();
print_highlight ("Transforming ");
transformPointCloud2 (*input, output, tform);
printElapsedTimeAndNumberOfPoints (tt.toc (), output.width, output.height);
}
void
saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &cloud, bool binary, bool use_camera)
{
TicToc tt;
tt.tic ();
print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
pcl::PLYWriter writer;
writer.write (filename, cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary, use_camera);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
}
bool
loadCloud (const std::string &filename, CloudT::Ptr &cloud)
{
TicToc tt;
print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
tt.tic ();
if (loadPCDFile (filename, *cloud) < 0)
return (false);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud->width * cloud->height); print_info (" points]\n");
return (true);
}
void
saveCloud (const string &filename, const pcl::PCLPointCloud2 &output)
{
TicToc tt;
tt.tic ();
print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
PCDWriter w;
w.writeBinaryCompressed (filename, output, translation, orientation);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
}
void
saveCloud (const std::string &filename, const pcl::PCLPointCloud2 &output)
{
TicToc tt;
tt.tic ();
print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
PCDWriter w;
w.writeBinaryCompressed (filename, output);
printElapsedTimeAndNumberOfPoints (tt.toc (), output.width, output.height);
}
template <typename T> void
saveCloud (const std::string &filename, const pcl::PointCloud<T> &cloud)
{
TicToc tt;
tt.tic ();
print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
pcl::PCDWriter writer;
writer.writeBinaryCompressed (filename, cloud);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
}
bool
loadCloud (const std::string &filename, sensor_msgs::PointCloud2 &cloud)
{
TicToc tt;
print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
tt.tic ();
if (loadPCDFile (filename, cloud) < 0)
return (false);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
return (true);
}
void
saveCloud (const std::string &filename, CloudLT::Ptr &output)
{
TicToc tt;
tt.tic ();
print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
PCDWriter w;
w.write (filename, *output);
print_info ("[done, ");
print_value ("%g", tt.toc ()); print_info (" ms : ");
print_value ("%d", output->width * output->height); print_info (" points]\n");
}
