void init(const OpenROVmessages::LaserMsg msg){
/********************* INITIALIZE CLOUDS *********************/
// Mandatory: Init he sizes of the point clouds
temp_cloud->width = msg.n_rois;
temp_cloud->height = 2;
temp_cloud->resize(temp_cloud->width * temp_cloud->height);
nr_points = temp_cloud->points.size();
/**************** INITIALIZE SEGMENTER ****************/
// Mandatory: use perpendicular plane to use setAxis() function
seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
// seg.setOptimizeCoefficients (true);
// Set method and threshold
seg.setMethodType (pcl::SAC_RANSAC);
// seg.setDistanceThreshold (msg.ransac_threshold);
seg.setDistanceThreshold (5);
// Set the axis for which to search for perpendicular planes
Eigen::Vector3f axis = Eigen::Vector3f(1.0,1.0,0.0);
// Set a allowed deviation angle from vector axis
seg.setEpsAngle((60*CV_PI)/180);
seg.setAxis(axis);
}
int main(int argc, char** argv) {
ros::init(argc, argv, "downsample");
ros::NodeHandle n;
downsample_pub = n.advertise<sensor_msgs::PointCloud2>("/downsample", 10);
depth_sub = n.subscribe("/camera/depth/points", 10, depthPointsCallback);
ros::Rate loop_rate(10);
// Set variables for filters
vox.setLeafSize (0.01, 0.01, 0.01);
sor.setMeanK (10);
sor.setStddevMulThresh (0.1);
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (20);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
//seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.02);
while(ros::ok()) {
ros::spin();
}
return 0;
}
int find_planes(const OpenROVmessages::LaserMsg msg){
ROS_INFO("Entered find_planes");
// Fill cloud with data
fill_cloud(msg);
// Find planes in the cloud
while(temp_cloud->points.size() > 0.1 * nr_points){
seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
seg.setOptimizeCoefficients (true);
// Set method and threshold
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (msg.ransac_threshold);
// Set the axis for which to search for perpendicular planes
Eigen::Vector3f axis = Eigen::Vector3f(1.0,0.0,0.0);
// Set a allowed deviation angle from vector axis
seg.setEpsAngle((60*CV_PI)/180);
seg.setAxis(axis);
// Segment the planes and insert the coefficients and inliers in vectors
seg.setInputCloud (temp_cloud);
seg.segment (*inliers, *coefficients);
// Check that we found a plane from the cloud
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// push coeffients onto stack
coef_vect.push_back(*coefficients);
// Extract the inliers
extract.setInputCloud (temp_cloud);
extract.setIndices (inliers);
// Extract found inliers from cloud
extract.setNegative(true);
extract.filter(*temp_cloud);
}
return 0;
}
int main(int argc, char **argv){
if (argc < 2){
printf("Error: Debe especificar un archivo .pcd\n");
exit(1);
}
// Cargar nube de puntos
PointCloud::Ptr cloud (new PointCloud);
pcl::io::loadPCDFile(argv[1],*cloud);
// Crear visualizador
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = simpleVis(cloud);
// Segmentar
segmentator.setOptimizeCoefficients(true);
segmentator.setModelType(pcl::SACMODEL_PLANE);
segmentator.setMethodType(pcl::SAC_RANSAC);
segmentator.setDistanceThreshold(SEG_THRESHOLD);
segmentator.setInputCloud(cloud);
pcl::ModelCoefficients::Ptr coefs (new pcl::ModelCoefficients());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices());
segmentator.segment(*inliers,*coefs);
// Ver qué se obtuvo
printf("Se obtienen %d puntos\n",(int)inliers->indices.size());
if (inliers->indices.size() == 0){
printf("Segmentación no sirvió de nada.\n");
exit(1);
}
viewer->addPlane(*coefs, "planito");
// Obtener centroide y dibujarlo
Eigen::Vector4f centroid;
compute3DCentroid(*cloud,centroid);
viewer->addSphere(pcl::PointXYZ(centroid(0),centroid(1),centroid(2)), 0.2, "centroide");
// Dibujar un plano en el viewer para indicar más menos qué se obtuvo
// http://docs.pointclouds.org/trunk/classpcl_1_1visualization_1_1_p_c_l_visualizer.html
while (!viewer->wasStopped()){
viewer->spinOnce(100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}
void depthPointsCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {
// Instantiate various clouds
pcl::PCLPointCloud2* cloud_intermediate = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud_intermediate);
pcl::PointCloud<pcl::PointXYZ> cloud;
// Convert to PCL data type
pcl_conversions::toPCL(*cloud_msg, *cloud_intermediate);
// Apply Voxel Filter on PCLPointCloud2
vox.setInputCloud (cloudPtr);
vox.setInputCloud (cloudPtr);
vox.filter (*cloud_intermediate);
// Convert PCL::PointCloud2 to PCL::PointCloud<PointXYZ>
pcl::fromPCLPointCloud2(*cloud_intermediate, cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p = cloud.makeShared();
// Apply Passthrough Filter
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.3, 1);
pass.setInputCloud (cloud_p);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_p);
// Apply Passthrough Filter
pass.setFilterFieldName ("x");
pass.setFilterLimits (-0.2, 0.2);
pass.setInputCloud (cloud_p);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.0, 3.0);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_p);
// Apply Statistical Noise Filter
sor.setInputCloud (cloud_p);
sor.filter (*cloud_p);
// Planar segmentation: Remove large planes? Or extract floor?
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
int nr_points = (int) cloud_p->points.size ();
Eigen::Vector3f lol (0, 1, 0);
seg.setEpsAngle( 30.0f * (3.14f/180.0f) );
seg.setAxis(lol);
//while(cloud_p->points.size () > 0.2 * nr_points) {
sor.setInputCloud (cloud_p);
sor.filter (*cloud_p);
// 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_p);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
//break;
}
else {
/*std::cout << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << "\n";*/
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_p);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*cloud_p);
}
//}
Eigen::Vector3f lol_p (0.5f, 0, 0.5f);
seg.setAxis(lol_p);
while(cloud_p->points.size () > 0.1 * nr_points) {
seg.setInputCloud (cloud_p);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
break;
}
else {
/*std::cout << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << "\n";*/
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_p);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*cloud_p);
}
}
// Apply Statistical Noise Filter
sor.setInputCloud (cloud_p);
sor.filter (*cloud_p);
if(cloud_p->points.size() > 0) {
std::vector<pcl::PointIndices> cluster_indices;
tree->setInputCloud (cloud_p);
ec.setInputCloud (cloud_p);
ec.extract (cluster_indices);
std::cout << "Clusters detected: " << cluster_indices.size() << "\n";
// Convert to ROS data type
}
// Convert to ROS data type
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*cloud_p, output);
// Publish the data
downsample_pub.publish(output);
}
