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, typename LeafContainerT, typename BranchContainerT> void
pcl::octree::OctreePointCloudSuperVoxel<PointT, LeafContainerT, BranchContainerT>::iterateVoxelLabels (const std::map<uint32_t,PointSuperVoxel> &supervoxel_centers)
{
typename OctreeSuperVoxelT::LeafNodeIterator leaf_itr;
LeafContainerT* leafContainer;
std::map<uint32_t, PointSuperVoxel>::const_iterator it;
for ( leaf_itr = this->leaf_begin (); leaf_itr != this->leaf_end (); ++leaf_itr)
{
leafContainer = dynamic_cast<LeafContainerT*> (*leaf_itr);
//If this node is labeled, try to spread its label to all of its neighbors
if (leafContainer->getLabel () != 0)
{
const PointSuperVoxel &point = leafContainer->getCentroid ();
//Check all neighbors!
it = supervoxel_centers.find (point.label);
//Make sure we found the label...
assert (it != supervoxel_centers.end ());
checkNeighbors (leafContainer, it->second);
}
}
for ( leaf_itr = this->leaf_begin (); leaf_itr != this->leaf_end (); ++leaf_itr)
{
leafContainer = dynamic_cast<LeafContainerT*> (*leaf_itr);
leafContainer->pushLabel ();
}
}
template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
pcl::octree::OctreePointCloudSuperVoxel<PointT, LeafContainerT, BranchContainerT>::updateCenters (std::map<uint32_t,PointSuperVoxel> &supervoxel_centers)
{
typename OctreeSuperVoxelT::LeafNodeIterator leaf_itr;
leaf_itr = this->leaf_begin ();
LeafContainerT* leafContainer;
//Reset the distances to 0, this stores the number of labels
std::map<uint32_t,PointSuperVoxel>::iterator it = supervoxel_centers.begin ();
std::map<uint32_t,PointSuperVoxel>::iterator it_end = supervoxel_centers.end ();
for ( ; it != it_end; ++it)
{
it->second.x = it->second.y = it->second.z = 0.0f;
it->second.R = it->second.G = it->second.B = 0.0f;
it->second.normal_x = it->second.normal_y = it->second.normal_z = 0.0f;
it->second.distance = 0.0f;
}
for ( ; leaf_itr != this->leaf_end (); ++leaf_itr)
{
leafContainer = dynamic_cast<LeafContainerT*> (*leaf_itr);
if (leafContainer->getLabel () != 0)
{
const PointSuperVoxel &point = leafContainer->getCentroid ();
//Check all neighbors!
it = supervoxel_centers.find (point.label);
assert (it != supervoxel_centers.end ());
//Add in the values, increment counter
it->second.x += point.x;
it->second.y += point.y;
it->second.z += point.z;
it->second.R += point.R;
it->second.G += point.G;
it->second.B += point.B;
it->second.normal_x += point.normal_x;
it->second.normal_y += point.normal_y;
it->second.normal_z += point.normal_z;
it->second.distance += 1.0f;
}
}
for ( it = supervoxel_centers.begin () ; it != it_end; ++it)
{
it->second.x /= it->second.distance;
it->second.y /= it->second.distance;
it->second.z /= it->second.distance;
it->second.R /= it->second.distance;
it->second.G /= it->second.distance;
it->second.B /= it->second.distance;
it->second.normal_x /= it->second.distance;
it->second.normal_y /= it->second.distance;
it->second.normal_z /= it->second.distance;
float norm = std::sqrt (it->second.normal_x * it->second.normal_x
+ it->second.normal_y * it->second.normal_y
+ it->second.normal_z * it->second.normal_z);
it->second.normal_x /= norm;
it->second.normal_y /= norm;
it->second.normal_z /= norm;
}
}
template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
pcl::octree::OctreePointCloudSuperVoxel<PointT, LeafContainerT, BranchContainerT>::getCentroidCloud (pcl::PointCloud<PointSuperVoxel>::Ptr &cloud )
{
bool use_existing_points = true;
if ((cloud == 0) || (cloud->size () != this->OctreeBaseT::getLeafCount ()))
{
cloud.reset ();
cloud = boost::make_shared<pcl::PointCloud<PointSuperVoxel> > ();
cloud->reserve (this->OctreeBaseT::getLeafCount ());
use_existing_points = false;
}
typename OctreeSuperVoxelT::LeafNodeIterator leaf_itr;
leaf_itr = this->leaf_begin ();
LeafContainerT* leafContainer;
PointSuperVoxel point;
for ( int index = 0; leaf_itr != this->leaf_end (); ++leaf_itr,++index)
{
leafContainer = dynamic_cast<LeafContainerT*> (*leaf_itr);
if (!use_existing_points)
{
leafContainer->getCentroid (point);
cloud->push_back (point);
}
else
{
leafContainer->getCentroid (cloud->points[index]);
}
}
}
template<typename PointT, typename LeafContainerT, typename BranchContainerT> size_t
pcl::octree::OctreePointCloudSuperVoxel<PointT, LeafContainerT, BranchContainerT>::getVoxelCentroids (std::vector<PointSuperVoxel, Eigen::aligned_allocator<PointSuperVoxel> > &voxel_centroids_arg) const
{
OctreeKey new_key;
// reset output vector
voxel_centroids_arg.clear ();
voxel_centroids_arg.reserve (this->leafCount_);
typename OctreeSuperVoxelT::LeafNodeIterator leaf_itr;
leaf_itr = this->leaf_begin ();
LeafContainerT* leafContainer;
int num = 0;
for ( ; leaf_itr != this->leaf_end (); ++leaf_itr)
{
leafContainer = dynamic_cast<LeafContainerT*> (*leaf_itr);
PointSuperVoxel new_centroid;
leafContainer->getCentroid (new_centroid);
voxel_centroids_arg.push_back (new_centroid);
if (leafContainer->getLabel() != 0)
++num;
}
//getVoxelCentroidsRecursive (this->rootNode_, new_key, voxel_centroid_list_arg );
// std::cout << "There are "<<num <<" labeled points!\n";
// return size of centroid vector
return (voxel_centroids_arg.size ());
}
template<typename PointT, typename LeafContainerT, typename BranchContainerT> bool
pcl::octree::OctreePointCloudVoxelCentroid<PointT, LeafContainerT, BranchContainerT>::getVoxelCentroidAtPoint (
const PointT& point_arg, PointT& voxel_centroid_arg) const
{
OctreeKey key;
LeafContainerT* leaf = NULL;
// generate key
genOctreeKeyforPoint (point_arg, key);
leaf = this->findLeaf (key);
if (leaf)
leaf->getCentroid (voxel_centroid_arg);
return (leaf != NULL);
}
template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
pcl::octree::OctreePointCloudSuperVoxel<PointT, LeafContainerT, BranchContainerT>::computeNeighbors ()
{
OctreeKey current_key, neighbor_key;
typename OctreeSuperVoxelT::LeafNodeIterator leaf_itr;
LeafContainerT *leaf_container, *neighbor_container;
//Now iterate through finding neighbors, and create edges
//This is to avoid connecting voxels where centroids are on the outer edges
//of the voxels, which can lead to "neighbors" that really aren't
//1.2 is just a fudging coefficient - this means it's sqrt(1.2) times the
//maximum distance from voxel center to voxel center (ie voxels that share only a vertex)
float max_dist_squared = this->resolution_ * this->resolution_ * 3.0f * 1.2f;
for ( leaf_itr = this->leaf_begin () ; leaf_itr != this->leaf_end (); ++leaf_itr)
{
current_key = leaf_itr.getCurrentOctreeKey ();
leaf_container = dynamic_cast<LeafContainerT*> (*leaf_itr);
PointSuperVoxel leaf_centroid = leaf_container->getCentroid ();
PointSuperVoxel neighbor_centroid;
for (int dx = -1; dx <= 1; ++dx)
{
for (int dy = -1; dy <= 1; ++dy)
{
for (int dz = -1; dz <= 1; ++dz)
{
neighbor_key.x = current_key.x + dx;
neighbor_key.y = current_key.y + dy;
neighbor_key.z = current_key.z + dz;
LeafContainerT *neighbor = OctreeBaseT::findLeaf (neighbor_key);
if (neighbor)
{
neighbor_centroid = neighbor->getCentroid ();
float dist = pow((neighbor_centroid.x - leaf_centroid.x),2)
+pow((neighbor_centroid.y - leaf_centroid.y),2)
+pow((neighbor_centroid.z - leaf_centroid.z),2);
if ( dist < max_dist_squared)
leaf_container->addNeighbor (neighbor_container);
}
}
}
}
}
}
template<typename PointT, typename LeafContainerT, typename BranchContainerT> const pcl::PointSuperVoxel&
pcl::octree::OctreePointCloudSuperVoxel<PointT, LeafContainerT, BranchContainerT>::setPointLabel (const PointSuperVoxel &point_arg, uint32_t label_arg)
{
OctreeKey key;
// generate key for point
PointT point;
point.x = point_arg.x;
point.y = point_arg.y;
point.z = point_arg.z;
this->genOctreeKeyforPoint (point, key);
// Make sure point exists
LeafContainerT* leaf = dynamic_cast<LeafContainerT*> (OctreeBaseT::findLeaf (key));
assert (leaf != 0);
leaf->setLabel (label_arg);
leaf->setTempLabel (label_arg);
leaf->setDistance (0.0f);
return leaf->getCentroid ();
}
template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
pcl::octree::OctreePointCloudSuperVoxel<PointT, LeafContainerT, BranchContainerT>::insertNormals (const std::vector<PointSuperVoxel, Eigen::aligned_allocator<PointSuperVoxel> > &voxel_centroids_arg)
{
assert (voxel_centroids_arg.size () == this->leaf_count_);
typename OctreeSuperVoxelT::LeafNodeIterator leaf_itr;
leaf_itr = this->leaf_begin ();
LeafContainerT* leafContainer;
std::vector<PointSuperVoxel, Eigen::aligned_allocator<PointSuperVoxel> >::const_iterator it_centroids = voxel_centroids_arg.begin ();
int num = 0;
for ( ; leaf_itr != this->leaf_end (); ++leaf_itr, ++it_centroids)
{
leafContainer = dynamic_cast<LeafContainerT*> (*leaf_itr);
leafContainer->setCentroid (*it_centroids);
if (leafContainer->getLabel() != 0)
++num;
}
// std::cout << "Num labeled in normal insert "<<num<<"\n";
}
template<typename PointT, typename LeafContainerT, typename BranchContainerT> bool
pcl::octree::OctreePointCloudSuperVoxel<PointT, LeafContainerT, BranchContainerT>::getVoxelCentroidAtPoint (
const PointT& point_arg, PointSuperVoxel& voxel_centroid_arg) const
{
OctreeKey key;
LeafNode* leaf = 0;
// generate key
genOctreeKeyforPoint (point_arg, key);
leaf = this->findLeaf (key);
if (leaf)
{
LeafContainerT* container = leaf;
container->getCentroid (voxel_centroid_arg);
}
return (leaf != 0);
}
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 ();
}
}
}
