40 #ifndef PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_
41 #define PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_HPP_
43 #include <pcl/segmentation/supervoxel_clustering.h>
44 #include <pcl/common/io.h>
47 template <
typename Po
intT>
49 resolution_ (voxel_resolution),
50 seed_resolution_ (seed_resolution),
52 voxel_centroid_cloud_ (),
53 color_importance_ (0.1f),
54 spatial_importance_ (0.4f),
55 normal_importance_ (1.0f),
56 use_default_transform_behaviour_ (true)
62 template <
typename Po
intT>
69 template <
typename Po
intT>
void
72 if ( cloud->
empty () )
74 PCL_ERROR (
"[pcl::SupervoxelClustering::setInputCloud] Empty cloud set, doing nothing \n");
79 adjacency_octree_->setInputCloud (cloud);
83 template <
typename Po
intT>
void
86 if ( normal_cloud->empty () )
88 PCL_ERROR (
"[pcl::SupervoxelClustering::setNormalCloud] Empty cloud set, doing nothing \n");
92 input_normals_ = normal_cloud;
96 template <
typename Po
intT>
void
102 bool segmentation_is_possible = initCompute ();
103 if ( !segmentation_is_possible )
110 segmentation_is_possible = prepareForSegmentation ();
111 if ( !segmentation_is_possible )
120 selectInitialSupervoxelSeeds (seed_indices);
122 createSupervoxelHelpers (seed_indices);
127 int max_depth =
static_cast<int> (1.8f*seed_resolution_/resolution_);
128 expandSupervoxels (max_depth);
132 makeSupervoxels (supervoxel_clusters);
148 template <
typename Po
intT>
void
151 if (supervoxel_helpers_.empty ())
153 PCL_ERROR (
"[pcl::SupervoxelClustering::refineVoxelNormals] Supervoxels not extracted, doing nothing - (Call extract first!) \n");
157 int max_depth =
static_cast<int> (1.8f*seed_resolution_/resolution_);
158 for (
int i = 0; i < num_itr; ++i)
160 for (
typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
162 sv_itr->refineNormals ();
165 reseedSupervoxels ();
166 expandSupervoxels (max_depth);
170 makeSupervoxels (supervoxel_clusters);
180 template <
typename Po
intT>
bool
185 if ( input_->points.empty () )
191 if ( (use_default_transform_behaviour_ && input_->isOrganized ())
192 || (!use_default_transform_behaviour_ && use_single_camera_transform_))
193 adjacency_octree_->setTransformFunction ([
this] (
PointT &p) { transformFunction (p); });
195 adjacency_octree_->addPointsFromInputCloud ();
208 template <
typename Po
intT>
void
212 voxel_centroid_cloud_->resize (adjacency_octree_->getLeafCount ());
213 typename LeafVectorT::iterator leaf_itr = adjacency_octree_->begin ();
215 for (
int idx = 0 ; leaf_itr != adjacency_octree_->end (); ++leaf_itr, ++cent_cloud_itr, ++idx)
217 VoxelData& new_voxel_data = (*leaf_itr)->getData ();
219 new_voxel_data.getPoint (*cent_cloud_itr);
221 new_voxel_data.idx_ = idx;
228 assert (input_normals_->size () == input_->size ());
230 typename NormalCloudT::const_iterator normal_itr = input_normals_->begin ();
234 if ( !pcl::isFinite<PointT> (*input_itr))
237 LeafContainerT* leaf = adjacency_octree_->getLeafContainerAtPoint (*input_itr);
240 VoxelData& voxel_data = leaf->getData ();
242 voxel_data.normal_ += normal_itr->getNormalVector4fMap ();
243 voxel_data.curvature_ += normal_itr->curvature;
246 for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
248 VoxelData& voxel_data = (*leaf_itr)->getData ();
249 voxel_data.normal_.normalize ();
250 voxel_data.owner_ =
nullptr;
251 voxel_data.distance_ = std::numeric_limits<float>::max ();
253 int num_points = (*leaf_itr)->getPointCounter ();
254 voxel_data.curvature_ /= num_points;
259 for (leaf_itr = adjacency_octree_->begin (); leaf_itr != adjacency_octree_->end (); ++leaf_itr)
261 VoxelData& new_voxel_data = (*leaf_itr)->getData ();
264 indices.reserve (81);
266 indices.push_back (new_voxel_data.idx_);
267 for (
typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
269 VoxelData& neighb_voxel_data = (*neighb_itr)->getData ();
271 indices.push_back (neighb_voxel_data.idx_);
273 for (
typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr)
275 VoxelData& neighb2_voxel_data = (*neighb_neighb_itr)->getData ();
276 indices.push_back (neighb2_voxel_data.idx_);
282 new_voxel_data.normal_[3] = 0.0f;
283 new_voxel_data.normal_.normalize ();
284 new_voxel_data.owner_ =
nullptr;
285 new_voxel_data.distance_ = std::numeric_limits<float>::max ();
293 template <
typename Po
intT>
void
298 for (
int i = 1; i < depth; ++i)
301 for (
typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
307 for (
typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); )
309 if (sv_itr->size () == 0)
311 sv_itr = supervoxel_helpers_.erase (sv_itr);
315 sv_itr->updateCentroid ();
325 template <
typename Po
intT>
void
328 supervoxel_clusters.clear ();
329 for (
typename HelperListT::iterator sv_itr = supervoxel_helpers_.begin (); sv_itr != supervoxel_helpers_.end (); ++sv_itr)
331 std::uint32_t label = sv_itr->getLabel ();
332 supervoxel_clusters[label].reset (
new Supervoxel<PointT>);
333 sv_itr->getXYZ (supervoxel_clusters[label]->centroid_.x,supervoxel_clusters[label]->centroid_.y,supervoxel_clusters[label]->centroid_.z);
334 sv_itr->getRGB (supervoxel_clusters[label]->centroid_.rgba);
335 sv_itr->getNormal (supervoxel_clusters[label]->normal_);
336 sv_itr->getVoxels (supervoxel_clusters[label]->voxels_);
337 sv_itr->getNormals (supervoxel_clusters[label]->normals_);
343 template <
typename Po
intT>
void
347 supervoxel_helpers_.clear ();
348 for (std::size_t i = 0; i < seed_indices.size (); ++i)
350 supervoxel_helpers_.push_back (
new SupervoxelHelper(i+1,
this));
352 LeafContainerT* seed_leaf = adjacency_octree_->at(seed_indices[i]);
355 supervoxel_helpers_.back ().addLeaf (seed_leaf);
359 PCL_WARN (
"Could not find leaf in pcl::SupervoxelClustering<PointT>::createSupervoxelHelpers - supervoxel will be deleted \n");
365 template <
typename Po
intT>
void
374 seed_octree.setInputCloud (voxel_centroid_cloud_);
375 seed_octree.addPointsFromInputCloud ();
377 std::vector<PointT, Eigen::aligned_allocator<PointT> > voxel_centers;
378 int num_seeds = seed_octree.getOccupiedVoxelCenters(voxel_centers);
381 std::vector<int> seed_indices_orig;
382 seed_indices_orig.resize (num_seeds, 0);
383 seed_indices.clear ();
384 std::vector<int> closest_index;
386 closest_index.resize(1,0);
391 voxel_kdtree_ ->setInputCloud (voxel_centroid_cloud_);
394 for (
int i = 0; i < num_seeds; ++i)
396 voxel_kdtree_->nearestKSearch (voxel_centers[i], 1, closest_index,
distance);
397 seed_indices_orig[i] = closest_index[0];
400 std::vector<int> neighbors;
401 std::vector<float> sqr_distances;
402 seed_indices.reserve (seed_indices_orig.size ());
403 float search_radius = 0.5f*seed_resolution_;
406 float min_points = 0.05f * (search_radius)*(search_radius) * 3.1415926536f / (resolution_*resolution_);
407 for (
const auto &index_orig : seed_indices_orig)
409 int num = voxel_kdtree_->radiusSearch (index_orig, search_radius , neighbors, sqr_distances);
410 if ( num > min_points)
412 seed_indices.push_back (index_orig);
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 ();
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)
497 std::uint32_t label = sv_itr->getLabel ();
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)
539 std::uint32_t label = sv_itr->getLabel ();
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 for (
auto i_labeled = labeled_cloud->
begin (); i_labeled != labeled_cloud->
end (); ++i_labeled,++i_input)
590 if ( !pcl::isFinite<PointT> (*i_input))
591 i_labeled->label = 0;
594 i_labeled->label = 0;
595 LeafContainerT *leaf = adjacency_octree_->getLeafContainerAtPoint (*i_input);
598 i_labeled->label = voxel_data.
owner_->getLabel ();
604 return (labeled_cloud);
612 normal_cloud->
resize (supervoxel_clusters.size ());
614 for (
auto sv_itr = supervoxel_clusters.cbegin (), sv_itr_end = supervoxel_clusters.cend ();
615 sv_itr != sv_itr_end; ++sv_itr, ++normal_cloud_itr)
617 (sv_itr->second)->getCentroidPointNormal (*normal_cloud_itr);
623 template <
typename Po
intT>
float
626 return (resolution_);
630 template <
typename Po
intT>
void
633 resolution_ = resolution;
638 template <
typename Po
intT>
float
641 return (seed_resolution_);
645 template <
typename Po
intT>
void
648 seed_resolution_ = seed_resolution;
653 template <
typename Po
intT>
void
656 color_importance_ = val;
660 template <
typename Po
intT>
void
663 spatial_importance_ = val;
667 template <
typename Po
intT>
void
670 normal_importance_ = val;
674 template <
typename Po
intT>
void
677 use_default_transform_behaviour_ =
false;
678 use_single_camera_transform_ = val;
682 template <
typename Po
intT>
int
686 for (
typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
688 int temp = sv_itr->getLabel ();
689 if (temp > max_label)
737 template<
typename Po
intT>
void
741 point_arg.x = xyz_[0];
742 point_arg.y = xyz_[1];
743 point_arg.z = xyz_[2];
747 template <
typename Po
intT>
void
750 normal_arg.normal_x = normal_[0];
751 normal_arg.normal_y = normal_[1];
752 normal_arg.normal_z = normal_[2];
760 template <
typename Po
intT>
void
763 leaves_.insert (leaf_arg);
764 VoxelData& voxel_data = leaf_arg->getData ();
765 voxel_data.owner_ =
this;
769 template <
typename Po
intT>
void
772 leaves_.erase (leaf_arg);
776 template <
typename Po
intT>
void
779 for (
auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
781 VoxelData& voxel = ((*leaf_itr)->getData ());
782 voxel.owner_ =
nullptr;
783 voxel.distance_ = std::numeric_limits<float>::max ();
789 template <
typename Po
intT>
void
794 std::vector<LeafContainerT*> new_owned;
795 new_owned.reserve (leaves_.size () * 9);
797 for (
auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
800 for (
typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
803 VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
805 if(neighbor_voxel.owner_ ==
this)
808 float dist = parent_->voxelDataDistance (centroid_, neighbor_voxel);
811 if (dist < neighbor_voxel.distance_)
813 neighbor_voxel.distance_ = dist;
814 if (neighbor_voxel.owner_ !=
this)
816 if (neighbor_voxel.owner_)
817 (neighbor_voxel.owner_)->removeLeaf(*neighb_itr);
818 neighbor_voxel.owner_ =
this;
819 new_owned.push_back (*neighb_itr);
825 for (
auto new_owned_itr = new_owned.cbegin (); new_owned_itr != new_owned.cend (); ++new_owned_itr)
827 leaves_.insert (*new_owned_itr);
832 template <
typename Po
intT>
void
836 for (
auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
838 VoxelData& voxel_data = (*leaf_itr)->getData ();
840 indices.reserve (81);
842 indices.push_back (voxel_data.idx_);
843 for (
typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
846 VoxelData& neighbor_voxel_data = ((*neighb_itr)->getData ());
848 if (neighbor_voxel_data.owner_ ==
this)
850 indices.push_back (neighbor_voxel_data.idx_);
852 for (
typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr)
854 VoxelData& neighb_neighb_voxel_data = (*neighb_neighb_itr)->getData ();
855 if (neighb_neighb_voxel_data.owner_ ==
this)
856 indices.push_back (neighb_neighb_voxel_data.idx_);
865 voxel_data.normal_[3] = 0.0f;
866 voxel_data.normal_.normalize ();
871 template <
typename Po
intT>
void
874 centroid_.normal_ = Eigen::Vector4f::Zero ();
875 centroid_.xyz_ = Eigen::Vector3f::Zero ();
876 centroid_.rgb_ = Eigen::Vector3f::Zero ();
877 for (
auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
879 const VoxelData& leaf_data = (*leaf_itr)->getData ();
880 centroid_.normal_ += leaf_data.normal_;
881 centroid_.xyz_ += leaf_data.xyz_;
882 centroid_.rgb_ += leaf_data.rgb_;
884 centroid_.normal_.normalize ();
885 centroid_.xyz_ /=
static_cast<float> (leaves_.size ());
886 centroid_.rgb_ /=
static_cast<float> (leaves_.size ());
890 template <
typename Po
intT>
void
895 voxels->
resize (leaves_.size ());
897 for (
auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr, ++voxel_itr)
899 const VoxelData& leaf_data = (*leaf_itr)->getData ();
900 leaf_data.getPoint (*voxel_itr);
905 template <
typename Po
intT>
void
910 normals->
resize (leaves_.size ());
912 for (
auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr, ++normal_itr)
914 const VoxelData& leaf_data = (*leaf_itr)->getData ();
915 leaf_data.getNormal (*normal_itr);
920 template <
typename Po
intT>
void
923 neighbor_labels.clear ();
925 for (
auto leaf_itr = leaves_.cbegin (); leaf_itr != leaves_.cend (); ++leaf_itr)
928 for (
typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr)
931 VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());
933 if (neighbor_voxel.owner_ !=
this && neighbor_voxel.owner_)
935 neighbor_labels.insert (neighbor_voxel.owner_->getLabel ());
942 #endif // PCL_SUPERVOXEL_CLUSTERING_HPP_