Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of Willow Garage, Inc. nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * Author : jpapon@gmail.com 00036 * Email : jpapon@gmail.com 00037 * 00038 */ 00039 00040 #ifndef PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_ 00041 #define PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_ 00042 00043 #include <pcl/segmentation/supervoxel_clustering.h> 00044 00045 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00046 template <typename PointT> 00047 pcl::SupervoxelClustering<PointT>::SupervoxelClustering (float voxel_resolution, float seed_resolution, bool use_single_camera_transform) : 00048 resolution_ (voxel_resolution), 00049 seed_resolution_ (seed_resolution), 00050 adjacency_octree_ (), 00051 voxel_centroid_cloud_ (), 00052 color_importance_(0.1f), 00053 spatial_importance_(0.4f), 00054 normal_importance_(1.0f), 00055 label_colors_ (0) 00056 { 00057 label_colors_.reserve (MAX_LABEL); 00058 label_colors_.push_back (static_cast<uint32_t>(255) << 16 | static_cast<uint32_t>(0) << 8 | static_cast<uint32_t>(0)); 00059 00060 srand (static_cast<unsigned int> (time (0))); 00061 for (size_t i_segment = 0; i_segment < MAX_LABEL - 1; i_segment++) 00062 { 00063 uint8_t r = static_cast<uint8_t>( (rand () % 256)); 00064 uint8_t g = static_cast<uint8_t>( (rand () % 256)); 00065 uint8_t b = static_cast<uint8_t>( (rand () % 256)); 00066 label_colors_.push_back (static_cast<uint32_t>(r) << 16 | static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b)); 00067 } 00068 00069 adjacency_octree_ = boost::make_shared <OctreeAdjacencyT> (resolution_); 00070 if (use_single_camera_transform) 00071 adjacency_octree_->setTransformFunction (boost::bind (&SupervoxelClustering::transformFunction, this, _1)); 00072 } 00073 00074 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00075 template <typename PointT> 00076 pcl::SupervoxelClustering<PointT>::~SupervoxelClustering () 00077 { 00078 00079 } 00080 00081 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00082 template <typename PointT> void 00083 pcl::SupervoxelClustering<PointT>::setInputCloud (typename pcl::PointCloud<PointT>::ConstPtr cloud) 00084 { 00085 if ( cloud->size () == 0 ) 00086 { 00087 PCL_ERROR ("[pcl::SupervoxelClustering::setInputCloud] Empty cloud set, doing nothing \n"); 00088 return; 00089 } 00090 00091 input_ = cloud; 00092 adjacency_octree_->setInputCloud (cloud); 00093 } 00094 00095 00096 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00097 template <typename PointT> void 00098 pcl::SupervoxelClustering<PointT>::extract (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters) 00099 { 00100 //timer_.reset (); 00101 //double t_start = timer_.getTime (); 00102 //std::cout << "Init compute \n"; 00103 bool segmentation_is_possible = initCompute (); 00104 if ( !segmentation_is_possible ) 00105 { 00106 deinitCompute (); 00107 return; 00108 } 00109 00110 //std::cout << "Preparing for segmentation \n"; 00111 segmentation_is_possible = prepareForSegmentation (); 00112 if ( !segmentation_is_possible ) 00113 { 00114 deinitCompute (); 00115 return; 00116 } 00117 00118 //double t_prep = timer_.getTime (); 00119 //std::cout << "Placing Seeds" << std::endl; 00120 std::vector<PointT, Eigen::aligned_allocator<PointT> > seed_points; 00121 selectInitialSupervoxelSeeds (seed_points); 00122 //std::cout << "Creating helpers "<<std::endl; 00123 createSupervoxelHelpers (seed_points); 00124 //double t_seeds = timer_.getTime (); 00125 00126 00127 //std::cout << "Expanding the supervoxels" << std::endl; 00128 int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_); 00129 expandSupervoxels (max_depth); 00130 //double t_iterate = timer_.getTime (); 00131 00132 //std::cout << "Making Supervoxel structures" << std::endl; 00133 makeSupervoxels (supervoxel_clusters); 00134 //double t_supervoxels = timer_.getTime (); 00135 00136 // std::cout << "--------------------------------- Timing Report --------------------------------- \n"; 00137 // std::cout << "Time to prep (normals, neighbors, voxelization)="<<t_prep-t_start<<" ms\n"; 00138 // std::cout << "Time to seed clusters ="<<t_seeds-t_prep<<" ms\n"; 00139 // std::cout << "Time to expand clusters ="<<t_iterate-t_seeds<<" ms\n"; 00140 // std::cout << "Time to create supervoxel structures ="<<t_supervoxels-t_iterate<<" ms\n"; 00141 // std::cout << "Total run time ="<<t_supervoxels-t_start<<" ms\n"; 00142 // std::cout << "--------------------------------------------------------------------------------- \n"; 00143 00144 deinitCompute (); 00145 } 00146 00147 00148 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00149 template <typename PointT> void 00150 pcl::SupervoxelClustering<PointT>::refineSupervoxels (int num_itr, std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters) 00151 { 00152 if (supervoxel_helpers_.size () == 0) 00153 { 00154 PCL_ERROR ("[pcl::SupervoxelClustering::refineVoxelNormals] Supervoxels not extracted, doing nothing - (Call extract first!) \n"); 00155 return; 00156 } 00157 00158 int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_); 00159 for (int i = 0; i < num_itr; ++i) 00160 { 00161 for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr) 00162 { 00163 sv_itr->refineNormals (); 00164 } 00165 00166 reseedSupervoxels (); 00167 expandSupervoxels (max_depth); 00168 } 00169 00170 00171 makeSupervoxels (supervoxel_clusters); 00172 00173 } 00174 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00175 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00176 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00177 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00178 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00179 00180 00181 template <typename PointT> bool 00182 pcl::SupervoxelClustering<PointT>::prepareForSegmentation () 00183 { 00184 00185 // if user forgot to pass point cloud or if it is empty 00186 if ( input_->points.size () == 0 ) 00187 return (false); 00188 00189 //Add the new cloud of data to the octree 00190 //std::cout << "Populating adjacency octree with new cloud \n"; 00191 //double prep_start = timer_.getTime (); 00192 adjacency_octree_->addPointsFromInputCloud (); 00193 //double prep_end = timer_.getTime (); 00194 //std::cout<<"Time elapsed populating octree with next frame ="<<prep_end-prep_start<<" ms\n"; 00195 00196 //Compute normals and insert data for centroids into data field of octree 00197 //double normals_start = timer_.getTime (); 00198 computeVoxelData (); 00199 //double normals_end = timer_.getTime (); 00200 //std::cout << "Time elapsed finding normals and pushing into octree ="<<normals_end-normals_start<<" ms\n"; 00201 00202 return true; 00203 } 00204 00205 00206 template <typename PointT> void 00207 pcl::SupervoxelClustering<PointT>::computeVoxelData () 00208 { 00209 voxel_centroid_cloud_ = boost::make_shared<PointCloudT> (); 00210 voxel_centroid_cloud_->resize (adjacency_octree_->getLeafCount ()); 00211 typename LeafVectorT::iterator leaf_itr = adjacency_octree_->begin (); 00212 typename PointCloudT::iterator cent_cloud_itr = voxel_centroid_cloud_->begin (); 00213 for (int idx = 0 ; leaf_itr != adjacency_octree_->end (); ++leaf_itr, ++cent_cloud_itr, ++idx) 00214 { 00215 VoxelData& new_voxel_data = (*leaf_itr)->getData (); 00216 //Add the point to the centroid cloud 00217 new_voxel_data.getPoint (*cent_cloud_itr); 00218 //voxel_centroid_cloud_->push_back(new_voxel_data.getPoint ()); 00219 new_voxel_data.idx_ = idx; 00220 } 00221 00222 00223 for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr) 00224 { 00225 VoxelData& new_voxel_data = (*leaf_itr)->getData (); 00226 //For every point, get its neighbors, build an index vector, compute normal 00227 std::vector<int> indices; 00228 indices.reserve (81); 00229 //Push this point 00230 indices.push_back (new_voxel_data.idx_); 00231 for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr) 00232 { 00233 VoxelData& neighb_voxel_data = (*neighb_itr)->getData (); 00234 //Push neighbor index 00235 indices.push_back (neighb_voxel_data.idx_); 00236 //Get neighbors neighbors, push onto cloud 00237 for (typename LeafContainerT::iterator neighb_neighb_itr=(*neighb_itr)->begin (); neighb_neighb_itr!=(*neighb_itr)->end (); ++neighb_neighb_itr) 00238 { 00239 VoxelData& neighb2_voxel_data = (*neighb_neighb_itr)->getData (); 00240 indices.push_back (neighb2_voxel_data.idx_); 00241 } 00242 } 00243 //Compute normal 00244 pcl::computePointNormal (*voxel_centroid_cloud_, indices, new_voxel_data.normal_, new_voxel_data.curvature_); 00245 pcl::flipNormalTowardsViewpoint (voxel_centroid_cloud_->points[new_voxel_data.idx_], 0.0f,0.0f,0.0f, new_voxel_data.normal_); 00246 new_voxel_data.normal_[3] = 0.0f; 00247 new_voxel_data.normal_.normalize (); 00248 new_voxel_data.owner_ = 0; 00249 new_voxel_data.distance_ = std::numeric_limits<float>::max (); 00250 } 00251 00252 00253 } 00254 00255 00256 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00257 template <typename PointT> void 00258 pcl::SupervoxelClustering<PointT>::expandSupervoxels ( int depth ) 00259 { 00260 00261 00262 for (int i = 1; i < depth; ++i) 00263 { 00264 //Expand the the supervoxels by one iteration 00265 for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr) 00266 { 00267 sv_itr->expand (); 00268 } 00269 00270 //Update the centers to reflect new centers 00271 for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ) 00272 { 00273 if (sv_itr->size () == 0) 00274 { 00275 sv_itr = supervoxel_helpers_.erase (sv_itr); 00276 } 00277 else 00278 { 00279 sv_itr->updateCentroid (); 00280 ++sv_itr; 00281 } 00282 } 00283 00284 } 00285 00286 } 00287 00288 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00289 template <typename PointT> void 00290 pcl::SupervoxelClustering<PointT>::makeSupervoxels (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters) 00291 { 00292 supervoxel_clusters.clear (); 00293 for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr) 00294 { 00295 uint32_t label = sv_itr->getLabel (); 00296 supervoxel_clusters[label] = boost::make_shared<Supervoxel<PointT> > (); 00297 sv_itr->getXYZ (supervoxel_clusters[label]->centroid_.x,supervoxel_clusters[label]->centroid_.y,supervoxel_clusters[label]->centroid_.z); 00298 sv_itr->getRGB (supervoxel_clusters[label]->centroid_.rgba); 00299 sv_itr->getNormal (supervoxel_clusters[label]->normal_); 00300 sv_itr->getVoxels (supervoxel_clusters[label]->voxels_); 00301 sv_itr->getNormals (supervoxel_clusters[label]->normals_); 00302 } 00303 } 00304 00305 00306 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00307 template <typename PointT> void 00308 pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers (std::vector<PointT, Eigen::aligned_allocator<PointT> > &seed_points) 00309 { 00310 00311 supervoxel_helpers_.clear (); 00312 for (int i = 0; i < seed_points.size (); ++i) 00313 { 00314 supervoxel_helpers_.push_back (new SupervoxelHelper(i+1,this)); 00315 //Find which leaf corresponds to this seed index 00316 LeafContainerT* seed_leaf = adjacency_octree_->getLeafContainerAtPoint (seed_points[i]); 00317 if (seed_leaf) 00318 { 00319 supervoxel_helpers_.back ().addLeaf (seed_leaf); 00320 } 00321 else 00322 { 00323 PCL_WARN ("Could not find leaf in pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers - supervoxel will be deleted \n"); 00324 } 00325 } 00326 00327 } 00328 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00329 template <typename PointT> void 00330 pcl::SupervoxelClustering<PointT>::selectInitialSupervoxelSeeds (std::vector<PointT, Eigen::aligned_allocator<PointT> > &seed_points) 00331 { 00332 //TODO THIS IS BAD - SEEDING SHOULD BE BETTER 00333 //TODO Switch to assigning leaves! Don't use Octree! 00334 00335 // std::cout << "Size of centroid cloud="<<voxel_centroid_cloud_->size ()<<", seeding resolution="<<seed_resolution_<<"\n"; 00336 //Initialize octree with voxel centroids 00337 pcl::octree::OctreePointCloudSearch <PointT> seed_octree (seed_resolution_); 00338 seed_octree.setInputCloud (voxel_centroid_cloud_); 00339 seed_octree.addPointsFromInputCloud (); 00340 // std::cout << "Size of octree ="<<seed_octree.getLeafCount ()<<"\n"; 00341 std::vector<PointT, Eigen::aligned_allocator<PointT> > voxel_centers; 00342 int num_seeds = seed_octree.getOccupiedVoxelCenters(voxel_centers); 00343 //std::cout << "Number of seed points before filtering="<<voxel_centers.size ()<<std::endl; 00344 00345 std::vector<int> seed_indices_orig; 00346 seed_indices_orig.resize (num_seeds, 0); 00347 seed_points.clear (); 00348 std::vector<int> closest_index; 00349 std::vector<float> distance; 00350 closest_index.resize(1,0); 00351 distance.resize(1,0); 00352 if (voxel_kdtree_ == 0) 00353 { 00354 voxel_kdtree_ = boost::make_shared< pcl::search::KdTree<PointT> >(); 00355 voxel_kdtree_ ->setInputCloud (voxel_centroid_cloud_); 00356 } 00357 00358 for (int i = 0; i < num_seeds; ++i) 00359 { 00360 voxel_kdtree_->nearestKSearch (voxel_centers[i], 1, closest_index, distance); 00361 seed_indices_orig[i] = closest_index[0]; 00362 } 00363 00364 std::vector<int> neighbors; 00365 std::vector<float> sqr_distances; 00366 seed_points.reserve (seed_indices_orig.size ()); 00367 float search_radius = 0.5f*seed_resolution_; 00368 // This is number of voxels which fit in a planar slice through search volume 00369 // Area of planar slice / area of voxel side 00370 float min_points = 0.05f * (search_radius)*(search_radius) * 3.1415926536f / (resolution_*resolution_); 00371 for (int i = 0; i < seed_indices_orig.size (); ++i) 00372 { 00373 int num = voxel_kdtree_->radiusSearch (seed_indices_orig[i], search_radius , neighbors, sqr_distances); 00374 int min_index = seed_indices_orig[i]; 00375 if ( num > min_points) 00376 { 00377 seed_points.push_back (voxel_centroid_cloud_->points[min_index]); 00378 } 00379 00380 } 00381 // std::cout << "Number of seed points after filtering="<<seed_points.size ()<<std::endl; 00382 00383 } 00384 00385 00386 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00387 template <typename PointT> void 00388 pcl::SupervoxelClustering<PointT>::reseedSupervoxels () 00389 { 00390 //Go through each supervoxel and remove all it's leaves 00391 for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr) 00392 { 00393 sv_itr->removeAllLeaves (); 00394 } 00395 00396 std::vector<int> closest_index; 00397 std::vector<float> distance; 00398 //Now go through each supervoxel, find voxel closest to its center, add it in 00399 for (typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr) 00400 { 00401 PointT point; 00402 sv_itr->getXYZ (point.x, point.y, point.z); 00403 voxel_kdtree_->nearestKSearch (point, 1, closest_index, distance); 00404 00405 LeafContainerT* seed_leaf = adjacency_octree_->getLeafContainerAtPoint (voxel_centroid_cloud_->points[closest_index[0]]); 00406 if (seed_leaf) 00407 { 00408 sv_itr->addLeaf (seed_leaf); 00409 } 00410 } 00411 00412 } 00413 00414 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00415 template <typename PointT> void 00416 pcl::SupervoxelClustering<PointT>::transformFunction (PointT &p) 00417 { 00418 p.x /= p.z; 00419 p.y /= p.z; 00420 p.z = std::log (p.z); 00421 } 00422 00423 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00424 template <typename PointT> float 00425 pcl::SupervoxelClustering<PointT>::voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const 00426 { 00427 00428 float spatial_dist = (v1.xyz_ - v2.xyz_).norm () / seed_resolution_; 00429 float color_dist = (v1.rgb_ - v2.rgb_).norm () / 255.0f; 00430 float cos_angle_normal = 1.0f - std::abs (v1.normal_.dot (v2.normal_)); 00431 // std::cout << "s="<<spatial_dist<<" c="<<color_dist<<" an="<<cos_angle_normal<<"\n"; 00432 return cos_angle_normal * normal_importance_ + color_dist * color_importance_+ spatial_dist * spatial_importance_; 00433 00434 } 00435 00436 00437 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00438 ///////// GETTER FUNCTIONS 00439 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00440 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00441 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00442 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00443 template <typename PointT> void 00444 pcl::SupervoxelClustering<PointT>::getSupervoxelAdjacencyList (VoxelAdjacencyList &adjacency_list_arg) const 00445 { 00446 adjacency_list_arg.clear (); 00447 //Add a vertex for each label, store ids in map 00448 std::map <uint32_t, VoxelID> label_ID_map; 00449 for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr) 00450 { 00451 VoxelID node_id = add_vertex (adjacency_list_arg); 00452 adjacency_list_arg[node_id] = (sv_itr->getLabel ()); 00453 label_ID_map.insert (std::make_pair (sv_itr->getLabel (), node_id)); 00454 } 00455 00456 for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr) 00457 { 00458 uint32_t label = sv_itr->getLabel (); 00459 std::set<uint32_t> neighbor_labels; 00460 sv_itr->getNeighborLabels (neighbor_labels); 00461 for (std::set<uint32_t>::iterator label_itr = neighbor_labels.begin (); label_itr != neighbor_labels.end (); ++label_itr) 00462 { 00463 bool edge_added; 00464 EdgeID edge; 00465 VoxelID u = (label_ID_map.find (label))->second; 00466 VoxelID v = (label_ID_map.find (*label_itr))->second; 00467 boost::tie (edge, edge_added) = add_edge (u,v,adjacency_list_arg); 00468 //Calc distance between centers, set as edge weight 00469 if (edge_added) 00470 { 00471 VoxelData centroid_data = (sv_itr)->getCentroid (); 00472 //Find the neighbhor with this label 00473 VoxelData neighb_centroid_data; 00474 00475 for (typename HelperListT::const_iterator neighb_itr = supervoxel_helpers_.cbegin (); neighb_itr != supervoxel_helpers_.cend (); ++neighb_itr) 00476 { 00477 if (neighb_itr->getLabel () == (*label_itr)) 00478 { 00479 neighb_centroid_data = neighb_itr->getCentroid (); 00480 break; 00481 } 00482 } 00483 00484 float length = voxelDataDistance (centroid_data, neighb_centroid_data); 00485 adjacency_list_arg[edge] = length; 00486 } 00487 } 00488 00489 } 00490 00491 } 00492 00493 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00494 template <typename PointT> void 00495 pcl::SupervoxelClustering<PointT>::getSupervoxelAdjacency (std::multimap<uint32_t, uint32_t> &label_adjacency) const 00496 { 00497 label_adjacency.clear (); 00498 for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr) 00499 { 00500 uint32_t label = sv_itr->getLabel (); 00501 std::set<uint32_t> neighbor_labels; 00502 sv_itr->getNeighborLabels (neighbor_labels); 00503 for (std::set<uint32_t>::iterator label_itr = neighbor_labels.begin (); label_itr != neighbor_labels.end (); ++label_itr) 00504 label_adjacency.insert (std::pair<uint32_t,uint32_t> (label, *label_itr) ); 00505 //if (neighbor_labels.size () == 0) 00506 // std::cout << label<<"(size="<<sv_itr->size () << ") has "<<neighbor_labels.size () << "\n"; 00507 } 00508 00509 } 00510 00511 00512 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00513 template <typename PointT> pcl::PointCloud<pcl::PointXYZRGBA>::Ptr 00514 pcl::SupervoxelClustering<PointT>::getColoredCloud () const 00515 { 00516 00517 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud = boost::make_shared <pcl::PointCloud<pcl::PointXYZRGBA> >(); 00518 pcl::copyPointCloud (*input_,*colored_cloud); 00519 00520 pcl::PointCloud <pcl::PointXYZRGBA>::iterator i_colored; 00521 typename pcl::PointCloud <PointT>::const_iterator i_input = input_->begin (); 00522 std::vector <int> indices; 00523 std::vector <float> sqr_distances; 00524 for (i_colored = colored_cloud->begin (); i_colored != colored_cloud->end (); ++i_colored,++i_input) 00525 { 00526 if ( !pcl::isFinite<PointT> (*i_input)) 00527 i_colored->rgb = 0; 00528 else 00529 { 00530 i_colored->rgb = 0; 00531 LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input); 00532 VoxelData& voxel_data = leaf->getData (); 00533 if (voxel_data.owner_) 00534 i_colored->rgba = label_colors_[voxel_data.owner_->getLabel ()]; 00535 00536 } 00537 00538 } 00539 00540 return (colored_cloud); 00541 } 00542 00543 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00544 template <typename PointT> pcl::PointCloud<pcl::PointXYZRGBA>::Ptr 00545 pcl::SupervoxelClustering<PointT>::getColoredVoxelCloud () const 00546 { 00547 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud = boost::make_shared< pcl::PointCloud<pcl::PointXYZRGBA> > (); 00548 for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr) 00549 { 00550 typename PointCloudT::Ptr voxels; 00551 sv_itr->getVoxels (voxels); 00552 pcl::PointCloud<pcl::PointXYZRGBA> rgb_copy; 00553 copyPointCloud (*voxels, rgb_copy); 00554 00555 pcl::PointCloud<pcl::PointXYZRGBA>::iterator rgb_copy_itr = rgb_copy.begin (); 00556 for ( ; rgb_copy_itr != rgb_copy.end (); ++rgb_copy_itr) 00557 rgb_copy_itr->rgba = label_colors_ [sv_itr->getLabel ()]; 00558 00559 *colored_cloud += rgb_copy; 00560 } 00561 00562 return colored_cloud; 00563 } 00564 00565 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00566 template <typename PointT> typename pcl::PointCloud<PointT>::Ptr 00567 pcl::SupervoxelClustering<PointT>::getVoxelCentroidCloud () const 00568 { 00569 typename PointCloudT::Ptr centroid_copy = boost::make_shared<PointCloudT> (); 00570 copyPointCloud (*voxel_centroid_cloud_, *centroid_copy); 00571 return centroid_copy; 00572 } 00573 00574 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00575 template <typename PointT> pcl::PointCloud<pcl::PointXYZL>::Ptr 00576 pcl::SupervoxelClustering<PointT>::getLabeledVoxelCloud () const 00577 { 00578 pcl::PointCloud<pcl::PointXYZL>::Ptr labeled_voxel_cloud = boost::make_shared< pcl::PointCloud<pcl::PointXYZL> > (); 00579 for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr) 00580 { 00581 typename PointCloudT::Ptr voxels; 00582 sv_itr->getVoxels (voxels); 00583 pcl::PointCloud<pcl::PointXYZL> xyzl_copy; 00584 copyPointCloud (*voxels, xyzl_copy); 00585 00586 pcl::PointCloud<pcl::PointXYZL>::iterator xyzl_copy_itr = xyzl_copy.begin (); 00587 for ( ; xyzl_copy_itr != xyzl_copy.end (); ++xyzl_copy_itr) 00588 xyzl_copy_itr->label = sv_itr->getLabel (); 00589 00590 *labeled_voxel_cloud += xyzl_copy; 00591 } 00592 00593 return labeled_voxel_cloud; 00594 } 00595 00596 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00597 template <typename PointT> pcl::PointCloud<pcl::PointXYZL>::Ptr 00598 pcl::SupervoxelClustering<PointT>::getLabeledCloud () const 00599 { 00600 pcl::PointCloud<pcl::PointXYZL>::Ptr labeled_cloud = boost::make_shared <pcl::PointCloud<pcl::PointXYZL> >(); 00601 pcl::copyPointCloud (*input_,*labeled_cloud); 00602 00603 pcl::PointCloud <pcl::PointXYZL>::iterator i_labeled; 00604 typename pcl::PointCloud <PointT>::const_iterator i_input = input_->begin (); 00605 std::vector <int> indices; 00606 std::vector <float> sqr_distances; 00607 for (i_labeled = labeled_cloud->begin (); i_labeled != labeled_cloud->end (); ++i_labeled,++i_input) 00608 { 00609 if ( !pcl::isFinite<PointT> (*i_input)) 00610 i_labeled->label = 0; 00611 else 00612 { 00613 i_labeled->label = 0; 00614 LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input); 00615 VoxelData& voxel_data = leaf->getData (); 00616 if (voxel_data.owner_) 00617 i_labeled->label = voxel_data.owner_->getLabel (); 00618 00619 } 00620 00621 } 00622 00623 return (labeled_cloud); 00624 } 00625 00626 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00627 template <typename PointT> pcl::PointCloud<pcl::PointNormal>::Ptr 00628 pcl::SupervoxelClustering<PointT>::makeSupervoxelNormalCloud (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters) 00629 { 00630 pcl::PointCloud<pcl::PointNormal>::Ptr normal_cloud = boost::make_shared<pcl::PointCloud<pcl::PointNormal> > (); 00631 normal_cloud->resize (supervoxel_clusters.size ()); 00632 typename std::map <uint32_t, typename pcl::Supervoxel<PointT>::Ptr>::iterator sv_itr,sv_itr_end; 00633 sv_itr = supervoxel_clusters.begin (); 00634 sv_itr_end = supervoxel_clusters.end (); 00635 pcl::PointCloud<pcl::PointNormal>::iterator normal_cloud_itr = normal_cloud->begin (); 00636 for ( ; sv_itr != sv_itr_end; ++sv_itr, ++normal_cloud_itr) 00637 { 00638 (sv_itr->second)->getCentroidPointNormal (*normal_cloud_itr); 00639 } 00640 return normal_cloud; 00641 } 00642 00643 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00644 template <typename PointT> float 00645 pcl::SupervoxelClustering<PointT>::getVoxelResolution () const 00646 { 00647 return (resolution_); 00648 } 00649 00650 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00651 template <typename PointT> void 00652 pcl::SupervoxelClustering<PointT>::setVoxelResolution (float resolution) 00653 { 00654 resolution_ = resolution; 00655 00656 } 00657 00658 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00659 template <typename PointT> float 00660 pcl::SupervoxelClustering<PointT>::getSeedResolution () const 00661 { 00662 return (resolution_); 00663 } 00664 00665 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00666 template <typename PointT> void 00667 pcl::SupervoxelClustering<PointT>::setSeedResolution (float seed_resolution) 00668 { 00669 seed_resolution_ = seed_resolution; 00670 } 00671 00672 00673 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00674 template <typename PointT> void 00675 pcl::SupervoxelClustering<PointT>::setColorImportance (float val) 00676 { 00677 color_importance_ = val; 00678 } 00679 00680 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00681 template <typename PointT> void 00682 pcl::SupervoxelClustering<PointT>::setSpatialImportance (float val) 00683 { 00684 spatial_importance_ = val; 00685 } 00686 00687 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00688 template <typename PointT> void 00689 pcl::SupervoxelClustering<PointT>::setNormalImportance (float val) 00690 { 00691 normal_importance_ = val; 00692 } 00693 00694 00695 00696 namespace pcl 00697 { 00698 00699 namespace octree 00700 { 00701 //Explicit overloads for RGB types 00702 template<> 00703 void 00704 pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGB,pcl::SupervoxelClustering<pcl::PointXYZRGB>::VoxelData>::addPoint (const pcl::PointXYZRGB &new_point) 00705 { 00706 ++num_points_; 00707 //Same as before here 00708 data_.xyz_[0] += new_point.x; 00709 data_.xyz_[1] += new_point.y; 00710 data_.xyz_[2] += new_point.z; 00711 //Separate sums for r,g,b since we cant sum in uchars 00712 data_.rgb_[0] += static_cast<float> (new_point.r); 00713 data_.rgb_[1] += static_cast<float> (new_point.g); 00714 data_.rgb_[2] += static_cast<float> (new_point.b); 00715 } 00716 00717 template<> 00718 void 00719 pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGBA,pcl::SupervoxelClustering<pcl::PointXYZRGBA>::VoxelData>::addPoint (const pcl::PointXYZRGBA &new_point) 00720 { 00721 ++num_points_; 00722 //Same as before here 00723 data_.xyz_[0] += new_point.x; 00724 data_.xyz_[1] += new_point.y; 00725 data_.xyz_[2] += new_point.z; 00726 //Separate sums for r,g,b since we cant sum in uchars 00727 data_.rgb_[0] += static_cast<float> (new_point.r); 00728 data_.rgb_[1] += static_cast<float> (new_point.g); 00729 data_.rgb_[2] += static_cast<float> (new_point.b); 00730 } 00731 00732 00733 00734 //Explicit overloads for RGB types 00735 template<> void 00736 pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGB,pcl::SupervoxelClustering<pcl::PointXYZRGB>::VoxelData>::computeData () 00737 { 00738 data_.rgb_[0] /= (static_cast<float> (num_points_)); 00739 data_.rgb_[1] /= (static_cast<float> (num_points_)); 00740 data_.rgb_[2] /= (static_cast<float> (num_points_)); 00741 data_.xyz_[0] /= (static_cast<float> (num_points_)); 00742 data_.xyz_[1] /= (static_cast<float> (num_points_)); 00743 data_.xyz_[2] /= (static_cast<float> (num_points_)); 00744 } 00745 00746 template<> void 00747 pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZRGBA,pcl::SupervoxelClustering<pcl::PointXYZRGBA>::VoxelData>::computeData () 00748 { 00749 data_.rgb_[0] /= (static_cast<float> (num_points_)); 00750 data_.rgb_[1] /= (static_cast<float> (num_points_)); 00751 data_.rgb_[2] /= (static_cast<float> (num_points_)); 00752 data_.xyz_[0] /= (static_cast<float> (num_points_)); 00753 data_.xyz_[1] /= (static_cast<float> (num_points_)); 00754 data_.xyz_[2] /= (static_cast<float> (num_points_)); 00755 } 00756 00757 } 00758 } 00759 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00760 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00761 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00762 namespace pcl 00763 { 00764 00765 template<> void 00766 pcl::SupervoxelClustering<pcl::PointXYZRGB>::VoxelData::getPoint (pcl::PointXYZRGB &point_arg) const 00767 { 00768 point_arg.rgba = static_cast<uint32_t>(rgb_[0]) << 16 | 00769 static_cast<uint32_t>(rgb_[1]) << 8 | 00770 static_cast<uint32_t>(rgb_[2]); 00771 point_arg.x = xyz_[0]; 00772 point_arg.y = xyz_[1]; 00773 point_arg.z = xyz_[2]; 00774 } 00775 00776 template<> void 00777 pcl::SupervoxelClustering<pcl::PointXYZRGBA>::VoxelData::getPoint (pcl::PointXYZRGBA &point_arg ) const 00778 { 00779 point_arg.rgba = static_cast<uint32_t>(rgb_[0]) << 16 | 00780 static_cast<uint32_t>(rgb_[1]) << 8 | 00781 static_cast<uint32_t>(rgb_[2]); 00782 point_arg.x = xyz_[0]; 00783 point_arg.y = xyz_[1]; 00784 point_arg.z = xyz_[2]; 00785 } 00786 00787 template<typename PointT> void 00788 pcl::SupervoxelClustering<PointT>::VoxelData::getPoint (PointT &point_arg ) const 00789 { 00790 //XYZ is required or this doesn't make much sense... 00791 point_arg.x = xyz_[0]; 00792 point_arg.y = xyz_[1]; 00793 point_arg.z = xyz_[2]; 00794 } 00795 00796 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00797 template <typename PointT> void 00798 pcl::SupervoxelClustering<PointT>::VoxelData::getNormal (Normal &normal_arg) const 00799 { 00800 normal_arg.normal_x = normal_[0]; 00801 normal_arg.normal_y = normal_[1]; 00802 normal_arg.normal_z = normal_[2]; 00803 normal_arg.curvature = curvature_; 00804 } 00805 } 00806 00807 00808 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00809 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00810 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00811 template <typename PointT> void 00812 pcl::SupervoxelClustering<PointT>::SupervoxelHelper::addLeaf (LeafContainerT* leaf_arg) 00813 { 00814 leaves_.insert (leaf_arg); 00815 VoxelData& voxel_data = leaf_arg->getData (); 00816 voxel_data.owner_ = this; 00817 } 00818 00819 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00820 template <typename PointT> void 00821 pcl::SupervoxelClustering<PointT>::SupervoxelHelper::removeLeaf (LeafContainerT* leaf_arg) 00822 { 00823 leaves_.erase (leaf_arg); 00824 } 00825 00826 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00827 template <typename PointT> void 00828 pcl::SupervoxelClustering<PointT>::SupervoxelHelper::removeAllLeaves () 00829 { 00830 typename std::set<LeafContainerT*>::iterator leaf_itr; 00831 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr) 00832 { 00833 VoxelData& voxel = ((*leaf_itr)->getData ()); 00834 voxel.owner_ = 0; 00835 voxel.distance_ = std::numeric_limits<float>::max (); 00836 } 00837 leaves_.clear (); 00838 } 00839 00840 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00841 template <typename PointT> void 00842 pcl::SupervoxelClustering<PointT>::SupervoxelHelper::expand () 00843 { 00844 //std::cout << "Expanding sv "<<label_<<", owns "<<leaves_.size ()<<" voxels\n"; 00845 //Buffer of new neighbors - initial size is just a guess of most possible 00846 std::vector<LeafContainerT*> new_owned; 00847 new_owned.reserve (leaves_.size () * 9); 00848 //For each leaf belonging to this supervoxel 00849 typename std::set<LeafContainerT*>::iterator leaf_itr; 00850 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr) 00851 { 00852 //for each neighbor of the leaf 00853 for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr) 00854 { 00855 //Get a reference to the data contained in the leaf 00856 VoxelData& neighbor_voxel = ((*neighb_itr)->getData ()); 00857 //TODO this is a shortcut, really we should always recompute distance 00858 if(neighbor_voxel.owner_ == this) 00859 continue; 00860 //Compute distance to the neighbor 00861 float dist = parent_->voxelDataDistance (centroid_, neighbor_voxel); 00862 //If distance is less than previous, we remove it from its owner's list 00863 //and change the owner to this and distance (we *steal* it!) 00864 if (dist < neighbor_voxel.distance_) 00865 { 00866 neighbor_voxel.distance_ = dist; 00867 if (neighbor_voxel.owner_ != this) 00868 { 00869 if (neighbor_voxel.owner_) 00870 (neighbor_voxel.owner_)->removeLeaf(*neighb_itr); 00871 neighbor_voxel.owner_ = this; 00872 new_owned.push_back (*neighb_itr); 00873 } 00874 } 00875 } 00876 } 00877 //Push all new owned onto the owned leaf set 00878 typename std::vector<LeafContainerT*>::iterator new_owned_itr; 00879 for (new_owned_itr=new_owned.begin (); new_owned_itr!=new_owned.end (); ++new_owned_itr) 00880 { 00881 leaves_.insert (*new_owned_itr); 00882 } 00883 00884 } 00885 00886 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00887 template <typename PointT> void 00888 pcl::SupervoxelClustering<PointT>::SupervoxelHelper::refineNormals () 00889 { 00890 typename std::set<LeafContainerT*>::iterator leaf_itr; 00891 //For each leaf belonging to this supervoxel, get its neighbors, build an index vector, compute normal 00892 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr) 00893 { 00894 VoxelData& voxel_data = (*leaf_itr)->getData (); 00895 std::vector<int> indices; 00896 indices.reserve (81); 00897 //Push this point 00898 indices.push_back (voxel_data.idx_); 00899 for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr) 00900 { 00901 //Get a reference to the data contained in the leaf 00902 VoxelData& neighbor_voxel_data = ((*neighb_itr)->getData ()); 00903 //If the neighbor is in this supervoxel, use it 00904 if (neighbor_voxel_data.owner_ == this) 00905 { 00906 indices.push_back (neighbor_voxel_data.idx_); 00907 //Also check its neighbors 00908 for (typename LeafContainerT::iterator neighb_neighb_itr=(*neighb_itr)->begin (); neighb_neighb_itr!=(*neighb_itr)->end (); ++neighb_neighb_itr) 00909 { 00910 VoxelData& neighb_neighb_voxel_data = (*neighb_neighb_itr)->getData (); 00911 if (neighb_neighb_voxel_data.owner_ == this) 00912 indices.push_back (neighb_neighb_voxel_data.idx_); 00913 } 00914 00915 00916 } 00917 } 00918 //Compute normal 00919 pcl::computePointNormal (*parent_->voxel_centroid_cloud_, indices, voxel_data.normal_, voxel_data.curvature_); 00920 pcl::flipNormalTowardsViewpoint (parent_->voxel_centroid_cloud_->points[voxel_data.idx_], 0.0f,0.0f,0.0f, voxel_data.normal_); 00921 voxel_data.normal_[3] = 0.0f; 00922 voxel_data.normal_.normalize (); 00923 } 00924 } 00925 00926 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00927 template <typename PointT> void 00928 pcl::SupervoxelClustering<PointT>::SupervoxelHelper::updateCentroid () 00929 { 00930 centroid_.normal_ = Eigen::Vector4f::Zero (); 00931 centroid_.xyz_ = Eigen::Vector3f::Zero (); 00932 centroid_.rgb_ = Eigen::Vector3f::Zero (); 00933 typename std::set<LeafContainerT*>::iterator leaf_itr = leaves_.begin (); 00934 if (leaves_.size () < 20) //Just a check to see if we have enough points to calculate a good normal 00935 { 00936 for ( ; leaf_itr!= leaves_.end (); ++leaf_itr) 00937 { 00938 const VoxelData& leaf_data = (*leaf_itr)->getData (); 00939 centroid_.normal_ += leaf_data.normal_; 00940 centroid_.xyz_ += leaf_data.xyz_; 00941 centroid_.rgb_ += leaf_data.rgb_; 00942 } 00943 centroid_.normal_.normalize (); 00944 } 00945 else 00946 { 00947 std::vector<int> indices; 00948 indices.reserve (leaves_.size ()); 00949 for ( ; leaf_itr!= leaves_.end (); ++leaf_itr) 00950 { 00951 const VoxelData& leaf_data = (*leaf_itr)->getData (); 00952 centroid_.xyz_ += leaf_data.xyz_; 00953 centroid_.rgb_ += leaf_data.rgb_; 00954 indices.push_back (leaf_data.idx_); 00955 } 00956 pcl::computePointNormal (*parent_->voxel_centroid_cloud_, indices, centroid_.normal_, centroid_.curvature_); 00957 PointT temp; 00958 this->getXYZ (temp.x, temp.y, temp.z); 00959 pcl::flipNormalTowardsViewpoint (temp, 0.0f,0.0f,0.0f, centroid_.normal_); 00960 centroid_.normal_[3] = 0.0f; 00961 centroid_.normal_.normalize (); 00962 } 00963 00964 centroid_.xyz_ /= static_cast<float> (leaves_.size ()); 00965 centroid_.rgb_ /= static_cast<float> (leaves_.size ()); 00966 00967 } 00968 00969 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00970 template <typename PointT> void 00971 pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getVoxels (typename pcl::PointCloud<PointT>::Ptr &voxels) const 00972 { 00973 voxels = boost::make_shared<pcl::PointCloud<PointT> > (); 00974 voxels->clear (); 00975 voxels->resize (leaves_.size ()); 00976 typename pcl::PointCloud<PointT>::iterator voxel_itr = voxels->begin (); 00977 typename std::set<LeafContainerT*>::iterator leaf_itr; 00978 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++voxel_itr) 00979 { 00980 const VoxelData& leaf_data = (*leaf_itr)->getData (); 00981 leaf_data.getPoint (*voxel_itr); 00982 } 00983 } 00984 00985 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00986 template <typename PointT> void 00987 pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const 00988 { 00989 normals = boost::make_shared<pcl::PointCloud<Normal> > (); 00990 normals->clear (); 00991 normals->resize (leaves_.size ()); 00992 typename std::set<LeafContainerT*>::iterator leaf_itr; 00993 typename pcl::PointCloud<Normal>::iterator normal_itr = normals->begin (); 00994 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++normal_itr) 00995 { 00996 const VoxelData& leaf_data = (*leaf_itr)->getData (); 00997 leaf_data.getNormal (*normal_itr); 00998 } 00999 } 01000 01001 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 01002 template <typename PointT> void 01003 pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNeighborLabels (std::set<uint32_t> &neighbor_labels) const 01004 { 01005 neighbor_labels.clear (); 01006 //For each leaf belonging to this supervoxel 01007 typename std::set<LeafContainerT*>::iterator leaf_itr; 01008 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr) 01009 { 01010 //for each neighbor of the leaf 01011 for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr) 01012 { 01013 //Get a reference to the data contained in the leaf 01014 VoxelData& neighbor_voxel = ((*neighb_itr)->getData ()); 01015 //If it has an owner, and it's not us - get it's owner's label insert into set 01016 if (neighbor_voxel.owner_ != this && neighbor_voxel.owner_) 01017 { 01018 neighbor_labels.insert (neighbor_voxel.owner_->getLabel ()); 01019 } 01020 } 01021 } 01022 } 01023 01024 01025 #endif // PCL_SUPERVOXEL_CLUSTERING_HPP_ 01026