Restructure and add functionality to PCLPointCloud2 filters

filters/include/pcl/filters/impl/crop_box.hpp

@@ -57,8 +57,8 @@ pcl::CropBox<PointT>::applyFilter (PointCloud &output)
     extract_removed_indices_ = temp;
 
     output = *input_;
-    for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii)  // rii = removed indices iterator
-      output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_;
+    for (const auto ri : *removed_indices_)  // ri = removed index
+      output.points[ri].x = output.points[ri].y = output.points[ri].z = user_filter_value_;
     if (!std::isfinite (user_filter_value_))
       output.is_dense = false;
   }
@@ -94,15 +94,15 @@ pcl::CropBox<PointT>::applyFilter (std::vector<int> &indices)
   bool translation_is_zero = (translation_ == Eigen::Vector3f::Zero ());
   bool inverse_transform_matrix_is_identity = inverse_transform.matrix ().isIdentity ();
 
-  for (std::size_t index = 0; index < indices_->size (); ++index)
+  for (const auto index : *indices_)
   {
     if (!input_->is_dense)
       // Check if the point is invalid
-      if (!isFinite (input_->points[(*indices_)[index]]))
+      if (!isFinite (input_->points[index]))
         continue;
 
     // Get local point
-    PointT local_pt = input_->points[(*indices_)[index]];
+    PointT local_pt = input_->points[index];
 
     // Transform point to world space
     if (!transform_matrix_is_identity)
@@ -124,17 +124,17 @@ pcl::CropBox<PointT>::applyFilter (std::vector<int> &indices)
          (local_pt.x > max_pt_[0] || local_pt.y > max_pt_[1] || local_pt.z > max_pt_[2]))
     {
       if (negative_)
-        indices[indices_count++] = (*indices_)[index];
+        indices[indices_count++] = index;
       else if (extract_removed_indices_)
-        (*removed_indices_)[removed_indices_count++] = static_cast<int> ((*indices_)[index]);
+        (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
     }
     // If inside the cropbox
     else
     {
       if (negative_ && extract_removed_indices_)
-        (*removed_indices_)[removed_indices_count++] = static_cast<int> ((*indices_)[index]);
+        (*removed_indices_)[removed_indices_count++] = static_cast<int> (index);
       else if (!negative_) 
-        indices[indices_count++] = (*indices_)[index];
+        indices[indices_count++] = index;
     }
   }
   indices.resize (indices_count);

filters/include/pcl/filters/impl/extract_indices.hpp

@@ -89,9 +89,9 @@ pcl::ExtractIndices<PointT>::applyFilter (PointCloud &output)
     output = *input_;
     std::vector<pcl::PCLPointField> fields;
     pcl::for_each_type<FieldList> (pcl::detail::FieldAdder<PointT> (fields));
-    for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii)  // rii = removed indices iterator
+    for (const auto ri : *removed_indices_)  // ri = removed index
     {
-      std::size_t pt_index = (std::size_t)(*removed_indices_)[rii];
+      std::size_t pt_index = (std::size_t)ri;
       if (pt_index >= input_->points.size ())
       {
         PCL_ERROR ("[pcl::%s::applyFilter] The index exceeds the size of the input. Do nothing.\n",

filters/include/pcl/filters/impl/passthrough.hpp

@@ -56,8 +56,8 @@ pcl::PassThrough<PointT>::applyFilter (PointCloud &output)
     extract_removed_indices_ = temp;
 
     output = *input_;
-    for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii)  // rii = removed indices iterator
-      output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_;
+    for (const auto ri : *removed_indices_)  // ri = removed index
+      output.points[ri].x = output.points[ri].y = output.points[ri].z = user_filter_value_;
     if (!std::isfinite (user_filter_value_))
       output.is_dense = false;
   }
@@ -82,18 +82,18 @@ pcl::PassThrough<PointT>::applyFilterIndices (std::vector<int> &indices)
   if (filter_field_name_.empty ())
   {
     // Only filter for non-finite entries then
-    for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)  // iii = input indices iterator
+    for (const auto ii : *indices_)  // ii = input index
     {
       // Non-finite entries are always passed to removed indices
-      if (!std::isfinite (input_->points[(*indices_)[iii]].x) ||
-          !std::isfinite (input_->points[(*indices_)[iii]].y) ||
-          !std::isfinite (input_->points[(*indices_)[iii]].z))
+      if (!std::isfinite (input_->points[ii].x) ||
+          !std::isfinite (input_->points[ii].y) ||
+          !std::isfinite (input_->points[ii].z))
       {
         if (extract_removed_indices_)
-          (*removed_indices_)[rii++] = (*indices_)[iii];
+          (*removed_indices_)[rii++] = ii;
         continue;
       }
-      indices[oii++] = (*indices_)[iii];
+      indices[oii++] = ii;
     }
   }
   else
@@ -110,49 +110,49 @@ pcl::PassThrough<PointT>::applyFilterIndices (std::vector<int> &indices)
     }
 
     // Filter for non-finite entries and the specified field limits
-    for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)  // iii = input indices iterator
+    for (const auto ii : *indices_)  // ii = input index
     {
       // Non-finite entries are always passed to removed indices
-      if (!std::isfinite (input_->points[(*indices_)[iii]].x) ||
-          !std::isfinite (input_->points[(*indices_)[iii]].y) ||
-          !std::isfinite (input_->points[(*indices_)[iii]].z))
+      if (!std::isfinite (input_->points[ii].x) ||
+          !std::isfinite (input_->points[ii].y) ||
+          !std::isfinite (input_->points[ii].z))
       {
         if (extract_removed_indices_)
-          (*removed_indices_)[rii++] = (*indices_)[iii];
+          (*removed_indices_)[rii++] = ii;
         continue;
       }
 
       // Get the field's value
-      const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&input_->points[(*indices_)[iii]]);
+      const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&input_->points[ii]);
       float field_value = 0;
       memcpy (&field_value, pt_data + fields[distance_idx].offset, sizeof (float));
 
       // Remove NAN/INF/-INF values. We expect passthrough to output clean valid data.
       if (!std::isfinite (field_value))
       {
         if (extract_removed_indices_)
-          (*removed_indices_)[rii++] = (*indices_)[iii];
+          (*removed_indices_)[rii++] = ii;
         continue;
       }
 
       // Outside of the field limits are passed to removed indices
       if (!negative_ && (field_value < filter_limit_min_ || field_value > filter_limit_max_))
       {
         if (extract_removed_indices_)
-          (*removed_indices_)[rii++] = (*indices_)[iii];
+          (*removed_indices_)[rii++] = ii;
         continue;
       }
 
       // Inside of the field limits are passed to removed indices if negative was set
       if (negative_ && field_value >= filter_limit_min_ && field_value <= filter_limit_max_)
       {
         if (extract_removed_indices_)
-          (*removed_indices_)[rii++] = (*indices_)[iii];
+          (*removed_indices_)[rii++] = ii;
         continue;
       }
 
       // Otherwise it was a normal point for output (inlier)
-      indices[oii++] = (*indices_)[iii];
+      indices[oii++] = ii;
     }
   }
 

filters/include/pcl/filters/impl/radius_outlier_removal.hpp

@@ -56,8 +56,8 @@ pcl::RadiusOutlierRemoval<PointT>::applyFilter (PointCloud &output)
     extract_removed_indices_ = temp;
 
     output = *input_;
-    for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii)  // rii = removed indices iterator
-      output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_;
+    for (const auto ri : *removed_indices_)  // ri = removed index
+      output.points[ri].x = output.points[ri].y = output.points[ri].z = user_filter_value_;
     if (!std::isfinite (user_filter_value_))
       output.is_dense = false;
   }

filters/include/pcl/filters/impl/random_sample.hpp

@@ -67,16 +67,16 @@ pcl::RandomSample<PointT>::applyFilter (PointCloud &output)
     }
     // For every "removed" point, set the x,y,z fields to user_filter_value_
     const static float user_filter_value = user_filter_value_;
-    for (std::size_t rii = 0; rii < removed_indices_->size (); ++rii)
+    for (const auto ri : *removed_indices_)  // ri = removed index
     {
-      std::uint8_t* pt_data = reinterpret_cast<std::uint8_t*> (&output[(*removed_indices_)[rii]]);
+      std::uint8_t* pt_data = reinterpret_cast<std::uint8_t*> (&output[ri]);
       for (const unsigned long &offset : offsets)
       {
         memcpy (pt_data + offset, &user_filter_value, sizeof (float));
       }
-      if (!std::isfinite (user_filter_value_))
-        output.is_dense = false;
     }
+    if (!std::isfinite (user_filter_value_))
+      output.is_dense = false;
   }
   else
   {

filters/include/pcl/filters/impl/statistical_outlier_removal.hpp

@@ -56,8 +56,8 @@ pcl::StatisticalOutlierRemoval<PointT>::applyFilter (PointCloud &output)
     extract_removed_indices_ = temp;
 
     output = *input_;
-    for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii)  // rii = removed indices iterator
-      output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_;
+    for (const auto ri : *removed_indices_)  // ri = removed index
+      output.points[ri].x = output.points[ri].y = output.points[ri].z = user_filter_value_;
     if (!std::isfinite (user_filter_value_))
       output.is_dense = false;
   }

filters/include/pcl/filters/passthrough.h

@@ -225,7 +225,7 @@ namespace pcl
     * \ingroup filters
     */
   template<>
-  class PCL_EXPORTS PassThrough<pcl::PCLPointCloud2> : public Filter<pcl::PCLPointCloud2>
+  class PCL_EXPORTS PassThrough<pcl::PCLPointCloud2> : public FilterIndices<pcl::PCLPointCloud2>
   {
     using PCLPointCloud2 = pcl::PCLPointCloud2;
     using PCLPointCloud2Ptr = PCLPointCloud2::Ptr;
@@ -237,46 +237,12 @@ namespace pcl
     public:
       /** \brief Constructor. */
       PassThrough (bool extract_removed_indices = false) :
-        Filter<pcl::PCLPointCloud2>::Filter (extract_removed_indices), keep_organized_ (false),
-        user_filter_value_ (std::numeric_limits<float>::quiet_NaN ()),
-        filter_field_name_ (""), filter_limit_min_ (-FLT_MAX), filter_limit_max_ (FLT_MAX),
-        filter_limit_negative_ (false)
+        FilterIndices<pcl::PCLPointCloud2>::FilterIndices (extract_removed_indices),
+        filter_field_name_ (""), filter_limit_min_ (-FLT_MAX), filter_limit_max_ (FLT_MAX)
       {
         filter_name_ = "PassThrough";
       }
 
-      /** \brief Set whether the filtered points should be kept and set to the
-        * value given through \a setUserFilterValue (default: NaN), or removed
-        * from the PointCloud, thus potentially breaking its organized
-        * structure. By default, points are removed.
-        *
-        * \param[in] val set to true whether the filtered points should be kept and
-        * set to a user given value (default: NaN)
-        */
-      inline void
-      setKeepOrganized (bool val)
-      {
-        keep_organized_ = val;
-      }
-
-      /** \brief Obtain the value of the internal \a keep_organized_ parameter. */
-      inline bool
-      getKeepOrganized () const
-      {
-        return (keep_organized_);
-      }
-
-      /** \brief Provide a value that the filtered points should be set to
-        * instead of removing them.  Used in conjunction with \a
-        * setKeepOrganized ().
-        * \param[in] val the user given value that the filtered point dimensions should be set to
-        */
-      inline void
-      setUserFilterValue (float val)
-      {
-        user_filter_value_ = val;
-      }
-
       /** \brief Provide the name of the field to be used for filtering data. In conjunction with  \a setFilterLimits,
         * points having values outside this interval will be discarded.
         * \param[in] field_name the name of the field that contains values used for filtering
@@ -320,45 +286,41 @@ namespace pcl
         * Default: false.
         * \param[in] limit_negative return data inside the interval (false) or outside (true)
         */
+      [[deprecated("use inherited FilterIndices::setNegative() instead")]]
       inline void
       setFilterLimitsNegative (const bool limit_negative)
       {
-        filter_limit_negative_ = limit_negative;
+        negative_ = limit_negative;
       }
 
       /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
         * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise
         */
+      [[deprecated("use inherited FilterIndices::getNegative() instead")]]
       inline void
       getFilterLimitsNegative (bool &limit_negative) const
       {
-        limit_negative = filter_limit_negative_;
+        limit_negative = negative_;
       }
 
       /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
         * \return true if data \b outside the interval [min; max] is to be returned, false otherwise
         */
+      [[deprecated("use inherited FilterIndices::getNegative() instead")]]
       inline bool
       getFilterLimitsNegative () const
       {
-        return (filter_limit_negative_);
+        return (negative_);
       }
 
     protected:
       void
       applyFilter (PCLPointCloud2 &output) override;
 
-    private:
-      /** \brief Keep the structure of the data organized, by setting the
-        * filtered points to a user given value (NaN by default).
-        */
-      bool keep_organized_;
-
-      /** \brief User given value to be set to any filtered point. Casted to
-        * the correct field type.
-        */
-      float user_filter_value_;
+      void
+      applyFilter (std::vector<int> &indices) override;
 
+    private:
       /** \brief The desired user filter field name. */
       std::string filter_field_name_;
 
@@ -368,9 +330,6 @@ namespace pcl
       /** \brief The maximum allowed filter value a point will be considered from. */
       double filter_limit_max_;
 
-      /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */
-      bool filter_limit_negative_;
-
   };
 }
 

filters/include/pcl/filters/radius_outlier_removal.h

@@ -80,7 +80,7 @@ namespace pcl
 
       using Ptr = boost::shared_ptr<RadiusOutlierRemoval<PointT> >;
       using ConstPtr = boost::shared_ptr<const RadiusOutlierRemoval<PointT> >;
-  
+
 
       /** \brief Constructor.
         * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false).
@@ -189,7 +189,7 @@ namespace pcl
     * \ingroup filters
     */
   template<>
-  class PCL_EXPORTS RadiusOutlierRemoval<pcl::PCLPointCloud2> : public Filter<pcl::PCLPointCloud2>
+  class PCL_EXPORTS RadiusOutlierRemoval<pcl::PCLPointCloud2> : public FilterIndices<pcl::PCLPointCloud2>
   {
     using Filter<pcl::PCLPointCloud2>::filter_name_;
     using Filter<pcl::PCLPointCloud2>::getClassName;
@@ -207,7 +207,7 @@ namespace pcl
     public:
       /** \brief Empty constructor. */
       RadiusOutlierRemoval (bool extract_removed_indices = false) :
-        Filter<pcl::PCLPointCloud2>::Filter (extract_removed_indices),
+        FilterIndices<pcl::PCLPointCloud2>::FilterIndices (extract_removed_indices),
         search_radius_ (0.0), min_pts_radius_ (1)
       {
         filter_name_ = "RadiusOutlierRemoval";
@@ -240,7 +240,7 @@ namespace pcl
       }
 
       /** \brief Get the minimum number of neighbors that a point needs to have in the given search radius to be
-        * considered an inlier and avoid being filtered. 
+        * considered an inlier and avoid being filtered.
         */
       inline double
       getMinNeighborsInRadius ()
@@ -253,15 +253,18 @@ namespace pcl
       double search_radius_;
 
       /** \brief The minimum number of neighbors that a point needs to have in the given search radius to be considered
-        * an inlier. 
+        * an inlier.
         */
       int min_pts_radius_;
 
       /** \brief A pointer to the spatial search object. */
-      KdTreePtr tree_;
+      KdTreePtr searcher_;
 
       void
       applyFilter (PCLPointCloud2 &output) override;
+
+      void
+      applyFilter (std::vector<int> &indices) override;
   };
 }