[filters] Add a filter accepting a functor to reduce boiler plate code for simple filters

common/include/pcl/type_traits.h

@@ -41,7 +41,9 @@
 
 #include <cstddef>  // for std::size_t
 #include <cstdint>  // for std::uint8_t
-#include <string>  // for std::string
+
+#include <functional>   // for std::function, needed till C++17
+#include <string>       // for std::string
 #include <type_traits>  // for std::false_type, std::true_type
 
 namespace pcl
@@ -260,5 +262,32 @@ namespace pcl
   template <typename T> struct has_custom_allocator<T, void_t<typename T::_custom_allocator_type_trait>> : std::true_type {};
 
 #endif
-}
 
+  /**
+   * \todo: Remove in C++17
+   */
+#ifndef __cpp_lib_is_invocable
+  template <typename F, typename... Args>
+  constexpr bool is_invocable_v =
+      std::is_constructible<std::function<void(Args...)>,
+                            std::reference_wrapper<std::remove_reference_t<F>>>::value;
+
+  template <typename R, typename F, typename... Args>
+  constexpr bool is_invocable_r_v =
+      std::is_constructible<std::function<R(Args...)>,
+                            std::reference_wrapper<std::remove_reference_t<F>>>::value;
+#else
+  using std::is_invocable_v;
+  using std::is_invocable_r_v;
+#endif
+
+  /**
+   * \todo: Remove in C++17
+   */
+#ifndef __cpp_lib_remove_cvref
+  template <typename T>
+  using remove_cvref_t = std::remove_cv_t<std::remove_reference_t<T>>;
+#else
+  using std::remove_cvref_t;
+#endif
+}

filters/include/pcl/filters/lambda_filter_indices.h

@@ -0,0 +1,166 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2020-, Open Perception
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#pragma once
+
+#include <pcl/filters/filter_indices.h>
+#include <pcl/pcl_macros.h>
+#include <pcl/type_traits.h> // for is_invocable
+
+#ifndef __cpp_lib_is_invocable
+#include <boost/hof/is_invocable.hpp>
+#endif
+
+#include <type_traits>
+
+#include <utility>
+
+namespace pcl {
+namespace detail {
+template <typename PointT, typename Function>
+using PointFilterLambda =
+    std::enable_if_t<pcl::is_invocable_r_v<bool,
+                                           Function,
+                                           const pcl::remove_cvref_t<PointT>&,
+                                           pcl::index_t>,
+                     bool>;
+
+// can't use this for SFINAE since Derived isn't properly defined
+// but this can be used after the class definition to test it
+/*
+template <class Base, class Derived>
+constexpr auto IsValidLambdaFilter = std::enable_if_t<
+    std::is_base_of<Base, Derived>::value &&
+        pcl::is_invocable_v<std::declval<Derived>().get_lambda, void> &&
+        std::is_same<PointFilterLambda<std::declval<Derived>().get_lambda()>,
+bool>, bool>;
+*/
+
+/**
+ * \brief LambdaFilterIndices filters point clouds and indices based on a
+ * function pointer passed to filter command \ingroup filters
+ */
+template <typename PointT, typename Derived>
+struct LambdaFilterIndicesImpl : public FilterIndices<PointT> {
+private:
+  using Base = FilterIndices<PointT>;
+  using PCLBase = pcl::PCLBase<PointT>;
+
+protected:
+  using PointCloud = typename FilterIndices<PointT>::PointCloud;
+  using PointCloudPtr = typename PointCloud::Ptr;
+  using PointCloudConstPtr = typename PointCloud::ConstPtr;
+  using FieldList = typename pcl::traits::fieldList<PointT>::type;
+
+  using Base::extract_removed_indices_;
+  using Base::removed_indices_;
+  using PCLBase::indices_;
+  using PCLBase::input_;
+
+protected:
+  // to prevent instantiation of impl class
+  LambdaFilterIndicesImpl(bool extract_removed_indices)
+  : FilterIndices<PointT>(extract_removed_indices)
+  {}
+  /** \brief Filtered results are indexed by an indices array.
+   * \param[out] indices The resultant indices.
+   */
+  void
+  applyFilter(std::vector<int>& indices) override
+  {
+    indices.clear();
+    indices.reserve(input_->points.size());
+    if (extract_removed_indices_) {
+      removed_indices_->clear();
+      removed_indices_->reserve(input_->points.size());
+    }
+
+    const auto& lambda = static_cast<Derived*>(this)->get_lambda();
+
+    for (const auto index : *indices_) {
+      // lambda returns true for points that shoud be selected
+      if (!(this->negative_) == lambda(input_->points[index], index)) {
+        indices.push_back(index);
+      }
+      else {
+        if (extract_removed_indices_) {
+          removed_indices_->push_back(index);
+        }
+      }
+    }
+  }
+};
+} // namespace detail
+
+template <typename PointT, typename Functor>
+struct LambdaFilterIndices
+: public detail::LambdaFilterIndicesImpl<PointT, LambdaFilterIndices<PointT, Functor>> {
+private:
+  using Self = LambdaFilterIndices<PointT, Functor>;
+  using Base = detail::LambdaFilterIndicesImpl<PointT, Self>;
+
+public:
+  using FunctorT = Functor;
+  // using in type would complicate signature
+  static_assert(std::is_same<detail::PointFilterLambda<PointT, FunctorT>, bool>::value,
+                "Functor needs to be able to satisfy the callable constraint");
+
+protected:
+  using Base::filter_name_;
+  // need to hold a value because lambdas can only be copy or move constructed
+  const FunctorT lambda_;
+
+public:
+  /** \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).
+   */
+  LambdaFilterIndices(FunctorT lambda, bool extract_removed_indices = false)
+  : Base(extract_removed_indices), lambda_(lambda)
+  {
+    filter_name_ = "lambda_filter_indices";
+  }
+
+  const FunctorT&
+  get_lambda() const noexcept
+  {
+    return lambda_;
+  }
+};
+} // namespace pcl