C++ LeafContainerT::getCentroid方法代码示例

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