Implementation of the iterator 'OctreeFixedDepthIterator'.

octree/CMakeLists.txt

@@ -46,7 +46,7 @@ if(build)
 
     set(LIB_NAME "pcl_${SUBSYS_NAME}")
     PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs})
-    target_link_libraries("${LIB_NAME}")
+    target_link_libraries("${LIB_NAME}" pcl_common)
     target_include_directories(${LIB_NAME} PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include")
     PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}"
       "${SUBSYS_DEPS}" "" "" "" "")

octree/include/pcl/octree/impl/octree_iterator.hpp

@@ -39,6 +39,8 @@
 #ifndef PCL_OCTREE_ITERATOR_HPP_
 #define PCL_OCTREE_ITERATOR_HPP_
 
+#include <pcl/console/print.h>
+
 namespace pcl
 {
   namespace octree
@@ -269,6 +271,58 @@ namespace pcl
 
       return (*this);
     }
+
+    //////////////////////////////////////////////////////////////////////////////////////////////
+    template<typename OctreeT>
+    OctreeFixedDepthIterator<OctreeT>::OctreeFixedDepthIterator () :
+        OctreeBreadthFirstIterator<OctreeT> (0u), fixed_depth_ (0u)
+    {}
+
+    //////////////////////////////////////////////////////////////////////////////////////////////
+    template<typename OctreeT>
+    OctreeFixedDepthIterator<OctreeT>::OctreeFixedDepthIterator (OctreeT* octree_arg, unsigned int fixed_depth_arg) :
+        OctreeBreadthFirstIterator<OctreeT> (octree_arg, fixed_depth_arg), fixed_depth_ (fixed_depth_arg)
+    {
+      this->reset (fixed_depth_arg);
+    }
+
+    //////////////////////////////////////////////////////////////////////////////////////////////
+    template<typename OctreeT>
+    void OctreeFixedDepthIterator<OctreeT>::reset (unsigned int fixed_depth_arg)
+    {
+      // Set the desired depth to walk through
+      fixed_depth_ = fixed_depth_arg;
+
+      if (!this->octree_)
+      {
+        return;
+      }
+
+      // If I'm nowhere, reset
+      // If I'm somewhere and I can't guarantee I'm before the first node, reset
+      if ((!this->current_state_) || (fixed_depth_ <= this->getCurrentOctreeDepth ()))
+        OctreeBreadthFirstIterator<OctreeT>::reset ();
+
+      if (this->octree_->getTreeDepth () < fixed_depth_)
+      {
+        PCL_WARN ("[pcl::octree::FixedDepthIterator] The requested fixed depth was bigger than the octree's depth.\n");
+        PCL_WARN ("[pcl::octree::FixedDepthIterator] fixed_depth = %d (instead of %d)\n", this->octree_->getTreeDepth (), fixed_depth_);
+      }
+
+      // By default for the parent class OctreeBreadthFirstIterator, if the
+      // depth argument is equal to 0, the iterator would run over every node.
+      // For the OctreeFixedDepthIterator, whatever the depth ask, set the
+      // max_octree_depth_ accordingly
+      this->max_octree_depth_ = std::min (fixed_depth_, this->octree_->getTreeDepth ());
+
+      // Restore previous state in case breath first iterator had child nodes already set up
+      if (FIFO_.size ())
+        this->current_state_ = &FIFO_.front ();
+
+      // Iterate all the way to the desired level
+      while (this->current_state_ && (this->getCurrentOctreeDepth () != fixed_depth_))
+        OctreeBreadthFirstIterator<OctreeT>::operator++ ();
+    }
   }
 }
 

octree/include/pcl/octree/octree_base.h

@@ -104,6 +104,7 @@ namespace pcl
         friend class OctreeIteratorBase<OctreeT> ;
         friend class OctreeDepthFirstIterator<OctreeT> ;
         friend class OctreeBreadthFirstIterator<OctreeT> ;
+        friend class OctreeFixedDepthIterator<OctreeT> ;
         friend class OctreeLeafNodeIterator<OctreeT> ;
 
         // Octree default iterators
@@ -162,6 +163,19 @@ namespace pcl
           return BreadthFirstIterator (this, 0, NULL);
         };
 
+        // Octree breadth iterators at a given depth
+        typedef OctreeFixedDepthIterator<OctreeT> FixedDepthIterator;
+        typedef const OctreeFixedDepthIterator<OctreeT> ConstFixedDepthIterator;
+
+        FixedDepthIterator fixed_depth_begin (unsigned int fixed_depth_arg = 0u)
+        {
+          return FixedDepthIterator (this, fixed_depth_arg);
+        };
+
+        const FixedDepthIterator fixed_depth_end ()
+        {
+          return FixedDepthIterator (this, 0, NULL);
+        };
 
         /** \brief Empty constructor. */
         OctreeBase ();

octree/include/pcl/octree/octree_iterator.h

@@ -3,6 +3,7 @@
  *
  *  Point Cloud Library (PCL) - www.pointclouds.org
  *  Copyright (c) 2010-2012, Willow Garage, Inc.
+ *  Copyright (c) 2017-, Open Perception, Inc.
  *
  *  All rights reserved.
  *
@@ -62,7 +63,7 @@ namespace pcl
     struct IteratorState{
         OctreeNode* node_;
         OctreeKey key_;
-        unsigned char depth_;
+        unsigned int depth_;
     };
 
 
@@ -474,12 +475,11 @@ namespace pcl
     template<typename OctreeT>
       class OctreeBreadthFirstIterator : public OctreeIteratorBase<OctreeT>
       {
+      public:
         // public typedefs
         typedef typename OctreeIteratorBase<OctreeT>::BranchNode BranchNode;
         typedef typename OctreeIteratorBase<OctreeT>::LeafNode LeafNode;
 
-
-      public:
         /** \brief Empty constructor.
          * \param[in] max_depth_arg Depth limitation during traversal
          */
@@ -568,6 +568,92 @@ namespace pcl
         std::deque<IteratorState> FIFO_;
       };
 
+    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    /** \brief @b Octree iterator class
+     * \note Iterator  over all existing nodes at a given depth. It walks across an octree
+     *       in a breadth-first manner.
+     * \ingroup octree
+     * \author Fabien Rozar (fabien.rozar@gmail.com)
+     */
+    template<typename OctreeT>
+    class OctreeFixedDepthIterator : public OctreeBreadthFirstIterator<OctreeT>
+    {
+    public:
+
+      // public typedefs
+      using typename OctreeBreadthFirstIterator<OctreeT>::BranchNode;
+      using typename OctreeBreadthFirstIterator<OctreeT>::LeafNode;
+
+      /** \brief Empty constructor.
+       */
+      OctreeFixedDepthIterator ();
+
+      /** \brief Constructor.
+       * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
+       * \param[in] fixed_depth_arg Depth level during traversal
+       */
+      explicit
+      OctreeFixedDepthIterator (OctreeT* octree_arg, unsigned int fixed_depth_arg = 0);
+
+      /** \brief Constructor.
+       * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
+       * \param[in] fixed_depth_arg Depth level during traversal
+       * \param[in] current_state A pointer to the current iterator state
+       * \param[in] fifo Internal container of octree node to go through
+       *
+       *  \warning For advanced users only.
+       */
+      OctreeFixedDepthIterator (OctreeT* octree_arg,
+                                unsigned int fixed_depth_arg,
+                                IteratorState* current_state,
+                                const std::deque<IteratorState>& fifo = std::deque<IteratorState> ())
+        : OctreeBreadthFirstIterator<OctreeT> (octree_arg, fixed_depth_arg, current_state, fifo)
+        , fixed_depth_ (fixed_depth_arg)
+      {}
+
+      /** \brief Copy Constructor.
+       * \param[in] other Another OctreeFixedDepthIterator to copy from
+       */
+      OctreeFixedDepthIterator (const OctreeFixedDepthIterator& other)
+        : OctreeBreadthFirstIterator<OctreeT> (other)
+      {
+        this->fixed_depth_ = other.fixed_depth_;
+      }
+
+      /** \brief Copy assignment.
+       * \param[in] src the iterator to copy into this
+       * \return pointer to the current octree node
+       */
+      inline OctreeFixedDepthIterator&
+      operator = (const OctreeFixedDepthIterator& src)
+      {
+        OctreeBreadthFirstIterator<OctreeT>::operator= (src);
+        this->fixed_depth_ = src.fixed_depth_;
+
+        return (*this);
+      }
+
+      /** \brief Reset the iterator to the first node at the depth given as parameter
+       * \param[in] fixed_depth_arg Depth level during traversal
+       */
+      void
+      reset (unsigned int fixed_depth_arg);
+
+      /** \brief Reset the iterator to the first node at the current depth
+       */
+      void
+      reset ()
+      {
+        this->reset (fixed_depth_);
+      }
+
+    protected:
+      using OctreeBreadthFirstIterator<OctreeT>::FIFO_;
+
+      /** \brief Given level of the node to be iterated */
+      unsigned int fixed_depth_;
+    };
+
     //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
     /** \brief Octree leaf node iterator class
      * \note This class implements a forward iterator for traversing the leaf nodes of an octree data structure.

octree/include/pcl/octree/octree_key.h

@@ -79,6 +79,15 @@ namespace pcl
         return ((b.x == this->x) && (b.y == this->y) && (b.z == this->z));
       }
 
+      /** \brief Inequal comparison operator
+       * \param[in] other OctreeIteratorBase to compare with
+       * \return "true" if the current and other iterators are different ; "false" otherwise.
+       */
+      bool operator!= (const OctreeKey& other) const
+      {
+        return !operator== (other);
+      }
+
       /** \brief Operator<= for comparing octree keys with each other.
        *  \return "true" if key indices are not greater than the key indices of b  ; "false" otherwise.
        * */