Ejemplo n.º 1
0
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;
  }
  
  
}
Ejemplo n.º 2
0
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 ();
  }
}
Ejemplo n.º 3
0
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 ());
}
Ejemplo n.º 4
0
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";
}