QList <pcl::cloud_composer::CloudComposerItem*> pcl::cloud_composer::VoxelGridDownsampleTool::performAction (ConstItemList input_data, PointTypeFlags::PointType) { QList <CloudComposerItem*> output; const CloudComposerItem* input_item; // Check input data length if ( input_data.size () == 0) { qCritical () << "Empty input in VoxelGridDownsampleTool!"; return output; } else if ( input_data.size () > 1) { qWarning () << "Input vector has more than one item in VoxelGridDownsampleTool"; } input_item = input_data.value (0); if (input_item->type () == CloudComposerItem::CLOUD_ITEM) { double leaf_x = parameter_model_->getProperty("Leaf Size x").toDouble (); double leaf_y = parameter_model_->getProperty("Leaf Size y").toDouble (); double leaf_z = parameter_model_->getProperty("Leaf Size z").toDouble (); pcl::PCLPointCloud2::ConstPtr input_cloud = input_item->data (ItemDataRole::CLOUD_BLOB).value <pcl::PCLPointCloud2::ConstPtr> (); //////////////// THE WORK - FILTERING OUTLIERS /////////////////// // Create the filtering object pcl::VoxelGrid<pcl::PCLPointCloud2> vox_grid; vox_grid.setInputCloud (input_cloud); vox_grid.setLeafSize (float (leaf_x), float (leaf_y), float (leaf_z)); //Create output cloud pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2); //Filter! vox_grid.filter (*cloud_filtered); ////////////////////////////////////////////////////////////////// //Get copies of the original origin and orientation Eigen::Vector4f source_origin = input_item->data (ItemDataRole::ORIGIN).value<Eigen::Vector4f> (); Eigen::Quaternionf source_orientation = input_item->data (ItemDataRole::ORIENTATION).value<Eigen::Quaternionf> (); //Put the modified cloud into an item, stick in output CloudItem* cloud_item = new CloudItem (input_item->text () + tr (" vox ds") , cloud_filtered , source_origin , source_orientation); output.append (cloud_item); } else { qDebug () << "Input item in VoxelGridDownsampleTool is not a cloud!!!"; } return output; }
QList <pcl::cloud_composer::CloudComposerItem*> pcl::cloud_composer::NormalEstimationTool::performAction (ConstItemList input_data, PointTypeFlags::PointType type) { QList <CloudComposerItem*> output; const CloudComposerItem* input_item; // Check input data length if ( input_data.size () == 0) { qCritical () << "Empty input in Normal Estimation Tool!"; return output; } else if ( input_data.size () > 1) { qWarning () << "Input vector has more than one item in Normal Estimation!"; } input_item = input_data.value (0); sensor_msgs::PointCloud2::ConstPtr input_cloud; if (input_item->type () == CloudComposerItem::CLOUD_ITEM) { double radius = parameter_model_->getProperty("Radius").toDouble(); qDebug () << "Received Radius = " <<radius; sensor_msgs::PointCloud2::ConstPtr input_cloud = input_item->data (ItemDataRole::CLOUD_BLOB).value <sensor_msgs::PointCloud2::ConstPtr> (); qDebug () << "Got cloud size = "<<input_cloud->width; //////////////// THE WORK - COMPUTING NORMALS /////////////////// pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg (*input_cloud, *cloud); // Create the normal estimation class, and pass the input dataset to it pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne; ne.setInputCloud (cloud); // Create an empty kdtree representation, and pass it to the normal estimation object. // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given). pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ()); ne.setSearchMethod (tree); // Output datasets pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); // Use all neighbors in a sphere of radius 3cm ne.setRadiusSearch (radius); // Compute the features ne.compute (*cloud_normals); ////////////////////////////////////////////////////////////////// NormalsItem* normals_item = new NormalsItem (tr("Normals r=%1").arg(radius),cloud_normals,radius); output.append (normals_item); qDebug () << "Calced normals"; } else { qDebug () << "Input item in Normal Estimation is not a cloud!!!"; } return output; }
QList <pcl::cloud_composer::CloudComposerItem*> pcl::cloud_composer::SanitizeCloudTool::performAction (ConstItemList input_data, PointTypeFlags::PointType type) { QList <CloudComposerItem*> output; const CloudComposerItem* input_item; // Check input data length if ( input_data.size () == 0) { qCritical () << "Empty input in SanitizeCloudTool!"; return output; } input_item = input_data.value (0); if (input_item->type () == CloudComposerItem::CLOUD_ITEM ) { sensor_msgs::PointCloud2::ConstPtr input_cloud = input_item->data (ItemDataRole::CLOUD_BLOB).value <sensor_msgs::PointCloud2::ConstPtr> (); bool keep_organized = parameter_model_->getProperty("Keep Organized").toBool (); //////////////// THE WORK - FILTERING NANS /////////////////// // Create the filtering object pcl::PassThrough<sensor_msgs::PointCloud2> pass_filter; pass_filter.setInputCloud (input_cloud); pass_filter.setKeepOrganized (keep_organized); //Create output cloud sensor_msgs::PointCloud2::Ptr cloud_filtered = boost::make_shared<sensor_msgs::PointCloud2> (); //Filter! pass_filter.filter (*cloud_filtered); ////////////////////////////////////////////////////////////////// //Get copies of the original origin and orientation Eigen::Vector4f source_origin = input_item->data (ItemDataRole::ORIGIN).value<Eigen::Vector4f> (); Eigen::Quaternionf source_orientation = input_item->data (ItemDataRole::ORIENTATION).value<Eigen::Quaternionf> (); //Put the modified cloud into an item, stick in output CloudItem* cloud_item = new CloudItem (input_item->text () + tr (" sanitized") , cloud_filtered , source_origin , source_orientation); output.append (cloud_item); } else { qDebug () << "Input item in StatisticalOutlierRemovalTool is not a cloud!!!"; } return output; }
QList <pcl::cloud_composer::CloudComposerItem*> pcl::cloud_composer::StatisticalOutlierRemovalTool::performAction (ConstItemList input_data, PointTypeFlags::PointType) { QList <CloudComposerItem*> output; const CloudComposerItem* input_item; // Check input data length if ( input_data.size () == 0) { qCritical () << "Empty input in StatisticalOutlierRemovalTool!"; return output; } else if ( input_data.size () > 1) { qWarning () << "Input vector has more than one item in StatisticalOutlierRemovalTool"; } input_item = input_data.value (0); if ( !input_item->isSanitized () ) { qCritical () << "StatisticalOutlierRemovalTool requires sanitized input!"; return output; } if (input_item->type () == CloudComposerItem::CLOUD_ITEM ) { pcl::PCLPointCloud2::ConstPtr input_cloud = input_item->data (ItemDataRole::CLOUD_BLOB).value <pcl::PCLPointCloud2::ConstPtr> (); int mean_k = parameter_model_->getProperty("Mean K").toInt (); double std_dev_thresh = parameter_model_->getProperty ("Std Dev Thresh").toDouble (); //////////////// THE WORK - FILTERING OUTLIERS /////////////////// // Create the filtering object pcl::StatisticalOutlierRemoval<pcl::PCLPointCloud2> sor; sor.setInputCloud (input_cloud); sor.setMeanK (mean_k); sor.setStddevMulThresh (std_dev_thresh); //Create output cloud pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2); //Filter! sor.filter (*cloud_filtered); ////////////////////////////////////////////////////////////////// //Get copies of the original origin and orientation Eigen::Vector4f source_origin = input_item->data (ItemDataRole::ORIGIN).value<Eigen::Vector4f> (); Eigen::Quaternionf source_orientation = input_item->data (ItemDataRole::ORIENTATION).value<Eigen::Quaternionf> (); //Put the modified cloud into an item, stick in output CloudItem* cloud_item = new CloudItem (input_item->text () + tr (" sor filtered") , cloud_filtered , source_origin , source_orientation); output.append (cloud_item); } else { qDebug () << "Input item in StatisticalOutlierRemovalTool is not a cloud!!!"; } return output; }
QList <pcl::cloud_composer::CloudComposerItem*> pcl::cloud_composer::FPFHEstimationTool::performAction (ConstItemList input_data, PointTypeFlags::PointType) { QList <CloudComposerItem*> output; const CloudComposerItem* input_item; // Check input data length if ( input_data.size () == 0) { qCritical () << "Empty input in FPFH Estimation Tool!"; return output; } else if ( input_data.size () > 1) { qWarning () << "Input vector has more than one item in FPFH Estimation!"; } input_item = input_data.value (0); if (input_item->type () == CloudComposerItem::CLOUD_ITEM) { //Check if this cloud has normals computed! QList <CloudComposerItem*> normals_list = input_item->getChildren (CloudComposerItem::NORMALS_ITEM); if ( normals_list.size () == 0 ) { qCritical () << "No normals item child found in this cloud item"; return output; } qDebug () << "Found item text="<<normals_list.at(0)->text(); double radius = parameter_model_->getProperty("Radius").toDouble(); pcl::PCLPointCloud2::ConstPtr input_cloud = input_item->data (ItemDataRole::CLOUD_BLOB).value <pcl::PCLPointCloud2::ConstPtr> (); //Get the cloud in template form pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::fromPCLPointCloud2 (*input_cloud, *cloud); //Get the normals cloud, we just use the first normals that were found if there are more than one pcl::PointCloud<pcl::Normal>::ConstPtr input_normals = normals_list.value(0)->data(ItemDataRole::CLOUD_TEMPLATED).value <pcl::PointCloud<pcl::Normal>::ConstPtr> (); pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh; // qDebug () << "Input cloud size = "<<cloud->size (); //////////////// THE WORK - COMPUTING FPFH /////////////////// // Create the FPFH estimation class, and pass the input dataset+normals to it fpfh.setInputCloud (cloud); fpfh.setInputNormals (input_normals); // Create an empty kdtree representation, and pass it to the FPFH estimation object. // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given). qDebug () << "Building KD Tree"; pcl::search::KdTree<PointXYZ>::Ptr tree (new pcl::search::KdTree<PointXYZ>); fpfh.setSearchMethod (tree); // Output datasets pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33> ()); // Use all neighbors in a sphere of radius 5cm // IMPORTANT: the radius used here has to be larger than the radius used to estimate the surface normals!!! fpfh.setRadiusSearch (radius); // Compute the features qDebug () << "Computing FPFH features"; fpfh.compute (*fpfhs); qDebug () << "Size of computed features ="<<fpfhs->width; ////////////////////////////////////////////////////////////////// FPFHItem* fpfh_item = new FPFHItem (tr("FPFH r=%1").arg(radius),fpfhs,radius); output.append (fpfh_item); } else { qCritical () << "Input item in FPFH Estimation is not a cloud!!!"; } return output; }