void greedy_proj () {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2 ());
pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
pcl::io::loadPCDFile ("input.pcd", *cloud_blob);
pcl::fromPCLPointCloud2 (*cloud_blob, *cloud);
cloud_blob = cloud_filtered;
// Normal estimation
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud);
n.setInputCloud (cloud);
n.setSearchMethod (tree);
n.setKSearch (20);
n.compute (*normals);
// Concatenate the XYZ and normal fields
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
// Create search tree
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
tree2->setInputCloud (cloud_with_normals);
// Initialize objects
pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
pcl::PolygonMesh triangles;
// Set the maximum distance between connected points (maximum edge length)
gp3.setSearchRadius (1);
// Set typical values for the parameters
gp3.setMu (2.5);
gp3.setMaximumNearestNeighbors (10);
gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
gp3.setMinimumAngle(M_PI/18); // 10 degrees
gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
gp3.setNormalConsistency(false);
// Get result
gp3.setInputCloud (cloud_with_normals);
gp3.setSearchMethod (tree2);
gp3.reconstruct (triangles);
// Additional vertex information
std::vector<int> parts = gp3.getPartIDs();
std::vector<int> states = gp3.getPointStates();
pcl::io::saveVTKFile("output.vtk", triangles);
pcl::io::savePolygonFileSTL("output.stl", triangles);
}
vector<PCPolygonPtr> PCPolyhedron::detectPolygons(const PCPtr& cloud, float planeTolerance, float pointsTolerance, bool limitFaces)
{
float DOT_EPSILON = 0.15;
saveCloud("1_toDetect.pcd", *cloud);
PCPtr cloudTemp(cloud);
float maxFaces = limitFaces ? MAX_FACES : 100;
sensor_msgs::PointCloud2::Ptr cloud_blob (new sensor_msgs::PointCloud2());
sensor_msgs::PointCloud2::Ptr cloud_filtered_blob (new sensor_msgs::PointCloud2);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
vector<PCPolygonPtr> nuevos;
PCPtr cloudP (new PC());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::ExtractIndices<pcl::PointXYZ> extract;
std::vector<ofVec3f> vCloudHull;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (50);
seg.setDistanceThreshold (planeTolerance); //original: 0.01
// Create the filtering object
int i = 0, nrPoints = cloudTemp->points.size ();
// mientras 7% de la nube no se haya procesad+o
int numFaces = 0;
while (cloudTemp->points.size () > 0.07 * nrPoints && numFaces < maxFaces)
{
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloudTemp);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0) {
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
//FIX
PCPtr cloudFilteredTempInliers (new PC());
PCPtr cloudFilteredTempOutliers (new PC());
if(inliers->indices.size() != cloudTemp->size())
{
// Extract the inliers
extract.setInputCloud (cloudTemp);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloudFilteredTempInliers);
cloudP = cloudFilteredTempInliers;
}
else
cloudP = cloudTemp;
// Create the filtering object
extract.setInputCloud (cloudTemp);
extract.setIndices (inliers);
extract.setNegative (true);
if(cloudP->size() != cloudTemp->size())
extract.filter (*cloudFilteredTempOutliers);
cloudTemp = cloudFilteredTempOutliers;
saveCloud("2_DetectedPol" + ofToString(i) + ".pcd", *cloudP);
//Remove outliers by clustering
vector<pcl::PointIndices> clusterIndices(findClusters(cloudP, pointsTolerance, 10, 10000));
int debuccount = 0;
PCPtr cloudPFiltered (new PC());
if(clusterIndices.size() > 0)
{
cloudPFiltered = getCloudFromIndices(cloudP, clusterIndices.at(0));
}
saveCloud("3_Postfilter_pol" + ofToString(i) + ".pcd",*cloudPFiltered);
if (cloudPFiltered->size() < 4)
break;
//Controlo que las normales sean perpendiculares
ofVec3f norm (coefficients->values[0],coefficients->values[1],coefficients->values[2]);
norm.normalize();
bool normalCheck = true;
for(int i = 0; i < nuevos.size() && normalCheck; i++)
{
float dot = abs(nuevos[i]->getNormal().dot(norm));
if( dot > DOT_EPSILON)
{
normalCheck = false;
}
}
if(normalCheck)
{
//proyecto los puntos sobre el plano
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType(pcl::SACMODEL_PLANE);
PCPtr projectedCloud (new PC());
proj.setInputCloud(cloudPFiltered);
proj.setModelCoefficients(coefficients);
proj.filter(*projectedCloud);
saveCloud("4_Proy_Pol" + ofToString(i) + ".pcd",*projectedCloud);
PCPolygonPtr pcp(new PCQuadrilateral(*coefficients, projectedCloud));
pcp->detectPolygon();
nuevos.push_back(pcp);
numFaces++;
}
i++;
}
return nuevos;
}
void ReadFileWorker::doWork(const QString &filename)
{
bool is_success(false);
QByteArray ba = filename.toLocal8Bit();
std::string* strfilename = new std::string(ba.data());
sensor_msgs::PointCloud2::Ptr cloud_blob(new sensor_msgs::PointCloud2);
dataLibrary::Status = STATUS_OPENPCD;
dataLibrary::start = clock();
//begin of processing
if(!pcl::io::loadPCDFile (*strfilename, *cloud_blob))
{
dataLibrary::clearall();
pcl::fromROSMsg (*cloud_blob, *dataLibrary::cloudxyz);
if(pcl::getFieldIndex (*cloud_blob, "rgb")<0)
{
dataLibrary::cloudID = *strfilename;
if(!this->getMuteMode())
{
emit ReadFileReady(CLOUDXYZ);
}
is_success = true;
}
else
{
pcl::fromROSMsg (*cloud_blob, *dataLibrary::cloudxyzrgb);
dataLibrary::cloudID = *strfilename;
if(!this->getMuteMode())
{
emit ReadFileReady(CLOUDXYZRGB);
}
is_success = true;
}
}
else
{
emit showErrors("Error opening pcd file.");
}
//end of processing
dataLibrary::finish = clock();
if(this->getWriteLogMpde()&&is_success)
{
std::string string_filename = filename.toUtf8().constData();
std::string log_text = string_filename + "\n\tReading PCD file costs: ";
std::ostringstream strs;
strs << (double)(dataLibrary::finish-dataLibrary::start)/CLOCKS_PER_SEC;
log_text += (strs.str() +" seconds.");
dataLibrary::write_text_to_log_file(log_text);
}
dataLibrary::Status = STATUS_READY;
emit showReadyStatus();
delete strfilename;
if(this->getWorkFlowMode()&&is_success)
{
this->Sleep(1000);
emit GoWorkFlow();
}
}
int main (int argc, char** argv)
{
sensor_msgs::PointCloud2::Ptr cloud_blob (new sensor_msgs::PointCloud2), cloud_filtered_blob (new sensor_msgs::PointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the cloud data
pcl::PCDReader reader;
reader.read ("table_scene_lms400.pcd", *cloud_blob);
std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl;
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
sor.setInputCloud (cloud_blob);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered_blob);
// Convert to the templated PointCloud
pcl::fromROSMsg (*cloud_filtered_blob, *cloud_filtered);
std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
// Write the downsampled version to disk
pcl::PCDWriter writer;
writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);
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.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;
int i = 0, nr_points = (int) cloud_filtered->points.size ();
// While 30% of the original cloud is still there
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the inliers
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;
std::stringstream ss;
ss << "table_scene_lms400_plane_" << i << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_filtered);
i++;
}
return (0);
}
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), cloud_filtered_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
pcl_conversions::toPCL(*input, *cloud_blob);
// Fill in the cloud data
//pcl::PCDReader reader;
//reader.read ("table_scene_lms400.pcd", *cloud_blob);
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud_blob);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered_blob);
// Convert to the templated PointCloud
pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
//std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
// Write the downsampled version to disk
//pcl::PCDWriter writer;
//writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);
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.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;
// int i = 0, nr_points = (int) cloud_filtered->points.size ();
// While 30% of the original cloud is still there
//while (cloud_filtered->points.size () > 0.3 * nr_points)
for (int i = 0; i < 2; i++)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
ROS_ERROR("Could not estimate a planar model for the given dataset.");
break;
}
// Extract the inliers
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
// std::stringstream ss;
// ss << "table_scene_lms400_plane_" << i << ".pcd";
// writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_f);
cloud_filtered.swap (cloud_f);
//i++;
}
//pcl::PCLPointCloud2 pre_output;
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*cloud_p, output);
//pcl_conversions::fromPCL(pre_output, output);
pub.publish(output);
}
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// Container for original & filtered data
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
// pcl::PCLPointCloud2 cloud_filtered;
pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
// Convert to PCL data type
pcl_conversions::toPCL(*input, *cloud_blob);
// Perform the actual filtering
pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud_blob);
sor.setLeafSize (0.05, 0.05, 0.05);
sor.setFilterFieldName("z");
sor.setFilterLimits(0.01, 0.3);
sor.filter (*cloud_filtered_blob);
// // Remove outlier X
// pcl::PCLPointCloud2::Ptr cloud_filtered_blobx (new pcl::PCLPointCloud2);
// pcl::VoxelGrid<pcl::PCLPointCloud2> sorx;
// sorx.setInputCloud(cloud_filtered_blobz);
// sorx.setFilterFieldName("x");
// sorx.setFilterLimits(-1, 1);
// sorx.filter(*cloud_filtered_blobx);
// // Remove outlier Y
// pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
// pcl::VoxelGrid<pcl::PCLPointCloud2> sory;
// sory.setInputCloud(cloud_filtered_blobx);
// sory.setFilterFieldName("y");
// sory.setFilterLimits(-1, 1);
// sory.filter(*cloud_filtered_blob);
// Convert to the templated PointCloud
pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
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);
Eigen::Vector3f upVector(0, 0, 1);
seg.setAxis(upVector);
seg.setEpsAngle(1.5708);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.05);
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
return;
}
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;
// Extract the inliers
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
// Publish inliers
// sensor_msgs::PointCloud2 inlierpc;
// pcl_conversions::fromPCL(cloud_p, inlierpc);
pub.publish (*cloud_p);
// Publish the model coefficients
pcl_msgs::ModelCoefficients ros_coefficients;
pcl_conversions::fromPCL(*coefficients, ros_coefficients);
pubCoef.publish (ros_coefficients);
// ==========================================
// // Convert to ROS data type
// sensor_msgs::PointCloud2 downpc;
// pcl_conversions::fromPCL(cloud_filtered, downpc);
// // Publish the data
// pub.publish (downpc);
// pcl::ModelCoefficients coefficients;
// pcl::PointIndices inliers;
// //.makeShared()
// // Create the segmentation object
// pcl::SACSegmentation<pcl::PointXYZ> seg;
// seg.setInputCloud (&cloudPtr);
// seg.setOptimizeCoefficients (true); // Optional
// seg.setModelType (pcl::SACMODEL_PLANE); // Mandatory
// seg.setMethodType (pcl::SAC_RANSAC);
// seg.setDistanceThreshold (0.1);
// seg.segment (inliers, coefficients);
// if (inliers.indices.size () == 0)
// {
// PCL_ERROR ("Could not estimate a planar model for the given dataset.");
// return;
// }
// std::cerr << "Model coefficients: " << coefficients.values[0] << " "
// << coefficients.values[1] << " "
// << coefficients.values[2] << " "
// << coefficients.values[3] << std::endl;
// // Publish the model coefficients
// pcl_msgs::ModelCoefficients ros_coefficients;
// pcl_conversions::fromPCL(coefficients, ros_coefficients);
// pubCoef.publish (ros_coefficients);
}
