pcl::PointCloud<PointT>::Ptr cropCloud(const pcl::PointCloud<PointT>::Ptr cloud, const pcl::ModelCoefficients::Ptr planeCoef, float elev)
{
pcl::PointCloud<PointT>::Ptr cloud_f(new pcl::PointCloud<PointT>());
pcl::copyPointCloud(*cloud, *cloud_f);
pcl::ProjectInliers<PointT> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setInputCloud (cloud_f);
proj.setModelCoefficients (planeCoef);
pcl::PointCloud<PointT>::Ptr cloud_projected(new pcl::PointCloud<PointT>());
proj.filter (*cloud_projected);
for(size_t i = 0 ; i < cloud_f->size() ; i++ )
{
if( pcl_isfinite(cloud_f->at(i).z) == true )
{
float diffx = cloud_f->at(i).x-cloud_projected->at(i).x;
float diffy = cloud_f->at(i).y-cloud_projected->at(i).y;
float diffz = cloud_f->at(i).z-cloud_projected->at(i).z;
float dist = sqrt( diffx*diffx + diffy*diffy + diffz*diffz);
if ( dist <= elev )
{
cloud_f->at(i).x = NAN;
cloud_f->at(i).y = NAN;
cloud_f->at(i).z = NAN;
}
}
}
cloud_f->is_dense = false;
return cloud_f;
}
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>),
cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>),
cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCDReader reader;
reader.read ("table_scene_mug_stereo_textured.pcd", *cloud);
// Build a filter to remove spurious NaNs
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 1.1);
pass.filter (*cloud_filtered);
std::cerr << "PointCloud after filtering has: "
<< cloud_filtered->points.size () << " data points." << std::endl;
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
std::cerr << "PointCloud after segmentation has: "
<< inliers->indices.size () << " inliers." << std::endl;
// Project the model inliers
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setIndices (inliers);
proj.setInputCloud (cloud_filtered);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
std::cerr << "PointCloud after projection has: "
<< cloud_projected->points.size () << " data points." << std::endl;
// Create a Concave Hull representation of the projected inliers
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConcaveHull<pcl::PointXYZ> chull;
chull.setInputCloud (cloud_projected);
chull.setAlpha (0.1);
chull.reconstruct (*cloud_hull);
std::cerr << "Concave hull has: " << cloud_hull->points.size ()
<< " data points." << std::endl;
pcl::PCDWriter writer;
writer.write ("table_scene_mug_stereo_textured_hull.pcd", *cloud_hull, false);
return (0);
}
void DynModelExporter2::createMarkerForConvexHull(pcl::PointCloud<pcl::PointXYZ>& plane_cloud, pcl::ModelCoefficients::Ptr& plane_coefficients, visualization_msgs::Marker& marker)
{
// init marker
marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
marker.action = visualization_msgs::Marker::ADD;
// project the points of the plane on the plane
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setInputCloud (plane_cloud.makeShared());
proj.setModelCoefficients (plane_coefficients);
proj.filter(*cloud_projected);
// create the convex hull in the plane
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::ConvexHull<pcl::PointXYZ > chull;
chull.setInputCloud (cloud_projected);
chull.reconstruct(*cloud_hull);
// work around known bug in ROS Diamondback perception_pcl: convex hull is centered around centroid of input cloud (fixed in pcl svn revision 443)
// thus: we shift the mean of cloud_hull to the mean of cloud_projected (fill dx, dy, dz and apply when creating the marker points)
Eigen::Vector4f meanPointCH, meanPointCP;
pcl::compute3DCentroid(*cloud_projected, meanPointCP);
pcl::compute3DCentroid(*cloud_hull, meanPointCH);
//float dx = 0;//meanPointCP[0]-meanPointCH[0];
//float dy = 0;//meanPointCP[1]-meanPointCH[1];
//float dz = 0;//meanPointCP[2]-meanPointCH[2];
// create colored part of plane by creating marker for each triangle between neighbored points on contour of convex hull an midpoint
marker.points.clear();
for (unsigned int j = 0; j < cloud_hull->points.size(); ++j)
{
geometry_msgs::Point p;
p.x = cloud_hull->points[j].x; p.y = cloud_hull->points[j].y; p.z = cloud_hull->points[j].z;
marker.points.push_back( p );
p.x = cloud_hull->points[(j+1)%cloud_hull->points.size() ].x; p.y = cloud_hull->points[(j+1)%cloud_hull->points.size()].y; p.z = cloud_hull->points[(j+1)%cloud_hull->points.size()].z;
marker.points.push_back( p );
p.x = meanPointCP[0]; p.y = meanPointCP[1]; p.z = meanPointCP[2];
marker.points.push_back( p );
}
// scale of the marker
marker.scale.x = 1;
marker.scale.y = 1;
marker.scale.z = 1;
}
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the cloud data
cloud->width = 5;
cloud->height = 1;
cloud->points.resize (cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size (); ++i)
{
cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
}
std::cerr << "Cloud before projection: " << std::endl;
for (size_t i = 0; i < cloud->points.size (); ++i)
std::cerr << " " << cloud->points[i].x << " "
<< cloud->points[i].y << " "
<< cloud->points[i].z << std::endl;
// Create a set of planar coefficients with X=Y=0,Z=1
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
coefficients->values.resize (4);
coefficients->values[0] = coefficients->values[1] = 0;
coefficients->values[2] = 1.0;
coefficients->values[3] = 0;
// Create the filtering object
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setInputCloud (cloud);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
std::cerr << "Cloud after projection: " << std::endl;
for (size_t i = 0; i < cloud_projected->points.size (); ++i)
std::cerr << " " << cloud_projected->points[i].x << " "
<< cloud_projected->points[i].y << " "
<< cloud_projected->points[i].z << std::endl;
return (0);
}
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// Do data processing here...
// run pass through filter to shrink point cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_passthrough(new pcl::PointCloud<pcl::PointXYZRGB>);
//passthrough_z(input, cloud_passthrough);
passthrough_y(input,cloud_passthrough);
//passthrough_x(cloud_passthrough);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_outlier(new pcl::PointCloud<pcl::PointXYZRGB>);
passthrough_oulier(input,cloud_outlier);
pub_out.publish(*cloud_outlier);
// run ransac to find floor
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZRGB>);
ransac(cloud_passthrough, cloud_projected);
pub.publish(*cloud_projected);
}
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr calc_convex_hull(pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud, pcl::PointIndices::Ptr inliers, pcl::ModelCoefficients::Ptr coefficients){
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZRGBA>),
cloud_projected (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::ConvexHull<pcl::PointXYZRGBA> chull;
// Project the model inliers
pcl::ProjectInliers<pcl::PointXYZRGBA> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setIndices (inliers);
proj.setInputCloud (cloud);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
// Create a Convex Hull representation of the projected inliers
chull.setInputCloud (cloud_projected);
//chull.setAlpha (0.1);
chull.reconstruct (*cloud_hull);
return cloud_hull;
}
PlaneExt::tVertices PlaneExt::ComputeConcaveHull(pcl::PointCloud<pcl::PointXYZ>::Ptr &plane_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &plane_hull)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ> ());
// project all points onto current plane
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setInputCloud (plane_cloud);
proj.setModelCoefficients (planeCoefficients);
proj.filter(*cloud_projected);
// create the concave hull of the plane
pcl::ConcaveHull<pcl::PointXYZ > chull;
chull.setInputCloud (cloud_projected);
chull.setAlpha(0.05);
tVertices polys;
chull.reconstruct(*plane_hull, polys);
return polys;
}
PointI
Cylinder::projectToAxis(const PointI& p)
{
pcl::ModelCoefficients::Ptr coefficients_line (new pcl::ModelCoefficients);
coefficients_line->values.push_back(values[0]);
coefficients_line->values.push_back(values[1]);
coefficients_line->values.push_back(values[2]);
coefficients_line->values.push_back(values[3]);
coefficients_line->values.push_back(values[4]);
coefficients_line->values.push_back(values[5]);
pcl::PointCloud<PointI>::Ptr cloud_projected (new pcl::PointCloud<PointI>);
pcl::PointCloud<PointI>::Ptr cloud_toProject (new pcl::PointCloud<PointI>);
cloud_toProject->push_back (p);
pcl::ProjectInliers<PointI> proj;
proj.setModelType (pcl::SACMODEL_LINE);
proj.setInputCloud (cloud_toProject);
proj.setModelCoefficients (coefficients_line);
proj.filter (*cloud_projected);
return cloud_projected->points[0];
}
int HeightAboveGroundKernel::execute()
{
// we require separate contexts for the input and ground files
PointContextRef input_ctx;
PointContextRef ground_ctx;
// because we are appending HeightAboveGround to the input buffer, we must
// register it's Dimension
input_ctx.registerDim(Dimension::Id::HeightAboveGround);
// StageFactory will be used to create required stages
StageFactory f;
// setup the reader, inferring driver type from the filename
std::string reader_driver = f.inferReaderDriver(m_input_file);
std::unique_ptr<Reader> input(f.createReader(reader_driver));
Options readerOptions;
readerOptions.add("filename", m_input_file);
input->setOptions(readerOptions);
// go ahead and execute to get the PointBuffer
input->prepare(input_ctx);
PointBufferSet pbSetInput = input->execute(input_ctx);
PointBufferPtr input_buf = *pbSetInput.begin();
PointBufferSet pbSetGround;
PointBufferPtr ground_buf;
if (m_use_classification)
{
// the user has indicated that the classification dimension exists, so
// we will find all ground returns
Option source("source",
"import numpy as np\n"
"def yow1(ins,outs):\n"
" cls = ins['Classification']\n"
" keep_classes = [2]\n"
" keep = np.equal(cls, keep_classes[0])\n"
" outs['Mask'] = keep\n"
" return True\n"
);
Option module("module", "MyModule");
Option function("function", "yow1");
Options opts;
opts.add(source);
opts.add(module);
opts.add(function);
// and create a PointBuffer of only ground returns
std::unique_ptr<Filter> pred(f.createFilter("filters.predicate"));
pred->setOptions(opts);
pred->setInput(input.get());
pred->prepare(ground_ctx);
pbSetGround = pred->execute(ground_ctx);
ground_buf = *pbSetGround.begin();
}
else
{
// the user has provided a file containing only ground returns, setup
// the reader, inferring driver type from the filename
std::string ground_driver = f.inferReaderDriver(m_ground_file);
std::unique_ptr<Reader> ground(f.createReader(ground_driver));
Options ro;
ro.add("filename", m_ground_file);
ground->setOptions(ro);
// go ahead and execute to get the PointBuffer
ground->prepare(ground_ctx);
pbSetGround = ground->execute(ground_ctx);
ground_buf = *pbSetGround.begin();
}
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> Cloud;
typedef Cloud::Ptr CloudPtr;
// convert the input PointBuffer to a PointCloud
CloudPtr cloud(new Cloud);
BOX3D const& bounds = input_buf->calculateBounds();
pclsupport::PDALtoPCD(*input_buf, *cloud, bounds);
// convert the ground PointBuffer to a PointCloud
CloudPtr cloud_g(new Cloud);
// here, we offset the ground cloud by the input bounds so that the two are aligned
pclsupport::PDALtoPCD(*ground_buf, *cloud_g, bounds);
// create a set of planar coefficients with X=Y=0,Z=1
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
coefficients->values.resize(4);
coefficients->values[0] = coefficients->values[1] = 0;
coefficients->values[2] = 1.0;
coefficients->values[3] = 0;
// create the filtering object and project ground returns into xy plane
pcl::ProjectInliers<PointT> proj;
proj.setModelType(pcl::SACMODEL_PLANE);
proj.setInputCloud(cloud_g);
proj.setModelCoefficients(coefficients);
CloudPtr cloud_projected(new Cloud);
proj.filter(*cloud_projected);
// setup the KdTree
pcl::KdTreeFLANN<PointT> tree;
tree.setInputCloud(cloud_projected);
// loop over all points in the input cloud, finding the nearest neighbor in
// the ground returns (XY plane only), and calculating the difference in z
int32_t k = 1;
for (size_t idx = 0; idx < cloud->points.size(); ++idx)
{
// Search for nearesrt neighbor of the query point
std::vector<int32_t> neighbors(k);
std::vector<float> distances(k);
PointT temp_pt = cloud->points[idx];
temp_pt.z = 0.0f;
int num_neighbors = tree.nearestKSearch(temp_pt, k, neighbors, distances);
double hag = cloud->points[idx].z - cloud_g->points[neighbors[0]].z;
input_buf->setField(Dimension::Id::HeightAboveGround, idx, hag);
}
// populate BufferReader with the input PointBuffer, which now has the
// HeightAboveGround dimension
BufferReader bufferReader;
bufferReader.addBuffer(input_buf);
// we require that the output be BPF for now, to house our non-standard
// dimension
Options wo;
wo.add("filename", m_output_file);
std::unique_ptr<Writer> writer(f.createWriter("writers.bpf"));
writer->setOptions(wo);
writer->setInput(&bufferReader);
writer->prepare(input_ctx);
writer->execute(input_ctx);
return 0;
}
void FilterPlanes::filterPlanes(vector<BasicPlane> &plane_stack){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>);
if (verbose_text>0){
cout << "PointCloud before filtering: " << cloud->width * cloud->height << " data points." << std::endl;
}
#if DO_TIMING_PROFILE
vector<int64_t> tic_toc;
tic_toc.push_back(bot_timestamp_now());
#endif
/* Ptcoll_cfg ptcoll_cfg;
ptcoll_cfg.point_lists_id =pose_element_id; //bot_timestamp_now();
ptcoll_cfg.collection = pose_coll_id;
ptcoll_cfg.element_id = pose_element_id; */
//1. Downsample the dataset using a leaf size of 1cm
// this is 99% of the cpu time
pcl::VoxelGrid<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud);
// for table dataset: sor.setLeafSize (0.01, 0.01, 0.01);
sor.setLeafSize (0.05, 0.05, 0.05);
// sor.setLeafSize (0.1, 0.1, 0.1);
sor.filter (*cloud_filtered);
if (verbose_text>0){
cout << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
}
#if DO_TIMING_PROFILE
tic_toc.push_back(bot_timestamp_now());
#endif
if (verbose_lcm>2){
/* ptcoll_cfg.id = 200;
ptcoll_cfg.reset=true;
ptcoll_cfg.name ="cloud_downsampled";
ptcoll_cfg.npoints = cloud_filtered->points.size();
float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0};
ptcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float));
ptcoll_cfg.type =1;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg, *cloud_filtered);*/
}
// 2. Set up the Ransac Plane Fitting Object:
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100); // was 4000
seg.setDistanceThreshold (distance_threshold_); // 0.01 for table data set
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
#if DO_TIMING_PROFILE
tic_toc.push_back(bot_timestamp_now());
#endif
int n_major = 0, nr_points = cloud_filtered->points.size ();
//vector<BasicPlaneX> plane_stack;
BasicPlane one_plane;
// Extract the primary planes:
// major planes are the coarse planes extracted via ransac. they might contain disconnected points
// these are broken up into minor planes which are portions of major planes
if(verbose_lcm > 2){
for (int i=0;i<7;i++){
char n_major_char[10];
sprintf(n_major_char,"%d",i);
/* ptcoll_cfg.id =210+ i+3;
ptcoll_cfg.reset=true;
ptcoll_cfg.name =(char*) "cloud_p ";
ptcoll_cfg.name.append(n_major_char);
ptcoll_cfg.npoints = 0;
float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0};
ptcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float));
ptcoll_cfg.type=1;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg, *cloud_filtered);
*/
}
for (int i=0;i<7;i++){
/*
Ptcoll_cfg ptcoll_cfg2;
// the i below accounts for super planes in the same utime
ptcoll_cfg2.point_lists_id =pose_element_id; //bot_timestamp_now();
ptcoll_cfg2.collection = pose_coll_id;
ptcoll_cfg2.element_id = pose_element_id;
if (i==0){
ptcoll_cfg2.reset=true;
}else{
ptcoll_cfg2.reset=false;
}
ptcoll_cfg2.id =500;
ptcoll_cfg2.name.assign("Grown Stack Final");
ptcoll_cfg2.npoints = 0;
ptcoll_cfg2.type =1;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg2, *cloud_filtered);
*/
}
// TODO: this will rest this object. need to add publish of reset
// at start of this block and set rese ttofal
for (int i=0;i<7;i++){
/*
Ptcoll_cfg ptcoll_cfg2;
// the i below accounts for super planes in the same utime
ptcoll_cfg2.point_lists_id =pose_element_id; //bot_timestamp_now();
ptcoll_cfg2.collection = pose_coll_id;
ptcoll_cfg2.element_id = pose_element_id;
if (i==0){
ptcoll_cfg2.reset=true;
}else{
ptcoll_cfg2.reset=false;
}
ptcoll_cfg2.id =501;
ptcoll_cfg2.name.assign("Grown Stack Final Hull");
ptcoll_cfg2.npoints = 0;
ptcoll_cfg2.type =3;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg2, *cloud_filtered);
*/
}
}
#if DO_TIMING_PROFILE
tic_toc.push_back(bot_timestamp_now());
#endif
while (cloud_filtered->points.size () > stop_proportion_ * nr_points) {
// While XX% of the original cloud is still there
char n_major_char[10];
sprintf(n_major_char,"%d",n_major);
if (n_major >6) {
if (verbose_text >0){
std::cout << n_major << " is the max number of planes to find, quitting" << std::endl;
}
break;
}
//a Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
if (inliers->indices.size () < stop_cloud_size_) // stop when the plane is only a few points
{
//std::cerr << "No remaining planes in this set" << std::endl;
break;
}
//b Extract the inliers
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
if (verbose_text>0){
std::cout << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;
}
// Create the filtering object
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud_p);
sor.setMeanK (30);
sor.setStddevMulThresh (0.5); //was 1.0
sor.filter (*cloud_p);
if(verbose_lcm > 2){
/*
ptcoll_cfg.id =210+ n_major+3;
ptcoll_cfg.reset=true;
ptcoll_cfg.name ="cloud_p ";
ptcoll_cfg.name.append(n_major_char);
ptcoll_cfg.npoints = cloud_p->points.size();
float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0};
ptcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float));
ptcoll_cfg.type=1;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg, *cloud_p);
*/
}
vector<BasicPlane> grown_stack;
vector<BasicPlane> * grown_stack_ptr;
grown_stack_ptr = &grown_stack;
GrowCloud grow;
grow.setInputCloud(cloud_p);
grow.setMinCloudSize(stop_cloud_size_); // useing stop cloud size here too
grow.setLCM(publish_lcm);
grow.doGrowCloud(*grown_stack_ptr);
if (verbose_text>0){
cout << "grow_cloud new found " << grown_stack.size() << " seperate clouds\n";
}
// Spit raw clouds out to LCM:
if(verbose_lcm > 2){
for (int i=0;i<grown_stack.size();i++){
/*
Ptcoll_cfg ptcoll_cfg2;
ptcoll_cfg2.reset=false;
// the i below accounts for super planes in the same utime
ptcoll_cfg2.point_lists_id =10*n_major + i; //filterplanes->pose_element_id;
ptcoll_cfg2.collection = pose_coll_id;
ptcoll_cfg2.element_id = pose_element_id;
ptcoll_cfg2.id =500;
ptcoll_cfg2.name.assign("Grown Stack Final");
ptcoll_cfg2.npoints = grown_stack[i].cloud.points.size ();
ptcoll_cfg2.type =1;
int id = plane_stack.size() + i; // 10*n_major + i;
float colorm_temp0[] ={colors[3*(id%num_colors)],colors[3*(id%num_colors)+1],colors[3*(id%num_colors)+2] ,0.0};
ptcoll_cfg2.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float));
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg2, grown_stack[i].cloud);
*/
}
}
BasicPlane one_plane;
int n_minor=0;
BOOST_FOREACH( BasicPlane one_plane, grown_stack ){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZRGB>);
// c Project the model inliers (seems to be necessary to fitting convex hull
pcl::ProjectInliers<pcl::PointXYZRGB> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp (new pcl::PointCloud<pcl::PointXYZRGB> ());
*temp = (one_plane.cloud);
proj.setInputCloud (temp);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
std::vector <pcl::Vertices> vertices;
if (1==1){ // convex:
pcl::ConvexHull<pcl::PointXYZRGB> chull;
chull.setInputCloud (cloud_projected);
chull.setDimension(2);
chull.reconstruct (*cloud_hull,vertices);
}else { // concave
pcl::ConcaveHull<pcl::PointXYZRGB> chull;
chull.setInputCloud (cloud_projected);
chull.setKeepInformation(true);
chull.setAlpha(0.5);
// for arch way:
// 1.1 too few
// 0.7 a little to few but much better
chull.reconstruct (*cloud_hull,vertices);
}
//std::cout << "Hull has: " << cloud_hull->points.size () << " vertices." << std::endl;
if (cloud_hull->points.size () ==0){
cout <<"ERROR: CONVEX HULL HAS NO POINTS! - NEED TO RESOLVE THIS\n";
}
// d.2 Find the mean colour of the hull:
int rgba[]={0,0,0,0};
for(int i=0;i<temp->points.size();i++){
int rgba_one = *reinterpret_cast<int*>(&temp->points[i].rgba);
rgba[3] += ((rgba_one >> 24) & 0xff);
rgba[2] += ((rgba_one >> 16) & 0xff);
rgba[1] += ((rgba_one >> 8) & 0xff);
rgba[0] += (rgba_one & 0xff);
}
double scale = ((double) temp->points.size());
rgba[3] =(int) round(((double) rgba[3]) / scale);
rgba[2] =(int) round(((double) rgba[2]) / scale);
rgba[1] =(int) round(((double) rgba[1]) / scale);
rgba[0] =(int) round(((double) rgba[0]) / scale);
// Write the plane to file for experimentation:
//stringstream oss;
//oss << ptcoll_cfg.element_id <<"_" << ptcoll_cfg.name << ".pcd";
//writer.write (oss.str(), *this_hull, false);
for(int i=0;i<cloud_hull->points.size();i++){
unsigned char* rgba_ptr = (unsigned char*)&cloud_hull->points[i].rgba;
(*rgba_ptr) = rgba[0] ;
(*(rgba_ptr+1)) = rgba[1];
(*(rgba_ptr+2)) = rgba[2];
(*(rgba_ptr+3)) = rgba[3];
}
(one_plane.coeffs) = *coefficients;
one_plane.cloud = *cloud_hull;
one_plane.utime = pose_element_id;
one_plane.major = n_major;
one_plane.minor = n_minor;
one_plane.n_source_points = cloud_projected->points.size();
plane_stack.push_back(one_plane);
n_minor++;
// int stop;
// cout << "int to cont: ";
// cin >> stop;
}
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_filtered);
n_major++;
}
//input should be point cloud that is amplitude filetered, statistical outlier filtered, voxel filtered, coordinate system should be /map
void
PlaneExtraction::extractPlanes(const pcl::PointCloud<Point>::ConstPtr& pc_in,
std::vector<pcl::PointCloud<Point>, Eigen::aligned_allocator<pcl::PointCloud<Point> > >& v_cloud_hull,
std::vector<std::vector<pcl::Vertices> >& v_hull_polygons,
std::vector<pcl::ModelCoefficients>& v_coefficients_plane)
{
static int ctr=0;
static double time=0;
PrecisionStopWatch t;
t.precisionStart();
std::stringstream ss;
ROS_DEBUG("Extract planes");
ROS_DEBUG("Saving files: %d", save_to_file_);
if(save_to_file_)
{
ss.str("");
ss.clear();
ss << file_path_ << "/planes/pc_" << ctr_ << ".pcd";
pcl::io::savePCDFileASCII (ss.str(), *pc_in);
}
//ROS_INFO("pc_in size: %d" , pc_in->size());
// Extract Eucledian clusters
std::vector<pcl::PointIndices> clusters;
cluster_.setInputCloud (pc_in);
cluster_.extract (clusters);
//extractClusters (pc_in, clusters);
ROS_DEBUG ("Number of clusters found: %d", (int)clusters.size ());
//ROS_INFO("Clustering took %f s", t.precisionStop());
//t.precisionStart();
// Estimate point normals
//normal_estimator_.setInputCloud(pc_in);
//normalEstimator.setNumberOfThreads(4);
//pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal> ());
//normal_estimator_.compute(*cloud_normals);
//ROS_INFO("Normal estimation took %f s", t.precisionStop());
//t.precisionStart();
seg_.setInputCloud (pc_in);
//seg_.setInputNormals (cloud_normals);
proj_.setInputCloud (pc_in);
// Go through all clusters and search for planes
extracted_planes_indices_.clear();
for(unsigned int i = 0; i < clusters.size(); ++i)
{
ROS_DEBUG("Processing cluster no. %u", i);
// Extract cluster points
/*pcl::PointCloud<Point> cluster;
extract_.setInputCloud (pc_in);
extract_.setIndices (boost::make_shared<const pcl::PointIndices> (clusters[i]));
extract_.setNegative (false);
extract_.filter (cluster);
ROS_INFO("Extraction1 took %f s", t.precisionStop());
t.precisionStart();*/
/*pcl::PointCloud<Point>::Ptr cluster_ptr = cluster.makeShared();
if(save_to_file_)
{
ss.str("");
ss.clear();
ss << file_path_ << "/planes/cluster_" << ctr_ << ".pcd";
pcl::io::savePCDFileASCII (ss.str(), cluster);
}*/
// Find plane
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ());
//pcl::PointIndices::Ptr clusters_ptr(&clusters[i]);
//pcl::PointIndices::Ptr consider_in_cluster(new pcl::PointIndices ());
//for(unsigned int idx_ctr=0; idx_ctr < clusters[i].indices.size(); idx_ctr++)
// consider_in_cluster->indices.push_back(clusters[i].indices[idx_ctr]);
int ctr = 0;
// iterate over cluster to find all planes until cluster is too small
while(clusters[i].indices.size() > min_plane_size_)
{
//ROS_INFO("Cluster size: %d", (int)consider_in_cluster->indices.size());
seg_.setIndices(boost::make_shared<const pcl::PointIndices> (clusters[i]));
// Obtain the plane inliers and coefficients
pcl::ModelCoefficients coefficients_plane;
seg_.segment (*inliers_plane, coefficients_plane);
// Evaluate plane
if (coefficients_plane.values.size () <=3)
{
//ROS_INFO("Failed to detect plane in scan, skipping cluster");
break;
}
//TODO: parameter
if ( inliers_plane->indices.size() < min_plane_size_)
{
//std::cout << "Plane coefficients: " << *coefficients_plane << std::endl;
//ROS_INFO("Plane detection has %d inliers, below min threshold of %d, skipping cluster", (int)inliers_plane->indices.size(), 150);
break;
}
bool validPlane = false;
validPlane = isValidPlane (coefficients_plane);
//std::cout << " Plane valid = " << validPlane << std::endl;
if(validPlane)
{
// Extract plane points, only needed for storing bag file
ROS_DEBUG("Plane has %d inliers", (int)inliers_plane->indices.size());
/*pcl::PointCloud<Point> dominant_plane;
extract_.setInputCloud(pc_in);
extract_.setIndices(inliers_plane);
extract_.filter(dominant_plane);
if(save_to_file_)
{
ss.str("");
ss.clear();
ss << file_path_ << "/planes/plane_" << ctr_ << "_" << ctr << ".pcd";
pcl::io::savePCDFileASCII (ss.str(), dominant_plane);
}*/
// Project the model inliers
pcl::PointCloud<Point>::Ptr cloud_projected (new pcl::PointCloud<Point>);
proj_.setModelCoefficients (boost::make_shared<pcl::ModelCoefficients>(coefficients_plane));
proj_.setIndices(inliers_plane);
proj_.filter (*cloud_projected);
if(save_to_file_)
{
ss.str("");
ss.clear();
ss << file_path_ << "/planes/plane_pr_" << ctr_ << "_" << ctr << ".pcd";
pcl::io::savePCDFileASCII (ss.str(), *cloud_projected);
}
std::vector<pcl::PointIndices> plane_clusters;
cluster_plane_.setInputCloud (cloud_projected);
cluster_plane_.extract (plane_clusters);
extract_.setInputCloud(cloud_projected);
/*std::cout << "projected: " << cloud_projected->size() << std::endl;
std::cout << "inliers_plane: " << inliers_plane->indices.size() << std::endl;*/
for(unsigned int j=0; j<plane_clusters.size(); j++)
{
pcl::PointCloud<Point> plane_cluster;
extract_.setIndices(boost::make_shared<const pcl::PointIndices> (plane_clusters[j]));
extract_.filter(plane_cluster);
pcl::PointCloud<Point>::Ptr plane_cluster_ptr = plane_cluster.makeShared();
if(plane_cluster_ptr->size() < min_plane_size_) continue;
//else std::cout << "plane cluster has " << plane_cluster_ptr->size() << " points" << std::endl;
// <evaluation_stuff>
extracted_planes_indices_.push_back(std::vector<int>());
//std::cout << "plane_cluster: " << plane_clusters[j].indices.size() << std::endl;
for (size_t idx = 0; idx < plane_clusters[j].indices.size(); ++idx)
{
//std::cout << plane_clusters[j].indices[idx] << " ";
extracted_planes_indices_.back().push_back(inliers_plane->indices[ plane_clusters[j].indices[idx] ]);
}
// </evaluation_stuff>
// Create a Concave Hull representation of the projected inliers
pcl::PointCloud<Point> cloud_hull;
std::vector< pcl::Vertices > hull_polygons;
chull_.setInputCloud (plane_cluster_ptr);
//TODO: parameter
chull_.reconstruct (cloud_hull, hull_polygons);
/*std::cout << "Hull: " << cloud_hull.size() << ", " << hull_polygons[0].vertices.size()
<< ", "<< plane_cluster_ptr->size() << std::endl;*/
if(hull_polygons.size() > 1)
{
extracted_planes_indices_.pop_back();
continue;
ROS_WARN("Extracted Polygon %d contours, separating ...", hull_polygons.size());
pcl::PointCloud<Point>::Ptr cloud_hull_ptr = cloud_hull.makeShared();
pcl::ExtractIndices<Point> extract_2;
extract_2.setInputCloud(cloud_hull_ptr);
for( unsigned int z=0; z<hull_polygons.size(); z++)
{
//ROS_WARN("\tC%d size: %d", z,hull_polygons[z].vertices.size());
pcl::PointCloud<Point> cloud_hull_2;
std::vector< pcl::Vertices > hull_polygons_2;
pcl::PointIndices hull_poly_indices;
for (unsigned int x=0; x<hull_polygons[z].vertices.size(); x++)
hull_poly_indices.indices.push_back(hull_polygons[z].vertices[x]);
//ROS_INFO("Size indices: %d", hull_poly_indices.indices.size());
extract_2.setIndices(boost::make_shared<const pcl::PointIndices> (hull_poly_indices));
extract_2.filter(cloud_hull_2);
//ROS_INFO("Hull 2 size: %d", cloud_hull_2.size());
pcl::Vertices verts;
for(unsigned int y=0; y<cloud_hull_2.size(); y++)
verts.vertices.push_back(y);
verts.vertices.push_back(0);
//ROS_INFO("Verts size: %d", verts.vertices.size());
hull_polygons_2.push_back(verts);
v_cloud_hull.push_back(cloud_hull_2);
v_hull_polygons.push_back(hull_polygons_2);
v_coefficients_plane.push_back(coefficients_plane);
}
}
else
{
//ROS_INFO("Hull size: %d", cloud_hull.size());
//ROS_INFO("Verts size: %d", hull_polygons[0].vertices.size());
v_cloud_hull.push_back(cloud_hull);
v_hull_polygons.push_back(hull_polygons);
v_coefficients_plane.push_back(coefficients_plane);
}
ROS_DEBUG("v_cloud_hull size: %d", (unsigned int)v_cloud_hull.size());
if(save_to_file_)
{
saveHulls(cloud_hull, hull_polygons, ctr);
}
ctr++;
}
}
// Remove plane inliers from indices vector
for(unsigned int idx_ctr1=0; idx_ctr1 < clusters[i].indices.size(); idx_ctr1++)
{
for(unsigned int idx_ctr2=0; idx_ctr2 < inliers_plane->indices.size(); idx_ctr2++)
{
if(clusters[i].indices[idx_ctr1] == inliers_plane->indices[idx_ctr2])
clusters[i].indices.erase(clusters[i].indices.begin()+idx_ctr1);
}
}
//ctr_++;
}
//ROS_INFO("Plane estimation took %f s", t.precisionStop());
//t.precisionStart();
/*if(clusters[i].indices.size() >0 )
{
if(save_to_file_)
{
ss.str("");
ss.clear();
ss << file_path_ << "/planes/rem_pts_" << ctr_ << "_" << ctr << ".pcd";
if(consider_in_cluster->indices.size() == cluster_ptr->size())
pcl::io::savePCDFileASCII (ss.str(), *cluster_ptr);
else
{
pcl::PointCloud<Point> remaining_pts;
extract_.setInputCloud (cluster_ptr);
extract_.setIndices (consider_in_cluster);
extract_.filter (remaining_pts);
pcl::io::savePCDFileASCII (ss.str(), remaining_pts);
}
}
}*/
ctr_++;
}
double step_time = t.precisionStop();
//ROS_INFO("Plane extraction took %f", step_time);
time += step_time;
//ROS_INFO("[plane extraction] Accumulated time at step %d: %f s", ctr, time);
ctr++;
return;
}
void PclExtractor::cloudCallback(const Cloud2Msg::ConstPtr& cloud2Msg_input)
{
boost::mutex::scoped_lock(mutex_);
sensor_msgs::PointCloud2 output;
sensor_msgs::PointCloud2 convex_hull;
sensor_msgs::PointCloud2 object_msg;
sensor_msgs::PointCloud2 nonObject_msg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*cloud2Msg_input, *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// *** Normal estimation
// Create the normal estimation class and pass the input dataset to it
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
// Creating the kdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
ne.setSearchMethod(tree);
// output dataset
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
// use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch(0.3);
// compute the features
ne.compute(*cloud_normals);
// *** End of normal estimation
// *** Plane Estimation From Normals Start
// Create the segmentation object
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
// seg.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
seg.setModelType(pcl::SACMODEL_PLANE);
// seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
//y가 z
// const Eigen::Vector3f z_axis(0,-1.0,0);
// seg.setAxis(z_axis);
// seg.setEpsAngle(seg_setEpsAngle_);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations (seg_setMaxIterations_);
seg.setDistanceThreshold(seg_setDistanceThreshold_);
seg.setNormalDistanceWeight(seg_setNormalDistanceWeight_);
// seg.setProbability(seg_probability_);
seg.setInputCloud((*cloud).makeShared());
seg.setInputNormals(cloud_normals);
seg.segment(*inliers, *coefficients);
std::cerr <<"input: "<<cloud->width*cloud->height<<"Model inliers: " << inliers->indices.size () << std::endl;
if (inliers->indices.size () == 0)
{
ROS_ERROR ("Could not estimate a planar model for the given dataset.");
}
// *** End of Plane Estimation
// *** Plane Estimation Start
// Create the segmentation object
/* pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
//seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
// seg.setModelType(pcl::SACMODEL_PLANE);
// seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
//y가 z
const Eigen::Vector3f z_axis(0,-1.0,0);
seg.setAxis(z_axis);
seg.setEpsAngle(seg_setEpsAngle_);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations (seg_setMaxIterations_);
seg.setDistanceThreshold(seg_setDistanceThreshold_);
seg.setInputCloud((*cloud).makeShared());
seg.segment(*inliers, *coefficients);
std::cerr <<"input: "<<cloud->width*cloud->height<<"Model inliers: " << inliers->indices.size () << std::endl;
if (inliers->indices.size () == 0)
{
ROS_ERROR ("Could not estimate a planar model for the given dataset.");
}
// *** End of Plane Estimation
*/
pcl::ExtractIndices<pcl::PointXYZ> extract;
// Extrat the inliers
extract.setInputCloud(cloud);
extract.setIndices(inliers);
pcl::PointCloud<pcl::PointXYZ>::Ptr ground_hull(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);
if ((int)inliers->indices.size() > minPoints_)
{
extract.setNegative(false);
extract.filter(*cloud_p);
pcl::toROSMsg(*cloud_p, output);
std::cerr << "PointCloud representing the planar component: "
<< cloud_p->width * cloud_p->height << " data points." << std::endl;
// Step 3c. Project the ground inliers
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType(pcl::SACMODEL_PLANE);
proj.setInputCloud(cloud_p);
proj.setModelCoefficients(coefficients);
proj.filter(*cloud_projected);
// Step 3d. Create a Convex Hull representation of the projected inliers
pcl::ConvexHull<pcl::PointXYZ> chull;
//chull.setInputCloud(cloud_p);
chull.setInputCloud(cloud_projected);
chull.setDimension(chull_setDimension_);//2D
chull.reconstruct(*ground_hull);
pcl::toROSMsg(*ground_hull, convex_hull);
//pcl::toROSMsg(*ground_hull, convex_hull);
ROS_INFO ("Convex hull has: %d data points.", (int) ground_hull->points.size ());
// Publish the data
plane_pub_.publish (output);
hull_pub_.publish(convex_hull);
}
// Create the filtering object
extract.setNegative(true);
extract.filter(*cloud_f);
ROS_INFO ("Cloud_f has: %d data points.", (int) cloud_f->points.size ());
// cloud.swap(cloud_f);
pcl::PointCloud<pcl::PointXYZ>::Ptr object(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr nonObject(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointIndices::Ptr object_indices(new pcl::PointIndices);
// cloud statictical filter
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudStatisticalFiltered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud_f);
sor.setMeanK(sor_setMeanK_);
sor.setStddevMulThresh(sor_setStddevMulThresh_);
sor.filter(*cloudStatisticalFiltered);
pcl::ExtractIndices<pcl::PointXYZ> exObjectIndices;
//exObjectIndices.setInputCloud(cloud_f);
exObjectIndices.setInputCloud(cloudStatisticalFiltered);
exObjectIndices.setIndices(object_indices);
exObjectIndices.filter(*object);
exObjectIndices.setNegative(true);
exObjectIndices.filter(*nonObject);
ROS_INFO ("Object has: %d data points.", (int) object->points.size ());
pcl::toROSMsg(*object, object_msg);
object_pub_.publish(object_msg);
//pcl::toROSMsg(*nonObject, nonObject_msg);
//pcl::toROSMsg(*cloud_f, nonObject_msg);
pcl::toROSMsg(*cloudStatisticalFiltered, nonObject_msg);
nonObject_pub_.publish(nonObject_msg);
}
// input: cloudInput
// input: pointCloudNormals
// output: cloudOutput
// output: pointCloudNormalsFiltered
void extractHandles(PointCloud::Ptr& cloudInput, PointCloud::Ptr& cloudOutput, PointCloudNormal::Ptr& pointCloudNormals, std::vector<int>& handles) {
// PCL objects
//pcl::PassThrough<Point> vgrid_; // Filtering + downsampling object
pcl::VoxelGrid<Point> vgrid_; // Filtering + downsampling object
pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_; // Planar segmentation object
pcl::SACSegmentation<Point> seg_line_; // Planar segmentation object
pcl::ProjectInliers<Point> proj_; // Inlier projection object
pcl::ExtractIndices<Point> extract_; // Extract (too) big tables
pcl::ConvexHull<Point> chull_;
pcl::ExtractPolygonalPrismData<Point> prism_;
pcl::PointCloud<Point> cloud_objects_;
pcl::EuclideanClusterExtraction<Point> cluster_, handle_cluster_;
pcl::PCDWriter writer;
double sac_distance_, normal_distance_weight_;
double eps_angle_, seg_prob_;
int max_iter_;
sac_distance_ = 0.05; //0.02
normal_distance_weight_ = 0.05;
max_iter_ = 500;
eps_angle_ = 30.0; //20.0
seg_prob_ = 0.99;
btVector3 axis(0.0, 0.0, 1.0);
seg_.setModelType(pcl::SACMODEL_NORMAL_PARALLEL_PLANE);
seg_.setMethodType(pcl::SAC_RANSAC);
seg_.setDistanceThreshold(sac_distance_);
seg_.setNormalDistanceWeight(normal_distance_weight_);
seg_.setOptimizeCoefficients(true);
seg_.setAxis(Eigen::Vector3f(fabs(axis.getX()), fabs(axis.getY()), fabs(axis.getZ())));
seg_.setEpsAngle(pcl::deg2rad(eps_angle_));
seg_.setMaxIterations(max_iter_);
seg_.setProbability(seg_prob_);
cluster_.setClusterTolerance(0.03);
cluster_.setMinClusterSize(200);
KdTreePtr clusters_tree_(new KdTree);
clusters_tree_->setEpsilon(1);
cluster_.setSearchMethod(clusters_tree_);
seg_line_.setModelType(pcl::SACMODEL_LINE);
seg_line_.setMethodType(pcl::SAC_RANSAC);
seg_line_.setDistanceThreshold(0.05);
seg_line_.setOptimizeCoefficients(true);
seg_line_.setMaxIterations(max_iter_);
seg_line_.setProbability(seg_prob_);
//filter cloud
double leafSize = 0.001;
pcl::VoxelGrid<Point> sor;
sor.setInputCloud (cloudInput);
sor.setLeafSize (leafSize, leafSize, leafSize);
sor.filter (*cloudOutput);
//sor.setInputCloud (pointCloudNormals);
//sor.filter (*pointCloudNormalsFiltered);
//std::vector<int> tempIndices;
//pcl::removeNaNFromPointCloud(*cloudInput, *cloudOutput, tempIndices);
//pcl::removeNaNFromPointCloud(*pointCloudNormals, *pointCloudNormalsFiltered, tempIndices);
// Segment planes
pcl::OrganizedMultiPlaneSegmentation<Point, PointNormal, pcl::Label> mps;
ROS_INFO("Segmenting planes");
mps.setMinInliers (20000);
mps.setMaximumCurvature(0.02);
mps.setInputNormals (pointCloudNormals);
mps.setInputCloud (cloudInput);
std::vector<pcl::PlanarRegion<Point> > regions;
std::vector<pcl::PointIndices> regionPoints;
std::vector< pcl::ModelCoefficients > planes_coeff;
mps.segment(planes_coeff, regionPoints);
ROS_INFO_STREAM("Number of regions:" << regionPoints.size());
if ((int) regionPoints.size() < 1) {
ROS_ERROR("no planes found");
return;
}
std::stringstream filename;
for (size_t plane = 0; plane < regionPoints.size (); plane++)
{
filename.str("");
filename << "plane" << plane << ".pcd";
writer.write(filename.str(), *cloudInput, regionPoints[plane].indices, true);
ROS_INFO("Plane model: [%f, %f, %f, %f] with %d inliers.",
planes_coeff[plane].values[0], planes_coeff[plane].values[1],
planes_coeff[plane].values[2], planes_coeff[plane].values[3], (int)regionPoints[plane].indices.size ());
//Project Points into the Perfect plane
PointCloud::Ptr cloud_projected(new PointCloud());
pcl::PointIndicesPtr cloudPlaneIndicesPtr(new pcl::PointIndices(regionPoints[plane]));
pcl::ModelCoefficientsPtr coeff(new pcl::ModelCoefficients(planes_coeff[plane]));
proj_.setInputCloud(cloudInput);
proj_.setIndices(cloudPlaneIndicesPtr);
proj_.setModelCoefficients(coeff);
proj_.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
proj_.filter(*cloud_projected);
PointCloud::Ptr cloud_hull(new PointCloud());
// Create a Convex Hull representation of the projected inliers
chull_.setInputCloud(cloud_projected);
chull_.reconstruct(*cloud_hull);
ROS_INFO("Convex hull has: %d data points.", (int)cloud_hull->points.size ());
if ((int) cloud_hull->points.size() == 0)
{
ROS_WARN("Convex hull has: %d data points. Returning.", (int)cloud_hull->points.size ());
return;
}
// Extract the handle clusters using a polygonal prism
pcl::PointIndices::Ptr handlesIndicesPtr(new pcl::PointIndices());
prism_.setHeightLimits(0.05, 0.1);
prism_.setInputCloud(cloudInput);
prism_.setInputPlanarHull(cloud_hull);
prism_.segment(*handlesIndicesPtr);
ROS_INFO("Number of handle candidates: %d.", (int)handlesIndicesPtr->indices.size ());
if((int)handlesIndicesPtr->indices.size () < 1100)
continue;
/*######### handle clustering code
handle_clusters.clear();
handle_cluster_.setClusterTolerance(0.03);
handle_cluster_.setMinClusterSize(200);
handle_cluster_.setSearchMethod(clusters_tree_);
handle_cluster_.setInputCloud(handles);
handle_cluster_.setIndices(handlesIndicesPtr);
handle_cluster_.extract(handle_clusters);
for(size_t j = 0; j < handle_clusters.size(); j++)
{
filename.str("");
filename << "handle" << (int)i << "-" << (int)j << ".pcd";
writer.write(filename.str(), *handles, handle_clusters[j].indices, true);
}*/
pcl::StatisticalOutlierRemoval<Point> sor;
PointCloud::Ptr cloud_filtered (new pcl::PointCloud<Point>);
sor.setInputCloud (cloudInput);
sor.setIndices(handlesIndicesPtr);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud_filtered);
PointCloudNormal::Ptr cloud_filtered_hack (new PointCloudNormal);
pcl::copyPointCloud(*cloud_filtered, *cloud_filtered_hack);
// Our handle is in cloud_filtered. We want indices of a cloud (also filtered for NaNs)
pcl::KdTreeFLANN<PointNormal> kdtreeNN;
std::vector<int> pointIdxNKNSearch(1);
std::vector<float> pointNKNSquaredDistance(1);
kdtreeNN.setInputCloud(pointCloudNormals);
for(size_t j = 0; j < cloud_filtered_hack->points.size(); j++)
{
kdtreeNN.nearestKSearch(cloud_filtered_hack->points[j], 1, pointIdxNKNSearch, pointNKNSquaredDistance);
handles.push_back(pointIdxNKNSearch[0]);
}
}
}
/**
* extract handles from a pointcloud
* @param[pointCloudIn] input point cloud
* @param[pointCloudNormals] normals for the input cloud
* @param[handle_indices] vector of indices, each item being a set of indices representing a handle
* @param[handle_coefficients] vector of cofficients, each item being the coefficients of the line representing the handle
*/
void HandleExtractor::extractHandles(PointCloud::Ptr &cloudInput, PointCloudNormal::Ptr &pointCloudNormals,
std::vector< pcl::PointIndices> &handle_indices, std::vector< pcl::ModelCoefficients> &handle_coefficients
)
{
// setup handle cluster object
pcl::EuclideanClusterExtraction<Point> handle_cluster_;
KdTreePtr clusters_tree_(new KdTree);
handle_cluster_.setClusterTolerance(handle_cluster_tolerance);
handle_cluster_.setMinClusterSize(min_handle_cluster_size);
handle_cluster_.setSearchMethod(clusters_tree_);
handle_cluster_.setInputCloud(cloudInput);
pcl::PointCloud<Point>::Ptr cloud_filtered(new pcl::PointCloud<Point>());
pcl::VoxelGrid<Point> vg;
vg.setInputCloud(cloudInput);
vg.setLeafSize(0.01, 0.01, 0.01);
vg.filter(*cloud_filtered);
// setup line ransac object
pcl::SACSegmentation<Point> seg_line_;
seg_line_.setModelType(pcl::SACMODEL_LINE);
seg_line_.setMethodType(pcl::SAC_RANSAC);
seg_line_.setInputCloud(cloudInput);
seg_line_.setDistanceThreshold(line_ransac_distance);
seg_line_.setOptimizeCoefficients(true);
seg_line_.setMaxIterations(line_ransac_max_iter);
// setup Inlier projection object
pcl::ProjectInliers<Point> proj_;
proj_.setInputCloud(cloud_filtered);
proj_.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
// setup polygonal prism
pcl::ExtractPolygonalPrismData<Point> prism_;
prism_.setHeightLimits(min_handle_height, max_handle_height);
prism_.setInputCloud(cloudInput);
//setup Plane Segmentation
outInfo("Segmenting planes");
pcl::SACSegmentation<Point> seg_plane_;
seg_plane_.setOptimizeCoefficients(true);
seg_plane_.setModelType(pcl::SACMODEL_PLANE);
seg_plane_.setMethodType(pcl::SAC_RANSAC);
seg_plane_.setDistanceThreshold(0.03);
seg_plane_.setMaxIterations(500);
seg_plane_.setInputCloud(cloud_filtered);
pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients());
pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices());
seg_plane_.segment(*plane_inliers, *plane_coefficients);
if(plane_inliers->indices.size() != 0)
{
outDebug("Plane model: "
<< plane_coefficients->values[0] << ","
<< plane_coefficients->values[1] << ","
<< plane_coefficients->values[2] << ","
<< plane_coefficients->values[3] << " with "
<< (int)plane_inliers->indices.size() << "inliers ");
//Project inliers of the planes into a perfect plane
PointCloud::Ptr cloud_projected(new PointCloud());
proj_.setIndices(plane_inliers);
proj_.setModelCoefficients(plane_coefficients);
proj_.filter(*cloud_projected);
// Create a Convex Hull representation using the projected inliers
PointCloud::Ptr cloud_hull(new PointCloud());
pcl::ConvexHull<Point> chull_;
chull_.setDimension(2);
chull_.setInputCloud(cloud_projected);
chull_.reconstruct(*cloud_hull);
outInfo("Convex hull has: " << (int)cloud_hull->points.size() << " data points.");
if((int) cloud_hull->points.size() == 0)
{
outInfo("Convex hull has: no data points. Returning.");
return;
}
// Extract the handle clusters using a polygonal prism
pcl::PointIndices::Ptr handlesIndicesPtr(new pcl::PointIndices());
prism_.setInputPlanarHull(cloud_hull);
prism_.segment(*handlesIndicesPtr);
// cluster the points found in the prism
std::vector< pcl::PointIndices> handle_clusters;
handle_cluster_.setIndices(handlesIndicesPtr);
handle_cluster_.extract(handle_clusters);
for(size_t j = 0; j < handle_clusters.size(); j++)
{
// for each cluster in the prism, attempt to fit a line using ransac
pcl::PointIndices single_handle_indices;
pcl::ModelCoefficients handle_line_coefficients;
seg_line_.setIndices(getIndicesPointerFromObject(handle_clusters[j]));
seg_line_.segment(single_handle_indices, handle_line_coefficients);
if(single_handle_indices.indices.size() > 0)
{
outInfo("Found a handle with " << single_handle_indices.indices.size() << " inliers.");
outDebug("Handle Line coefficients: " << handle_line_coefficients);
handle_indices.push_back(single_handle_indices);
handle_coefficients.push_back(handle_line_coefficients);
}
}
}
else
{
outInfo("no planes found");
return;
}
}
