C++ LeafContainerT类代码示例

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