int main()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
//... read, pass in or create a point 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::KdTreeFLANN<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ> ());
//ne.setSearchMethod (tree);
ne.setSearchMethod(pcl::search::KdTree<pcl::PointXYZ>::Ptr (new pcl::search::KdTree<pcl::PointXYZ>));
// 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 (0.03);
// Compute the features
ne.compute (*cloud_normals);
// cloud_normals->points.size () should have the same size as the input cloud->points.size ()
}
int main (int argc, char** argv)
{
typedef pcl::PointCloud<pcl::PointXYZ> PointXYZCloud;
typedef pcl::PointCloud<pcl::PointNormal> PointNormalCloud;
PointXYZCloud::Ptr cloud (new PointXYZCloud);
cloud->width = 2;
cloud->height = 5;
cloud->points.resize(cloud->width * cloud->height);
cloud->is_dense = true;
// Create the normal estimation class, and pass the input dataset to it
pcl::NormalEstimation<PointXYZCloud::PointType, PointNormalCloud::PointType> normalEstimation;
normalEstimation.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).
typedef pcl::search::KdTree<PointXYZCloud::PointType> TreeType;
TreeType::Ptr tree (new TreeType);
//normalEstimation.setSearchMethod (tree);
//normalEstimation.setRadiusSearch (0.03);
normalEstimation.setKSearch (3);
// Compute the normals
PointNormalCloud::Ptr cloud_normals (new PointNormalCloud);
normalEstimation.compute (*cloud_normals);
return 0;
}
//==================================================================
void pcl_visualization::cloud_cb (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
if (viewer.wasStopped() || m_stopFlag)
return;
// normalize
normalize_cloud (cloud);
// Viewer
qDebug() << "cloud_cb_: cloud->width" << cloud->width;
viewer.addPointCloud<pcl::PointXYZ> (cloud, "cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");
// All the objects needed
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
qDebug() << "Estimating point normals.";
// Estimate point normals
ne.setSearchMethod (tree);
ne.setInputCloud (cloud);
ne.setKSearch (10);
ne.compute (*cloud_normals);
qDebug() << "Adding visualizations to viewer.";
viewer.setBackgroundColor (0, 0, 0);
viewer.addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud, cloud_normals, 1, 0.1, "normals");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "normals");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 2, "normals");
viewer.addCoordinateSystem(1.0);
viewer.initCameraParameters();
}
pcl::ihs::InputDataProcessing::CloudProcessedPtr
pcl::ihs::InputDataProcessing::calculateNormals (const CloudInputConstPtr& cloud) const
{
const CloudProcessedPtr cloud_out (new CloudProcessed ());
// Calculate the normals
CloudNormalsPtr cloud_normals (new CloudNormals ());
normal_estimation_->setInputCloud (cloud);
normal_estimation_->compute (*cloud_normals);
// Copy the input cloud and normals into the output cloud.
// While we are already iterating over the whole input we can also remove some points.
cloud_out->resize (cloud->size ());
cloud_out->header = cloud->header;
cloud_out->width = cloud->width;
cloud_out->height = cloud->height;
cloud_out->is_dense = cloud->is_dense && cloud_normals->is_dense;
cloud_out->sensor_origin_ = cloud->sensor_origin_;
cloud_out->sensor_orientation_ = cloud->sensor_orientation_;
CloudInput::const_iterator it_in = cloud->begin ();
CloudNormals::const_iterator it_n = cloud_normals->begin ();
CloudProcessed::iterator it_out = cloud_out->begin ();
for (; it_in!=cloud->end (); ++it_in, ++it_n, ++it_out)
{
it_out->getVector4fMap () = it_in->getVector4fMap ();
it_out->rgba = it_in->rgba;
it_out->getNormalVector4fMap () = it_n->getNormalVector4fMap ();
}
return (cloud_out);
}
void
SACNormalsPlaneExtractor<PointT>::compute()
{
CHECK ( cloud_ ) << "Input cloud is not organized!";
all_planes_.clear();
// ---[ PassThroughFilter
typename pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT> ());
pcl::PassThrough<PointT> pass;
pass.setFilterLimits (0, max_z_bounds_);
pass.setFilterFieldName ("z");
pass.setInputCloud (cloud_);
pass.filter (*cloud_filtered);
if ( cloud_filtered->points.size () < k_)
{
PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %lu points! Aborting.",
cloud_filtered->points.size ());
return;
}
// Downsample the point cloud
typename pcl::PointCloud<PointT>::Ptr cloud_downsampled (new pcl::PointCloud<PointT> ());
pcl::VoxelGrid<PointT> grid;
grid.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
grid.setDownsampleAllData (false);
grid.setInputCloud (cloud_filtered);
grid.filter (*cloud_downsampled);
// ---[ Estimate the point normals
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
pcl::NormalEstimation<PointT, pcl::Normal> n3d;
typename pcl::search::KdTree<PointT>::Ptr normals_tree_ (new pcl::search::KdTree<PointT>);
n3d.setKSearch ( (int) k_);
n3d.setSearchMethod (normals_tree_);
n3d.setInputCloud (cloud_downsampled);
n3d.compute (*cloud_normals);
// ---[ Perform segmentation
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
seg.setDistanceThreshold (sac_distance_threshold_);
seg.setMaxIterations (2000);
seg.setNormalDistanceWeight (0.1);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setProbability (0.99);
seg.setInputCloud (cloud_downsampled);
seg.setInputNormals (cloud_normals);
pcl::PointIndices table_inliers;
pcl::ModelCoefficients coefficients;
seg.segment ( table_inliers, coefficients);
Eigen::Vector4f plane = Eigen::Vector4f(coefficients.values[0], coefficients.values[1],
coefficients.values[2], coefficients.values[3]);
all_planes_.resize(1);
all_planes_[0] = plane;
}
//Function that computes the Viewpoint Feature Histogram
void VFH::computeVFHistogram(pcl::PointCloud<PointT>::Ptr cloud, const pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs)
{
//---compute normals---
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
//create normal estimation class, and pass the input cloud
pcl::NormalEstimation<PointT, pcl::Normal> ne;
ne.setInputCloud (cloud);
//Create empty kdetree representation, and pass it to the normal estimation object.
//its content will be filled inside the object based on the given input.
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT> ());
ne.setSearchMethod (tree);
//set radious of the neighbors to use (1 cm)
ne.setRadiusSearch(10);
//computing normals
ne.compute(*cloud_normals);
//---proceed to compute VFH---
//Create the VFH estimation class and pas the input to it
pcl::VFHEstimation<PointT, pcl::Normal, pcl::VFHSignature308> vfh;
vfh.setInputCloud (cloud);
vfh.setInputNormals (cloud_normals);
//create an empty kdtree representation and pass it to the vfh estimation object
//its content will be filled inside the object based on the given input.
pcl::search::KdTree<PointT>::Ptr vfhtree (new pcl::search::KdTree<PointT> ());
vfh.setSearchMethod (vfhtree);
//compute the features
vfh.compute (*vfhs);
}
int
main (int, char **argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
pcl::PCDWriter writer;
if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloud_ptr) == -1)
{
std::cout<<"Couldn't read the file "<<argv[1]<<std::endl;
return (-1);
}
std::cout << "Loaded pcd file " << argv[1] << " with " << cloud_ptr->points.size () << std::endl;
// Normal estimation
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud_ptr);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n (new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod (tree_n);
ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
std::cout << "Estimated the normals" << std::endl;
// Creating the kdtree object for the search method of the extraction
boost::shared_ptr<pcl::KdTree<pcl::PointXYZ> > tree_ec (new pcl::KdTreeFLANN<pcl::PointXYZ> ());
tree_ec->setInputCloud (cloud_ptr);
// Extracting Euclidean clusters using cloud and its normals
std::vector<int> indices;
std::vector<pcl::PointIndices> cluster_indices;
const float tolerance = 0.5f; // 50cm tolerance in (x, y, z) coordinate system
const double eps_angle = 5 * (M_PI / 180.0); // 5degree tolerance in normals
const unsigned int min_cluster_size = 50;
pcl::extractEuclideanClusters (*cloud_ptr, *cloud_normals, tolerance, tree_ec, cluster_indices, eps_angle, min_cluster_size);
std::cout << "No of clusters formed are " << cluster_indices.size () << std::endl;
// Saving the clusters in seperate pcd files
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_ptr->points[*pit]);
cloud_cluster->width = static_cast<uint32_t> (cloud_cluster->points.size ());
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster using xyzn: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
ss << "./cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false);
j++;
}
return (0);
}
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;
}
pcl::IndicesPtr normalFiltering(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
float radiusSearch,
const Eigen::Vector4f & viewpoint)
{
typedef typename pcl::search::KdTree<PointT> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
pcl::NormalEstimation<PointT, pcl::Normal> ne;
ne.setInputCloud (cloud);
if(indices->size())
{
ne.setIndices(indices);
}
KdTreePtr tree (new KdTree(false));
if(indices->size())
{
tree->setInputCloud(cloud, indices);
}
else
{
tree->setInputCloud(cloud);
}
ne.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (radiusSearch);
if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
{
ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
}
ne.compute (*cloud_normals);
pcl::IndicesPtr output(new std::vector<int>(cloud_normals->size()));
int oi = 0; // output iterator
Eigen::Vector3f n(normal[0], normal[1], normal[2]);
for(unsigned int i=0; i<cloud_normals->size(); ++i)
{
Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
float angle = pcl::getAngle3D(normal, v);
if(angle < angleMax)
{
output->at(oi++) = indices->size()!=0?indices->at(i):i;
}
}
output->resize(oi);
return output;
}
pcl::ihs::InputDataProcessing::CloudProcessedPtr
pcl::ihs::InputDataProcessing::process (const CloudInputConstPtr& cloud) const
{
const CloudProcessedPtr cloud_out (new CloudProcessed ());
// Calculate the normals
CloudNormalsPtr cloud_normals (new CloudNormals ());
normal_estimation_->setInputCloud (cloud);
normal_estimation_->compute (*cloud_normals);
// Copy the input cloud and normals into the output cloud.
// While we are already iterating over the whole input we can also remove some points.
cloud_out->resize (cloud->size ());
cloud_out->header = cloud->header;
cloud_out->width = cloud->width;
cloud_out->height = cloud->height;
cloud_out->is_dense = false; /*cloud->is_dense && cloud_normals->is_dense;*/
cloud_out->sensor_origin_ = cloud->sensor_origin_;
cloud_out->sensor_orientation_ = cloud->sensor_orientation_;
CloudInput::const_iterator it_in = cloud->begin ();
CloudNormals::const_iterator it_n = cloud_normals->begin ();
CloudProcessed::iterator it_out = cloud_out->begin ();
for (; it_in!=cloud->end (); ++it_in, ++it_n, ++it_out)
{
if (pcl::isFinite (*it_in) && pcl::isFinite (*it_n) &&
it_in->x >= x_min_ && it_in->x <= x_max_ &&
it_in->y >= y_min_ && it_in->y <= y_max_ &&
it_in->z >= z_min_ && it_in->z <= z_max_)
{
it_out->getVector4fMap () = it_in->getVector4fMap ();
it_out->rgba = it_in->rgba;
it_out->getNormalVector4fMap () = it_n->getNormalVector4fMap ();
}
else
{
// Fourth value should stay 1 (default)
it_out->getVector3fMap () = Eigen::Vector3f (std::numeric_limits <float>::quiet_NaN (),
std::numeric_limits <float>::quiet_NaN (),
std::numeric_limits <float>::quiet_NaN ());
// it_out->r = 0;
// it_out->g = 0;
// it_out->b = 0;
// it_out->a = 255;
// Fourth value should stay 0 (default)
it_out->getNormalVector3fMap () = Eigen::Vector3f (std::numeric_limits <float>::quiet_NaN (),
std::numeric_limits <float>::quiet_NaN (),
std::numeric_limits <float>::quiet_NaN ());
}
}
return (cloud_out);
}
pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointNormal>); // Output datasets
pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne;
ne.setNormalEstimationMethod( ne.AVERAGE_3D_GRADIENT);
ne.setMaxDepthChangeFactor(0.02f);
ne.setNormalSmoothingSize(10.0f);
ne.setInputCloud(cloud);
ne.compute(*cloud_normals);
copyPointCloud(*cloud, *cloud_normals);
return cloud_normals;
}
pcl::PointCloud<PointTNormal>::Ptr addNormalsToPointCloud(pcl::PointCloud<PointT>::Ptr cloud_input){
pcl::PointCloud<PointTNormal>::Ptr cloudWithNormals(new pcl::PointCloud<PointTNormal>());
pcl::NormalEstimation<PointT, pcl::Normal> ne;
ne.setInputCloud (cloud_input);
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
ne.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
pcl::concatenateFields(*cloud_input,*cloud_normals,*cloudWithNormals);
return cloudWithNormals;
}
pcl::PointCloud<pcl::Normal>::Ptr compute_normals(pcl::PointCloud<pcl::PointXYZ>::Ptr pcloud){
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (pcloud);
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 (7.0);
ne.compute (*cloud_normals);
return cloud_normals;
}
int
main (int, char** av)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_no_nans (new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_segmented (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PCDWriter writer;
if (pcl::io::loadPCDFile(av[1], *cloud_ptr)==-1)
{
std::cout << "Couldn't find the file " << av[1] << std::endl;
return -1;
}
std::cout << "Loaded cloud " << av[1] << " of size " << cloud_ptr->points.size() << std::endl;
// Remove the nans
cloud_ptr->is_dense = false;
cloud_no_nans->is_dense = false;
std::vector<int> indices;
pcl::removeNaNFromPointCloud (*cloud_ptr, *cloud_no_nans, indices);
std::cout << "Removed nans from " << cloud_ptr->points.size () << " to " << cloud_no_nans->points.size () << std::endl;
// Estimate the normals
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud_no_nans);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n (new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod (tree_n);
ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
std::cout << "Normals are computed and size is " << cloud_normals->points.size () << std::endl;
// Region growing
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> rg;
rg.setSmoothModeFlag (false); // Depends on the cloud being processed
rg.setInputCloud (cloud_no_nans);
rg.setInputNormals (cloud_normals);
std::vector <pcl::PointIndices> clusters;
rg.extract (clusters);
cloud_segmented = rg.getColoredCloud ();
// Writing the resulting cloud into a pcd file
std::cout << "No of segments done is " << clusters.size () << std::endl;
writer.write<pcl::PointXYZRGB> ("segment_result.pcd", *cloud_segmented, false);
return (0);
}
void Normals::computeNormalsXYZRGB() {
LOG(LWARNING) << "Normals::computeNormalsXYZRGB";
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = in_cloud_xyzrgb.read();
LOG(LWARNING) << "Normals: input size: " << cloud->size();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr copy(new pcl::PointCloud<pcl::PointXYZRGB>());
// Remove NaNs.
std::vector<int> indicesNANs;
cloud->is_dense = false;
pcl::removeNaNFromPointCloud(*cloud, *copy, indicesNANs);
LOG(LWARNING) << "Normals: input size: " << copy->size();
// compute centroid
// Eigen::Vector4f centroid;
// pcl::compute3DCentroid(*copy,centroid);
//LOG(LDEBUG) << "Normals: centroid computed: " << centroid;
// compute normals for centroid as viewpoint
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normalEstimation;
normalEstimation.setInputCloud (copy);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
normalEstimation.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
normalEstimation.setRadiusSearch (radius);
// normalEstimation.setViewPoint(centroid[0], centroid[1], centroid[2]);
LOG(LWARNING) << "Normals: compute normals!";
normalEstimation.compute (*cloud_normals);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_xyz_normals (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::copyPointCloud(*copy, *cloud_xyz_normals);
pcl::copyPointCloud(*cloud_normals, *cloud_xyz_normals);
LOG(LWARNING) << "Normals: out_cloud_xyzrgb_normals.write " << cloud_normals->size();
out_cloud_xyzrgb_normals.write(cloud_xyz_normals);
LOG(LWARNING) << "Normals: out_cloud_normals.write " << cloud_normals->size();
out_cloud_normals.write(cloud_normals);
}
// --> clase que llama run() y que pasa la 'cloud' obtenida del openni_grabber <--
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
{
if(inicializacion == false)
{
viewer.addPointCloud(cloud,"cloud_RGBA-D");
ne.setInputCloud (cloud);
ne.setRadiusSearch (0.03);
inicializacion = true;
}else{
viewer.removePointCloud(); // se carga "cloud", osease cloud_normals
}
if (!viewer.wasStopped())
{
viewer.updatePointCloud(cloud);
pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA> ());
ne.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_out (new pcl::PointCloud<pcl::Normal>);
ne.compute (*cloud_normals);
/*
// --> eliminar lineas nan <--
std::vector<int> index;
pcl::removeNaNNormalsFromPointCloud(*cloud_normals,*cloud_normals_out, index);
// remover outliers de las normales
// probar a recorrer cloud_normals y aquellos indices que sean NAN, ponerlos a 0
*/
viewer.addPointCloudNormals<pcl::PointXYZRGBA,pcl::Normal>(cloud, cloud_normals); // asigna "cloud" a cloud_normals
viewer.spinOnce(100);
}
}
void mpcl_extract_VFH(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
// http://virtuemarket-lab.blogspot.jp/2015/03/viewpoint-feature-histogram.html
// pcl::PointCloud<pcl::VFHSignature308>::Ptr Extract_VFH(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308> ());
// cloud_normals = surface_normals(cloud);
vfh.setInputCloud (cloud);
vfh.setInputNormals (cloud_normals);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
vfh.setSearchMethod (tree);
vfh.compute (*vfhs);
// return vfhs;
}
/** \brief Loads an n-D histogram file as a VFH signature
* \param path the input file name
* \param vfh the resultant VFH model
*/
bool
loadHist (const boost::filesystem::path &path, vfh_model &vfh)
{
pcl::PointCloud<PointType>::Ptr cloud (new pcl::PointCloud<PointType>);
pcl::PointCloud<NormalType>::Ptr cloud_normals (new pcl::PointCloud<NormalType> ());
if (pcl::io::loadPCDFile<PointType> (path.string(), *cloud) == -1) {
PCL_ERROR ("Couldn't read file %s.pcd \n", path.string().c_str());
return false;
}
pcl::PointCloud<PointType>::Ptr cloud_filtered (new pcl::PointCloud<PointType>);
std::vector<int> filter_index;
pcl::removeNaNFromPointCloud (*cloud, *cloud_filtered, filter_index);
cloud = cloud_filtered;
pcl::NormalEstimationOMP<PointType, NormalType> norm_est;
norm_est.setKSearch (10);
norm_est.setInputCloud (cloud);
norm_est.compute (*cloud_normals);
pcl::VFHEstimation<PointType, NormalType, pcl::VFHSignature308> vfh_est;
vfh_est.setInputCloud (cloud);
vfh_est.setInputNormals (cloud_normals);
pcl::search::KdTree<PointType>::Ptr tree (new pcl::search::KdTree<PointType> ());
vfh_est.setSearchMethod (tree);
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308> ());
vfh_est.compute (*vfhs);
vfh.second.resize (308);
for (size_t i = 0; i < pcl::VFHSignature308::descriptorSize(); ++i)
{
vfh.second[i] = vfhs->points[0].histogram[i];
}
vfh.first = path.string ();
return (true);
}
int
main (int argc, char** argv)
{
std::string filename = argv[1];
std::cout << "Reading " << filename << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud) == -1) // load the file
{
PCL_ERROR ("Couldn't read file");
return -1;
}
std::cout << "points: " << cloud->points.size () << std::endl;
// Create the normal estimation class, and pass the input dataset to it
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
normal_estimation.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>);
normal_estimation.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
normal_estimation.setRadiusSearch (0.03);
// Compute the features
normal_estimation.compute (*cloud_normals);
// cloud_normals->points.size () should have the same size as the input cloud->points.size ()
std::cout << "cloud_normals->points.size (): " << cloud_normals->points.size () << std::endl;
return 0;
}
void Normals::computeNormalsXYZ() {
LOG(LTRACE) << "Normals::computeNormalsXYZ";
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = in_cloud_xyz.read();
pcl::PointCloud<pcl::PointXYZ>::Ptr copy(new pcl::PointCloud<pcl::PointXYZ>());
// Remove NaNs.
std::vector<int> indicesNANs;
cloud->is_dense = false;
pcl::removeNaNFromPointCloud(*cloud, *copy, indicesNANs);
// compute centroid
// Eigen::Vector4f centroid;
// pcl::compute3DCentroid(*copy,centroid);
// compute normals for centroid as viewpoint
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;
normalEstimation.setInputCloud (copy);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
normalEstimation.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
normalEstimation.setRadiusSearch (radius);
// normalEstimation.setViewPoint(centroid[0], centroid[1], centroid[2]);
normalEstimation.compute (*cloud_normals);
// pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_xyz_normals (new pcl::PointCloud<pcl::PointXYZINormal>);
// pcl::copyPointCloud(*copy, *cloud_xyz_normals);
// pcl::copyPointCloud(*cloud_normals, *cloud_xyz_normals);
//
// out_cloud_xyz_normals.write(cloud_xyz_normals);
out_cloud_normals.write(cloud_normals);
}
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::io::loadPCDFile (argv[1], *cloud);
pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> ne;
ne.setInputCloud (cloud);
cv::Mat input_cloud;
input_cloud = cv::Mat(480, 640, CV_8UC3);
if (!cloud->empty()) {
for (int h=0; h<cloud->height; h++) {
for (int w=0; w<cloud->width; w++) {
pcl::PointXYZRGBA point = cloud->at(w, h);
Eigen::Vector3i rgb = point.getRGBVector3i();
int x_pos = 320 + point.x/point.z *480;
int y_pos = 240 + point.y/point.z * 480;
//std::cout << "x pos" << x_pos << "y pos" << y_pos << std::endl;
if( x_pos > 0 and x_pos < 640 and y_pos > 0 and y_pos < 480){
input_cloud.at<cv::Vec3b>(y_pos,x_pos)[0] = rgb[2];
input_cloud.at<cv::Vec3b>(y_pos,x_pos)[1] = rgb[1];
input_cloud.at<cv::Vec3b>(y_pos,x_pos)[2] = rgb[0];
}
}
}
}
std::cout << " saving input image " << std::endl;
cv::imwrite( "image_no_pespective_correction.png", input_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::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA> ());
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 (0.03);
std::cout << " starting computation " << std::endl;
// Compute the features
ne.compute (*cloud_normals);
std::cout << "cloud_normals->points.size (): " << cloud_normals->points.size () << std::endl;
pcl::Normal normal_point = cloud_normals->at(320, 240);
pcl::PointXYZRGBA _point = cloud->at(320, 240);
std::cout << "cloud_normal at point: " << normal_point << std::endl;
std::cout << "z of cloud at point : " << _point.z << std::endl;
Eigen::Vector3f z_axis(normal_point.normal_x,normal_point.normal_y,normal_point.normal_z);
Eigen::Vector3f x_axis_standard(1,0,0);
Eigen::Vector3f x_proj = x_axis_standard - x_axis_standard.dot(z_axis)*z_axis;
Eigen::Affine3f translate_z;
Eigen::Affine3f transformation;
Eigen::Vector3f tmp0 = x_proj.normalized();
Eigen::Vector3f tmp1 = (z_axis.normalized().cross(x_proj.normalized())).normalized();
Eigen::Vector3f tmp2 = z_axis.normalized();
translate_z(0,0)=1; translate_z(0,1)=0; translate_z(0,2)=0; translate_z(0,3)=0.0f;
translate_z(1,0)=0; translate_z(1,1)=1; translate_z(1,2)=0; translate_z(1,3)=0.0f;
translate_z(2,0)=0; translate_z(2,1)=0; translate_z(2,2)=1; translate_z(2,3)=-_point.z;
translate_z(3,0)=0.0f; translate_z(3,1)=0.0f; translate_z(3,2)=0.0f; translate_z(3,3)=1.0f;
transformation(0,0)=tmp0[0]; transformation(0,1)=tmp0[1]; transformation(0,2)=tmp0[2]; transformation(0,3)=0.0f;
transformation(1,0)=tmp1[0]; transformation(1,1)=tmp1[1]; transformation(1,2)=tmp1[2]; transformation(1,3)=0.0f;
transformation(2,0)=tmp2[0]; transformation(2,1)=tmp2[1]; transformation(2,2)=tmp2[2]; transformation(2,3)=0.0f;
transformation(3,0)=0.0f; transformation(3,1)=0.0f; transformation(3,2)=0.0f; transformation(3,3)=1.0f;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr transformed_cloud1 (new pcl::PointCloud<pcl::PointXYZRGBA>);
// You can either apply transform_1 or transform_2; they are the same
pcl::transformPointCloud (*cloud, *transformed_cloud1, translate_z);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr transformed_cloud2 (new pcl::PointCloud<pcl::PointXYZRGBA>);
// You can either apply transform_1 or transform_2; they are the same
pcl::transformPointCloud (*transformed_cloud1, *transformed_cloud2, transformation);
translate_z(2,3)=_point.z;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr transformed_cloud3 (new pcl::PointCloud<pcl::PointXYZRGBA>);
// You can either apply transform_1 or transform_2; they are the same
pcl::transformPointCloud (*transformed_cloud2, *transformed_cloud3, translate_z);
_point = transformed_cloud3->at(320, 240);
std::cout << "cloud at point: " << _point << std::endl;
cv::Mat output_cloud;
output_cloud = cv::Mat(480, 640, CV_8UC3, cv::Scalar(0,0,255) );
cv::Mat distance;
distance = cv::Mat(480, 640, CV_32FC1, -1.2); //::zeros
std::cout << "depth " << distance.at<float>(0,0) << std::endl;
if (!transformed_cloud3->empty())
{
for (int h=0; h<transformed_cloud3->height; h++)
{
for (int w=0; w<transformed_cloud3->width; w++)
{
pcl::PointXYZRGBA point = transformed_cloud3->at(w, h);
Eigen::Vector3i rgb = point.getRGBVector3i();
int x_pos = 320 - point.x/point.z * 480;
int y_pos = 240 + point.y/point.z * 480;
if( x_pos > 0 and x_pos < 640 and y_pos > 0 and y_pos < 480)
{
if( point.z > distance.at<float>(y_pos,x_pos) and point.z < -0.8 )
{
output_cloud.at<cv::Vec3b>(y_pos,x_pos)[0] = rgb[2];
output_cloud.at<cv::Vec3b>(y_pos,x_pos)[1] = rgb[1];
output_cloud.at<cv::Vec3b>(y_pos,x_pos)[2] = rgb[0];
distance.at<float>(y_pos,x_pos) = point.z;
}
}
}
}
}
std::cout << " saving output image " << std::endl;
cv::imwrite( "transformed_image_no_interpolation.png", output_cloud );
//cv::Mat input_cloud;
input_cloud = cv::Mat(480, 640, CV_8UC3);
for (int h=0; h< output_cloud.rows ; h++)
{
for (int w=0; w< output_cloud.cols ; w++)
{
cv::Vec3b color = output_cloud.at<cv::Vec3b>(cv::Point(w,h));
if( !( (int)output_cloud.at<cv::Vec3b>(h, w)[0] == 0 and (int)output_cloud.at<cv::Vec3b>(h, w)[1] == 0 and (int)output_cloud.at<cv::Vec3b>(h, w)[2] == 255) )
{
input_cloud.at<cv::Vec3b>(h, w)[0] = output_cloud.at<cv::Vec3b>(h, w)[0];
input_cloud.at<cv::Vec3b>(h, w)[1] = output_cloud.at<cv::Vec3b>(h, w)[1];
input_cloud.at<cv::Vec3b>(h, w)[2] = output_cloud.at<cv::Vec3b>(h, w)[2];
}
else
{
// std::cout << " oh god" << std::endl;
int left_pos = - 1;
int right_pos = 1;
while( left_pos + w > 0
and ( (int)output_cloud.at<cv::Vec3b>(h, left_pos + w)[0] == 0 and (int)output_cloud.at<cv::Vec3b>(h, left_pos + w)[1] == 0 and (int)output_cloud.at<cv::Vec3b>(h, left_pos + w)[2] == 255) )
{
//std::cout << "ha" << std::endl;
left_pos--;
}
while( right_pos + w < 640 -1
and ( (int)output_cloud.at<cv::Vec3b>(h, right_pos + w)[0] == 0 and (int)output_cloud.at<cv::Vec3b>(h, right_pos + w)[1] == 0 and (int)output_cloud.at<cv::Vec3b>(h, right_pos + w)[2] == 255) )
{
//std::cout << "ha" << std::endl;
right_pos++;
}
if( left_pos + w >= 0 and right_pos + w < 640)
{
if( !( (int)output_cloud.at<cv::Vec3b>(h, left_pos + w)[0] == 0 and (int)output_cloud.at<cv::Vec3b>(h, left_pos + w)[1] == 0 and (int)output_cloud.at<cv::Vec3b>(h, left_pos + w)[2] == 255)
and !( (int)output_cloud.at<cv::Vec3b>(h, right_pos + w)[0] == 0 and (int)output_cloud.at<cv::Vec3b>(h, right_pos + w)[1] == 0 and (int)output_cloud.at<cv::Vec3b>(h, right_pos + w)[2] == 255) )
{
float left_right_weight = -left_pos/right_pos;
int pixel_color = (int) output_cloud.at<cv::Vec3b>(h, left_pos + w)[0]/2 + output_cloud.at<cv::Vec3b>(h, right_pos + w)[0]/2 ;
input_cloud.at<cv::Vec3b>(h, w)[0] = pixel_color;
input_cloud.at<cv::Vec3b>(h, w)[1] = pixel_color;
input_cloud.at<cv::Vec3b>(h, w)[2] = pixel_color;
}
else
{
input_cloud.at<cv::Vec3b>(h, w)[0] = 255;
input_cloud.at<cv::Vec3b>(h, w)[1] = 0;
input_cloud.at<cv::Vec3b>(h, w)[2] = 0;
}
}
}
}
}
std::cout << " saving output image " << std::endl;
cv::imwrite( "transformed_image_perspective.png", input_cloud );
return 0;
}
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
{
PCProc<pcl::PointXYZRGBA> pc1;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr output1 (new pcl::PointCloud<pcl::PointXYZRGBA>);
if(bVoxelGrid)
{
pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr inputc = cloud;//.makeShared();
pc1.downSampling(inputc,output1 , leafsz,leafsz,leafsz);
}
else
{
output1 = cloud->makeShared();
}
///////////////////////////////////////
// passthrough
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr output2 (new pcl::PointCloud<pcl::PointXYZRGBA>);
pc1.PassThroughZ(output1, output2, 1, 3);
cloud_filtered = output2;
///////////////////////////////////////
// normal display
pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> ne;
ne.setInputCloud (output2);
// 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::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA> ());
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 (0.03);
// Compute the features
ne.compute (*cloud_normals);
//viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(cloud, cloud_normals, 1, 0.01, "normals1", 0);
int nf = cloud_filtered->size();
int n = cloud->size();
lpMapping[0] = n;
lpMapping[1] = nf;
lpMapping[2] = bVoxelGrid;
lpMapping[3] = leafsz;
;
int i;
for(i=0; i<nf; i++)//DATA_LEN/6
{
float x = cloud_filtered->points[i].x;
lpMapping[HEADER_LEN + 6*i +0] = cloud_filtered->points[i].x;
lpMapping[HEADER_LEN + 6*i +1] = cloud_filtered->points[i].y;
lpMapping[HEADER_LEN + 6*i +2] = cloud_filtered->points[i].z;
lpMapping[HEADER_LEN + 6*i +3] = cloud_filtered->points[i].r;
lpMapping[HEADER_LEN + 6*i +4] = cloud_filtered->points[i].g;
lpMapping[HEADER_LEN + 6*i +5] = cloud_filtered->points[i].b;
}
//viewer.addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
if (!viewer.wasStopped())
{
viewer.showCloud (cloud_filtered);
}
}
void callback(const sensor_msgs::PointCloud2ConstPtr& cloud)
{
ros::Time whole_start = ros::Time::now();
ros::Time declare_types_start = ros::Time::now();
// filter
pcl::VoxelGrid<sensor_msgs::PointCloud2> voxel_grid;
pcl::PassThrough<sensor_msgs::PointCloud2> pass;
pcl::ExtractIndices<pcl::PointXYZ> extract_indices;
pcl::ExtractIndices<pcl::Normal> extract_normals;
// Normal estimation
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> segmentation_from_normals;
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ> ());
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree3 (new pcl::search::KdTree<pcl::PointXYZ> ());
// The plane and sphere coefficients
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ());
// The plane and sphere inliers
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_cylinder (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_sphere (new pcl::PointIndices ());
// The point clouds
sensor_msgs::PointCloud2::Ptr voxelgrid_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr plane_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_cloud_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr cylinder_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2);
// The PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_output (new pcl::PointCloud<pcl::PointXYZ>);
// The cloud normals
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ()); // for plane
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal> ()); // for cylinder
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals3 (new pcl::PointCloud<pcl::Normal> ()); // for sphere
ros::Time declare_types_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Voxel grid Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// // Create VoxelGrid filtering
// voxel_grid.setInputCloud (cloud);
// voxel_grid.setLeafSize (0.01, 0.01, 0.01);
// voxel_grid.filter (*voxelgrid_filtered);
//
// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*voxelgrid_filtered, *transformed_cloud);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Passthrough Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// pass through filter
// pass.setInputCloud (cloud);
// pass.setFilterFieldName ("z");
// pass.setFilterLimits (0, 1.5);
// pass.filter (*cloud_filtered);
//
// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*cloud_filtered, *transformed_cloud);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Estimate point normals
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ros::Time estimate_start = ros::Time::now();
//
// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*cloud, *transformed_cloud);
//
// // Estimate point normals
// normal_estimation.setSearchMethod (tree);
// normal_estimation.setInputCloud (transformed_cloud);
// normal_estimation.setKSearch (50);
// normal_estimation.compute (*cloud_normals);
//
// ros::Time estimate_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Create and processing the plane extraction
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ros::Time plane_start = ros::Time::now();
//
// // Create the segmentation object for the planar model and set all the parameters
// segmentation_from_normals.setOptimizeCoefficients (true);
// segmentation_from_normals.setModelType (pcl::SACMODEL_NORMAL_PLANE);
// segmentation_from_normals.setNormalDistanceWeight (0.1);
// segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
// segmentation_from_normals.setMaxIterations (100);
// segmentation_from_normals.setDistanceThreshold (0.03);
// segmentation_from_normals.setInputCloud (transformed_cloud);
// segmentation_from_normals.setInputNormals (cloud_normals);
//
// // Obtain the plane inliers and coefficients
// segmentation_from_normals.segment (*inliers_plane, *coefficients_plane);
// //std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
//
// // Extract the planar inliers from the input cloud
// extract_indices.setInputCloud (transformed_cloud);
// extract_indices.setIndices (inliers_plane);
// extract_indices.setNegative (false);
// extract_indices.filter (*cloud_plane);
//
// pcl::toROSMsg (*cloud_plane, *plane_output_cloud);
// plane_pub.publish(plane_output_cloud);
//
// ros::Time plane_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time plane_start = ros::Time::now();
pcl::fromROSMsg (*cloud, *transformed_cloud);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (transformed_cloud);
seg.segment (*inliers_plane, *coefficients_plane);
extract_indices.setInputCloud(transformed_cloud);
extract_indices.setIndices(inliers_plane);
extract_indices.setNegative(false);
extract_indices.filter(*cloud_plane);
pcl::toROSMsg (*cloud_plane, *plane_output_cloud);
plane_pub.publish(plane_output_cloud);
ros::Time plane_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Extract rest plane and passthrough filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time rest_pass_start = ros::Time::now();
// Create the filtering object
// Remove the planar inliers, extract the rest
extract_indices.setNegative (true);
extract_indices.filter (*remove_transformed_cloud);
transformed_cloud.swap (remove_transformed_cloud);
// publish result of Removal the planar inliers, extract the rest
pcl::toROSMsg (*transformed_cloud, *rest_output_cloud);
rest_pub.publish(rest_output_cloud);
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*rest_output_cloud, *cylinder_cloud);
// pass through filter
pass.setInputCloud (rest_output_cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 2.5);
pass.filter (*rest_cloud_filtered);
ros::Time rest_pass_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for cylinder features
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*rest_cloud_filtered, *cylinder_cloud);
// Estimate point normals
normal_estimation.setSearchMethod (tree2);
normal_estimation.setInputCloud (cylinder_cloud);
normal_estimation.setKSearch (50);
normal_estimation.compute (*cloud_normals2);
// Create the segmentation object for sphere segmentation and set all the paopennirameters
segmentation_from_normals.setOptimizeCoefficients (true);
segmentation_from_normals.setModelType (pcl::SACMODEL_CYLINDER);
segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
segmentation_from_normals.setNormalDistanceWeight (0.1);
segmentation_from_normals.setMaxIterations (10000);
segmentation_from_normals.setDistanceThreshold (0.05);
segmentation_from_normals.setRadiusLimits (0, 0.5);
segmentation_from_normals.setInputCloud (cylinder_cloud);
segmentation_from_normals.setInputNormals (cloud_normals2);
// Obtain the sphere inliers and coefficients
segmentation_from_normals.segment (*inliers_cylinder, *coefficients_cylinder);
//std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
// Publish the sphere cloud
extract_indices.setInputCloud (cylinder_cloud);
extract_indices.setIndices (inliers_cylinder);
extract_indices.setNegative (false);
extract_indices.filter (*cylinder_output);
if (cylinder_output->points.empty ())
std::cerr << "Can't find the cylindrical component." << std::endl;
pcl::toROSMsg (*cylinder_output, *cylinder_output_cloud);
cylinder_pub.publish(cylinder_output_cloud);
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for sphere features
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time sphere_start = ros::Time::now();
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*rest_cloud_filtered, *sphere_cloud);
// Estimate point normals
normal_estimation.setSearchMethod (tree3);
normal_estimation.setInputCloud (sphere_cloud);
normal_estimation.setKSearch (50);
normal_estimation.compute (*cloud_normals3);
// Create the segmentation object for sphere segmentation and set all the paopennirameters
segmentation_from_normals.setOptimizeCoefficients (true);
//segmentation_from_normals.setModelType (pcl::SACMODEL_SPHERE);
segmentation_from_normals.setModelType (pcl::SACMODEL_SPHERE);
segmentation_from_normals.setMethodType (pcl::SAC_RANSAC);
segmentation_from_normals.setNormalDistanceWeight (0.1);
segmentation_from_normals.setMaxIterations (10000);
segmentation_from_normals.setDistanceThreshold (0.05);
segmentation_from_normals.setRadiusLimits (0, 0.2);
segmentation_from_normals.setInputCloud (sphere_cloud);
segmentation_from_normals.setInputNormals (cloud_normals3);
// Obtain the sphere inliers and coefficients
segmentation_from_normals.segment (*inliers_sphere, *coefficients_sphere);
//std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl;
// Publish the sphere cloud
extract_indices.setInputCloud (sphere_cloud);
extract_indices.setIndices (inliers_sphere);
extract_indices.setNegative (false);
extract_indices.filter (*sphere_output);
if (sphere_output->points.empty ())
std::cerr << "Can't find the sphere component." << std::endl;
pcl::toROSMsg (*sphere_output, *sphere_output_cloud);
sphere_pub.publish(sphere_output_cloud);
ros::Time sphere_end = ros::Time::now();
std::cout << "cloud size : " << cloud->width * cloud->height << std::endl;
std::cout << "plane size : " << transformed_cloud->width * transformed_cloud->height << std::endl;
//std::cout << "plane size : " << cloud_normals->width * cloud_normals->height << std::endl;
//std::cout << "cylinder size : " << cloud_normals2->width * cloud_normals2->height << std::endl;
std::cout << "sphere size : " << cloud_normals3->width * cloud_normals3->height << std::endl;
ros::Time whole_now = ros::Time::now();
printf("\n");
std::cout << "whole time : " << whole_now - whole_start << " sec" << std::endl;
std::cout << "declare types time : " << declare_types_end - declare_types_start << " sec" << std::endl;
//std::cout << "estimate time : " << estimate_end - estimate_start << " sec" << std::endl;
std::cout << "plane time : " << plane_end - plane_start << " sec" << std::endl;
std::cout << "rest and pass time : " << rest_pass_end - rest_pass_start << " sec" << std::endl;
std::cout << "sphere time : " << sphere_end - sphere_start << " sec" << std::endl;
printf("\n");
}
bool SnapIt::processModelCylinder(jsk_pcl_ros::CallSnapIt::Request& req,
jsk_pcl_ros::CallSnapIt::Response& res) {
pcl::PointCloud<pcl::PointXYZ>::Ptr candidate_points (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
Eigen::Vector3f n, C_orig;
if (!extractPointsInsideCylinder(req.request.center,
req.request.direction,
req.request.radius,
req.request.height,
inliers, n, C_orig,
1.3)) {
return false;
}
if (inliers->indices.size() < 3) {
NODELET_ERROR("not enough points inside of the target_plane");
return false;
}
geometry_msgs::PointStamped centroid;
centroid.point.x = C_orig[0];
centroid.point.y = C_orig[1];
centroid.point.z = C_orig[2];
centroid.header = req.request.header;
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(input_);
extract.setIndices(inliers);
extract.filter(*candidate_points);
publishPointCloud(debug_candidate_points_pub_, candidate_points);
// first, to remove plane we estimate the plane
pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices);
extractPlanePoints(candidate_points, plane_inliers, plane_coefficients,
n, req.request.eps_angle);
if (plane_inliers->indices.size() == 0) {
NODELET_ERROR ("plane estimation failed");
return false;
}
// remove the points blonging to the plane
pcl::PointCloud<pcl::PointXYZ>::Ptr points_inside_pole_wo_plane (new pcl::PointCloud<pcl::PointXYZ>);
extract.setInputCloud (candidate_points);
extract.setIndices (plane_inliers);
extract.setNegative (true);
extract.filter (*points_inside_pole_wo_plane);
publishPointCloud(debug_candidate_points_pub2_, points_inside_pole_wo_plane);
// normal estimation
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);
ne.setInputCloud (points_inside_pole_wo_plane);
ne.setKSearch (50);
ne.compute (*cloud_normals);
// segmentation
pcl::ModelCoefficients::Ptr cylinder_coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr cylinder_inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_CYLINDER);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setRadiusLimits (0.01, req.request.radius * 1.2);
seg.setDistanceThreshold (0.05);
seg.setInputCloud(points_inside_pole_wo_plane);
seg.setInputNormals (cloud_normals);
seg.setMaxIterations (10000);
seg.setNormalDistanceWeight (0.1);
seg.setAxis(n);
if (req.request.eps_angle != 0.0) {
seg.setEpsAngle(req.request.eps_angle);
}
else {
seg.setEpsAngle(0.35);
}
seg.segment (*cylinder_inliers, *cylinder_coefficients);
if (cylinder_inliers->indices.size () == 0)
{
NODELET_ERROR ("Could not estimate a cylinder model for the given dataset.");
return false;
}
debug_centroid_pub_.publish(centroid);
pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_points (new pcl::PointCloud<pcl::PointXYZ>);
extract.setInputCloud (points_inside_pole_wo_plane);
extract.setIndices (cylinder_inliers);
extract.setNegative (false);
extract.filter (*cylinder_points);
publishPointCloud(debug_candidate_points_pub3_, cylinder_points);
Eigen::Vector3f n_prime;
Eigen::Vector3f C_new;
for (size_t i = 0; i < 3; i++) {
C_new[i] = cylinder_coefficients->values[i];
n_prime[i] = cylinder_coefficients->values[i + 3];
}
double radius = fabs(cylinder_coefficients->values[6]);
// inorder to compute centroid, we project all the points to the center line.
// and after that, get the minimum and maximum points in the coordinate system of the center line
double min_alpha = DBL_MAX;
double max_alpha = -DBL_MAX;
for (size_t i = 0; i < cylinder_points->points.size(); i++ ) {
pcl::PointXYZ q = cylinder_points->points[i];
double alpha = (q.getVector3fMap() - C_new).dot(n_prime);
if (alpha < min_alpha) {
min_alpha = alpha;
}
if (alpha > max_alpha) {
max_alpha = alpha;
}
}
// the center of cylinder
Eigen::Vector3f C_new_prime = C_new + (max_alpha + min_alpha) / 2.0 * n_prime;
Eigen::Vector3f n_cross = n.cross(n_prime);
if (n.dot(n_prime)) {
n_cross = - n_cross;
}
double theta = asin(n_cross.norm());
Eigen::Quaternionf trans (Eigen::AngleAxisf(theta, n_cross.normalized()));
Eigen::Vector3f C = trans * C_orig;
Eigen::Affine3f A = Eigen::Translation3f(C) * Eigen::Translation3f(C_new_prime - C) * trans * Eigen::Translation3f(C_orig).inverse();
tf::poseEigenToMsg((Eigen::Affine3d)A, res.transformation);
geometry_msgs::PointStamped centroid_transformed;
centroid_transformed.point.x = C_new_prime[0];
centroid_transformed.point.y = C_new_prime[1];
centroid_transformed.point.z = C_new_prime[2];
centroid_transformed.header = req.request.header;
debug_centroid_after_trans_pub_.publish(centroid_transformed);
// publish marker
visualization_msgs::Marker marker;
marker.type = visualization_msgs::Marker::CYLINDER;
marker.scale.x = radius;
marker.scale.y = radius;
marker.scale.z = (max_alpha - min_alpha);
marker.pose.position.x = C_new_prime[0];
marker.pose.position.y = C_new_prime[1];
marker.pose.position.z = C_new_prime[2];
// n_prime -> z
// n_cross.normalized() -> x
Eigen::Vector3f z_axis = n_prime.normalized();
Eigen::Vector3f y_axis = n_cross.normalized();
Eigen::Vector3f x_axis = (y_axis.cross(z_axis)).normalized();
Eigen::Matrix3f M;
for (size_t i = 0; i < 3; i++) {
M(i, 0) = x_axis[i];
M(i, 1) = y_axis[i];
M(i, 2) = z_axis[i];
}
Eigen::Quaternionf q (M);
marker.pose.orientation.x = q.x();
marker.pose.orientation.y = q.y();
marker.pose.orientation.z = q.z();
marker.pose.orientation.w = q.w();
marker.color.g = 1.0;
marker.color.a = 1.0;
marker.header = input_header_;
marker_pub_.publish(marker);
return true;
}
bool cloud_cb(data_collection::process_cloud::Request &req,
data_collection::process_cloud::Response &res)
{
//Segment from cloud:
//May need to tweak segmentation parameters to get just the cluster I'm looking for.
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
nrg_object_recognition::segmentation seg_srv;
seg_srv.request.scene = req.in_cloud;
seg_srv.request.min_x = -.75, seg_srv.request.max_x = .4;
seg_srv.request.min_y = -5, seg_srv.request.max_y = .5;
seg_srv.request.min_z = 0.0, seg_srv.request.max_z = 1.15;
seg_client.call(seg_srv);
//SegmentCloud(req.in_cloud, clouds);
//For parameter tweaking, may be good to write all cluseters to file here for examination in pcd viewer.
pcl::PointCloud<pcl::PointXYZ>::Ptr cluster (new pcl::PointCloud<pcl::PointXYZ>);
//cluster = clouds.at(0);
pcl::fromROSMsg(seg_srv.response.clusters.at(0), *cluster);
//std::cout << "found " << clouds.size() << " clusters.\n";
std::cout << "cluster has " << cluster->height*cluster->width << " points.\n";
//Write raw pcd file (objecName_angle.pcd)
std::stringstream fileName_ss;
fileName_ss << "data/" << req.objectName << "_" << req.angle << ".pcd";
std::cout << "writing raw cloud to file...\n";
std::cout << fileName_ss.str() << std::endl;
pcl::io::savePCDFile(fileName_ss.str(), *cluster);
std::cout << "done.\n";
//Write vfh feature to file:
pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
vfh.setInputCloud (cluster);
//Estimate normals:
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cluster);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
vfh.setInputNormals (cloud_normals);
//Estimate vfh:
vfh.setSearchMethod (tree);
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308> ());
// Compute the feature
vfh.compute (*vfhs);
//Write to file: (objectName_angle_vfh.pcd)
fileName_ss.str("");
std::cout << "writing vfh descriptor to file...\n";
fileName_ss << "data/" << req.objectName << "_" << req.angle << "_vfh.pcd";
pcl::io::savePCDFile(fileName_ss.str(), *vfhs);
std::cout << "done.\n";
//Extract cph
std::vector<float> feature;
CPHEstimation cph(5,72);
cph.setInputCloud(cluster);
cph.compute(feature);
//Write cph to file. (objectName_angle.csv)
std::ofstream outFile;
fileName_ss.str("");
fileName_ss << "data/" << req.objectName << "_" << req.angle << ".csv";
outFile.open(fileName_ss.str().c_str());
std::cout << "writing cph descriptor to file...\n";
for(unsigned int j=0; j<feature.size(); j++){
outFile << feature.at(j) << " ";
}
outFile.close();
fileName_ss.str("");
std::cout << "done.\n";
res.result = 1;
}
void Segmentation::ransac(std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &clusters, Eigen::Vector3f axis, std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &vectorCloudInliers )
{
vectorCloudInliers.clear();
for(unsigned int i = 0; i < clusters.size(); i++)
{
pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr indices(new pcl::PointIndices);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_(new pcl::PointCloud<pcl::PointXYZRGBA>);
//pcl::PointCloud<pcl::PointXYZRGBA>::Ptr hullGround(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> ne;
pcl::SACSegmentationFromNormals<pcl::PointXYZRGBA, pcl::Normal> seg;
pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA> ());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
// Estimate Normals
ne.setSearchMethod(tree);
ne.setInputCloud(clusters[i]);
ne.setKSearch(50);
ne.compute(*cloud_normals);
// Create the segmentation object
//pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
//seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE); // We search a plane perpendicular to the y
seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);// We search a plane perpendicular to the y
seg.setNormalDistanceWeight(0.005);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.020); //0.25 / 35
//seg.setAxis(axis); // Axis Y 0, -1, 0
seg.setEpsAngle(20.0f * (M_PI/180.0f)); // 50 degrees of error
pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
seg.setInputCloud (clusters[i]);
seg.setInputNormals(cloud_normals);
seg.segment (*indices, *coeff);
if(normalDifferenceError(coeff))
{
std::cout << "coeff" << coeff->values[0] << " " << coeff->values[1] << " " << coeff->values[2] << std::endl;
if (indices->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model (Ground).");
//return false;
}
else
{
extract.setInputCloud(clusters[i]);
extract.setIndices(indices);
extract.setNegative(false);
extract.filter(*cloud_);
vectorCloudInliers.push_back(cloud_);
//return true;
}
}
}
}
void cloud_callback(const sensor_msgs::PointCloud2ConstPtr& input) {
ROS_DEBUG("Got new pointcloud.");
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
std_msgs::Header header = input->header;
pcl::fromROSMsg(*input, *cloud);
//all the objects needed
pcl::PCDReader reader;
pcl::PassThrough<PointT> pass;
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
pcl::PCDWriter writer;
pcl::ExtractIndices<PointT> extract;
pcl::ExtractIndices<pcl::Normal> extract_normals;
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
//data sets
pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_sphere (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_sphere (new pcl::PointIndices);
ROS_DEBUG("PointCloud before filtering has: %i data points.", (int)cloud->points.size());
//filter our NaNs
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 2.0);
pass.filter (*cloud_filtered);
ROS_DEBUG("PointCloud after filtering has: %i data points." , (int)cloud_filtered->points.size());
*cloud_filtered = *cloud;//remove the pass through filter basically
//estimate normal points
ne.setSearchMethod (tree);
ne.setInputCloud (cloud_filtered);
//ne.setKSearch(10);
ne.setRadiusSearch(0.025);
ne.compute (*cloud_normals);
// Create the segmentation object for the planar model and set all the parameters
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg.setNormalDistanceWeight (0.05);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (5.0);
seg.setInputCloud (cloud_filtered);
seg.setInputNormals (cloud_normals);
// Obtain the plane inliers and coefficients
seg.segment (*inliers_plane, *coefficients_plane);
std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
// Extract the planar inliers from the input cloud
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers_plane);
extract.setNegative (false);
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_filtered2);
extract_normals.setNegative (true);
extract_normals.setInputCloud (cloud_normals);
extract_normals.setIndices (inliers_plane);
extract_normals.filter (*cloud_normals2);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_SPHERE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setNormalDistanceWeight (0.1);
seg.setMaxIterations (100);
seg.setDistanceThreshold (5.0);
seg.setRadiusLimits (0, 1.0);
seg.setInputCloud (cloud_filtered2);
seg.setInputNormals (cloud_normals2);
extract.setInputCloud (cloud_filtered2);
extract.setIndices (inliers_sphere);
extract.setNegative (false);
pcl::PointCloud<PointT>::Ptr sphere_cloud (new pcl::PointCloud<PointT> ());
extract.filter(*sphere_cloud);
pcl::PointCloud<pcl::PointXYZ> sphere_cloud_in = *sphere_cloud;
sensor_msgs::PointCloud2 sphere_cloud_out;
pcl::toROSMsg(sphere_cloud_in, sphere_cloud_out);
sphere_cloud_out.header = header;
buoy_cloud_pub.publish(sphere_cloud_out);
}
void HintedHandleEstimator::estimate(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
const geometry_msgs::PointStampedConstPtr &point_msg)
{
boost::mutex::scoped_lock lock(mutex_);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
pcl::PassThrough<pcl::PointXYZ> pass;
int K = 1;
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
pcl::search::KdTree<pcl::PointXYZ>::Ptr kd_tree(new pcl::search::KdTree<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_msg, *cloud);
geometry_msgs::PointStamped transed_point;
ros::Time now = ros::Time::now();
try
{
listener_.waitForTransform(cloud->header.frame_id, point_msg->header.frame_id, now, ros::Duration(1.0));
listener_.transformPoint(cloud->header.frame_id, now, *point_msg, point_msg->header.frame_id, transed_point);
}
catch(tf::TransformException ex)
{
JSK_ROS_ERROR("%s", ex.what());
return;
}
pcl::PointXYZ searchPoint;
searchPoint.x = transed_point.point.x;
searchPoint.y = transed_point.point.y;
searchPoint.z = transed_point.point.z;
//remove too far cloud
pass.setInputCloud(cloud);
pass.setFilterFieldName("x");
pass.setFilterLimits(searchPoint.x - 3*handle.arm_w, searchPoint.x + 3*handle.arm_w);
pass.filter(*cloud);
pass.setInputCloud(cloud);
pass.setFilterFieldName("y");
pass.setFilterLimits(searchPoint.y - 3*handle.arm_w, searchPoint.y + 3*handle.arm_w);
pass.filter(*cloud);
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(searchPoint.z - 3*handle.arm_w, searchPoint.z + 3*handle.arm_w);
pass.filter(*cloud);
if(cloud->points.size() < 10){
JSK_ROS_INFO("points are too small");
return;
}
if(1){ //estimate_normal
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
ne.setSearchMethod(kd_tree);
ne.setRadiusSearch(0.02);
ne.setViewPoint(0, 0, 0);
ne.compute(*cloud_normals);
}
else{ //use normal of msg
}
if(! (kd_tree->nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)){
JSK_ROS_INFO("kdtree failed");
return;
}
float x = cloud->points[pointIdxNKNSearch[0]].x;
float y = cloud->points[pointIdxNKNSearch[0]].y;
float z = cloud->points[pointIdxNKNSearch[0]].z;
float v_x = cloud_normals->points[pointIdxNKNSearch[0]].normal_x;
float v_y = cloud_normals->points[pointIdxNKNSearch[0]].normal_y;
float v_z = cloud_normals->points[pointIdxNKNSearch[0]].normal_z;
double theta = acos(v_x);
// use normal for estimating handle direction
tf::Quaternion normal(0, v_z/NORM(0, v_y, v_z) * cos(theta/2), -v_y/NORM(0, v_y, v_z) * cos(theta/2), sin(theta/2));
tf::Quaternion final_quaternion = normal;
double min_theta_index = 0;
double min_width = 100;
tf::Quaternion min_qua(0, 0, 0, 1);
visualization_msgs::Marker debug_hand_marker;
debug_hand_marker.header = cloud_msg->header;
debug_hand_marker.ns = string("debug_grasp");
debug_hand_marker.id = 0;
debug_hand_marker.type = visualization_msgs::Marker::LINE_LIST;
debug_hand_marker.pose.orientation.w = 1;
debug_hand_marker.scale.x=0.003;
tf::Matrix3x3 best_mat;
//search 180 degree and calc the shortest direction
for(double theta_=0; theta_<3.14/2;
theta_+=3.14/2/30){
tf::Quaternion rotate_(sin(theta_), 0, 0, cos(theta_));
tf::Quaternion temp_qua = normal * rotate_;
tf::Matrix3x3 temp_mat(temp_qua);
geometry_msgs::Pose pose_respected_to_tf;
pose_respected_to_tf.position.x = x;
pose_respected_to_tf.position.y = y;
pose_respected_to_tf.position.z = z;
pose_respected_to_tf.orientation.x = temp_qua.getX();
pose_respected_to_tf.orientation.y = temp_qua.getY();
pose_respected_to_tf.orientation.z = temp_qua.getZ();
pose_respected_to_tf.orientation.w = temp_qua.getW();
Eigen::Affine3d box_pose_respected_to_cloud_eigend;
tf::poseMsgToEigen(pose_respected_to_tf, box_pose_respected_to_cloud_eigend);
Eigen::Affine3d box_pose_respected_to_cloud_eigend_inversed
= box_pose_respected_to_cloud_eigend.inverse();
Eigen::Matrix4f box_pose_respected_to_cloud_eigen_inversed_matrixf;
Eigen::Matrix4d box_pose_respected_to_cloud_eigen_inversed_matrixd
= box_pose_respected_to_cloud_eigend_inversed.matrix();
jsk_pcl_ros::convertMatrix4<Eigen::Matrix4d, Eigen::Matrix4f>(
box_pose_respected_to_cloud_eigen_inversed_matrixd,
box_pose_respected_to_cloud_eigen_inversed_matrixf);
Eigen::Affine3f offset = Eigen::Affine3f(box_pose_respected_to_cloud_eigen_inversed_matrixf);
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud, *output_cloud, offset);
pcl::PassThrough<pcl::PointXYZ> pass;
pcl::PointCloud<pcl::PointXYZ>::Ptr points_z(new pcl::PointCloud<pcl::PointXYZ>), points_yz(new pcl::PointCloud<pcl::PointXYZ>), points_xyz(new pcl::PointCloud<pcl::PointXYZ>);
pass.setInputCloud(output_cloud);
pass.setFilterFieldName("y");
pass.setFilterLimits(-handle.arm_w*2, handle.arm_w*2);
pass.filter(*points_z);
pass.setInputCloud(points_z);
pass.setFilterFieldName("z");
pass.setFilterLimits(-handle.finger_d, handle.finger_d);
pass.filter(*points_yz);
pass.setInputCloud(points_yz);
pass.setFilterFieldName("x");
pass.setFilterLimits(-(handle.arm_l-handle.finger_l), handle.finger_l);
pass.filter(*points_xyz);
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
for(size_t index=0; index<points_xyz->size(); index++){
points_xyz->points[index].x = points_xyz->points[index].z = 0;
}
if(points_xyz->points.size() == 0){JSK_ROS_INFO("points are empty");return;}
kdtree.setInputCloud(points_xyz);
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
pcl::PointXYZ search_point_tree;
search_point_tree.x=search_point_tree.y=search_point_tree.z=0;
if( kdtree.radiusSearch(search_point_tree, 10, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){
double before_w=10, temp_w;
for(size_t index = 0; index < pointIdxRadiusSearch.size(); ++index){
temp_w =sqrt(pointRadiusSquaredDistance[index]);
if(temp_w - before_w > handle.finger_w*2){
break; // there are small space for finger
}
before_w=temp_w;
}
if(before_w < min_width){
min_theta_index = theta_;
min_width = before_w;
min_qua = temp_qua;
best_mat = temp_mat;
}
//for debug view
geometry_msgs::Point temp_point;
std_msgs::ColorRGBA temp_color;
temp_color.r=0; temp_color.g=0; temp_color.b=1; temp_color.a=1;
temp_point.x=x-temp_mat.getColumn(1)[0] * before_w;
temp_point.y=y-temp_mat.getColumn(1)[1] * before_w;
temp_point.z=z-temp_mat.getColumn(1)[2] * before_w;
debug_hand_marker.points.push_back(temp_point);
debug_hand_marker.colors.push_back(temp_color);
temp_point.x+=2*temp_mat.getColumn(1)[0] * before_w;
temp_point.y+=2*temp_mat.getColumn(1)[1] * before_w;
temp_point.z+=2*temp_mat.getColumn(1)[2] * before_w;
debug_hand_marker.points.push_back(temp_point);
debug_hand_marker.colors.push_back(temp_color);
}
}
geometry_msgs::PoseStamped handle_pose_stamped;
handle_pose_stamped.header = cloud_msg->header;
handle_pose_stamped.pose.position.x = x;
handle_pose_stamped.pose.position.y = y;
handle_pose_stamped.pose.position.z = z;
handle_pose_stamped.pose.orientation.x = min_qua.getX();
handle_pose_stamped.pose.orientation.y = min_qua.getY();
handle_pose_stamped.pose.orientation.z = min_qua.getZ();
handle_pose_stamped.pose.orientation.w = min_qua.getW();
std_msgs::Float64 min_width_msg;
min_width_msg.data = min_width;
pub_pose_.publish(handle_pose_stamped);
pub_debug_marker_.publish(debug_hand_marker);
pub_debug_marker_array_.publish(make_handle_array(handle_pose_stamped, handle));
jsk_recognition_msgs::SimpleHandle simple_handle;
simple_handle.header = handle_pose_stamped.header;
simple_handle.pose = handle_pose_stamped.pose;
simple_handle.handle_width = min_width;
pub_handle_.publish(simple_handle);
}
int
main (int argc, char** argv)
{
// All the objects needed
pcl::PCDReader reader;
pcl::PassThrough<PointT> pass;
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;
pcl::PCDWriter writer;
pcl::ExtractIndices<PointT> extract;
pcl::ExtractIndices<pcl::Normal> extract_normals;
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
// Datasets
pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<PointT>::Ptr cloud_filtered2 (new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2 (new pcl::PointCloud<pcl::Normal>);
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients), coefficients_cylinder (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices), inliers_cylinder (new pcl::PointIndices);
// Read in the cloud data
reader.read ("table_scene_mug_stereo_textured.pcd", *cloud);
std::cerr << "PointCloud has: " << cloud->points.size () << " data points." << std::endl;
// Build a passthrough filter to remove spurious NaNs
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 1.5);
pass.filter (*cloud_filtered);
std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
// Estimate point normals
ne.setSearchMethod (tree);
ne.setInputCloud (cloud_filtered);
ne.setKSearch (50);
ne.compute (*cloud_normals);
// Create the segmentation object for the planar model and set all the parameters
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg.setNormalDistanceWeight (0.1);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.03);
seg.setInputCloud (cloud_filtered);
seg.setInputNormals (cloud_normals);
// Obtain the plane inliers and coefficients
seg.segment (*inliers_plane, *coefficients_plane);
std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;
// Extract the planar inliers from the input cloud
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers_plane);
extract.setNegative (false);
// Write the planar inliers to disk
pcl::PointCloud<PointT>::Ptr cloud_plane (new pcl::PointCloud<PointT> ());
extract.filter (*cloud_plane);
std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
writer.write ("table_scene_mug_stereo_textured_plane.pcd", *cloud_plane, false);
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_filtered2);
extract_normals.setNegative (true);
extract_normals.setInputCloud (cloud_normals);
extract_normals.setIndices (inliers_plane);
extract_normals.filter (*cloud_normals2);
// Create the segmentation object for cylinder segmentation and set all the parameters
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_CYLINDER);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setNormalDistanceWeight (0.1);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.05);
seg.setRadiusLimits (0, 0.1);
seg.setInputCloud (cloud_filtered2);
seg.setInputNormals (cloud_normals2);
// Obtain the cylinder inliers and coefficients
seg.segment (*inliers_cylinder, *coefficients_cylinder);
std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;
// Write the cylinder inliers to disk
extract.setInputCloud (cloud_filtered2);
extract.setIndices (inliers_cylinder);
extract.setNegative (false);
pcl::PointCloud<PointT>::Ptr cloud_cylinder (new pcl::PointCloud<PointT> ());
extract.filter (*cloud_cylinder);
if (cloud_cylinder->points.empty ())
std::cerr << "Can't find the cylindrical component." << std::endl;
else
{
std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size () << " data points." << std::endl;
writer.write ("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false);
}
return (0);
}
int main (int argc, char** argv)
{
/* DOWNSAMPLING ********************************************************************************************************************/
std::ofstream output_file("properties.txt");
std::ofstream curvature("curvature.txt");
std::ofstream normals_text("normals.txt");
/*
std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height
<< " data points (" << pcl::getFieldsList (*cloud) << ")." << std::endl;
// Create the filtering object
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered);
output_file << "Number of filtered points" << std::endl;
output_file << cloud_filtered->width * cloud_filtered->height<<std::endl;
std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height
<< " data points (" << pcl::getFieldsList (*cloud_filtered) << ")." << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr descriptor (new pcl::PointCloud<pcl::PointXYZ>);
descriptor->width = 5000 ;
descriptor->height = 1 ;
descriptor->points.resize (descriptor->width * descriptor->height) ;
std::cerr << descriptor->points.size() << std::endl ;
pcl::PCDWriter writer;
writer.write ("out.pcd", *cloud_filtered, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);
*/
/***********************************************************************************************************************************/
/* CALCULATING VOLUME AND SURFACE AREA ********************************************************************************************************************/
/*pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConcaveHull<pcl::PointXYZ> chull;
chull.setInputCloud(test);
chull.reconstruct(*cloud_hull);*/
/*for (size_t i=0; i < test->points.size (); i++)
{
std::cout<< test->points[i].x <<std::endl;
std::cout<< test->points[i].y <<std::endl;
std::cout<< test->points[i].z <<std::endl;
}*/
//std::cout<<data.size()<<std::endl;
// Load input file into a PointCloud<T> with an appropriate type
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2 cloud_blob;
pcl::io::loadPCDFile ("mini_soccer_ball_downsampled.pcd", cloud_blob);
pcl::fromPCLPointCloud2 (cloud_blob, *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2 (cloud_blob, *cloud1);
//* the data should be available in cloud
// 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);
//* normals should not contain the point normals + surface curvatures
// 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);
//* cloud_with_normals = cloud + 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 (0.025);
// Set typical values for the parameters
gp3.setMu (2.5);
gp3.setMaximumNearestNeighbors (100);
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::PolygonMesh::Ptr mesh(&triangles);
pcl::PointCloud<pcl::PointXYZ>::Ptr triangle_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(mesh->cloud, *triangle_cloud);
/* for(int i = 0; i < 2; i++){
std::cout<<triangles.polygons[i]<<std::endl;
}
*/ std::cout<<"first vertice "<<triangles.polygons[0].vertices[0] << std::endl;
//std::cout<<"Prolly not gonna work "<<triangle_cloud->points[triangles.polygons[0].vertices[0]] << std::endl;
//pcl::fromPCLPointCloud2(triangles.cloud, triangle_cloud);
std::cout << "size of points " << triangle_cloud->points.size() << std::endl ;
std::cout<<triangle_cloud->points[0]<<std::endl;
for(unsigned i = 0; i < triangle_cloud->points.size(); i++){
std::cout << triangles.polgyons[i].getVector3fMap() <<"test"<< std::endl;
}
//std::cout<<"surface: "<<calculateAreaPolygon(triangles, triangle_cloud)<<std::endl;
/******************************************************************************************************************************************/
/* CALCULATING CURVATURE AND NORMALS ********************************************************************************************************************/
// 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 tree3 (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree3);
// 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 (0.03);
// Compute the features
ne.compute (*cloud_normals);
output_file << "size of points " << cloud->points.size() << std::endl ;
output_file << "size of the normals " << cloud_normals->points.size() << std::endl ;
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
/************************************************************************************************************************************/
/* PARSING DATA ********************************************************************************************************************/
int k=0 ;
float dist ;
float square_of_dist ;
float x1,y1,z1,x2,y2,z2 ;
float nu[3], nv[3], pv_pu[3], pu_pv[3] ;
float highest = triangle_cloud->points[0].z;
float lowest = triangle_cloud->points[0].z;
for ( int i = 0; i < cloud_normals->points.size() ; i++)
{
output_file<<i<<": triangulated "<<triangle_cloud->points[i].x<<", "<<triangle_cloud->points[i].y<<", "<<triangle_cloud->points[i].z<<std::endl;
output_file<<i<<": normal"<<cloud1->points[i].x<<", "<<cloud1->points[i].y<<", "<<cloud1->points[i].z<<std::endl;
if(triangle_cloud->points[i].z > highest)
{
highest = triangle_cloud->points[i].z;
}
if(triangle_cloud->points[i].z < lowest)
{
lowest = triangle_cloud->points[i].z;
}
normals_text <<i+1 <<": "<<" x-normal-> "<<cloud_normals->points[i].normal_x<<" y-normal-> "<<cloud_normals->points[i].normal_y<<" z-normal-> "<<cloud_normals->points[i].normal_z<<std::endl;
curvature <<i+1 <<": curvature: "<<cloud_normals->points[i].curvature<<std::endl;
float x, y, z, dist, nx, ny, nz, ndist;
/*
if(i != cloud_normals->points.size()-1){
x = cloud->points[i+1].x - cloud->points[i].x;
y = cloud->points[i+1].y - cloud->points[i].y;
z = cloud->points[i+1].z - cloud->points[i].z;
dist = sqrt(pow(x, 2)+pow(y, 2) + pow(z, 2));
output_file << i+1 <<" -> "<< i+2 << " distance normal: " << dist <<std::endl;
nx = triangle_cloud[i+1].indices[0] - triangle_cloud.points[i].x;
ny = triangle_cloud.points[i+1].y - triangle_cloud.points[i].y;
nz = triangle_cloud.points[i+1].z - triangle_cloud.points[i].z;
ndist = sqrt(pow(nx, 2)+pow(ny, 2) + pow(nz, 2));
output_file << i+1 <<" -> "<< i+2 << " distance triangulated: " << ndist <<std::endl;
}
*/
/*
pcl::PointXYZRGB point;
point.x = cloud_filtered_converted->points[i].x;
point.y = cloud_filtered_converted->points[i].y;
point.z = cloud_filtered_converted->points[i].z;
point.r = 0;
point.g = 100;
point.b = 200;
point_cloud_ptr->points.push_back (point);
*/
}
output_file << "highest point: "<< highest<<std::endl;
output_file << "lowest point: "<< lowest<<std::endl;
//pcl::PointCloud<pcl::PointXYZ>::Ptr test (new pcl::PointCloud<pcl::PointXYZ>);
//float surface_area = calculateAreaPolygon(test);
//std::cout<< surface_area<<std::endl;
/*
descriptor->width = k ;
descriptor->height = 1 ;
descriptor->points.resize (descriptor->width * descriptor->height) ;
std::cerr << descriptor->points.size() << std::endl ;
float voxelSize = 0.01f ; // how to find appropriate voxel resolution
pcl::octree::OctreePointCloud<pcl::PointXYZ> octree (voxelSize);
octree.setInputCloud(descriptor) ;
ctree.defineBoundingBox(0.0,0.0,0.0,3.14,3.14,3.14) ; //octree.defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ)
octree.addPointsFromInputCloud (); // octree created for block
int k_ball=0 ;
float dist_ball ;
float square_of_dist_ball ;
double X,Y,Z ;
bool occupied ;
highest = cloud_ball->points[0].z;
for ( int i = 0; i < cloud_normals_ball->points.size() ; i++)
{
if(cloud->points[i].z > highest){
highest = cloud_ball->points[i].z;
}
for (int j = i+1 ; (j < cloud_normals_ball->points.size()) ; j++)
{
x1 = cloud_ball->points[i].x ;
y1 = cloud_ball->points[i].y ;
z1 = cloud_ball->points[i].z ;
nu[0] = cloud_normals_ball->points[i].normal_x ;
nu[1] = cloud_normals_ball->points[i].normal_y ;
nu[2] = cloud_normals_ball->points[i].normal_z ;
x2 = cloud_ball->points[j].x ;
y2 = cloud_ball->points[j].y ;
z2 = cloud_ball->points[j].z ;
nv[0] = cloud_normals_ball->points[j].normal_x ;
nv[1] = cloud_normals_ball->points[j].normal_y ;
nv[2] = cloud_normals_ball->points[j].normal_z ;
square_of_dist = ((x2-x1)*(x2-x1)) + ((y2-y1)*(y2-y1)) + ((z2-z1)*(z2-z1)) ;
dist = sqrt(square_of_dist) ;
//std::cerr << dist ;
pv_pu[0] = x2-x1 ;
pv_pu[1] = y2-y1 ;
pv_pu[2] = z2-z1 ;
pu_pv[0] = x1-x2 ;
pu_pv[1] = y1-y2 ;
pu_pv[2] = z1-z2 ;
if ((dist > 0.0099) && (dist < 0.0101))
{
X = angle_between_vectors (nu, nv) ;
Y = angle_between_vectors (nu, pv_pu) ;
Z = angle_between_vectors (nv, pu_pv) ;
// output_file << descriptor->points[k].x << "\t" << descriptor->points[k].y << "\t" << descriptor->points[k].z ;
// output_file << "\n";
//k_ball = k_ball + 1 ;
occupied = octree.isVoxelOccupiedAtPoint (X,Y,Z) ;
if (occupied == 1)
{
//k_ball = k_ball + 1 ;
std::cerr << "Objects Matched" << "\t" << k_ball << std::endl ;
return(0);
}
}
}
}
*/
/***********************************************************************************************************************************/
/*
points.open("secondItemPoints.txt");
myfile<<"Second point \n";
myfile<<"second highest "<<highest;
for(int i = 0; i < cloud_normals->points.size(); i++){
points<<cloud->points[i].x<<", "<<cloud->points[i].y<<", "<<cloud->points[i].z<<"\n";
if(cloud->points[i].z >= highest - (highest/100)){
myfile<<cloud->points[i].x<<", "<<cloud->points[i].y<<", "<<cloud->points[i].z<<"\n";
}
}
points.close();
myfile.close();
std::cerr << "Objects Not Matched" << "\t" << k_ball << std::endl ;
*/
//output_file <<"Volume: "<<volume <<std::endl;
//output_file <<"Surface Area: "<<surface_area <<std::endl;
return (0);
}
