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>::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> 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> 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";
}