bool
loadCloud (const std::string & filename, PointCloudXYZRGBA & cloud)
{
TicToc tt;
print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
tt.tic ();
if (loadPCDFile (filename, cloud) < 0)
return (false);
printElapsedTimeAndNumberOfPoints (tt.toc (), cloud.width, cloud.height);
print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
return (true);
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
std::string method,
int min_pts, double radius,
int mean_k, double std_dev_mul, bool negative)
{
PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()),
xyz_cloud (new pcl::PointCloud<PointXYZ> ());
fromROSMsg (*input, *xyz_cloud_pre);
std::vector<int> index_vector;
removeNaNFromPointCloud<PointXYZ> (*xyz_cloud_pre, *xyz_cloud, index_vector);
TicToc tt;
tt.tic ();
PointCloud<PointXYZ>::Ptr xyz_cloud_filtered (new PointCloud<PointXYZ> ());
if (method == "statistical")
{
StatisticalOutlierRemoval<PointXYZ> filter;
filter.setInputCloud (xyz_cloud);
filter.setMeanK (mean_k);
filter.setStddevMulThresh (std_dev_mul);
filter.setNegative (negative);
PCL_INFO ("Computing filtered cloud with mean_k %d, std_dev_mul %f, inliers %d\n", filter.getMeanK (), filter.getStddevMulThresh (), filter.getNegative ());
filter.filter (*xyz_cloud_filtered);
}
else if (method == "radius")
{
RadiusOutlierRemoval<PointXYZ> filter;
filter.setInputCloud (xyz_cloud);
filter.setRadiusSearch (radius);
filter.setMinNeighborsInRadius (min_pts);
PCL_INFO ("Computing filtered cloud with radius %f, min_pts %d\n", radius, min_pts);
filter.filter (*xyz_cloud_filtered);
}
else
{
PCL_ERROR ("%s is not a valid filter name! Quitting!\n", method.c_str ());
return;
}
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", xyz_cloud_filtered->width * xyz_cloud_filtered->height); print_info (" points]\n");
toROSMsg (*xyz_cloud_filtered, output);
}
void
saveCloud (const std::string &filename, const pcl::PCLPointCloud2 &output)
{
TicToc tt;
tt.tic ();
print_highlight ("Saving ");
print_value ("%s ", filename.c_str ());
pcl::io::savePCDFile (filename, output);
print_info ("[done, ");
print_value ("%g", tt.toc ());
print_info (" ms : ");
print_value ("%d", output.width * output.height);
print_info (" points]\n");
}
bool
loadCloud (const string &filename, pcl::PCLPointCloud2 &cloud)
{
TicToc tt;
print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
tt.tic ();
if (pcl::io::load (filename, cloud)) {
print_error ("Cannot found input file name (%s).\n", filename.c_str ());
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", getFieldsList (cloud).c_str ());
return (true);
}
bool
loadPCLZF (const std::string &filename_depth,
const std::string &filename_params,
pcl::PointCloud<pcl::PointXYZ> &cloud)
{
TicToc tt;
print_highlight ("Loading "); print_value ("%s ", filename_depth.c_str ());
tt.tic ();
pcl::io::LZFDepth16ImageReader depth;
depth.readParameters (filename_params);
depth.read (filename_depth, cloud);
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
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
float leaf_x, float leaf_y, float leaf_z, const std::string &field, double fmin, double fmax)
{
TicToc tt;
tt.tic ();
print_highlight ("Computing ");
VoxelGrid<sensor_msgs::PointCloud2> grid;
grid.setInputCloud (input);
grid.setFilterFieldName (field);
grid.setFilterLimits (fmin, fmax);
grid.setLeafSize (leaf_x, leaf_y, leaf_z);
grid.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
project (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output, float a, float b, float c, float d)
{
Eigen::Vector4f coeffs;
coeffs << a, b, c, d;
// Convert data to PointCloud<T>
PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>);
fromROSMsg (*input, *xyz);
// Estimate
TicToc tt;
tt.tic ();
//First, we'll find a point on the plane
print_highlight (stderr, "Projecting ");
PointCloud<PointXYZ>::Ptr projected_cloud_pcl (new PointCloud<PointXYZ>);
projected_cloud_pcl->width = xyz->width;
projected_cloud_pcl->height = xyz->height;
projected_cloud_pcl->is_dense = xyz->is_dense;
projected_cloud_pcl->sensor_origin_ = xyz->sensor_origin_;
projected_cloud_pcl->sensor_orientation_ = xyz->sensor_orientation_;
for(size_t i = 0; i < xyz->points.size(); ++i)
{
pcl::PointXYZ projection;
pcl::projectPoint<PointXYZ> (xyz->points[i], coeffs, projection);
projected_cloud_pcl->points.push_back(projection);
}
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : ");
pcl::io::savePCDFile ("foo.pcd", *projected_cloud_pcl);
// Convert data back
sensor_msgs::PointCloud2 projected_cloud;
toROSMsg (*projected_cloud_pcl, projected_cloud);
//we can actually use concatenate fields to inject our projection into the
//output, the second argument overwrites the first's fields for those that
//are shared
concatenateFields (*input, projected_cloud, output);
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
std::string field_name, float min, float max, bool inside)
{
// Estimate
TicToc tt;
tt.tic ();
print_highlight (stderr, "Computing ");
PassThrough<sensor_msgs::PointCloud2> passthrough_filter;
passthrough_filter.setInputCloud (input);
passthrough_filter.setFilterFieldName (field_name);
passthrough_filter.setFilterLimits (min, max);
passthrough_filter.setFilterLimitsNegative (!inside);
passthrough_filter.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
saveCloud (const std::string &filename, const std::vector<sensor_msgs::PointCloud2::Ptr> &output)
{
TicToc tt;
tt.tic ();
std::string basename = filename.substr (0, filename.length () - 4);
for (size_t i = 0; i < output.size (); i++)
{
std::string clustername = basename + boost::lexical_cast<std::string>(i) + ".pcd";
print_highlight ("Saving "); print_value ("%s ", clustername.c_str ());
pcl::io::savePCDFile (clustername, *(output[i]), translation, orientation, false);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output[i]->width * output[i]->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, translation, orientation);
w.writeASCII (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");
}
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, translation, orientation) < 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", getFieldsList (cloud).c_str ());
// Check if the dataset has normals
if (getFieldIndex (cloud, "normal_x") == -1)
{
print_error ("The input dataset does not contain normal information!\n");
return (false);
}
return (true);
}
bool
loadCloud (const std::string &filename, pcl::PCLPointCloud2 &cloud)
{
TicToc tt;
print_highlight ("Loading ");
print_value ("%s ", filename.c_str ());
tt.tic ();
if (loadPCDFile (filename, cloud, translation, orientation) < 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", getFieldsList (cloud).c_str ());
return (true);
}
void
transform (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output)
{
// Check for 'normals'
bool has_normals = false;
for (size_t i = 0; i < input->fields.size (); ++i)
if (input->fields[i].name == "normals")
has_normals = true;
// Estimate
TicToc tt;
tt.tic ();
print_highlight (stderr, "Transforming ");
// Convert data to PointCloud<T>
if (has_normals)
{
PointCloud<PointNormal> xyznormals;
fromROSMsg (*input, xyznormals);
pcl::transformPointCloud<PointNormal> (xyznormals, xyznormals, translation.head<3> (), orientation);
// Copy back the xyz and normals
sensor_msgs::PointCloud2 output_xyznormals;
toROSMsg (xyznormals, output_xyznormals);
concatenateFields (*input, output_xyznormals, output);
}
else
{
PointCloud<PointXYZ> xyz;
fromROSMsg (*input, xyz);
pcl::transformPointCloud<PointXYZ> (xyz, xyz, translation.head<3> (), orientation);
// Copy back the xyz and normals
sensor_msgs::PointCloud2 output_xyz;
toROSMsg (xyz, output_xyz);
concatenateFields (*input, output_xyz, output);
}
translation = Eigen::Vector4f::Zero ();
orientation = Eigen::Quaternionf::Identity ();
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
}
int main(int argc, char * argv[])
{
double portion = 0.9;
uint64 memory_size = getMemorySize() * portion;
uint64 page_size = sysconf(_SC_PAGESIZE);
uint64 page_nums = memory_size/page_size;
// Need to Configure Parse
bool debug = true;
if(debug)
{
printf("*--------------------------*\nMemory Information... \n*--------------------------*\n");
printf("Memory Size (bytes): %lu | ",memory_size);
printf("Page Size (bytes): %lu | ",page_size);
printf("Number of Pages: %lu \n",page_nums);
printf("Note: We allocte %f of the memory \n",portion*100);
}
// Allocate memory
Byte * memory = (Byte * ) malloc(memory_size);
double iterations = 10;
double total_mem_time = 0;
double total_alligned_page = 0;
double total_unalligned_page =0;
for(double j =0 ; j < iterations; j++)
{
TicToc timer;
timer.tic();
memset(memory,0,memory_size);
total_mem_time+= (timer.toc()/1000000);
timer.tic();
for(uint64 i =0 ; i < page_nums; i++ )
{
memset(memory + i* page_size,i,page_size);
}
total_alligned_page+= (timer.toc()/page_size);
timer.tic();
for(uint64 i =0 ; i < page_nums - 1 ; i++ )
{
memset(memory + i * page_size + 2,i,page_size);
}
total_unalligned_page+=(timer.toc()/(page_size - 1));
}
printf("*--------------------------*\nBenchmarking Starting... \n*--------------------------*\n");
printf("Memory Access Time: %f (secs)\n",total_mem_time/iterations);
printf("Alligned Page Access Time: %f (usec)\n",total_alligned_page/iterations);
printf("Unalligned Page Access Time: %f (usec)\n",total_unalligned_page/iterations);
// Free allocated memory
free(memory);
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, std::vector<sensor_msgs::PointCloud2::Ptr> &output,
int min, int max, double tolerance)
{
// Convert data to PointCloud<T>
PointCloud<pcl::PointXYZ>::Ptr xyz (new PointCloud<pcl::PointXYZ>);
fromROSMsg (*input, *xyz);
// Estimate
TicToc tt;
tt.tic ();
print_highlight (stderr, "Computing ");
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (xyz);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (tolerance);
ec.setMinClusterSize (min);
ec.setMaxClusterSize (max);
ec.setSearchMethod (tree);
ec.setInputCloud (xyz);
ec.extract (cluster_indices);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cluster_indices.size ()); print_info (" clusters]\n");
output.reserve (cluster_indices.size ());
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::ExtractIndices<sensor_msgs::PointCloud2> extract;
extract.setInputCloud (input);
extract.setIndices (boost::make_shared<const pcl::PointIndices> (*it));
sensor_msgs::PointCloud2::Ptr out (new sensor_msgs::PointCloud2);
extract.filter (*out);
output.push_back (out);
}
}
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);
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, PolygonMesh &output,
int depth, int solver_divide, int iso_divide)
{
PointCloud<PointNormal>::Ptr xyz_cloud (new pcl::PointCloud<PointNormal> ());
fromROSMsg (*input, *xyz_cloud);
Poisson<PointNormal> poisson;
poisson.setDepth (depth);
poisson.setSolverDivide (solver_divide);
poisson.setIsoDivide (iso_divide);
poisson.setInputCloud (xyz_cloud);
TicToc tt;
tt.tic ();
print_highlight ("Computing ");
poisson.reconstruct (output);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
float sigma_s, float sigma_r, bool early_division)
{
PointCloud<PointXYZRGBA>::Ptr cloud (new PointCloud<PointXYZRGBA> ());
fromROSMsg (*input, *cloud);
FastBilateralFilter<PointXYZRGBA> filter;
filter.setSigmaS (sigma_s);
filter.setSigmaR (sigma_r);
filter.setEarlyDivision (early_division);
filter.setInputCloud (cloud);
PointCloud<PointXYZRGBA>::Ptr out_cloud (new PointCloud<PointXYZRGBA> ());
TicToc tt;
tt.tic ();
filter.filter (*out_cloud);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", out_cloud->width * out_cloud->height); print_info (" points]\n");
toROSMsg (*out_cloud, output);
}
void
view (const CloudConstPtr &src, const CloudConstPtr &tgt, const CorrespondencesPtr &correspondences)
{
if (!visualize || !vis) return;
PointCloudColorHandlerCustom<PointT> green (tgt, 0, 255, 0);
if (!vis->updatePointCloud<PointT> (src, "source"))
{
vis->addPointCloud<PointT> (src, "source");
vis->resetCameraViewpoint ("source");
}
if (!vis->updatePointCloud<PointT> (tgt, green, "target")) vis->addPointCloud<PointT> (tgt, green, "target");
vis->setPointCloudRenderingProperties (PCL_VISUALIZER_OPACITY, 0.5, "source");
vis->setPointCloudRenderingProperties (PCL_VISUALIZER_OPACITY, 0.7, "target");
vis->setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 6, "source");
TicToc tt;
tt.tic ();
if (!vis->updateCorrespondences<PointT> (src, tgt, *correspondences, 1))
vis->addCorrespondences<PointT> (src, tgt, *correspondences, 1, "correspondences");
tt.toc_print ();
vis->setShapeRenderingProperties (PCL_VISUALIZER_LINE_WIDTH, 5, "correspondences");
//vis->setShapeRenderingProperties (PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "correspondences");
vis->spin ();
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
double standard_deviation)
{
TicToc tt;
tt.tic ();
print_highlight ("Adding Gaussian noise with mean 0.0 and standard deviation %f\n", standard_deviation);
PointCloud<PointXYZ>::Ptr xyz_cloud (new pcl::PointCloud<PointXYZ> ());
fromROSMsg (*input, *xyz_cloud);
PointCloud<PointXYZ>::Ptr xyz_cloud_filtered (new PointCloud<PointXYZ> ());
xyz_cloud_filtered->points.resize (xyz_cloud->points.size ());
xyz_cloud_filtered->header = xyz_cloud->header;
xyz_cloud_filtered->width = xyz_cloud->width;
xyz_cloud_filtered->height = xyz_cloud->height;
boost::mt19937 rng; rng.seed (time (0));
boost::normal_distribution<> nd (0, standard_deviation);
boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
for (size_t point_i = 0; point_i < xyz_cloud->points.size (); ++point_i)
{
xyz_cloud_filtered->points[point_i].x = xyz_cloud->points[point_i].x + var_nor ();
xyz_cloud_filtered->points[point_i].y = xyz_cloud->points[point_i].y + var_nor ();
xyz_cloud_filtered->points[point_i].z = xyz_cloud->points[point_i].z + var_nor ();
}
sensor_msgs::PointCloud2 input_xyz_filtered;
toROSMsg (*xyz_cloud_filtered, input_xyz_filtered);
concatenateFields (*input, input_xyz_filtered, 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 sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
float th_dd, int max_search)
{
CloudPtr cloud (new Cloud);
fromROSMsg (*input, *cloud);
pcl::PointCloud<pcl::Normal>::Ptr normal (new pcl::PointCloud<pcl::Normal>);
pcl::IntegralImageNormalEstimation<PointXYZRGBA, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setNormalSmoothingSize (10.0f);
ne.setBorderPolicy (ne.BORDER_POLICY_MIRROR);
ne.setInputCloud (cloud);
ne.compute (*normal);
TicToc tt;
tt.tic ();
//OrganizedEdgeBase<PointXYZRGBA, Label> oed;
//OrganizedEdgeFromRGB<PointXYZRGBA, Label> oed;
//OrganizedEdgeFromNormals<PointXYZRGBA, Normal, Label> oed;
OrganizedEdgeFromRGBNormals<PointXYZRGBA, Normal, Label> oed;
oed.setInputNormals (normal);
oed.setInputCloud (cloud);
oed.setDepthDisconThreshold (th_dd);
oed.setMaxSearchNeighbors (max_search);
oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE | oed.EDGELABEL_RGB_CANNY);
PointCloud<Label> labels;
std::vector<PointIndices> label_indices;
oed.compute (labels, label_indices);
print_info ("Detecting all edges... [done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");
// Make gray point clouds
for (int idx = 0; idx < (int)cloud->points.size (); idx++)
{
uint8_t gray = (cloud->points[idx].r + cloud->points[idx].g + cloud->points[idx].b)/3;
cloud->points[idx].r = cloud->points[idx].g = cloud->points[idx].b = gray;
}
// Display edges in PCLVisualizer
viewer.setSize (640, 480);
viewer.addCoordinateSystem (0.2f);
viewer.addPointCloud (cloud, "original point cloud");
viewer.registerKeyboardCallback(&keyboard_callback);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr occluding_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
occluded_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
nan_boundary_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
high_curvature_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
rgb_edges (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::copyPointCloud (*cloud, label_indices[0].indices, *nan_boundary_edges);
pcl::copyPointCloud (*cloud, label_indices[1].indices, *occluding_edges);
pcl::copyPointCloud (*cloud, label_indices[2].indices, *occluded_edges);
pcl::copyPointCloud (*cloud, label_indices[3].indices, *high_curvature_edges);
pcl::copyPointCloud (*cloud, label_indices[4].indices, *rgb_edges);
const int point_size = 2;
viewer.addPointCloud<pcl::PointXYZRGBA> (nan_boundary_edges, "nan boundary edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "nan boundary edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 0.0f, 1.0f, "nan boundary edges");
viewer.addPointCloud<pcl::PointXYZRGBA> (occluding_edges, "occluding edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges");
viewer.addPointCloud<pcl::PointXYZRGBA> (occluded_edges, "occluded edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges");
viewer.addPointCloud<pcl::PointXYZRGBA> (high_curvature_edges, "high curvature edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "high curvature edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 1.0f, 0.0f, "high curvature edges");
viewer.addPointCloud<pcl::PointXYZRGBA> (rgb_edges, "rgb edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges");
while (!viewer.wasStopped ())
{
viewer.spinOnce ();
pcl_sleep(0.1);
}
// Combine point clouds and edge labels
sensor_msgs::PointCloud2 output_edges;
toROSMsg (labels, output_edges);
concatenateFields (*input, output_edges, output);
}
void
compute (const pcl::PCLPointCloud2::ConstPtr &source,
const pcl::PCLPointCloud2::ConstPtr &target,
pcl::PCLPointCloud2 &transformed_source)
{
// Convert data to PointCloud<T>
PointCloud<PointNormal>::Ptr src (new PointCloud<PointNormal>);
PointCloud<PointNormal>::Ptr tgt (new PointCloud<PointNormal>);
fromPCLPointCloud2 (*source, *src);
fromPCLPointCloud2 (*target, *tgt);
// Estimate
TicToc tt;
tt.tic ();
print_highlight (stderr, "Computing ");
#define Scalar double
//#define Scalar float
TransformationEstimationLM<PointNormal, PointNormal, Scalar>::Ptr te (new TransformationEstimationLM<PointNormal, PointNormal, Scalar>);
//TransformationEstimationSVD<PointNormal, PointNormal, Scalar>::Ptr te (new TransformationEstimationSVD<PointNormal, PointNormal, Scalar>);
CorrespondenceEstimation<PointNormal, PointNormal, double>::Ptr cens (new CorrespondenceEstimation<PointNormal, PointNormal, double>);
//CorrespondenceEstimationNormalShooting<PointNormal, PointNormal, PointNormal>::Ptr cens (new CorrespondenceEstimationNormalShooting<PointNormal, PointNormal, PointNormal>);
//CorrespondenceEstimationNormalShooting<PointNormal, PointNormal, PointNormal, double>::Ptr cens (new CorrespondenceEstimationNormalShooting<PointNormal, PointNormal, PointNormal, double>);
cens->setInputSource (src);
cens->setInputTarget (tgt);
//cens->setSourceNormals (src);
CorrespondenceRejectorOneToOne::Ptr cor_rej_o2o (new CorrespondenceRejectorOneToOne);
CorrespondenceRejectorMedianDistance::Ptr cor_rej_med (new CorrespondenceRejectorMedianDistance);
cor_rej_med->setInputSource<PointNormal> (src);
cor_rej_med->setInputTarget<PointNormal> (tgt);
CorrespondenceRejectorSampleConsensus<PointNormal>::Ptr cor_rej_sac (new CorrespondenceRejectorSampleConsensus<PointNormal>);
cor_rej_sac->setInputSource (src);
cor_rej_sac->setInputTarget (tgt);
cor_rej_sac->setInlierThreshold (0.005);
cor_rej_sac->setMaximumIterations (10000);
CorrespondenceRejectorVarTrimmed::Ptr cor_rej_var (new CorrespondenceRejectorVarTrimmed);
cor_rej_var->setInputSource<PointNormal> (src);
cor_rej_var->setInputTarget<PointNormal> (tgt);
CorrespondenceRejectorTrimmed::Ptr cor_rej_tri (new CorrespondenceRejectorTrimmed);
IterativeClosestPoint<PointNormal, PointNormal, Scalar> icp;
icp.setCorrespondenceEstimation (cens);
icp.setTransformationEstimation (te);
icp.addCorrespondenceRejector (cor_rej_o2o);
//icp.addCorrespondenceRejector (cor_rej_var);
//icp.addCorrespondenceRejector (cor_rej_med);
//icp.addCorrespondenceRejector (cor_rej_tri);
//icp.addCorrespondenceRejector (cor_rej_sac);
icp.setInputSource (src);
icp.setInputTarget (tgt);
icp.setMaximumIterations (1000);
icp.setTransformationEpsilon (1e-10);
PointCloud<PointNormal> output;
icp.align (output);
print_info ("[done, ");
print_value ("%g", tt.toc ());
print_info (" ms : ");
print_value ("%d", output.width * output.height);
print_info (" points], has converged: ");
print_value ("%d", icp.hasConverged ());
print_info (" with score: %f\n", icp.getFitnessScore ());
Eigen::Matrix4d transformation = icp.getFinalTransformation ();
//Eigen::Matrix4f transformation = icp.getFinalTransformation ();
PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n",
transformation (0, 0), transformation (0, 1), transformation (0, 2), transformation (0, 3),
transformation (1, 0), transformation (1, 1), transformation (1, 2), transformation (1, 3),
transformation (2, 0), transformation (2, 1), transformation (2, 2), transformation (2, 3),
transformation (3, 0), transformation (3, 1), transformation (3, 2), transformation (3, 3));
// Convert data back
pcl::PCLPointCloud2 output_source;
toPCLPointCloud2 (output, output_source);
concatenateFields (*source, output_source, transformed_source);
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
double search_radius, bool sqr_gauss_param_set, double sqr_gauss_param,
bool use_polynomial_fit, int polynomial_order)
{
PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()),
xyz_cloud (new pcl::PointCloud<PointXYZ> ());
fromROSMsg (*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 = xyz_cloud->size ();
xyz_cloud->is_dense = false;
// io::savePCDFile ("test.pcd", *xyz_cloud);
PointCloud<PointXYZ>::Ptr xyz_cloud_smoothed (new PointCloud<PointXYZ> ());
MovingLeastSquares<PointXYZ, Normal> mls;
mls.setInputCloud (xyz_cloud);
mls.setSearchRadius (search_radius);
if (sqr_gauss_param_set) mls.setSqrGaussParam (sqr_gauss_param);
mls.setPolynomialFit (use_polynomial_fit);
mls.setPolynomialOrder (polynomial_order);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::SAMPLE_LOCAL_PLANE);
mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::UNIFORM_DENSITY);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::FILL_HOLES);
// mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, Normal>::NONE);
mls.setFillingStepSize (0.02);
mls.setPointDensity (50000*search_radius); // 300 points in a 5 cm radius
mls.setUpsamplingRadius (0.025);
mls.setUpsamplingStepSize (0.015);
search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> ());
mls.setSearchMethod (tree);
PointCloud<Normal>::Ptr mls_normals (new PointCloud<Normal> ());
mls.setOutputNormals (mls_normals);
PCL_INFO ("Computing smoothed surface and normals with search_radius %f , sqr_gaussian_param %f, polynomial fitting %d, polynomial order %d\n",
mls.getSearchRadius(), mls.getSqrGaussParam(), mls.getPolynomialFit(), mls.getPolynomialOrder());
TicToc tt;
tt.tic ();
mls.reconstruct (*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");
sensor_msgs::PointCloud2 output_positions, output_normals;
// printf ("sizes: %d %d %d\n", xyz_cloud_smoothed->width, xyz_cloud_smoothed->height, xyz_cloud_smoothed->size ());
toROSMsg (*xyz_cloud_smoothed, output_positions);
toROSMsg (*mls_normals, output_normals);
concatenateFields (output_positions, output_normals, output);
PointCloud<PointXYZ> xyz_vg;
VoxelGrid<PointXYZ> vg;
vg.setInputCloud (xyz_cloud_smoothed);
vg.setLeafSize (0.005, 0.005, 0.005);
vg.filter (xyz_vg);
sensor_msgs::PointCloud2 xyz_vg_2;
toROSMsg (xyz_vg, xyz_vg_2);
pcl::io::savePCDFile ("cloud_vg.pcd", xyz_vg_2, Eigen::Vector4f::Zero (),
Eigen::Quaternionf::Identity (), true);
}
void
compute (const pcl::PCLPointCloud2::ConstPtr &input,pcl::PCLPointCloud2::Ptr &output, const double sigma_r, const double sigma_s)
{
TicToc tt;
tt.tic ();
print_highlight ("Bilaterally smoothing the intensity field of the cloud with distance sigma %f, and intensity sigma %f \n", sigma_s, sigma_r);
//Convert blob to XYZI cloud
PointCloud<PointXYZI>::Ptr xyzi_cloud (new pcl::PointCloud<PointXYZI> ());
fromPCLPointCloud2 (*input, *xyzi_cloud);
PointCloud<PointXYZI>::Ptr xyzi_cloud_filtered (new PointCloud<PointXYZI> ());
//Create the filter
pcl::BilateralFilter<PointXYZI> filter;
//Configure output cloud
// xyzi_cloud_filtered->points.resize (xyzi_cloud->points.size ());
// xyzi_cloud_filtered->header = xyzi_cloud->header;
// xyzi_cloud_filtered->width = xyzi_cloud->width;
// xyzi_cloud_filtered->height = xyzi_cloud->height;
// pcl::KdTreeFLANN<PointXYZI>::ConstPtr tree (new pcl::KdTreeFLANN<PointXYZI>);
filter.setInputCloud (xyzi_cloud);
// filter.setSearchMethod (*tree);
filter.setHalfSize (sigma_s);
filter.setStdDev (sigma_r);
filter.filter (*xyzi_cloud_filtered);
//set up the filter
/*filter.setInputCloud(xyzi_cloud);
filter.setHalfSize(sigma_s);
filter.setStdDev(sigma_r);*/
// filter.applyFilter();
// boost::mt19937 rng; rng.seed (time (0));
// boost::normal_distribution<> nd (0, standard_deviation);
// boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
//
// for (size_t point_i = 0; point_i < xyz_cloud->points.size (); ++point_i)
// {
// xyz_cloud_filtered->points[point_i].x = xyz_cloud->points[point_i].x + var_nor ();
// xyz_cloud_filtered->points[point_i].y = xyz_cloud->points[point_i].y + var_nor ();
// xyz_cloud_filtered->points[point_i].z = xyz_cloud->points[point_i].z + var_nor ();
// }
// pcl::PCLPointCloud2 output;
pcl::toPCLPointCloud2 (*xyzi_cloud_filtered, *output);
// concatenateFields (*input, *xyzi_cloud_filtered, output); TODO add this for keep the fields
// 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 sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
int max_iterations = 1000, double threshold = 0.05, bool negative = false)
{
// Convert data to PointCloud<T>
PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>);
fromROSMsg (*input, *xyz);
// Estimate
TicToc tt;
print_highlight (stderr, "Computing ");
tt.tic ();
// Refine the plane indices
typedef SampleConsensusModelPlane<PointXYZ>::Ptr SampleConsensusModelPlanePtr;
SampleConsensusModelPlanePtr model (new SampleConsensusModelPlane<PointXYZ> (xyz));
RandomSampleConsensus<PointXYZ> sac (model, threshold);
sac.setMaxIterations (max_iterations);
bool res = sac.computeModel ();
vector<int> inliers;
sac.getInliers (inliers);
Eigen::VectorXf coefficients;
sac.getModelCoefficients (coefficients);
if (!res || inliers.empty ())
{
PCL_ERROR ("No planar model found. Relax thresholds and continue.\n");
return;
}
sac.refineModel (2, 50);
sac.getInliers (inliers);
sac.getModelCoefficients (coefficients);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms, plane has : "); print_value ("%zu", inliers.size ()); print_info (" points]\n");
print_info ("Model coefficients: [");
print_value ("%g %g %g %g", coefficients[0], coefficients[1], coefficients[2], coefficients[3]); print_info ("]\n");
// Instead of returning the planar model as a set of inliers, return the outliers, but perform a cluster segmentation first
if (negative)
{
// Remove the plane indices from the data
PointIndices::Ptr everything_but_the_plane (new PointIndices);
std::vector<int> indices_fullset (xyz->size ());
for (int p_it = 0; p_it < static_cast<int> (indices_fullset.size ()); ++p_it)
indices_fullset[p_it] = p_it;
std::sort (inliers.begin (), inliers.end ());
set_difference (indices_fullset.begin (), indices_fullset.end (),
inliers.begin (), inliers.end (),
inserter (everything_but_the_plane->indices, everything_but_the_plane->indices.begin ()));
// Extract largest cluster minus the plane
vector<PointIndices> cluster_indices;
EuclideanClusterExtraction<PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setInputCloud (xyz);
ec.setIndices (everything_but_the_plane);
ec.extract (cluster_indices);
// Convert data back
copyPointCloud (*input, cluster_indices[0].indices, output);
}
else
{
// Convert data back
PointCloud<PointXYZ> output_inliers;
copyPointCloud (*input, inliers, output);
}
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &cloud_source, const sensor_msgs::PointCloud2::ConstPtr &cloud_target,
sensor_msgs::PointCloud2 &output, std::string correspondence_type)
{
// Estimate
TicToc tt;
tt.tic ();
PointCloud<PointXYZ>::Ptr xyz_source (new PointCloud<PointXYZ> ());
fromROSMsg (*cloud_source, *xyz_source);
PointCloud<PointXYZ>::Ptr xyz_target (new PointCloud<PointXYZ> ());
fromROSMsg (*cloud_target, *xyz_target);
PointCloud<PointXYZI>::Ptr output_xyzi (new PointCloud<PointXYZI> ());
output_xyzi->points.resize (xyz_source->points.size ());
output_xyzi->height = cloud_source->height;
output_xyzi->width = cloud_source->width;
float rmse = 0.0f;
if (correspondence_type == "index")
{
// print_highlight (stderr, "Computing using the equal indices correspondence heuristic.\n");
if (xyz_source->points.size () != xyz_target->points.size ())
{
print_error ("Source and target clouds do not have the same number of points.\n");
return;
}
for (size_t point_i = 0; point_i < xyz_source->points.size (); ++point_i)
{
if (!pcl_isfinite (xyz_source->points[point_i].x) || !pcl_isfinite (xyz_source->points[point_i].y) || !pcl_isfinite (xyz_source->points[point_i].z))
continue;
if (!pcl_isfinite (xyz_target->points[point_i].x) || !pcl_isfinite (xyz_target->points[point_i].y) || !pcl_isfinite (xyz_target->points[point_i].z))
continue;
float dist = squaredEuclideanDistance (xyz_source->points[point_i], xyz_target->points[point_i]);
rmse += dist;
output_xyzi->points[point_i].x = xyz_source->points[point_i].x;
output_xyzi->points[point_i].y = xyz_source->points[point_i].y;
output_xyzi->points[point_i].z = xyz_source->points[point_i].z;
output_xyzi->points[point_i].intensity = dist;
}
rmse = sqrt (rmse / xyz_source->points.size ());
}
else if (correspondence_type == "nn")
{
// print_highlight (stderr, "Computing using the nearest neighbor correspondence heuristic.\n");
KdTreeFLANN<PointXYZ>::Ptr tree (new KdTreeFLANN<PointXYZ> ());
tree->setInputCloud (xyz_target);
for (size_t point_i = 0; point_i < xyz_source->points.size (); ++ point_i)
{
if (!pcl_isfinite (xyz_source->points[point_i].x) || !pcl_isfinite (xyz_source->points[point_i].y) || !pcl_isfinite (xyz_source->points[point_i].z))
continue;
std::vector<int> nn_indices (1);
std::vector<float> nn_distances (1);
if (!tree->nearestKSearch (xyz_source->points[point_i], 1, nn_indices, nn_distances))
continue;
size_t point_nn_i = nn_indices.front();
float dist = squaredEuclideanDistance (xyz_source->points[point_i], xyz_target->points[point_nn_i]);
rmse += dist;
output_xyzi->points[point_i].x = xyz_source->points[point_i].x;
output_xyzi->points[point_i].y = xyz_source->points[point_i].y;
output_xyzi->points[point_i].z = xyz_source->points[point_i].z;
output_xyzi->points[point_i].intensity = dist;
}
rmse = sqrt (rmse / xyz_source->points.size ());
}
else if (correspondence_type == "nnplane")
{
// print_highlight (stderr, "Computing using the nearest neighbor plane projection correspondence heuristic.\n");
PointCloud<Normal>::Ptr normals_target (new PointCloud<Normal> ());
fromROSMsg (*cloud_target, *normals_target);
KdTreeFLANN<PointXYZ>::Ptr tree (new KdTreeFLANN<PointXYZ> ());
tree->setInputCloud (xyz_target);
for (size_t point_i = 0; point_i < xyz_source->points.size (); ++ point_i)
{
if (!pcl_isfinite (xyz_source->points[point_i].x) || !pcl_isfinite (xyz_source->points[point_i].y) || !pcl_isfinite (xyz_source->points[point_i].z))
continue;
std::vector<int> nn_indices (1);
std::vector<float> nn_distances (1);
if (!tree->nearestKSearch (xyz_source->points[point_i], 1, nn_indices, nn_distances))
continue;
size_t point_nn_i = nn_indices.front();
Eigen::Vector3f normal_target = normals_target->points[point_nn_i].getNormalVector3fMap (),
point_source = xyz_source->points[point_i].getVector3fMap (),
point_target = xyz_target->points[point_nn_i].getVector3fMap ();
float dist = normal_target.dot (point_source - point_target);
rmse += dist * dist;
output_xyzi->points[point_i].x = xyz_source->points[point_i].x;
output_xyzi->points[point_i].y = xyz_source->points[point_i].y;
output_xyzi->points[point_i].z = xyz_source->points[point_i].z;
output_xyzi->points[point_i].intensity = dist * dist;
}
rmse = sqrt (rmse / xyz_source->points.size ());
}
else
{
// print_error ("Unrecognized correspondence type. Check legal arguments by using the -h option\n");
return;
}
toROSMsg (*output_xyzi, output);
// print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" seconds]\n");
print_highlight ("RMSE Error: %f\n", rmse);
}
/** \brief Given a plane, and the set of inlier indices representing it,
* segment out the object of intererest supported by it.
*
* \param[in] picked_idx the index of a point on the object
* \param[in] cloud the full point cloud dataset
* \param[in] plane_indices a set of indices representing the plane supporting the object of interest
* \param[out] object the segmented resultant object
*/
void
segmentObject (int picked_idx,
const typename PointCloud<PointT>::ConstPtr &cloud,
const PointIndices::Ptr &plane_indices,
PointCloud<PointT> &object)
{
typename PointCloud<PointT>::Ptr plane_hull (new PointCloud<PointT>);
// Compute the convex hull of the plane
ConvexHull<PointT> chull;
chull.setDimension (2);
chull.setInputCloud (cloud);
chull.setIndices (plane_indices);
chull.reconstruct (*plane_hull);
// Remove the plane indices from the data
typename PointCloud<PointT>::Ptr plane (new PointCloud<PointT>);
ExtractIndices<PointT> extract (true);
extract.setInputCloud (cloud);
extract.setIndices (plane_indices);
extract.setNegative (false);
extract.filter (*plane);
PointIndices::Ptr indices_but_the_plane (new PointIndices);
extract.getRemovedIndices (*indices_but_the_plane);
// Extract all clusters above the hull
PointIndices::Ptr points_above_plane (new PointIndices);
ExtractPolygonalPrismData<PointT> exppd;
exppd.setInputCloud (cloud);
exppd.setIndices (indices_but_the_plane);
exppd.setInputPlanarHull (plane_hull);
exppd.setViewPoint (cloud->points[picked_idx].x, cloud->points[picked_idx].y, cloud->points[picked_idx].z);
exppd.setHeightLimits (0.001, 0.5); // up to half a meter
exppd.segment (*points_above_plane);
vector<PointIndices> euclidean_label_indices;
// Prefer a faster method if the cloud is organized, over EuclidanClusterExtraction
if (cloud_->isOrganized ())
{
// Use an organized clustering segmentation to extract the individual clusters
typename EuclideanClusterComparator<PointT, Label>::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator<PointT, Label>);
euclidean_cluster_comparator->setInputCloud (cloud);
euclidean_cluster_comparator->setDistanceThreshold (0.03f, false);
// Set the entire scene to false, and the inliers of the objects located on top of the plane to true
Label l; l.label = 0;
PointCloud<Label>::Ptr scene (new PointCloud<Label> (cloud->width, cloud->height, l));
// Mask the objects that we want to split into clusters
for (const int &index : points_above_plane->indices)
scene->points[index].label = 1;
euclidean_cluster_comparator->setLabels (scene);
boost::shared_ptr<std::set<uint32_t> > exclude_labels = boost::make_shared<std::set<uint32_t> > ();
exclude_labels->insert (0);
euclidean_cluster_comparator->setExcludeLabels (exclude_labels);
OrganizedConnectedComponentSegmentation<PointT, Label> euclidean_segmentation (euclidean_cluster_comparator);
euclidean_segmentation.setInputCloud (cloud);
PointCloud<Label> euclidean_labels;
euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
}
else
{
print_highlight (stderr, "Extracting individual clusters from the points above the reference plane ");
TicToc tt; tt.tic ();
EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setSearchMethod (search_);
ec.setInputCloud (cloud);
ec.setIndices (points_above_plane);
ec.extract (euclidean_label_indices);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", euclidean_label_indices.size ()); print_info (" clusters]\n");
}
// For each cluster found
bool cluster_found = false;
for (const auto &euclidean_label_index : euclidean_label_indices)
{
if (cluster_found)
break;
// Check if the point that we picked belongs to it
for (size_t j = 0; j < euclidean_label_index.indices.size (); ++j)
{
if (picked_idx != euclidean_label_index.indices[j])
continue;
copyPointCloud (*cloud, euclidean_label_index.indices, object);
cluster_found = true;
break;
}
}
}
void
display ()
{
float* reference = new float[range_likelihood_->getRowHeight () * range_likelihood_->getColWidth ()];
const float* depth_buffer = range_likelihood_->getDepthBuffer ();
// Copy one image from our last as a reference.
for (int i = 0, n = 0; i < range_likelihood_->getRowHeight (); ++i)
{
for (int j = 0; j < range_likelihood_->getColWidth (); ++j)
{
reference[n++] = depth_buffer[ (i + range_likelihood_->getRowHeight () * range_likelihood_->getRows () / 2) * range_likelihood_->getWidth ()
+ j + range_likelihood_->getColWidth () * range_likelihood_->getCols () / 2];
}
}
float* reference_vis = new float[range_likelihood_visualization_->getRowHeight () * range_likelihood_visualization_->getColWidth ()];
const float* depth_buffer_vis = range_likelihood_visualization_->getDepthBuffer ();
// Copy one image from our last as a reference.
for (int i = 0, n = 0; i < range_likelihood_visualization_->getRowHeight (); ++i)
{
for (int j = 0; j < range_likelihood_visualization_->getColWidth (); ++j)
{
reference_vis[n++] = depth_buffer_vis[i*range_likelihood_visualization_->getWidth () + j];
}
}
std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
std::vector<float> scores;
// Render a single pose for visualization
poses.clear ();
poses.push_back (camera_->getPose ());
range_likelihood_visualization_->computeLikelihoods (reference_vis, poses, scores);
glDrawBuffer (GL_BACK);
glReadBuffer (GL_BACK);
// Draw the resulting images from the range_likelihood
glViewport (range_likelihood_visualization_->getWidth (), 0,
range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
glMatrixMode (GL_PROJECTION);
glLoadIdentity ();
glMatrixMode (GL_MODELVIEW);
glLoadIdentity ();
// Draw the color image
glColorMask (true, true, true, true);
glClearColor (0, 0, 0, 0);
glClearDepth (1);
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable (GL_DEPTH_TEST);
glRasterPos2i (-1,-1);
glDrawPixels (range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight (),
GL_RGB, GL_UNSIGNED_BYTE, range_likelihood_visualization_->getColorBuffer ());
// Draw the depth image
glViewport (0, 0, range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
glMatrixMode (GL_PROJECTION);
glLoadIdentity ();
glMatrixMode (GL_MODELVIEW);
glLoadIdentity ();
display_depth_image (range_likelihood_visualization_->getDepthBuffer (),
range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
poses.clear ();
for (int i = 0; i < range_likelihood_->getRows (); ++i)
{
for (int j = 0; j < range_likelihood_->getCols (); ++j)
{
Camera camera (*camera_);
camera.move ((j - range_likelihood_->getCols () / 2.0) * 0.1,
(i - range_likelihood_->getRows () / 2.0) * 0.1,
0.0);
poses.push_back (camera.getPose ());
}
}
std::cout << std::endl;
TicToc tt;
tt.tic();
range_likelihood_->computeLikelihoods (reference, poses, scores);
tt.toc();
tt.toc_print();
if (gllib::getGLError () != GL_NO_ERROR)
{
std::cerr << "GL Error: RangeLikelihood::computeLikelihoods: finished" << std::endl;
}
#if 0
std::cout << "score: ";
for (size_t i = 0; i < scores.size (); ++i)
{
std::cout << " " << scores[i];
}
std::cout << std::endl;
#endif
std::cout << "camera: " << camera_->getX ()
<< " " << camera_->getY ()
<< " " << camera_->getZ ()
<< " " << camera_->getRoll ()
<< " " << camera_->getPitch ()
<< " " << camera_->getYaw ()
<< std::endl;
delete [] reference_vis;
delete [] reference;
if (gllib::getGLError () != GL_NO_ERROR)
{
std::cerr << "GL Error: before buffers" << std::endl;
}
glBindFramebuffer (GL_FRAMEBUFFER, 0);
glDrawBuffer (GL_BACK);
glReadBuffer (GL_BACK);
if (gllib::getGLError () != GL_NO_ERROR)
{
std::cerr << "GL Error: after buffers" << std::endl;
}
glMatrixMode (GL_PROJECTION);
glLoadIdentity ();
glMatrixMode (GL_MODELVIEW);
glLoadIdentity ();
if (gllib::getGLError () != GL_NO_ERROR)
{
std::cerr << "GL Error: before viewport" << std::endl;
}
// Draw the score image for the particles
glViewport (0, range_likelihood_visualization_->getHeight (),
range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
if (gllib::getGLError () != GL_NO_ERROR)
{
std::cerr << "GL Error: after viewport" << std::endl;
}
display_score_image (range_likelihood_->getScoreBuffer ());
// Draw the depth image for the particles
glViewport (range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight (),
range_likelihood_visualization_->getWidth (), range_likelihood_visualization_->getHeight ());
display_score_image (range_likelihood_->getDepthBuffer ());
glutSwapBuffers ();
}
void
segment (const PointT &picked_point,
int picked_idx,
PlanarRegion<PointT> ®ion,
typename PointCloud<PointT>::Ptr &object)
{
object.reset ();
// Segment out all planes
vector<ModelCoefficients> model_coefficients;
vector<PointIndices> inlier_indices, boundary_indices;
vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > > regions;
// Prefer a faster method if the cloud is organized, over RANSAC
if (cloud_->isOrganized ())
{
print_highlight (stderr, "Estimating normals ");
TicToc tt; tt.tic ();
// Estimate normals
PointCloud<Normal>::Ptr normal_cloud (new PointCloud<Normal>);
estimateNormals (cloud_, *normal_cloud);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", normal_cloud->size ()); print_info (" points]\n");
OrganizedMultiPlaneSegmentation<PointT, Normal, Label> mps;
mps.setMinInliers (1000);
mps.setAngularThreshold (deg2rad (3.0)); // 3 degrees
mps.setDistanceThreshold (0.03); // 2 cm
mps.setMaximumCurvature (0.001); // a small curvature
mps.setProjectPoints (true);
mps.setComparator (plane_comparator_);
mps.setInputNormals (normal_cloud);
mps.setInputCloud (cloud_);
// Use one of the overloaded segmentAndRefine calls to get all the information that we want out
PointCloud<Label>::Ptr labels (new PointCloud<Label>);
vector<PointIndices> label_indices;
mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
}
else
{
SACSegmentation<PointT> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (SACMODEL_PLANE);
seg.setMethodType (SAC_RANSAC);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.005);
// Copy XYZ and Normals to a new cloud
typename PointCloud<PointT>::Ptr cloud_segmented (new PointCloud<PointT> (*cloud_));
typename PointCloud<PointT>::Ptr cloud_remaining (new PointCloud<PointT>);
ModelCoefficients coefficients;
ExtractIndices<PointT> extract;
PointIndices::Ptr inliers (new PointIndices ());
// Up until 30% of the original cloud is left
int i = 1;
while (double (cloud_segmented->size ()) > 0.3 * double (cloud_->size ()))
{
seg.setInputCloud (cloud_segmented);
print_highlight (stderr, "Searching for the largest plane (%2.0d) ", i++);
TicToc tt; tt.tic ();
seg.segment (*inliers, coefficients);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", inliers->indices.size ()); print_info (" points]\n");
// No datasets could be found anymore
if (inliers->indices.empty ())
break;
// Save this plane
PlanarRegion<PointT> region;
region.setCoefficients (coefficients);
regions.push_back (region);
inlier_indices.push_back (*inliers);
model_coefficients.push_back (coefficients);
// Extract the outliers
extract.setInputCloud (cloud_segmented);
extract.setIndices (inliers);
extract.setNegative (true);
extract.filter (*cloud_remaining);
cloud_segmented.swap (cloud_remaining);
}
}
print_highlight ("Number of planar regions detected: %lu for a cloud of %lu points\n", regions.size (), cloud_->size ());
double max_dist = numeric_limits<double>::max ();
// Compute the distances from all the planar regions to the picked point, and select the closest region
int idx = -1;
for (size_t i = 0; i < regions.size (); ++i)
{
double dist = pointToPlaneDistance (picked_point, regions[i].getCoefficients ());
if (dist < max_dist)
{
max_dist = dist;
idx = static_cast<int> (i);
}
}
// Get the plane that holds the object of interest
if (idx != -1)
{
plane_indices_.reset (new PointIndices (inlier_indices[idx]));
if (cloud_->isOrganized ())
{
approximatePolygon (regions[idx], region, 0.01f, false, true);
print_highlight ("Planar region: %lu points initial, %lu points after refinement.\n", regions[idx].getContour ().size (), region.getContour ().size ());
}
else
{
// Save the current region
region = regions[idx];
print_highlight (stderr, "Obtaining the boundary points for the region ");
TicToc tt; tt.tic ();
// Project the inliers to obtain a better hull
typename PointCloud<PointT>::Ptr cloud_projected (new PointCloud<PointT>);
ModelCoefficients::Ptr coefficients (new ModelCoefficients (model_coefficients[idx]));
ProjectInliers<PointT> proj;
proj.setModelType (SACMODEL_PLANE);
proj.setInputCloud (cloud_);
proj.setIndices (plane_indices_);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
// Compute the boundary points as a ConvexHull
ConvexHull<PointT> chull;
chull.setDimension (2);
chull.setInputCloud (cloud_projected);
PointCloud<PointT> plane_hull;
chull.reconstruct (plane_hull);
region.setContour (plane_hull);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", plane_hull.size ()); print_info (" points]\n");
}
}
// Segment the object of interest
if (plane_indices_ && !plane_indices_->indices.empty ())
{
plane_.reset (new PointCloud<PointT>);
copyPointCloud (*cloud_, plane_indices_->indices, *plane_);
object.reset (new PointCloud<PointT>);
segmentObject (picked_idx, cloud_, plane_indices_, *object);
}
}
int main (int argc, char** argv)
{
print_info ("Estimate occlusion using pcl::VoxelGridOcclusionEstimation. For more information, use: %s -h\n", argv[0]);
if (argc < 2)
{
printHelp (argc, argv);
return (-1);
}
// Command line parsing
float leaf_x = default_leaf_size,
leaf_y = default_leaf_size,
leaf_z = default_leaf_size;
std::vector<double> values;
parse_x_arguments (argc, argv, "-leaf", values);
if (values.size () == 1)
{
leaf_x = static_cast<float> (values[0]);
leaf_y = static_cast<float> (values[0]);
leaf_z = static_cast<float> (values[0]);
}
else if (values.size () == 3)
{
leaf_x = static_cast<float> (values[0]);
leaf_y = static_cast<float> (values[1]);
leaf_z = static_cast<float> (values[2]);
}
else
{
print_error ("Leaf size must be specified with either 1 or 3 numbers (%lu given).\n", values.size ());
}
print_info ("Using a leaf size of: "); print_value ("%f, %f, %f\n", leaf_x, leaf_y, leaf_z);
// input cloud
CloudT::Ptr input_cloud (new CloudT);
loadPCDFile (argv[1], *input_cloud);
// VTK actors
vtkSmartPointer<vtkActorCollection> coll = vtkActorCollection::New ();
// voxel grid
VoxelGridOcclusionEstimation<PointT> vg;
vg.setInputCloud (input_cloud);
vg.setLeafSize (leaf_x, leaf_y, leaf_z);
vg.initializeVoxelGrid ();
Eigen::Vector3f b_min, b_max;
b_min = vg.getMinBoundCoordinates ();
b_max = vg.getMaxBoundCoordinates ();
TicToc tt;
print_highlight ("Ray-Traversal ");
tt.tic ();
// estimate the occluded space
std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > occluded_voxels;
vg.occlusionEstimationAll (occluded_voxels);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", (int)occluded_voxels.size ()); print_info (" occluded voxels]\n");
CloudT::Ptr occ_centroids (new CloudT);
occ_centroids->width = static_cast<int> (occluded_voxels.size ());
occ_centroids->height = 1;
occ_centroids->is_dense = false;
occ_centroids->points.resize (occluded_voxels.size ());
for (size_t i = 0; i < occluded_voxels.size (); ++i)
{
Eigen::Vector4f xyz = vg.getCentroidCoordinate (occluded_voxels[i]);
PointT point;
point.x = xyz[0];
point.y = xyz[1];
point.z = xyz[2];
occ_centroids->points[i] = point;
}
CloudT::Ptr cloud_centroids (new CloudT);
cloud_centroids->width = static_cast<int> (input_cloud->points.size ());
cloud_centroids->height = 1;
cloud_centroids->is_dense = false;
cloud_centroids->points.resize (input_cloud->points.size ());
for (size_t i = 0; i < input_cloud->points.size (); ++i)
{
float x = input_cloud->points[i].x;
float y = input_cloud->points[i].y;
float z = input_cloud->points[i].z;
Eigen::Vector3i c = vg.getGridCoordinates (x, y, z);
Eigen::Vector4f xyz = vg.getCentroidCoordinate (c);
PointT point;
point.x = xyz[0];
point.y = xyz[1];
point.z = xyz[2];
cloud_centroids->points[i] = point;
}
// visualization
Eigen::Vector3f red (1.0, 0.0, 0.0);
Eigen::Vector3f blue (0.0, 0.0, 1.0);
// draw point cloud voxels
getVoxelActors (*cloud_centroids, leaf_x, red, coll);
// draw the bounding box of the voxel grid
displayBoundingBox (b_min, b_max, coll);
// draw the occluded voxels
getVoxelActors (*occ_centroids, leaf_x, blue, coll);
// Create the RenderWindow, Renderer and RenderWindowInteractor
vtkSmartPointer<vtkRenderer> renderer = vtkSmartPointer<vtkRenderer>::New();
vtkSmartPointer<vtkRenderWindow> renderWindow =
vtkSmartPointer<vtkRenderWindow>::New();
renderWindow->AddRenderer (renderer);
vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor =
vtkSmartPointer<vtkRenderWindowInteractor>::New();
renderWindowInteractor->SetRenderWindow (renderWindow);
// Add the actors to the renderer, set the background and size
renderer->SetBackground (1.0, 1.0, 1.0);
renderWindow->SetSize (640, 480);
vtkActor* a;
coll->InitTraversal ();
a = coll->GetNextActor ();
while(a!=0)
{
renderer->AddActor (a);
a = coll->GetNextActor ();
}
renderWindow->Render ();
renderWindowInteractor->Start ();
return 0;
}
