template <typename PointT> pcl::PointCloud<pcl::PointXYZL>::Ptr
pcl::SupervoxelClustering<PointT>::getLabeledCloud () const
{
pcl::PointCloud<pcl::PointXYZL>::Ptr labeled_cloud (new pcl::PointCloud<pcl::PointXYZL>);
pcl::copyPointCloud (*input_,*labeled_cloud);
pcl::PointCloud <pcl::PointXYZL>::iterator i_labeled;
typename pcl::PointCloud <PointT>::const_iterator i_input = input_->begin ();
std::vector <int> indices;
std::vector <float> sqr_distances;
for (i_labeled = labeled_cloud->begin (); i_labeled != labeled_cloud->end (); ++i_labeled,++i_input)
{
if ( !pcl::isFinite<PointT> (*i_input))
i_labeled->label = 0;
else
{
i_labeled->label = 0;
LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
VoxelData& voxel_data = leaf->getData ();
if (voxel_data.owner_)
i_labeled->label = voxel_data.owner_->getLabel ();
}
}
return (labeled_cloud);
}
template <typename PointT> pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
pcl::SupervoxelClustering<PointT>::getColoredCloud () const
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud = boost::make_shared <pcl::PointCloud<pcl::PointXYZRGBA> >();
pcl::copyPointCloud (*input_,*colored_cloud);
pcl::PointCloud <pcl::PointXYZRGBA>::iterator i_colored;
typename pcl::PointCloud <PointT>::const_iterator i_input = input_->begin ();
std::vector <int> indices;
std::vector <float> sqr_distances;
for (i_colored = colored_cloud->begin (); i_colored != colored_cloud->end (); ++i_colored,++i_input)
{
if ( !pcl::isFinite<PointT> (*i_input))
i_colored->rgb = 0;
else
{
i_colored->rgb = 0;
LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
VoxelData& voxel_data = leaf->getData ();
if (voxel_data.owner_)
i_colored->rgba = label_colors_[voxel_data.owner_->getLabel ()];
}
}
return (colored_cloud);
}
template <typename PointT> void
pcl::SupervoxelClustering<PointT>::computeVoxelData ()
{
voxel_centroid_cloud_.reset (new PointCloudT);
voxel_centroid_cloud_->resize (adjacency_octree_->getLeafCount ());
typename LeafVectorT::iterator leaf_itr = adjacency_octree_->begin ();
typename PointCloudT::iterator cent_cloud_itr = voxel_centroid_cloud_->begin ();
for (int idx = 0 ; leaf_itr != adjacency_octree_->end (); ++leaf_itr, ++cent_cloud_itr, ++idx)
{
VoxelData& new_voxel_data = (*leaf_itr)->getData ();
//Add the point to the centroid cloud
new_voxel_data.getPoint (*cent_cloud_itr);
//voxel_centroid_cloud_->push_back(new_voxel_data.getPoint ());
new_voxel_data.idx_ = idx;
}
//If normals were provided
if (input_normals_)
{
//Verify that input normal cloud size is same as input cloud size
assert (input_normals_->size () == input_->size ());
//For every point in the input cloud, find its corresponding leaf
typename NormalCloudT::const_iterator normal_itr = input_normals_->begin ();
for (typename PointCloudT::const_iterator input_itr = input_->begin (); input_itr != input_->end (); ++input_itr, ++normal_itr)
{
//If the point is not finite we ignore it
if ( !pcl::isFinite<PointT> (*input_itr))
continue;
//Otherwise look up its leaf container
LeafContainerT* leaf = adjacency_octree_->getLeafContainerAtPoint (*input_itr);
//Get the voxel data object
VoxelData& voxel_data = leaf->getData ();
//Add this normal in (we will normalize at the end)
voxel_data.normal_ += normal_itr->getNormalVector4fMap ();
voxel_data.curvature_ += normal_itr->curvature;
}
//Now iterate through the leaves and normalize
for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
{
VoxelData& voxel_data = (*leaf_itr)->getData ();
voxel_data.normal_.normalize ();
voxel_data.owner_ = 0;
voxel_data.distance_ = std::numeric_limits<float>::max ();
//Get the number of points in this leaf
int num_points = (*leaf_itr)->getPointCounter ();
voxel_data.curvature_ /= num_points;
}
}
else //Otherwise just compute the normals
{
for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
{
VoxelData& new_voxel_data = (*leaf_itr)->getData ();
//For every point, get its neighbors, build an index vector, compute normal
std::vector<int> indices;
indices.reserve (81);
//Push this point
indices.push_back (new_voxel_data.idx_);
for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
{
VoxelData& neighb_voxel_data = (*neighb_itr)->getData ();
//Push neighbor index
indices.push_back (neighb_voxel_data.idx_);
//Get neighbors neighbors, push onto cloud
for (typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr)
{
VoxelData& neighb2_voxel_data = (*neighb_neighb_itr)->getData ();
indices.push_back (neighb2_voxel_data.idx_);
}
}
//Compute normal
pcl::computePointNormal (*voxel_centroid_cloud_, indices, new_voxel_data.normal_, new_voxel_data.curvature_);
pcl::flipNormalTowardsViewpoint (voxel_centroid_cloud_->points[new_voxel_data.idx_], 0.0f,0.0f,0.0f, new_voxel_data.normal_);
new_voxel_data.normal_[3] = 0.0f;
new_voxel_data.normal_.normalize ();
new_voxel_data.owner_ = 0;
new_voxel_data.distance_ = std::numeric_limits<float>::max ();
}
}
}
