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> 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> 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> 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);
}
