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),
55 use_default_transform_behaviour_ (true)
61 template <
typename Po
intT>
68 template <
typename Po
intT>
void
71 if ( cloud->
empty () )
73 PCL_ERROR (
"[pcl::SupervoxelClustering::setInputCloud] Empty cloud set, doing nothing \n");
78 adjacency_octree_->setInputCloud (cloud);
82 template <
typename Po
intT>
void
85 if ( normal_cloud->empty () )
87 PCL_ERROR (
"[pcl::SupervoxelClustering::setNormalCloud] Empty cloud set, doing nothing \n");
91 input_normals_ = normal_cloud;
95 template <
typename Po
intT>
void
101 bool segmentation_is_possible = initCompute ();
102 if ( !segmentation_is_possible )
109 segmentation_is_possible = prepareForSegmentation ();
110 if ( !segmentation_is_possible )
118 std::vector<int> seed_indices;
119 selectInitialSupervoxelSeeds (seed_indices);
121 createSupervoxelHelpers (seed_indices);
126 int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_);
127 expandSupervoxels (max_depth);
131 makeSupervoxels (supervoxel_clusters);
147 template <
typename Po
intT>
void
150 if (supervoxel_helpers_.empty ())
152 PCL_ERROR (
"[pcl::SupervoxelClustering::refineVoxelNormals] Supervoxels not extracted, doing nothing - (Call extract first!) \n");
156 int max_depth = static_cast<int> (1.8f*seed_resolution_/resolution_);
157 for (
int i = 0; i < num_itr; ++i)
159 for (
typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
161 sv_itr->refineNormals ();
164 reseedSupervoxels ();
165 expandSupervoxels (max_depth);
169 makeSupervoxels (supervoxel_clusters);
179 template <
typename Po
intT>
bool
184 if ( input_->points.empty () )
190 if ( (use_default_transform_behaviour_ && input_->isOrganized ())
191 || (!use_default_transform_behaviour_ && use_single_camera_transform_))
192 adjacency_octree_->setTransformFunction ([
this] (
PointT &p) { transformFunction (p); });
194 adjacency_octree_->addPointsFromInputCloud ();
207 template <
typename Po
intT>
void
211 voxel_centroid_cloud_->resize (adjacency_octree_->getLeafCount ());
212 typename LeafVectorT::iterator leaf_itr = adjacency_octree_->begin ();
214 for (
int idx = 0 ; leaf_itr != adjacency_octree_->end (); ++leaf_itr, ++cent_cloud_itr, ++idx)
216 VoxelData& new_voxel_data = (*leaf_itr)->getData ();
218 new_voxel_data.getPoint (*cent_cloud_itr);
220 new_voxel_data.idx_ = idx;
227 assert (input_normals_->size () == input_->size ());
229 typename NormalCloudT::const_iterator normal_itr = input_normals_->begin ();
233 if ( !pcl::isFinite<PointT> (*input_itr))
236 LeafContainerT* leaf = adjacency_octree_->getLeafContainerAtPoint (*input_itr);
239 VoxelData& voxel_data = leaf->getData ();
241 voxel_data.normal_ += normal_itr->getNormalVector4fMap ();
242 voxel_data.curvature_ += normal_itr->curvature;
245 for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
247 VoxelData& voxel_data = (*leaf_itr)->getData ();
248 voxel_data.normal_.normalize ();
249 voxel_data.owner_ =
nullptr;
250 voxel_data.distance_ = std::numeric_limits<float>::max ();
252 int num_points = (*leaf_itr)->getPointCounter ();
253 voxel_data.curvature_ /= num_points;
258 for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
260 VoxelData& new_voxel_data = (*leaf_itr)->getData ();
262 std::vector<int> indices;
263 indices.reserve (81);
265 indices.push_back (new_voxel_data.idx_);
266 for (
typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
268 VoxelData& neighb_voxel_data = (*neighb_itr)->getData ();
270 indices.push_back (neighb_voxel_data.idx_);
272 for (
typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr)
274 VoxelData& neighb2_voxel_data = (*neighb_neighb_itr)->getData ();
275 indices.push_back (neighb2_voxel_data.idx_);
281 new_voxel_data.normal_[3] = 0.0f;
282 new_voxel_data.normal_.normalize ();
283 new_voxel_data.owner_ =
nullptr;
284 new_voxel_data.distance_ = std::numeric_limits<float>::max ();
292 template <
typename Po
intT>
void
297 for (
int i = 1; i < depth; ++i)
300 for (
typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
306 for (
typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); )
308 if (sv_itr->size () == 0)
310 sv_itr = supervoxel_helpers_.erase (sv_itr);
314 sv_itr->updateCentroid ();
324 template <
typename Po
intT>
void
327 supervoxel_clusters.clear ();
328 for (
typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
331 supervoxel_clusters[label].reset (
new Supervoxel<PointT>);
332 sv_itr->getXYZ (supervoxel_clusters[label]->centroid_.x,supervoxel_clusters[label]->centroid_.y,supervoxel_clusters[label]->centroid_.z);
333 sv_itr->getRGB (supervoxel_clusters[label]->centroid_.rgba);
334 sv_itr->getNormal (supervoxel_clusters[label]->normal_);
335 sv_itr->getVoxels (supervoxel_clusters[label]->voxels_);
336 sv_itr->getNormals (supervoxel_clusters[label]->normals_);
342 template <
typename Po
intT>
void
346 supervoxel_helpers_.clear ();
347 for (std::size_t i = 0; i < seed_indices.size (); ++i)
349 supervoxel_helpers_.push_back (
new SupervoxelHelper(i+1,
this));
351 LeafContainerT* seed_leaf = adjacency_octree_->at(seed_indices[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_indices.clear ();
383 std::vector<int> closest_index;
385 closest_index.resize(1,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_indices.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 (
const int &index_orig : seed_indices_orig)
408 int num = voxel_kdtree_->radiusSearch (index_orig, search_radius , neighbors, sqr_distances);
409 int min_index = index_orig;
410 if ( num > min_points)
412 seed_indices.push_back (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;
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_->at (closest_index[0]);
443 sv_itr->addLeaf (seed_leaf);
447 PCL_WARN (
"Could not find leaf in pcl::SupervoxelClustering<PointT>::reseedSupervoxels - supervoxel will be deleted \n");
454 template <
typename Po
intT>
void
459 p.z = std::log (p.z);
463 template <
typename Po
intT>
float
467 float spatial_dist = (v1.xyz_ - v2.xyz_).norm () / seed_resolution_;
468 float color_dist = (v1.rgb_ - v2.rgb_).norm () / 255.0f;
469 float cos_angle_normal = 1.0f - std::abs (v1.normal_.dot (v2.normal_));
471 return cos_angle_normal * normal_importance_ + color_dist * color_importance_+ spatial_dist * spatial_importance_;
482 template <
typename Po
intT>
void
485 adjacency_list_arg.clear ();
487 std::map <std::uint32_t, VoxelID> label_ID_map;
488 for (
typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
490 VoxelID node_id = add_vertex (adjacency_list_arg);
491 adjacency_list_arg[node_id] = (sv_itr->getLabel ());
492 label_ID_map.insert (std::make_pair (sv_itr->getLabel (), node_id));
495 for (
typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
498 std::set<std::uint32_t> neighbor_labels;
499 sv_itr->getNeighborLabels (neighbor_labels);
500 for (
const unsigned int &neighbor_label : neighbor_labels)
504 VoxelID u = (label_ID_map.find (label))->second;
505 VoxelID v = (label_ID_map.find (neighbor_label))->second;
506 boost::tie (edge, edge_added) = add_edge (u,v,adjacency_list_arg);
510 VoxelData centroid_data = (sv_itr)->getCentroid ();
514 for (
typename HelperListT::const_iterator neighb_itr = supervoxel_helpers_.cbegin (); neighb_itr != supervoxel_helpers_.cend (); ++neighb_itr)
516 if (neighb_itr->getLabel () == neighbor_label)
518 neighb_centroid_data = neighb_itr->getCentroid ();
523 float length = voxelDataDistance (centroid_data, neighb_centroid_data);
524 adjacency_list_arg[edge] = length;
533 template <
typename Po
intT>
void
536 label_adjacency.clear ();
537 for (
typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
540 std::set<std::uint32_t> neighbor_labels;
541 sv_itr->getNeighborLabels (neighbor_labels);
542 for (
const unsigned int &neighbor_label : neighbor_labels)
543 label_adjacency.insert (std::pair<std::uint32_t,std::uint32_t> (label, neighbor_label) );
555 return centroid_copy;
563 for (
typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
566 sv_itr->getVoxels (voxels);
571 for ( ; xyzl_copy_itr != xyzl_copy.
end (); ++xyzl_copy_itr)
572 xyzl_copy_itr->label = sv_itr->getLabel ();
574 *labeled_voxel_cloud += xyzl_copy;
577 return labeled_voxel_cloud;
588 std::vector <int> indices;
589 std::vector <float> sqr_distances;
590 for (
auto i_labeled = labeled_cloud->
begin (); i_labeled != labeled_cloud->
end (); ++i_labeled,++i_input)
592 if ( !pcl::isFinite<PointT> (*i_input))
593 i_labeled->label = 0;
596 i_labeled->label = 0;
597 LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
598 VoxelData& voxel_data = leaf->getData ();
600 i_labeled->label = voxel_data.
owner_->getLabel ();
606 return (labeled_cloud);
614 normal_cloud->
resize (supervoxel_clusters.size ());
616 for (
auto sv_itr = supervoxel_clusters.cbegin (), sv_itr_end = supervoxel_clusters.cend ();
617 sv_itr != sv_itr_end; ++sv_itr, ++normal_cloud_itr)
619 (sv_itr->second)->getCentroidPointNormal (*normal_cloud_itr);
625 template <
typename Po
intT>
float
628 return (resolution_);
632 template <
typename Po
intT>
void
635 resolution_ = resolution;
640 template <
typename Po
intT>
float
643 return (seed_resolution_);
647 template <
typename Po
intT>
void
650 seed_resolution_ = seed_resolution;
655 template <
typename Po
intT>
void
658 color_importance_ = val;
662 template <
typename Po
intT>
void
665 spatial_importance_ = val;
669 template <
typename Po
intT>
void
672 normal_importance_ = val;
676 template <
typename Po
intT>
void
679 use_default_transform_behaviour_ =
false;
680 use_single_camera_transform_ = val;
684 template <
typename Po
intT>
int
688 for (
typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
690 int temp = sv_itr->getLabel ();
691 if (temp > max_label)
739 template<
typename Po
intT>
void
743 point_arg.x = xyz_[0];
744 point_arg.y = xyz_[1];
745 point_arg.z = xyz_[2];
749 template <
typename Po
intT>
void
752 normal_arg.normal_x = normal_[0];
753 normal_arg.normal_y = normal_[1];
754 normal_arg.normal_z = normal_[2];
762 template <
typename Po
intT>
void
765 leaves_.insert (leaf_arg);
766 VoxelData& voxel_data = leaf_arg->getData ();
767 voxel_data.owner_ =
this;
771 template <
typename Po
intT>
void
774 leaves_.erase (leaf_arg);
778 template <
typename Po
intT>
void
781 for (
auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
783 VoxelData& voxel = ((*leaf_itr)->getData ());
784 voxel.owner_ =
nullptr;
785 voxel.distance_ = std::numeric_limits<float>::max ();
791 template <
typename Po
intT>
void
796 std::vector<LeafContainerT*> new_owned;
797 new_owned.reserve (leaves_.size () * 9);
799 for (
auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
802 for (
typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
805 VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
807 if(neighbor_voxel.owner_ ==
this)
810 float dist = parent_->voxelDataDistance (centroid_, neighbor_voxel);
813 if (dist < neighbor_voxel.distance_)
815 neighbor_voxel.distance_ = dist;
816 if (neighbor_voxel.owner_ !=
this)
818 if (neighbor_voxel.owner_)
819 (neighbor_voxel.owner_)->removeLeaf(*neighb_itr);
820 neighbor_voxel.owner_ =
this;
821 new_owned.push_back (*neighb_itr);
827 for (
auto new_owned_itr = new_owned.cbegin (); new_owned_itr != new_owned.cend (); ++new_owned_itr)
829 leaves_.insert (*new_owned_itr);
834 template <
typename Po
intT>
void
838 for (
auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
840 VoxelData& voxel_data = (*leaf_itr)->getData ();
841 std::vector<int> indices;
842 indices.reserve (81);
844 indices.push_back (voxel_data.idx_);
845 for (
typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
848 VoxelData& neighbor_voxel_data = ((*neighb_itr)->getData ());
850 if (neighbor_voxel_data.owner_ ==
this)
852 indices.push_back (neighbor_voxel_data.idx_);
854 for (
typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr)
856 VoxelData& neighb_neighb_voxel_data = (*neighb_neighb_itr)->getData ();
857 if (neighb_neighb_voxel_data.owner_ ==
this)
858 indices.push_back (neighb_neighb_voxel_data.idx_);
867 voxel_data.normal_[3] = 0.0f;
868 voxel_data.normal_.normalize ();
873 template <
typename Po
intT>
void
876 centroid_.normal_ = Eigen::Vector4f::Zero ();
877 centroid_.xyz_ = Eigen::Vector3f::Zero ();
878 centroid_.rgb_ = Eigen::Vector3f::Zero ();
879 for (
auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
881 const VoxelData& leaf_data = (*leaf_itr)->getData ();
882 centroid_.normal_ += leaf_data.normal_;
883 centroid_.xyz_ += leaf_data.xyz_;
884 centroid_.rgb_ += leaf_data.rgb_;
886 centroid_.normal_.normalize ();
887 centroid_.xyz_ /= static_cast<float> (leaves_.size ());
888 centroid_.rgb_ /= static_cast<float> (leaves_.size ());
892 template <
typename Po
intT>
void
897 voxels->
resize (leaves_.size ());
899 for (
auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr, ++voxel_itr)
901 const VoxelData& leaf_data = (*leaf_itr)->getData ();
902 leaf_data.getPoint (*voxel_itr);
907 template <
typename Po
intT>
void
912 normals->
resize (leaves_.size ());
914 for (
auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr, ++normal_itr)
916 const VoxelData& leaf_data = (*leaf_itr)->getData ();
917 leaf_data.getNormal (*normal_itr);
922 template <
typename Po
intT>
void
925 neighbor_labels.clear ();
927 for (
auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
930 for (
typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
933 VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
935 if (neighbor_voxel.owner_ !=
this && neighbor_voxel.owner_)
937 neighbor_labels.insert (neighbor_voxel.owner_->getLabel ());
944 #endif // PCL_SUPERVOXEL_CLUSTERING_HPP_