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;
}
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);
}
void rgb_pcl::cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){
// Container for original & filtered data
#if PR2
if(target_frame.find(base_frame) == std::string::npos){
getTransformCloud(input, *input);
}
sensor_msgs::PointCloud2 in = *input;
sensor_msgs::PointCloud2 out;
pcl_ros::transformPointCloud(target_frame, net_transform, in, out);
#endif
// ROS_INFO("Cloud acquired...");
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>),
cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>),
cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
Mat displayImage = cv::Mat(Size(640, 480), CV_8UC3);
displayImage = Scalar(120);
// Convert to PCL data type
#if PR2
pcl_conversions::toPCL(out, *cloud);
#endif
#if !PR2
pcl_conversions::toPCL(*input, *cloud);
#endif
// ROS_INFO("\t=>Cloud rotated...");
// Perform the actual filtering
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloudPtr);
sor.setLeafSize (0.005, 0.005, 0.005);
sor.filter (*cloud_filtered_blob);
pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
ModelCoefficientsPtr coefficients (new pcl::ModelCoefficients);
PointCloudPtr plane_points(new PointCloud), point_points_2d_hull(new PointCloud);
std::vector<PointCloudPtr> object_clouds;
pcl::PointCloud<pcl::PointXYZRGB> combinedCloud;
#if PR2
make_crop_box_marker(marker_publisher, base_frame, 0, 0.2, -1, 0.2, 1.3, 2, 1.3);
// Define your cube with two points in space:
Eigen::Vector4f minPoint;
minPoint[0]=0.2; // define minimum point x
minPoint[1]=-1; // define minimum point y
minPoint[2]=0.2; // define minimum point z
Eigen::Vector4f maxPoint;
maxPoint[0]=1.5; // define max point x
maxPoint[1]=1; // define max point y
maxPoint[2]=1.5; // define max point z
pcl::CropBox<pcl::PointXYZRGB> cropFilter;
cropFilter.setInputCloud (cloud_filtered);
cropFilter.setMin(minPoint);
cropFilter.setMax(maxPoint);
cropFilter.filter (*cloud_filtered);
#endif
#if !PR2
//Rotate the point cloud
Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
// Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
float theta = M_PI; // The angle of rotation in radians
// Define a translation of 0 meters on the x axis
transform_1.translation() << 0.0, 0.0, 1.0;
// The same rotation matrix as before; tetha radians arround X axis
transform_1.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
// Executing the transformation
pcl::transformPointCloud (*cloud_filtered, *cloud_filtered, transform_1);
#endif
interpretTableScene(cloud_filtered, coefficients, plane_points, point_points_2d_hull, object_clouds);
int c = 0;
#if PUBLISH_CLOUDS
int ID_object = -1;
#endif
for(auto cloudCluster: object_clouds){
// get_cloud_matching(cloudCluster); //histogram matching
#if PUBLISH_CLOUDS
ID_object = c;
#endif
combinedCloud += *cloudCluster;
combinedCloud.header = cloud_filtered->header;
c++;
}
#if DISPLAY
drawPointCloud(combinedCloud, displayImage);
#endif
getTracker(object_clouds, displayImage);
stateDetection();
// ROS_INFO("\t=>Cloud analysed...");
#if PUBLISH_CLOUDS
sensor_msgs::PointCloud2 output;
if(object_clouds.size() >= ID_object && ID_object >= 0){
pcl::toROSMsg(combinedCloud, output);
// Publish the data
pub.publish (output);
}
#endif
end = ros::Time::now();
std::stringstream ss;
ss <<(end-begin);
string s_FPS = ss.str();
#if DISPLAY
cv::putText(displayImage, "FPS: "+to_string((int)1/(stof(s_FPS))) + " Desired: "+to_string(DESIRED_FPS), cv::Point(10, 10), CV_FONT_HERSHEY_COMPLEX, 0.4, Scalar(0,0,0));
imshow("RGB", displayImage);
#endif
waitKey(1);
begin = ros::Time::now();
}
void cloud_cb (const pcl::PCLPointCloud2ConstPtr& cloud_blob)
{
std::cout<<"Received Kinect message\n";
pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud_blob);
boost::this_thread::sleep (boost::posix_time::microseconds (100));
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);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud_filtered);
int i = 0, nr_points = (int) cloud_filtered->points.size ();
pcl::IndicesPtr remaining (new std::vector<int>);
remaining->resize (nr_points);
for (size_t i = 0; i < remaining->size (); ++i) { (*remaining)[i] = static_cast<int>(i); }
std::cout << "here again" << std::endl;
// While 30% of the original cloud is still there
while (remaining->size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setIndices (remaining);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0) break;
// Extract the inliers
std::vector<int>::iterator it = remaining->begin();
for (size_t i = 0; i < inliers->indices.size (); ++i)
{
int curr = inliers->indices[i];
// Remove it from further consideration.
while (it != remaining->end() && *it < curr) { ++it; }
if (it == remaining->end()) break;
if (*it == curr) it = remaining->erase(it);
}
i++;
}
std::cout << "Found " << i << " planes." << std::endl;
// Color all the non-planar things.
for (std::vector<int>::iterator it = remaining->begin(); it != remaining->end(); ++it)
{
uint8_t r = 0, g = 255, b = 0;
uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
cloud_filtered->at(*it).rgb = *reinterpret_cast<float*>(&rgb);
}
pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
viewer.showCloud (cloud_filtered);
while (!viewer.wasStopped ())
{
}
// Publish the planes we found.
pcl::PCLPointCloud2 outcloud;
pcl::toPCLPointCloud2 (*cloud_filtered, outcloud);
pub.publish (outcloud);
}
