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>
47template <
typename Po
intT>
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)
62template <
typename Po
intT>
69template <
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);
83template <
typename Po
intT>
void
88 PCL_ERROR (
"[pcl::SupervoxelClustering::setNormalCloud] Empty cloud set, doing nothing \n");
96template <
typename Po
intT>
void
127 int max_depth =
static_cast<int> (1.8f*seed_resolution_/resolution_);
128 expandSupervoxels (max_depth);
148template <
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)
165 reseedSupervoxels ();
166 expandSupervoxels (max_depth);
180template <
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 ();
208template <
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 ();
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 ();
293template <
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 ();
325template <
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_);
343template <
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");
365template <
typename Po
intT>
void
373 pcl::octree::OctreePointCloudSearch <PointT> seed_octree (seed_resolution_);
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 ();
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];
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);
422template <
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");
454template <
typename Po
intT>
void
459 p.z = std::log (p.z);
463template <
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_;
482template <
typename Po
intT>
void
488 for (
typename HelperListT::const_iterator
sv_itr = supervoxel_helpers_.cbegin ();
sv_itr != supervoxel_helpers_.cend (); ++
sv_itr)
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 ();
533template <
typename Po
intT>
void
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 ();
563 for (
typename HelperListT::const_iterator
sv_itr = supervoxel_helpers_.cbegin ();
sv_itr != supervoxel_helpers_.cend (); ++
sv_itr)
623template <
typename Po
intT>
float
626 return (resolution_);
630template <
typename Po
intT>
void
633 resolution_ = resolution;
638template <
typename Po
intT>
float
641 return (seed_resolution_);
645template <
typename Po
intT>
void
653template <
typename Po
intT>
void
656 color_importance_ =
val;
660template <
typename Po
intT>
void
663 spatial_importance_ =
val;
667template <
typename Po
intT>
void
670 normal_importance_ =
val;
674template <
typename Po
intT>
void
677 use_default_transform_behaviour_ =
false;
678 use_single_camera_transform_ =
val;
682template <
typename Po
intT>
int
686 for (
typename HelperListT::const_iterator
sv_itr = supervoxel_helpers_.cbegin ();
sv_itr != supervoxel_helpers_.cend (); ++
sv_itr)
737 template<
typename Po
intT>
void
747 template <
typename Po
intT>
void
760template <
typename Po
intT>
void
769template <
typename Po
intT>
void
772 leaves_.erase (leaf_arg);
776template <
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 ();
789template <
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);
832template <
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 ();
871template <
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 ());
890template <
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);
905template <
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);
920template <
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 ());
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
PointCloud represents the base class in PCL for storing collections of 3D points.
typename VectorType::iterator iterator
typename VectorType::const_iterator const_iterator
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContain...
void getPoint(PointT &point_arg) const
Gets the data of in the form of a point.
void getNormal(Normal &normal_arg) const
Gets the data of in the form of a normal.
Implements a supervoxel algorithm based on voxel structure, normals, and rgb values.
static pcl::PointCloud< pcl::PointNormal >::Ptr makeSupervoxelNormalCloud(std::map< std::uint32_t, typename Supervoxel< PointT >::Ptr > &supervoxel_clusters)
Static helper function which returns a pointcloud of normals for the input supervoxels.
VoxelAdjacencyList::edge_descriptor EdgeID
int getMaxLabel() const
Returns the current maximum (highest) label.
virtual void refineSupervoxels(int num_itr, std::map< std::uint32_t, typename Supervoxel< PointT >::Ptr > &supervoxel_clusters)
This method refines the calculated supervoxels - may only be called after extract.
void setSpatialImportance(float val)
Set the importance of spatial distance for supervoxels.
float getVoxelResolution() const
Get the resolution of the octree voxels.
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 setColorImportance(float val)
Set the importance of color for supervoxels.
pcl::PointCloud< pcl::PointXYZL >::Ptr getLabeledVoxelCloud() const
Returns labeled voxelized cloud Points that belong to the same supervoxel have the same label.
void setVoxelResolution(float resolution)
Set the resolution of the octree voxels.
void setInputCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud) override
This method sets the cloud to be supervoxelized.
virtual void extract(std::map< std::uint32_t, typename Supervoxel< PointT >::Ptr > &supervoxel_clusters)
This method launches the segmentation algorithm and returns the supervoxels that were obtained during...
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, std::uint32_t, float > VoxelAdjacencyList
pcl::PointCloud< PointT >::Ptr getVoxelCentroidCloud() const
Returns a deep copy of the voxel centroid cloud.
void setSeedResolution(float seed_resolution)
Set the resolution of the octree seed voxels.
void getSupervoxelAdjacency(std::multimap< std::uint32_t, std::uint32_t > &label_adjacency) const
Get a multimap which gives supervoxel adjacency.
void setUseSingleCameraTransform(bool val)
Set whether or not to use the single camera transform.
SupervoxelClustering(float voxel_resolution, float seed_resolution)
Constructor that sets default values for member variables.
void getSupervoxelAdjacencyList(VoxelAdjacencyList &adjacency_list_arg) const
Gets the adjacency list (Boost Graph library) which gives connections between supervoxels.
~SupervoxelClustering()
This destructor destroys the cloud, normals and search method used for finding neighbors.
VoxelAdjacencyList::vertex_descriptor VoxelID
float getSeedResolution() const
Get the resolution of the octree seed voxels.
void setNormalImportance(float val)
Set the importance of scalar normal product for supervoxels.
pcl::PointCloud< PointXYZL >::Ptr getLabeledCloud() const
Returns labeled cloud Points that belong to the same supervoxel have the same label.
shared_ptr< Supervoxel< PointT > > Ptr
Octree adjacency leaf container class- stores a list of pointers to neighbors, number of points added...
Octree pointcloud voxel class which maintains adjacency information for its voxels.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
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.
bool 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...
float distance(const PointT &p1, const PointT &p2)
IndicesAllocator<> Indices
Type used for indices in PCL.
A point structure representing normal coordinates and the surface curvature estimate.
A point structure representing Euclidean xyz coordinates.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
A point structure representing Euclidean xyz coordinates, and the RGB color.