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;
}
