40 #ifndef PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_
41 #define PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_
43 #include <pcl/segmentation/supervoxel_clustering.h>
46 template <
typename Po
intT>
48 resolution_ (voxel_resolution),
49 seed_resolution_ (seed_resolution),
51 voxel_centroid_cloud_ (),
52 color_importance_(0.1f),
53 spatial_importance_(0.4f),
54 normal_importance_(1.0f),
58 if (use_single_camera_transform)
59 adjacency_octree_->setTransformFunction (boost::bind (&SupervoxelClustering::transformFunction,
this, _1));
63 template <
typename Po
intT>
70 template <
typename Po
intT>
void
73 if ( cloud->
size () == 0 )
75 PCL_ERROR (
"[pcl::SupervoxelClustering::setInputCloud] Empty cloud set, doing nothing \n");
80 adjacency_octree_->setInputCloud (cloud);
84 template <
typename Po
intT>
void
87 if ( normal_cloud->size () == 0 )
89 PCL_ERROR (
"[pcl::SupervoxelClustering::setNormalCloud] Empty cloud set, doing nothing \n");
93 input_normals_ = normal_cloud;
97 template <
typename Po
intT>
void
103 bool segmentation_is_possible = initCompute ();
104 if ( !segmentation_is_possible )
111 segmentation_is_possible = prepareForSegmentation ();
112 if ( !segmentation_is_possible )
120 std::vector<PointT, Eigen::aligned_allocator<PointT> > seed_points;
121 selectInitialSupervoxelSeeds (seed_points);
123 createSupervoxelHelpers (seed_points);
128 int max_depth =
static_cast<int> (1.8f*seed_resolution_/resolution_);
129 expandSupervoxels (max_depth);
133 makeSupervoxels (supervoxel_clusters);
149 template <
typename Po
intT>
void
152 if (supervoxel_helpers_.size () == 0)
154 PCL_ERROR (
"[pcl::SupervoxelClustering::refineVoxelNormals] Supervoxels not extracted, doing nothing - (Call extract first!) \n");
158 int max_depth =
static_cast<int> (1.8f*seed_resolution_/resolution_);
159 for (
int i = 0; i < num_itr; ++i)
161 for (
typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
163 sv_itr->refineNormals ();
166 reseedSupervoxels ();
167 expandSupervoxels (max_depth);
171 makeSupervoxels (supervoxel_clusters);
181 template <
typename Po
intT>
bool
186 if ( input_->points.size () == 0 )
192 adjacency_octree_->addPointsFromInputCloud ();
205 template <
typename Po
intT>
void
209 voxel_centroid_cloud_->resize (adjacency_octree_->getLeafCount ());
210 typename LeafVectorT::iterator leaf_itr = adjacency_octree_->begin ();
212 for (
int idx = 0 ; leaf_itr != adjacency_octree_->end (); ++leaf_itr, ++cent_cloud_itr, ++idx)
214 VoxelData& new_voxel_data = (*leaf_itr)->getData ();
216 new_voxel_data.getPoint (*cent_cloud_itr);
218 new_voxel_data.idx_ = idx;
225 assert (input_normals_->size () == input_->size ());
227 typename NormalCloudT::const_iterator normal_itr = input_normals_->begin ();
231 if ( !pcl::isFinite<PointT> (*input_itr))
234 LeafContainerT* leaf = adjacency_octree_->getLeafContainerAtPoint (*input_itr);
237 VoxelData& voxel_data = leaf->getData ();
239 voxel_data.normal_ += normal_itr->getNormalVector4fMap ();
240 voxel_data.curvature_ += normal_itr->curvature;
243 for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
245 VoxelData& voxel_data = (*leaf_itr)->getData ();
246 voxel_data.normal_.normalize ();
247 voxel_data.owner_ = 0;
248 voxel_data.distance_ = std::numeric_limits<float>::max ();
250 int num_points = (*leaf_itr)->getPointCounter ();
251 voxel_data.curvature_ /= num_points;
256 for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
258 VoxelData& new_voxel_data = (*leaf_itr)->getData ();
260 std::vector<int> indices;
261 indices.reserve (81);
263 indices.push_back (new_voxel_data.idx_);
264 for (
typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
266 VoxelData& neighb_voxel_data = (*neighb_itr)->getData ();
268 indices.push_back (neighb_voxel_data.idx_);
270 for (
typename LeafContainerT::iterator neighb_neighb_itr=(*neighb_itr)->begin (); neighb_neighb_itr!=(*neighb_itr)->end (); ++neighb_neighb_itr)
272 VoxelData& neighb2_voxel_data = (*neighb_neighb_itr)->getData ();
273 indices.push_back (neighb2_voxel_data.idx_);
279 new_voxel_data.normal_[3] = 0.0f;
280 new_voxel_data.normal_.normalize ();
281 new_voxel_data.owner_ = 0;
282 new_voxel_data.distance_ = std::numeric_limits<float>::max ();
290 template <
typename Po
intT>
void
295 for (
int i = 1; i < depth; ++i)
298 for (
typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
304 for (
typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); )
306 if (sv_itr->size () == 0)
308 sv_itr = supervoxel_helpers_.erase (sv_itr);
312 sv_itr->updateCentroid ();
322 template <
typename Po
intT>
void
325 supervoxel_clusters.clear ();
326 for (
typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
328 uint32_t label = sv_itr->getLabel ();
329 supervoxel_clusters[label].reset (
new Supervoxel<PointT>);
330 sv_itr->getXYZ (supervoxel_clusters[label]->centroid_.x,supervoxel_clusters[label]->centroid_.y,supervoxel_clusters[label]->centroid_.z);
331 sv_itr->getRGB (supervoxel_clusters[label]->centroid_.rgba);
332 sv_itr->getNormal (supervoxel_clusters[label]->normal_);
333 sv_itr->getVoxels (supervoxel_clusters[label]->voxels_);
334 sv_itr->getNormals (supervoxel_clusters[label]->normals_);
337 initializeLabelColors ();
342 template <
typename Po
intT>
void
346 supervoxel_helpers_.clear ();
347 for (
size_t i = 0; i < seed_points.size (); ++i)
349 supervoxel_helpers_.push_back (
new SupervoxelHelper(i+1,
this));
351 LeafContainerT* seed_leaf = adjacency_octree_->getLeafContainerAtPoint (seed_points[i]);
354 supervoxel_helpers_.back ().addLeaf (seed_leaf);
358 PCL_WARN (
"Could not find leaf in pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers - supervoxel will be deleted \n");
364 template <
typename Po
intT>
void
373 seed_octree.setInputCloud (voxel_centroid_cloud_);
374 seed_octree.addPointsFromInputCloud ();
376 std::vector<PointT, Eigen::aligned_allocator<PointT> > voxel_centers;
377 int num_seeds = seed_octree.getOccupiedVoxelCenters(voxel_centers);
380 std::vector<int> seed_indices_orig;
381 seed_indices_orig.resize (num_seeds, 0);
382 seed_points.clear ();
383 std::vector<int> closest_index;
384 std::vector<float> distance;
385 closest_index.resize(1,0);
386 distance.resize(1,0);
387 if (voxel_kdtree_ == 0)
390 voxel_kdtree_ ->setInputCloud (voxel_centroid_cloud_);
393 for (
int i = 0; i < num_seeds; ++i)
395 voxel_kdtree_->nearestKSearch (voxel_centers[i], 1, closest_index, distance);
396 seed_indices_orig[i] = closest_index[0];
399 std::vector<int> neighbors;
400 std::vector<float> sqr_distances;
401 seed_points.reserve (seed_indices_orig.size ());
402 float search_radius = 0.5f*seed_resolution_;
405 float min_points = 0.05f * (search_radius)*(search_radius) * 3.1415926536f / (resolution_*resolution_);
406 for (
size_t i = 0; i < seed_indices_orig.size (); ++i)
408 int num = voxel_kdtree_->radiusSearch (seed_indices_orig[i], search_radius , neighbors, sqr_distances);
409 int min_index = seed_indices_orig[i];
410 if ( num > min_points)
412 seed_points.push_back (voxel_centroid_cloud_->points[min_index]);
422 template <
typename Po
intT>
void
426 for (
typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
428 sv_itr->removeAllLeaves ();
431 std::vector<int> closest_index;
432 std::vector<float> distance;
434 for (
typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
437 sv_itr->getXYZ (point.x, point.y, point.z);
438 voxel_kdtree_->nearestKSearch (point, 1, closest_index, distance);
440 LeafContainerT* seed_leaf = adjacency_octree_->getLeafContainerAtPoint (voxel_centroid_cloud_->points[closest_index[0]]);
443 sv_itr->addLeaf (seed_leaf);
450 template <
typename Po
intT>
void
455 p.z = std::log (p.z);
459 template <
typename Po
intT>
float
463 float spatial_dist = (v1.xyz_ - v2.xyz_).norm () / seed_resolution_;
464 float color_dist = (v1.rgb_ - v2.rgb_).norm () / 255.0f;
465 float cos_angle_normal = 1.0f - std::abs (v1.normal_.dot (v2.normal_));
467 return cos_angle_normal * normal_importance_ + color_dist * color_importance_+ spatial_dist * spatial_importance_;
478 template <
typename Po
intT>
void
481 adjacency_list_arg.clear ();
483 std::map <uint32_t, VoxelID> label_ID_map;
484 for (
typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
486 VoxelID node_id = add_vertex (adjacency_list_arg);
487 adjacency_list_arg[node_id] = (sv_itr->getLabel ());
488 label_ID_map.insert (std::make_pair (sv_itr->getLabel (), node_id));
491 for (
typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
493 uint32_t label = sv_itr->getLabel ();
494 std::set<uint32_t> neighbor_labels;
495 sv_itr->getNeighborLabels (neighbor_labels);
496 for (std::set<uint32_t>::iterator label_itr = neighbor_labels.begin (); label_itr != neighbor_labels.end (); ++label_itr)
500 VoxelID u = (label_ID_map.find (label))->second;
501 VoxelID v = (label_ID_map.find (*label_itr))->second;
502 boost::tie (edge, edge_added) = add_edge (u,v,adjacency_list_arg);
506 VoxelData centroid_data = (sv_itr)->getCentroid ();
510 for (
typename HelperListT::const_iterator neighb_itr = supervoxel_helpers_.cbegin (); neighb_itr != supervoxel_helpers_.cend (); ++neighb_itr)
512 if (neighb_itr->getLabel () == (*label_itr))
514 neighb_centroid_data = neighb_itr->getCentroid ();
519 float length = voxelDataDistance (centroid_data, neighb_centroid_data);
520 adjacency_list_arg[edge] = length;
529 template <
typename Po
intT>
void
532 label_adjacency.clear ();
533 for (
typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
535 uint32_t label = sv_itr->getLabel ();
536 std::set<uint32_t> neighbor_labels;
537 sv_itr->getNeighborLabels (neighbor_labels);
538 for (std::set<uint32_t>::iterator label_itr = neighbor_labels.begin (); label_itr != neighbor_labels.end (); ++label_itr)
539 label_adjacency.insert (std::pair<uint32_t,uint32_t> (label, *label_itr) );
556 std::vector <int> indices;
557 std::vector <float> sqr_distances;
558 for (i_colored = colored_cloud->
begin (); i_colored != colored_cloud->
end (); ++i_colored,++i_input)
560 if ( !pcl::isFinite<PointT> (*i_input))
565 LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
566 VoxelData& voxel_data = leaf->getData ();
568 i_colored->rgba = label_colors_[voxel_data.
owner_->getLabel ()];
574 return (colored_cloud);
582 for (
typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
585 sv_itr->getVoxels (voxels);
590 for ( ; rgb_copy_itr != rgb_copy.
end (); ++rgb_copy_itr)
591 rgb_copy_itr->rgba = label_colors_ [sv_itr->getLabel ()];
593 *colored_cloud += rgb_copy;
596 return colored_cloud;
605 return centroid_copy;
613 for (
typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
616 sv_itr->getVoxels (voxels);
621 for ( ; xyzl_copy_itr != xyzl_copy.
end (); ++xyzl_copy_itr)
622 xyzl_copy_itr->label = sv_itr->getLabel ();
624 *labeled_voxel_cloud += xyzl_copy;
627 return labeled_voxel_cloud;
639 std::vector <int> indices;
640 std::vector <float> sqr_distances;
641 for (i_labeled = labeled_cloud->
begin (); i_labeled != labeled_cloud->
end (); ++i_labeled,++i_input)
643 if ( !pcl::isFinite<PointT> (*i_input))
644 i_labeled->label = 0;
647 i_labeled->label = 0;
648 LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
649 VoxelData& voxel_data = leaf->getData ();
651 i_labeled->label = voxel_data.
owner_->getLabel ();
657 return (labeled_cloud);
665 normal_cloud->
resize (supervoxel_clusters.size ());
666 typename std::map <uint32_t, typename pcl::Supervoxel<PointT>::Ptr>::iterator sv_itr,sv_itr_end;
667 sv_itr = supervoxel_clusters.begin ();
668 sv_itr_end = supervoxel_clusters.end ();
670 for ( ; sv_itr != sv_itr_end; ++sv_itr, ++normal_cloud_itr)
672 (sv_itr->second)->getCentroidPointNormal (*normal_cloud_itr);
678 template <
typename Po
intT>
float
681 return (resolution_);
685 template <
typename Po
intT>
void
688 resolution_ = resolution;
693 template <
typename Po
intT>
float
696 return (resolution_);
700 template <
typename Po
intT>
void
703 seed_resolution_ = seed_resolution;
708 template <
typename Po
intT>
void
711 color_importance_ = val;
715 template <
typename Po
intT>
void
718 spatial_importance_ = val;
722 template <
typename Po
intT>
void
725 normal_importance_ = val;
730 template <
typename Po
intT>
void
733 uint32_t max_label =
static_cast<uint32_t
> (getMaxLabel ());
735 if (label_colors_.size () > max_label)
739 label_colors_.reserve (max_label + 1);
740 srand (static_cast<unsigned int> (time (0)));
741 while (label_colors_.size () <= max_label )
743 uint8_t r =
static_cast<uint8_t
>( (rand () % 256));
744 uint8_t g =
static_cast<uint8_t
>( (rand () % 256));
745 uint8_t b =
static_cast<uint8_t
>( (rand () % 256));
746 label_colors_.push_back (static_cast<uint32_t>(r) << 16 | static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
751 template <
typename Po
intT>
int
755 for (
typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
757 int temp = sv_itr->getLabel ();
758 if (temp > max_label)
776 data_.xyz_[0] += new_point.x;
777 data_.xyz_[1] += new_point.y;
778 data_.xyz_[2] += new_point.z;
780 data_.rgb_[0] +=
static_cast<float> (new_point.r);
781 data_.rgb_[1] +=
static_cast<float> (new_point.g);
782 data_.rgb_[2] +=
static_cast<float> (new_point.b);
791 data_.xyz_[0] += new_point.x;
792 data_.xyz_[1] += new_point.y;
793 data_.xyz_[2] += new_point.z;
795 data_.rgb_[0] +=
static_cast<float> (new_point.r);
796 data_.rgb_[1] +=
static_cast<float> (new_point.g);
797 data_.rgb_[2] +=
static_cast<float> (new_point.b);
806 data_.rgb_[0] /= (
static_cast<float> (num_points_));
807 data_.rgb_[1] /= (
static_cast<float> (num_points_));
808 data_.rgb_[2] /= (
static_cast<float> (num_points_));
809 data_.xyz_[0] /= (
static_cast<float> (num_points_));
810 data_.xyz_[1] /= (
static_cast<float> (num_points_));
811 data_.xyz_[2] /= (
static_cast<float> (num_points_));
817 data_.rgb_[0] /= (
static_cast<float> (num_points_));
818 data_.rgb_[1] /= (
static_cast<float> (num_points_));
819 data_.rgb_[2] /= (
static_cast<float> (num_points_));
820 data_.xyz_[0] /= (
static_cast<float> (num_points_));
821 data_.xyz_[1] /= (
static_cast<float> (num_points_));
822 data_.xyz_[2] /= (
static_cast<float> (num_points_));
832 data_.xyz_[0] += new_point.x;
833 data_.xyz_[1] += new_point.y;
834 data_.xyz_[2] += new_point.z;
840 data_.xyz_[0] /= (
static_cast<float> (num_points_));
841 data_.xyz_[1] /= (
static_cast<float> (num_points_));
842 data_.xyz_[2] /= (
static_cast<float> (num_points_));
857 point_arg.rgba =
static_cast<uint32_t
>(rgb_[0]) << 16 |
858 static_cast<uint32_t>(rgb_[1]) << 8 |
859 static_cast<uint32_t
>(rgb_[2]);
860 point_arg.x = xyz_[0];
861 point_arg.y = xyz_[1];
862 point_arg.z = xyz_[2];
868 point_arg.rgba =
static_cast<uint32_t
>(rgb_[0]) << 16 |
869 static_cast<uint32_t>(rgb_[1]) << 8 |
870 static_cast<uint32_t
>(rgb_[2]);
871 point_arg.x = xyz_[0];
872 point_arg.y = xyz_[1];
873 point_arg.z = xyz_[2];
876 template<
typename Po
intT>
void
880 point_arg.x = xyz_[0];
881 point_arg.y = xyz_[1];
882 point_arg.z = xyz_[2];
886 template <
typename Po
intT>
void
889 normal_arg.normal_x = normal_[0];
890 normal_arg.normal_y = normal_[1];
891 normal_arg.normal_z = normal_[2];
900 template <
typename Po
intT>
void
903 leaves_.insert (leaf_arg);
904 VoxelData& voxel_data = leaf_arg->getData ();
905 voxel_data.owner_ =
this;
909 template <
typename Po
intT>
void
912 leaves_.erase (leaf_arg);
916 template <
typename Po
intT>
void
919 typename std::set<LeafContainerT*>::iterator leaf_itr;
920 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
922 VoxelData& voxel = ((*leaf_itr)->getData ());
924 voxel.distance_ = std::numeric_limits<float>::max ();
930 template <
typename Po
intT>
void
935 std::vector<LeafContainerT*> new_owned;
936 new_owned.reserve (leaves_.size () * 9);
938 typename std::set<LeafContainerT*>::iterator leaf_itr;
939 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
942 for (
typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
945 VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
947 if(neighbor_voxel.owner_ ==
this)
950 float dist = parent_->voxelDataDistance (centroid_, neighbor_voxel);
953 if (dist < neighbor_voxel.distance_)
955 neighbor_voxel.distance_ = dist;
956 if (neighbor_voxel.owner_ !=
this)
958 if (neighbor_voxel.owner_)
959 (neighbor_voxel.owner_)->removeLeaf(*neighb_itr);
960 neighbor_voxel.owner_ =
this;
961 new_owned.push_back (*neighb_itr);
967 typename std::vector<LeafContainerT*>::iterator new_owned_itr;
968 for (new_owned_itr=new_owned.begin (); new_owned_itr!=new_owned.end (); ++new_owned_itr)
970 leaves_.insert (*new_owned_itr);
976 template <
typename Po
intT>
void
979 typename std::set<LeafContainerT*>::iterator leaf_itr;
981 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
983 VoxelData& voxel_data = (*leaf_itr)->getData ();
984 std::vector<int> indices;
985 indices.reserve (81);
987 indices.push_back (voxel_data.idx_);
988 for (
typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
991 VoxelData& neighbor_voxel_data = ((*neighb_itr)->getData ());
993 if (neighbor_voxel_data.owner_ ==
this)
995 indices.push_back (neighbor_voxel_data.idx_);
997 for (
typename LeafContainerT::iterator neighb_neighb_itr=(*neighb_itr)->begin (); neighb_neighb_itr!=(*neighb_itr)->end (); ++neighb_neighb_itr)
999 VoxelData& neighb_neighb_voxel_data = (*neighb_neighb_itr)->getData ();
1000 if (neighb_neighb_voxel_data.owner_ ==
this)
1001 indices.push_back (neighb_neighb_voxel_data.idx_);
1008 pcl::computePointNormal (*parent_->voxel_centroid_cloud_, indices, voxel_data.normal_, voxel_data.curvature_);
1010 voxel_data.normal_[3] = 0.0f;
1011 voxel_data.normal_.normalize ();
1016 template <
typename Po
intT>
void
1019 centroid_.normal_ = Eigen::Vector4f::Zero ();
1020 centroid_.xyz_ = Eigen::Vector3f::Zero ();
1021 centroid_.rgb_ = Eigen::Vector3f::Zero ();
1022 typename std::set<LeafContainerT*>::iterator leaf_itr = leaves_.begin ();
1023 for ( ; leaf_itr!= leaves_.end (); ++leaf_itr)
1025 const VoxelData& leaf_data = (*leaf_itr)->getData ();
1026 centroid_.normal_ += leaf_data.normal_;
1027 centroid_.xyz_ += leaf_data.xyz_;
1028 centroid_.rgb_ += leaf_data.rgb_;
1030 centroid_.normal_.normalize ();
1031 centroid_.xyz_ /=
static_cast<float> (leaves_.size ());
1032 centroid_.rgb_ /=
static_cast<float> (leaves_.size ());
1037 template <
typename Po
intT>
void
1042 voxels->
resize (leaves_.size ());
1045 for (
typename std::set<LeafContainerT*>::const_iterator leaf_itr = leaves_.begin ();
1046 leaf_itr != leaves_.end ();
1047 ++leaf_itr, ++voxel_itr)
1049 const VoxelData& leaf_data = (*leaf_itr)->getData ();
1050 leaf_data.getPoint (*voxel_itr);
1055 template <
typename Po
intT>
void
1060 normals->
resize (leaves_.size ());
1061 typename std::set<LeafContainerT*>::const_iterator leaf_itr;
1063 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++normal_itr)
1065 const VoxelData& leaf_data = (*leaf_itr)->getData ();
1066 leaf_data.getNormal (*normal_itr);
1071 template <
typename Po
intT>
void
1074 neighbor_labels.clear ();
1076 typename std::set<LeafContainerT*>::const_iterator leaf_itr;
1077 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
1080 for (
typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
1083 VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
1085 if (neighbor_voxel.owner_ !=
this && neighbor_voxel.owner_)
1087 neighbor_labels.insert (neighbor_voxel.owner_->getLabel ());
1094 #endif // PCL_SUPERVOXEL_CLUSTERING_HPP_
A point structure representing normal coordinates and the surface curvature estimate.
void setSeedResolution(float seed_resolution)
Set the resolution of the octree seed voxels.
boost::shared_ptr< PointCloud< PointT > > Ptr
Octree adjacency leaf container class- stores set of pointers to neighbors, number of points added...
Octree pointcloud voxel class which maintains adjacency information for its voxels.
void setNormalImportance(float val)
Set the importance of scalar normal product for supervoxels.
void setColorImportance(float val)
Set the importance of color for supervoxels.
pcl::PointCloud< PointXYZL >::Ptr getLabeledCloud() const
Returns labeled cloud Points that belong to the same supervoxel have the same label.
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
pcl::PointCloud< PointT >::Ptr getVoxelCentroidCloud() const
Returns a deep copy of the voxel centroid cloud.
VoxelAdjacencyList::vertex_descriptor VoxelID
void setVoxelResolution(float resolution)
Set the resolution of the octree voxels.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
virtual void setInputCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud)
This method sets the cloud to be supervoxelized.
void flipNormalTowardsViewpoint(const PointT &point, float vp_x, float vp_y, float vp_z, Eigen::Matrix< Scalar, 4, 1 > &normal)
Flip (in place) the estimated normal of a point towards a given viewpoint.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
virtual void refineSupervoxels(int num_itr, std::map< uint32_t, typename Supervoxel< PointT >::Ptr > &supervoxel_clusters)
This method refines the calculated supervoxels - may only be called after extract.
VectorType::const_iterator const_iterator
virtual void setNormalCloud(typename NormalCloudT::ConstPtr normal_cloud)
This method sets the normals to be used for supervoxels (should be same size as input cloud) ...
SupervoxelClustering(float voxel_resolution, float seed_resolution, bool use_single_camera_transform=true)
Constructor that sets default values for member variables.
A point structure representing Euclidean xyz coordinates.
virtual void extract(std::map< uint32_t, typename Supervoxel< PointT >::Ptr > &supervoxel_clusters)
This method launches the segmentation algorithm and returns the supervoxels that were obtained during...
VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContain...
static pcl::PointCloud< pcl::PointNormal >::Ptr makeSupervoxelNormalCloud(std::map< uint32_t, typename Supervoxel< PointT >::Ptr > &supervoxel_clusters)
Static helper function which returns a pointcloud of normals for the input supervoxels.
SupervoxelHelper * owner_
void getNormal(Normal &normal_arg) const
Gets the data of in the form of a normal.
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr getColoredVoxelCloud() const
Returns an RGB colorized voxelized cloud showing superpixels Otherwise it returns an empty pointer...
pcl::PointCloud< pcl::PointXYZL >::Ptr getLabeledVoxelCloud() const
Returns labeled voxelized cloud Points that belong to the same supervoxel have the same label...
Implements a supervoxel algorithm based on voxel structure, normals, and rgb values.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void getSupervoxelAdjacency(std::multimap< uint32_t, uint32_t > &label_adjacency) const
Get a multimap which gives supervoxel adjacency.
virtual ~SupervoxelClustering()
This destructor destroys the cloud, normals and search method used for finding neighbors.
Octree pointcloud search class
VoxelAdjacencyList::edge_descriptor EdgeID
float getSeedResolution() const
Get the resolution of the octree seed voxels.
int getMaxLabel() const
Returns the current maximum (highest) label.
void getPoint(PointT &point_arg) const
Gets the data of in the form of a point.
void computePointNormal(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &plane_parameters, float &curvature)
Compute the Least-Squares plane fit for a given set of points, and return the estimated plane paramet...
A point structure representing Euclidean xyz coordinates, and the RGB color.
boost::shared_ptr< Supervoxel< PointT > > Ptr
void getSupervoxelAdjacencyList(VoxelAdjacencyList &adjacency_list_arg) const
Gets the adjacency list (Boost Graph library) which gives connections between supervoxels.
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, uint32_t, float > VoxelAdjacencyList
VectorType::iterator iterator
void resize(size_t n)
Resize the cloud.
pcl::PointCloud< PointXYZRGBA >::Ptr getColoredCloud() const
Returns an RGB colorized cloud showing superpixels Otherwise it returns an empty pointer.
float getVoxelResolution() const
Get the resolution of the octree voxels.
void clear()
Removes all points in a cloud and sets the width and height to 0.
void setSpatialImportance(float val)
Set the importance of spatial distance for supervoxels.