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