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),
57 adjacency_octree_ = boost::make_shared <OctreeAdjacencyT> (resolution_);
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
208 voxel_centroid_cloud_ = boost::make_shared<PointCloudT> ();
209 voxel_centroid_cloud_->resize (adjacency_octree_->getLeafCount ());
210 typename LeafVectorT::iterator leaf_itr = adjacency_octree_->begin ();
211 typename PointCloudT::iterator cent_cloud_itr = voxel_centroid_cloud_->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 ();
228 for (
typename PointCloudT::const_iterator input_itr = input_->begin (); input_itr != input_->end (); ++input_itr, ++normal_itr)
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] = boost::make_shared<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 (
int 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)
389 voxel_kdtree_ = boost::make_shared< pcl::search::KdTree<PointT> >();
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 (
int 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)
584 typename PointCloudT::Ptr voxels;
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;
603 typename PointCloudT::Ptr centroid_copy = boost::make_shared<PointCloudT> ();
605 return centroid_copy;
613 for (
typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
615 typename PointCloudT::Ptr voxels;
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 int max_label = 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_));
836 point_arg.rgba =
static_cast<uint32_t
>(rgb_[0]) << 16 |
837 static_cast<uint32_t>(rgb_[1]) << 8 |
838 static_cast<uint32_t
>(rgb_[2]);
839 point_arg.x = xyz_[0];
840 point_arg.y = xyz_[1];
841 point_arg.z = xyz_[2];
847 point_arg.rgba =
static_cast<uint32_t
>(rgb_[0]) << 16 |
848 static_cast<uint32_t>(rgb_[1]) << 8 |
849 static_cast<uint32_t
>(rgb_[2]);
850 point_arg.x = xyz_[0];
851 point_arg.y = xyz_[1];
852 point_arg.z = xyz_[2];
855 template<
typename Po
intT>
void
859 point_arg.x = xyz_[0];
860 point_arg.y = xyz_[1];
861 point_arg.z = xyz_[2];
865 template <
typename Po
intT>
void
868 normal_arg.normal_x = normal_[0];
869 normal_arg.normal_y = normal_[1];
870 normal_arg.normal_z = normal_[2];
879 template <
typename Po
intT>
void
882 leaves_.insert (leaf_arg);
883 VoxelData& voxel_data = leaf_arg->getData ();
884 voxel_data.owner_ =
this;
888 template <
typename Po
intT>
void
891 leaves_.erase (leaf_arg);
895 template <
typename Po
intT>
void
898 typename std::set<LeafContainerT*>::iterator leaf_itr;
899 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
901 VoxelData& voxel = ((*leaf_itr)->getData ());
903 voxel.distance_ = std::numeric_limits<float>::max ();
909 template <
typename Po
intT>
void
914 std::vector<LeafContainerT*> new_owned;
915 new_owned.reserve (leaves_.size () * 9);
917 typename std::set<LeafContainerT*>::iterator leaf_itr;
918 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
921 for (
typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
924 VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
926 if(neighbor_voxel.owner_ ==
this)
929 float dist = parent_->voxelDataDistance (centroid_, neighbor_voxel);
932 if (dist < neighbor_voxel.distance_)
934 neighbor_voxel.distance_ = dist;
935 if (neighbor_voxel.owner_ !=
this)
937 if (neighbor_voxel.owner_)
938 (neighbor_voxel.owner_)->removeLeaf(*neighb_itr);
939 neighbor_voxel.owner_ =
this;
940 new_owned.push_back (*neighb_itr);
946 typename std::vector<LeafContainerT*>::iterator new_owned_itr;
947 for (new_owned_itr=new_owned.begin (); new_owned_itr!=new_owned.end (); ++new_owned_itr)
949 leaves_.insert (*new_owned_itr);
955 template <
typename Po
intT>
void
958 typename std::set<LeafContainerT*>::iterator leaf_itr;
960 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
962 VoxelData& voxel_data = (*leaf_itr)->getData ();
963 std::vector<int> indices;
964 indices.reserve (81);
966 indices.push_back (voxel_data.idx_);
967 for (
typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
970 VoxelData& neighbor_voxel_data = ((*neighb_itr)->getData ());
972 if (neighbor_voxel_data.owner_ ==
this)
974 indices.push_back (neighbor_voxel_data.idx_);
976 for (
typename LeafContainerT::iterator neighb_neighb_itr=(*neighb_itr)->begin (); neighb_neighb_itr!=(*neighb_itr)->end (); ++neighb_neighb_itr)
978 VoxelData& neighb_neighb_voxel_data = (*neighb_neighb_itr)->getData ();
979 if (neighb_neighb_voxel_data.owner_ ==
this)
980 indices.push_back (neighb_neighb_voxel_data.idx_);
989 voxel_data.normal_[3] = 0.0f;
990 voxel_data.normal_.normalize ();
995 template <
typename Po
intT>
void
998 centroid_.normal_ = Eigen::Vector4f::Zero ();
999 centroid_.xyz_ = Eigen::Vector3f::Zero ();
1000 centroid_.rgb_ = Eigen::Vector3f::Zero ();
1001 typename std::set<LeafContainerT*>::iterator leaf_itr = leaves_.begin ();
1002 for ( ; leaf_itr!= leaves_.end (); ++leaf_itr)
1004 const VoxelData& leaf_data = (*leaf_itr)->getData ();
1005 centroid_.normal_ += leaf_data.normal_;
1006 centroid_.xyz_ += leaf_data.xyz_;
1007 centroid_.rgb_ += leaf_data.rgb_;
1009 centroid_.normal_.normalize ();
1010 centroid_.xyz_ /=
static_cast<float> (leaves_.size ());
1011 centroid_.rgb_ /=
static_cast<float> (leaves_.size ());
1016 template <
typename Po
intT>
void
1019 voxels = boost::make_shared<pcl::PointCloud<PointT> > ();
1021 voxels->
resize (leaves_.size ());
1023 typename std::set<LeafContainerT*>::iterator leaf_itr;
1024 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++voxel_itr)
1026 const VoxelData& leaf_data = (*leaf_itr)->getData ();
1027 leaf_data.getPoint (*voxel_itr);
1032 template <
typename Po
intT>
void
1035 normals = boost::make_shared<pcl::PointCloud<Normal> > ();
1037 normals->
resize (leaves_.size ());
1038 typename std::set<LeafContainerT*>::iterator leaf_itr;
1040 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++normal_itr)
1042 const VoxelData& leaf_data = (*leaf_itr)->getData ();
1043 leaf_data.getNormal (*normal_itr);
1048 template <
typename Po
intT>
void
1051 neighbor_labels.clear ();
1053 typename std::set<LeafContainerT*>::iterator leaf_itr;
1054 for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr)
1057 for (
typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr)
1060 VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
1062 if (neighbor_voxel.owner_ !=
this && neighbor_voxel.owner_)
1064 neighbor_labels.insert (neighbor_voxel.owner_->getLabel ());
1071 #endif // PCL_SUPERVOXEL_CLUSTERING_HPP_
void setVoxelResolution(float resolution)
Set the resolution of the octree voxels.
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr getColoredVoxelCloud() const
Returns an RGB colorized voxelized cloud showing superpixels Otherwise it returns an empty pointer...
int getMaxLabel() const
Returns the current maximum (highest) 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.
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.
VectorType::iterator iterator
VoxelAdjacencyList::edge_descriptor EdgeID
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Octree adjacency leaf container class- stores set of pointers to neighbors, number of points added...
pcl::PointCloud< PointXYZRGBA >::Ptr getColoredCloud() const
Returns an RGB colorized cloud showing superpixels Otherwise it returns an empty pointer.
SupervoxelHelper * owner_
float getSeedResolution() const
Get the resolution of the octree seed voxels.
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...
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.
pcl::PointCloud< pcl::PointXYZL >::Ptr getLabeledVoxelCloud() const
Returns labeled voxelized cloud Points that belong to the same supervoxel have the same label...
boost::shared_ptr< PointCloud< PointT > > Ptr
float getVoxelResolution() const
Get the resolution of the octree voxels.
void setSpatialImportance(float val)
Set the importance of spatial distance for supervoxels.
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) ...
void getNormal(Normal &normal_arg) const
Gets the data of in the form of a normal.
VectorType::const_iterator const_iterator
A point structure representing normal coordinates and the surface curvature estimate.
VoxelAdjacencyList::vertex_descriptor VoxelID
void setColorImportance(float val)
Set the importance of color for supervoxels.
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, uint32_t, float > VoxelAdjacencyList
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, and the RGBA color.
void getPoint(PointT &point_arg) const
Gets the data of in the form of a point.
void setSeedResolution(float seed_resolution)
Set the resolution of the octree seed voxels.
Octree pointcloud search class
void resize(size_t n)
Resize the cloud.
void getSupervoxelAdjacency(std::multimap< uint32_t, uint32_t > &label_adjacency) const
Get a multimap which gives supervoxel adjacency.
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...
pcl::PointCloud< PointXYZL >::Ptr getLabeledCloud() const
Returns labeled cloud Points that belong to the same supervoxel have the same label.
VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContain...
boost::shared_ptr< Supervoxel< PointT > > Ptr
Implements a supervoxel algorithm based on voxel structure, normals, and rgb values.
A point structure representing Euclidean xyz coordinates, and the RGB color.
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.
virtual ~SupervoxelClustering()
This destructor destroys the cloud, normals and search method used for finding neighbors.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void clear()
Removes all points in a cloud and sets the width and height to 0.
void getSupervoxelAdjacencyList(VoxelAdjacencyList &adjacency_list_arg) const
Gets the adjacency list (Boost Graph library) which gives connections between supervoxels.
pcl::PointCloud< PointT >::Ptr getVoxelCentroidCloud() const
Returns a deep copy of the voxel centroid cloud.
virtual void setInputCloud(typename pcl::PointCloud< PointT >::ConstPtr cloud)
This method sets the cloud to be supervoxelized.
void setNormalImportance(float val)
Set the importance of scalar normal product for supervoxels.