template <typename PointT> void
pcl::SupervoxelClustering<PointT>::reseedSupervoxels ()
{
//Go through each supervoxel and remove all it's leaves
for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
{
sv_itr->removeAllLeaves ();
}
std::vector<int> closest_index;
std::vector<float> distance;
//Now go through each supervoxel, find voxel closest to its center, add it in
for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
{
PointT point;
sv_itr->getXYZ (point.x, point.y, point.z);
voxel_kdtree_->nearestKSearch (point, 1, closest_index, distance);
LeafContainerT* seed_leaf = adjacency_octree_->at (closest_index[0]);
if (seed_leaf)
{
sv_itr->addLeaf (seed_leaf);
}
else
{
PCL_WARN ("Could not find leaf in pcl::SupervoxelClustering<PointT>::reseedSupervoxels - supervoxel will be deleted \n");
}
}
}
template <typename PointT> void
pcl::SupervoxelClustering<PointT>::refineSupervoxels (int num_itr, std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
{
if (supervoxel_helpers_.size () == 0)
{
PCL_ERROR ("[pcl::SupervoxelClustering::refineVoxelNormals] Supervoxels not extracted, doing nothing - (Call extract first!) \n");
return;
}
int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_);
for (int i = 0; i < num_itr; ++i)
{
for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
{
sv_itr->refineNormals ();
}
reseedSupervoxels ();
expandSupervoxels (max_depth);
}
makeSupervoxels (supervoxel_clusters);
}
template <typename PointT> void
pcl::SupervoxelClustering<PointT>::expandSupervoxels ( int depth )
{
for (int i = 1; i < depth; ++i)
{
//Expand the the supervoxels by one iteration
for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
{
sv_itr->expand ();
}
//Update the centers to reflect new centers
for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); )
{
if (sv_itr->size () == 0)
{
sv_itr = supervoxel_helpers_.erase (sv_itr);
}
else
{
sv_itr->updateCentroid ();
++sv_itr;
}
}
}
}
template <typename PointT> void
pcl::SupervoxelClustering<PointT>::makeSupervoxels (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
{
supervoxel_clusters.clear ();
for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
{
uint32_t label = sv_itr->getLabel ();
supervoxel_clusters[label].reset (new Supervoxel<PointT>);
sv_itr->getXYZ (supervoxel_clusters[label]->centroid_.x,supervoxel_clusters[label]->centroid_.y,supervoxel_clusters[label]->centroid_.z);
sv_itr->getRGB (supervoxel_clusters[label]->centroid_.rgba);
sv_itr->getNormal (supervoxel_clusters[label]->normal_);
sv_itr->getVoxels (supervoxel_clusters[label]->voxels_);
sv_itr->getNormals (supervoxel_clusters[label]->normals_);
}
}
template <typename PointT> void
pcl::SupervoxelClustering<PointT>::makeSupervoxels (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters)
{
supervoxel_clusters.clear ();
for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
{
uint32_t label = sv_itr->getLabel ();
supervoxel_clusters[label] = boost::make_shared<Supervoxel<PointT> > ();
sv_itr->getXYZ (supervoxel_clusters[label]->centroid_.x,supervoxel_clusters[label]->centroid_.y,supervoxel_clusters[label]->centroid_.z);
sv_itr->getRGB (supervoxel_clusters[label]->centroid_.rgba);
sv_itr->getNormal (supervoxel_clusters[label]->normal_);
sv_itr->getVoxels (supervoxel_clusters[label]->voxels_);
sv_itr->getNormals (supervoxel_clusters[label]->normals_);
}
//Make sure that color vector is big enough
initializeLabelColors ();
}
