From 5a6adabf1eca03611e23c72a428100cbd299c4a8 Mon Sep 17 00:00:00 2001 From: Jeremie Papon Date: Fri, 12 Sep 2014 16:35:37 +0200 Subject: [PATCH 1/2] Fix for problem where leaves could get lost, and warning would show up. Now we use leaf indices directly for seeding, instead of doing a look up Changed OctreePointcloud Adjacency to use std::list instead of std::set, added comparator to SupervoxelHelper so that the internal set of voxels owned by a helper is sorted by index instead of memory location Added brief comment explaining comparator --- .../pcl/octree/octree_pointcloud_adjacency.h | 3 +- .../octree_pointcloud_adjacency_container.h | 16 ++++--- .../impl/supervoxel_clustering.hpp | 44 ++++++++++--------- .../pcl/segmentation/supervoxel_clustering.h | 26 ++++++++--- 4 files changed, 56 insertions(+), 33 deletions(-) diff --git a/octree/include/pcl/octree/octree_pointcloud_adjacency.h b/octree/include/pcl/octree/octree_pointcloud_adjacency.h index b0bb33016be..0385da97421 100644 --- a/octree/include/pcl/octree/octree_pointcloud_adjacency.h +++ b/octree/include/pcl/octree/octree_pointcloud_adjacency.h @@ -131,7 +131,8 @@ namespace pcl inline iterator begin () { return (leaf_vector_.begin ()); } inline iterator end () { return (leaf_vector_.end ()); } - + inline LeafContainerT* at (size_t idx) { return leaf_vector_.at (idx); } + // Size of neighbors inline size_t size () const { return leaf_vector_.size (); } diff --git a/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h b/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h index 326c0f5464e..380e42e04dd 100644 --- a/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h +++ b/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h @@ -54,7 +54,7 @@ namespace pcl class OctreePointCloudAdjacencyContainer : public OctreeContainerBase { public: - typedef std::set NeighborSetT; + typedef std::list NeighborSetT; //iterators to neighbors typedef typename NeighborSetT::iterator iterator; typedef typename NeighborSetT::const_iterator const_iterator; @@ -64,8 +64,6 @@ namespace pcl inline const_iterator end () const { return (neighbors_.end ()); } //size of neighbors inline size_t size () const { return neighbors_.size (); } - //insert for neighbors - inline std::pair insert (OctreePointCloudAdjacencyContainer* neighbor) { return neighbors_.insert (neighbor); } /** \brief Class initialization. */ OctreePointCloudAdjacencyContainer () : @@ -131,7 +129,7 @@ namespace pcl void addNeighbor (OctreePointCloudAdjacencyContainer *neighbor) { - neighbors_.insert (neighbor); + neighbors_.push_back (neighbor); } /** \brief Remove neighbor from neighbor set. @@ -140,8 +138,14 @@ namespace pcl void removeNeighbor (OctreePointCloudAdjacencyContainer *neighbor) { - neighbors_.erase (neighbor); - + for (iterator neighb_it = neighbors_.begin(); neighb_it != neighbors_.end(); ++neighb_it) + { + if ( *neighb_it == neighbor) + { + neighbors_.erase (neighb_it); + return; + } + } } /** \brief Returns the number of neighbors this leaf has diff --git a/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp b/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp index 1c7386f8d79..f1790501db3 100644 --- a/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp +++ b/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp @@ -117,10 +117,10 @@ pcl::SupervoxelClustering::extract (std::map > seed_points; - selectInitialSupervoxelSeeds (seed_points); + std::vector seed_indices; + selectInitialSupervoxelSeeds (seed_indices); //std::cout << "Creating helpers "<::makeSupervoxels (std::map void -pcl::SupervoxelClustering::createSupervoxelHelpers (std::vector > &seed_points) +pcl::SupervoxelClustering::createSupervoxelHelpers (std::vector &seed_indices) { supervoxel_helpers_.clear (); - for (size_t i = 0; i < seed_points.size (); ++i) + for (size_t i = 0; i < seed_indices.size (); ++i) { supervoxel_helpers_.push_back (new SupervoxelHelper(i+1,this)); //Find which leaf corresponds to this seed index - LeafContainerT* seed_leaf = adjacency_octree_->getLeafContainerAtPoint (seed_points[i]); + LeafContainerT* seed_leaf = adjacency_octree_->at(seed_indices[i]);//adjacency_octree_->getLeafContainerAtPoint (seed_points[i]); if (seed_leaf) { supervoxel_helpers_.back ().addLeaf (seed_leaf); @@ -362,7 +362,7 @@ pcl::SupervoxelClustering::createSupervoxelHelpers (std::vector void -pcl::SupervoxelClustering::selectInitialSupervoxelSeeds (std::vector > &seed_points) +pcl::SupervoxelClustering::selectInitialSupervoxelSeeds (std::vector &seed_indices) { //TODO THIS IS BAD - SEEDING SHOULD BE BETTER //TODO Switch to assigning leaves! Don't use Octree! @@ -379,7 +379,7 @@ pcl::SupervoxelClustering::selectInitialSupervoxelSeeds (std::vector seed_indices_orig; seed_indices_orig.resize (num_seeds, 0); - seed_points.clear (); + seed_indices.clear (); std::vector closest_index; std::vector distance; closest_index.resize(1,0); @@ -398,7 +398,7 @@ pcl::SupervoxelClustering::selectInitialSupervoxelSeeds (std::vector neighbors; std::vector sqr_distances; - seed_points.reserve (seed_indices_orig.size ()); + seed_indices.reserve (seed_indices_orig.size ()); float search_radius = 0.5f*seed_resolution_; // This is number of voxels which fit in a planar slice through search volume // Area of planar slice / area of voxel side @@ -409,7 +409,7 @@ pcl::SupervoxelClustering::selectInitialSupervoxelSeeds (std::vector min_points) { - seed_points.push_back (voxel_centroid_cloud_->points[min_index]); + seed_indices.push_back (min_index); } } @@ -437,11 +437,15 @@ pcl::SupervoxelClustering::reseedSupervoxels () sv_itr->getXYZ (point.x, point.y, point.z); voxel_kdtree_->nearestKSearch (point, 1, closest_index, distance); - LeafContainerT* seed_leaf = adjacency_octree_->getLeafContainerAtPoint (voxel_centroid_cloud_->points[closest_index[0]]); + LeafContainerT* seed_leaf = adjacency_octree_->at (closest_index[0]); if (seed_leaf) { sv_itr->addLeaf (seed_leaf); } + else + { + PCL_WARN ("Could not find leaf in pcl::SupervoxelClustering::reseedSupervoxels - supervoxel will be deleted \n"); + } } } @@ -916,7 +920,7 @@ pcl::SupervoxelClustering::SupervoxelHelper::removeLeaf (LeafContainerT* template void pcl::SupervoxelClustering::SupervoxelHelper::removeAllLeaves () { - typename std::set::iterator leaf_itr; + typename SupervoxelHelper::iterator leaf_itr; for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr) { VoxelData& voxel = ((*leaf_itr)->getData ()); @@ -935,7 +939,7 @@ pcl::SupervoxelClustering::SupervoxelHelper::expand () std::vector new_owned; new_owned.reserve (leaves_.size () * 9); //For each leaf belonging to this supervoxel - typename std::set::iterator leaf_itr; + typename SupervoxelHelper::iterator leaf_itr; for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr) { //for each neighbor of the leaf @@ -976,7 +980,7 @@ pcl::SupervoxelClustering::SupervoxelHelper::expand () template void pcl::SupervoxelClustering::SupervoxelHelper::refineNormals () { - typename std::set::iterator leaf_itr; + typename SupervoxelHelper::iterator leaf_itr; //For each leaf belonging to this supervoxel, get its neighbors, build an index vector, compute normal for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr) { @@ -1019,7 +1023,7 @@ pcl::SupervoxelClustering::SupervoxelHelper::updateCentroid () centroid_.normal_ = Eigen::Vector4f::Zero (); centroid_.xyz_ = Eigen::Vector3f::Zero (); centroid_.rgb_ = Eigen::Vector3f::Zero (); - typename std::set::iterator leaf_itr = leaves_.begin (); + typename SupervoxelHelper::iterator leaf_itr = leaves_.begin (); for ( ; leaf_itr!= leaves_.end (); ++leaf_itr) { const VoxelData& leaf_data = (*leaf_itr)->getData (); @@ -1041,10 +1045,8 @@ pcl::SupervoxelClustering::SupervoxelHelper::getVoxels (typename pcl::Po voxels->clear (); voxels->resize (leaves_.size ()); typename pcl::PointCloud::iterator voxel_itr = voxels->begin (); - //typename std::set::iterator leaf_itr; - for (typename std::set::const_iterator leaf_itr = leaves_.begin (); - leaf_itr != leaves_.end (); - ++leaf_itr, ++voxel_itr) + typename SupervoxelHelper::const_iterator leaf_itr; + for ( leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++voxel_itr) { const VoxelData& leaf_data = (*leaf_itr)->getData (); leaf_data.getPoint (*voxel_itr); @@ -1058,7 +1060,7 @@ pcl::SupervoxelClustering::SupervoxelHelper::getNormals (typename pcl::P normals.reset (new pcl::PointCloud); normals->clear (); normals->resize (leaves_.size ()); - typename std::set::const_iterator leaf_itr; + typename SupervoxelHelper::const_iterator leaf_itr; typename pcl::PointCloud::iterator normal_itr = normals->begin (); for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++normal_itr) { @@ -1073,7 +1075,7 @@ pcl::SupervoxelClustering::SupervoxelHelper::getNeighborLabels (std::set { neighbor_labels.clear (); //For each leaf belonging to this supervoxel - typename std::set::const_iterator leaf_itr; + typename SupervoxelHelper::const_iterator leaf_itr; for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr) { //for each neighbor of the leaf diff --git a/segmentation/include/pcl/segmentation/supervoxel_clustering.h b/segmentation/include/pcl/segmentation/supervoxel_clustering.h index 09ea59df6ee..1080d226aad 100644 --- a/segmentation/include/pcl/segmentation/supervoxel_clustering.h +++ b/segmentation/include/pcl/segmentation/supervoxel_clustering.h @@ -329,16 +329,16 @@ namespace pcl prepareForSegmentation (); /** \brief This selects points to use as initial supervoxel centroids - * \param[out] seed_points The selected points + * \param[out] seed_indices The selected leaf indices */ void - selectInitialSupervoxelSeeds (std::vector > &seed_points); + selectInitialSupervoxelSeeds (std::vector &seed_indices); /** \brief This method creates the internal supervoxel helpers based on the provided seed points - * \param[in] seed_points The selected points + * \param[in] seed_indices Indices of the leaves to use as seeds */ void - createSupervoxelHelpers (std::vector > &seed_points); + createSupervoxelHelpers (std::vector &seed_indices); /** \brief This performs the superpixel evolution */ void @@ -399,6 +399,22 @@ namespace pcl class SupervoxelHelper { public: + /** \brief Comparator for LeafContainerT pointers - used for sorting set of leaves + * \note Compares by index in the overall leaf_vector. Order isn't important, so long as it is fixed. + */ + struct compareLeaves + { + bool operator() (LeafContainerT* const &left, LeafContainerT* const &right) const + { + const VoxelData& leaf_data_left = left->getData (); + const VoxelData& leaf_data_right = right->getData (); + return leaf_data_left.idx_ < leaf_data_right.idx_; + } + }; + typedef std::set LeafSetT; + typedef typename LeafSetT::iterator iterator; + typedef typename LeafSetT::const_iterator const_iterator; + SupervoxelHelper (uint32_t label, SupervoxelClustering* parent_arg): label_ (label), parent_ (parent_arg) @@ -479,7 +495,7 @@ namespace pcl size () const { return leaves_.size (); } private: //Stores leaves - std::set leaves_; + LeafSetT leaves_; uint32_t label_; VoxelData centroid_; SupervoxelClustering* parent_; From 6716c20d5e33cc54e3801920d244f5eee6a54781 Mon Sep 17 00:00:00 2001 From: Jeremie Papon Date: Mon, 15 Sep 2014 17:35:43 +0200 Subject: [PATCH 2/2] Removed public interface for modifying neighbors of OctreePointcloudAdjacencyContainer. Neighbors can now only be added/removed from within OctreePointcloudAdjacency. This does *not* mean neighbors can't be modified, just that the list of neighbors itself can't be modified. --- .../octree_pointcloud_adjacency_container.h | 91 ++++++++++--------- .../impl/supervoxel_clustering.hpp | 12 +-- 2 files changed, 53 insertions(+), 50 deletions(-) diff --git a/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h b/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h index 380e42e04dd..45782b236c1 100644 --- a/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h +++ b/octree/include/pcl/octree/octree_pointcloud_adjacency_container.h @@ -45,7 +45,7 @@ namespace pcl namespace octree { - /** \brief @b Octree adjacency leaf container class- stores set of pointers to neighbors, number of points added, and a DataT value + /** \brief @b Octree adjacency leaf container class- stores a list of pointers to neighbors, number of points added, and a DataT value * \note This class implements a leaf node that stores pointers to neighboring leaves * \note This class also has a virtual computeData function, which is called by octreePointCloudAdjacency::addPointsFromInputCloud. * \note You should make explicit instantiations of it for your pointtype/datatype combo (if needed) see supervoxel_clustering.hpp for an example of this @@ -53,15 +53,14 @@ namespace pcl template class OctreePointCloudAdjacencyContainer : public OctreeContainerBase { + template + friend class OctreePointCloudAdjacency; public: - typedef std::list NeighborSetT; - //iterators to neighbors - typedef typename NeighborSetT::iterator iterator; - typedef typename NeighborSetT::const_iterator const_iterator; - inline iterator begin () { return (neighbors_.begin ()); } - inline iterator end () { return (neighbors_.end ()); } - inline const_iterator begin () const { return (neighbors_.begin ()); } - inline const_iterator end () const { return (neighbors_.end ()); } + typedef std::list NeighborListT; + typedef typename NeighborListT::const_iterator const_iterator; + //const iterators to neighbors + inline const_iterator cbegin () const { return (neighbors_.begin ()); } + inline const_iterator cend () const { return (neighbors_.end ()); } //size of neighbors inline size_t size () const { return neighbors_.size (); } @@ -77,6 +76,43 @@ namespace pcl { } + /** \brief Returns the number of neighbors this leaf has + * \returns number of neighbors + */ + size_t + getNumNeighbors () const + { + return neighbors_.size (); + } + + /** \brief Gets the number of points contributing to this leaf */ + int + getPointCounter () const { return num_points_; } + + /** \brief Returns a reference to the data member to access it without copying */ + DataT& + getData () { return data_; } + + /** \brief Sets the data member + * \param[in] data_arg New value for data + */ + void + setData (const DataT& data_arg) { data_ = data_arg;} + + /** \brief virtual method to get size of container + * \return number of points added to leaf node container. + */ + virtual size_t + getSize () const + { + return num_points_; + } + protected: + //iterators to neighbors + typedef typename NeighborListT::iterator iterator; + inline iterator begin () { return (neighbors_.begin ()); } + inline iterator end () { return (neighbors_.end ()); } + /** \brief deep copy function */ virtual OctreePointCloudAdjacencyContainer * deepCopy () const @@ -106,10 +142,6 @@ namespace pcl { } - /** \brief Gets the number of points contributing to this leaf */ - int - getPointCounter () const { return num_points_; } - /** \brief Sets the number of points contributing to this leaf */ void setPointCounter (int points_arg) { num_points_ = points_arg; } @@ -148,47 +180,18 @@ namespace pcl } } - /** \brief Returns the number of neighbors this leaf has - * \returns number of neighbors - */ - size_t - getNumNeighbors () const - { - return neighbors_.size (); - } - /** \brief Sets the whole neighbor set * \param[in] neighbor_arg the new set */ void - setNeighbors (const NeighborSetT &neighbor_arg) + setNeighbors (const NeighborListT &neighbor_arg) { neighbors_ = neighbor_arg; } - /** \brief Returns a reference to the data member to access it without copying */ - DataT& - getData () { return data_; } - - /** \brief Sets the data member - * \param[in] data_arg New value for data - */ - void - setData (const DataT& data_arg) { data_ = data_arg;} - - /** \brief virtual method to get size of container - * \return number of points added to leaf node container. - */ - virtual size_t - getSize () const - { - return num_points_; - } - - private: int num_points_; - NeighborSetT neighbors_; + NeighborListT neighbors_; DataT data_; }; } diff --git a/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp b/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp index f1790501db3..909a29f2db3 100644 --- a/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp +++ b/segmentation/include/pcl/segmentation/impl/supervoxel_clustering.hpp @@ -261,13 +261,13 @@ pcl::SupervoxelClustering::computeVoxelData () indices.reserve (81); //Push this point indices.push_back (new_voxel_data.idx_); - for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr) + for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr) { VoxelData& neighb_voxel_data = (*neighb_itr)->getData (); //Push neighbor index indices.push_back (neighb_voxel_data.idx_); //Get neighbors neighbors, push onto cloud - for (typename LeafContainerT::iterator neighb_neighb_itr=(*neighb_itr)->begin (); neighb_neighb_itr!=(*neighb_itr)->end (); ++neighb_neighb_itr) + for (typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr) { VoxelData& neighb2_voxel_data = (*neighb_neighb_itr)->getData (); indices.push_back (neighb2_voxel_data.idx_); @@ -943,7 +943,7 @@ pcl::SupervoxelClustering::SupervoxelHelper::expand () for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr) { //for each neighbor of the leaf - for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr) + for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr) { //Get a reference to the data contained in the leaf VoxelData& neighbor_voxel = ((*neighb_itr)->getData ()); @@ -989,7 +989,7 @@ pcl::SupervoxelClustering::SupervoxelHelper::refineNormals () indices.reserve (81); //Push this point indices.push_back (voxel_data.idx_); - for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr) + for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr) { //Get a reference to the data contained in the leaf VoxelData& neighbor_voxel_data = ((*neighb_itr)->getData ()); @@ -998,7 +998,7 @@ pcl::SupervoxelClustering::SupervoxelHelper::refineNormals () { indices.push_back (neighbor_voxel_data.idx_); //Also check its neighbors - for (typename LeafContainerT::iterator neighb_neighb_itr=(*neighb_itr)->begin (); neighb_neighb_itr!=(*neighb_itr)->end (); ++neighb_neighb_itr) + for (typename LeafContainerT::const_iterator neighb_neighb_itr=(*neighb_itr)->cbegin (); neighb_neighb_itr!=(*neighb_itr)->cend (); ++neighb_neighb_itr) { VoxelData& neighb_neighb_voxel_data = (*neighb_neighb_itr)->getData (); if (neighb_neighb_voxel_data.owner_ == this) @@ -1079,7 +1079,7 @@ pcl::SupervoxelClustering::SupervoxelHelper::getNeighborLabels (std::set for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr) { //for each neighbor of the leaf - for (typename LeafContainerT::iterator neighb_itr=(*leaf_itr)->begin (); neighb_itr!=(*leaf_itr)->end (); ++neighb_itr) + for (typename LeafContainerT::const_iterator neighb_itr=(*leaf_itr)->cbegin (); neighb_itr!=(*leaf_itr)->cend (); ++neighb_itr) { //Get a reference to the data contained in the leaf VoxelData& neighbor_voxel = ((*neighb_itr)->getData ());