template <typename PointT> int
pcl::SupervoxelClustering<PointT>::getMaxLabel () const
{
int max_label = 0;
for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
{
int temp = sv_itr->getLabel ();
if (temp > max_label)
max_label = temp;
}
return max_label;
}
template <typename PointT> void
pcl::SupervoxelClustering<PointT>::getSupervoxelAdjacency (std::multimap<uint32_t, uint32_t> &label_adjacency) const
{
label_adjacency.clear ();
for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
{
uint32_t label = sv_itr->getLabel ();
std::set<uint32_t> neighbor_labels;
sv_itr->getNeighborLabels (neighbor_labels);
for (std::set<uint32_t>::iterator label_itr = neighbor_labels.begin (); label_itr != neighbor_labels.end (); ++label_itr)
label_adjacency.insert (std::pair<uint32_t,uint32_t> (label, *label_itr) );
//if (neighbor_labels.size () == 0)
// std::cout << label<<"(size="<<sv_itr->size () << ") has "<<neighbor_labels.size () << "\n";
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////// GETTER FUNCTIONS
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SupervoxelClustering<PointT>::getSupervoxelAdjacencyList (VoxelAdjacencyList &adjacency_list_arg) const
{
adjacency_list_arg.clear ();
//Add a vertex for each label, store ids in map
std::map <uint32_t, VoxelID> label_ID_map;
for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
{
VoxelID node_id = add_vertex (adjacency_list_arg);
adjacency_list_arg[node_id] = (sv_itr->getLabel ());
label_ID_map.insert (std::make_pair (sv_itr->getLabel (), node_id));
}
for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
{
uint32_t label = sv_itr->getLabel ();
std::set<uint32_t> neighbor_labels;
sv_itr->getNeighborLabels (neighbor_labels);
for (std::set<uint32_t>::iterator label_itr = neighbor_labels.begin (); label_itr != neighbor_labels.end (); ++label_itr)
{
bool edge_added;
EdgeID edge;
VoxelID u = (label_ID_map.find (label))->second;
VoxelID v = (label_ID_map.find (*label_itr))->second;
boost::tie (edge, edge_added) = add_edge (u,v,adjacency_list_arg);
//Calc distance between centers, set as edge weight
if (edge_added)
{
VoxelData centroid_data = (sv_itr)->getCentroid ();
//Find the neighbhor with this label
VoxelData neighb_centroid_data;
for (typename HelperListT::const_iterator neighb_itr = supervoxel_helpers_.cbegin (); neighb_itr != supervoxel_helpers_.cend (); ++neighb_itr)
{
if (neighb_itr->getLabel () == (*label_itr))
{
neighb_centroid_data = neighb_itr->getCentroid ();
break;
}
}
float length = voxelDataDistance (centroid_data, neighb_centroid_data);
adjacency_list_arg[edge] = length;
}
}
}
}
template <typename PointT> pcl::PointCloud<pcl::PointXYZL>::Ptr
pcl::SupervoxelClustering<PointT>::getLabeledVoxelCloud () const
{
pcl::PointCloud<pcl::PointXYZL>::Ptr labeled_voxel_cloud (new pcl::PointCloud<pcl::PointXYZL>);
for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
{
typename PointCloudT::Ptr voxels;
sv_itr->getVoxels (voxels);
pcl::PointCloud<pcl::PointXYZL> xyzl_copy;
copyPointCloud (*voxels, xyzl_copy);
pcl::PointCloud<pcl::PointXYZL>::iterator xyzl_copy_itr = xyzl_copy.begin ();
for ( ; xyzl_copy_itr != xyzl_copy.end (); ++xyzl_copy_itr)
xyzl_copy_itr->label = sv_itr->getLabel ();
*labeled_voxel_cloud += xyzl_copy;
}
return labeled_voxel_cloud;
}
template <typename PointT> pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
pcl::SupervoxelClustering<PointT>::getColoredVoxelCloud () const
{
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud = boost::make_shared< pcl::PointCloud<pcl::PointXYZRGBA> > ();
for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
{
typename PointCloudT::Ptr voxels;
sv_itr->getVoxels (voxels);
pcl::PointCloud<pcl::PointXYZRGBA> rgb_copy;
copyPointCloud (*voxels, rgb_copy);
pcl::PointCloud<pcl::PointXYZRGBA>::iterator rgb_copy_itr = rgb_copy.begin ();
for ( ; rgb_copy_itr != rgb_copy.end (); ++rgb_copy_itr)
rgb_copy_itr->rgba = label_colors_ [sv_itr->getLabel ()];
*colored_cloud += rgb_copy;
}
return colored_cloud;
}
