[filters] Add a filter accepting a functor to reduce boiler plate code for simple filters (#3890)

* Proposed lambda filter
* move traits to common header
* Adding tests
* Make test more deterministic
* Using multiple seeds
* Using Typed Test
* For older gtest
* Adding more macros for older gtest

Author: kunaltyagi

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,33 @@ 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
+  // Implementation taken from: https://stackoverflow.com/a/51188325
+  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/CMakeLists.txt

@@ -58,6 +58,7 @@ set(incs
   "include/pcl/${SUBSYS_NAME}/extract_indices.h"
   "include/pcl/${SUBSYS_NAME}/filter.h"
   "include/pcl/${SUBSYS_NAME}/filter_indices.h"
+  "include/pcl/${SUBSYS_NAME}/functor_filter.h"
   "include/pcl/${SUBSYS_NAME}/passthrough.h"
   "include/pcl/${SUBSYS_NAME}/shadowpoints.h"
   "include/pcl/${SUBSYS_NAME}/project_inliers.h"

filters/include/pcl/filters/functor_filter.h

@@ -0,0 +1,98 @@
+/*
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2020-, Open Perception
+ *
+ *  All rights reserved
+ */
+
+#pragma once
+
+#include <pcl/filters/filter_indices.h>
+#include <pcl/type_traits.h> // for is_invocable
+
+namespace pcl {
+template <typename PointT, typename Function>
+constexpr static bool is_functor_for_filter_v =
+    pcl::is_invocable_r_v<bool,
+                          Function,
+                          const pcl::remove_cvref_t<pcl::PointCloud<PointT>>&,
+                          pcl::index_t>;
+
+/**
+ * \brief Filter point clouds and indices based on a functor passed in the ctor
+ * \ingroup filters
+ */
+template <typename PointT, typename Functor>
+class FunctorFilter : public FilterIndices<PointT> {
+  using Base = FilterIndices<PointT>;
+  using PCLBase = pcl::PCLBase<PointT>;
+
+public:
+  using FunctorT = Functor;
+  // using in type would complicate signature
+  static_assert(is_functor_for_filter_v<PointT, FunctorT>,
+                "Functor signature must be similar to `bool(const PointCloud<PointT>&, "
+                "index_t)`");
+
+protected:
+  using Base::extract_removed_indices_;
+  using Base::filter_name_;
+  using Base::negative_;
+  using Base::removed_indices_;
+  using PCLBase::indices_;
+  using PCLBase::input_;
+
+  // need to hold a value because functors can only be copy or move constructed
+  FunctorT functor_;
+
+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).
+   */
+  FunctorFilter(FunctorT functor, bool extract_removed_indices = false)
+  : Base(extract_removed_indices), functor_(std::move(functor))
+  {
+    filter_name_ = "functor_filter";
+  }
+
+  const FunctorT&
+  getFunctor() const noexcept
+  {
+    return functor_;
+  }
+
+  FunctorT&
+  getFunctor() noexcept
+  {
+    return functor_;
+  }
+
+  /**
+   * \brief Filtered results are indexed by an indices array.
+   * \param[out] indices The resultant indices.
+   */
+  void
+  applyFilter(Indices& indices) override
+  {
+    indices.clear();
+    indices.reserve(input_->size());
+    if (extract_removed_indices_) {
+      removed_indices_->clear();
+      removed_indices_->reserve(input_->size());
+    }
+
+    for (const auto index : *indices_) {
+      // functor returns true for points that should be selected
+      if (negative_ != functor_(*input_, index)) {
+        indices.push_back(index);
+      }
+      else if (extract_removed_indices_) {
+        removed_indices_->push_back(index);
+      }
+    }
+  }
+};
+} // namespace pcl

test/filters/CMakeLists.txt

@@ -21,6 +21,10 @@ PCL_ADD_TEST(filters_morphological test_morphological
              FILES test_morphological.cpp
              LINK_WITH pcl_gtest pcl_common pcl_filters)
 
+PCL_ADD_TEST(filters_functor test_filters_functor
+             FILES test_functor_filter.cpp
+             LINK_WITH pcl_gtest pcl_common pcl_filters)
+
 PCL_ADD_TEST(filters_local_maximum test_filters_local_maximum
              FILES test_local_maximum.cpp
              LINK_WITH pcl_gtest pcl_common pcl_filters pcl_search pcl_octree)

test/filters/test_functor_filter.cpp

@@ -0,0 +1,197 @@
+/*
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * Point CLoud Library (PCL) - www.pointclouds.org
+ * Copyright (c) 2020-, Open Perception
+ *
+ * All rights reserved
+ */
+
+#include <pcl/common/generate.h>
+#include <pcl/filters/functor_filter.h>
+#include <pcl/test/gtest.h>
+#include <pcl/point_types.h>
+
+#include <random>
+
+using namespace pcl;
+
+TEST(FunctorFilterTrait, CheckCompatibility)
+{
+  const auto copy_all = [](PointCloud<PointXYZ>, index_t) { return 0; };
+  EXPECT_TRUE((is_functor_for_filter_v<PointXYZ, decltype(copy_all)>));
+
+  const auto ref_all = [](PointCloud<PointXYZ>&, index_t&) { return 0; };
+  EXPECT_FALSE((is_functor_for_filter_v<PointXYZ, decltype(ref_all)>));
+
+  const auto ref_cloud = [](PointCloud<PointXYZ>&, index_t) { return 0; };
+  EXPECT_FALSE((is_functor_for_filter_v<PointXYZ, decltype(ref_cloud)>));
+
+  const auto const_ref_cloud = [](const PointCloud<PointXYZ>&, index_t) { return 0; };
+  EXPECT_TRUE((is_functor_for_filter_v<PointXYZ, decltype(const_ref_cloud)>));
+
+  const auto const_ref_all = [](const PointCloud<PointXYZ>&, const index_t&) {
+    return 0;
+  };
+  EXPECT_TRUE((is_functor_for_filter_v<PointXYZ, decltype(const_ref_all)>));
+}
+
+struct FunctorFilterRandom : public testing::TestWithParam<std::uint32_t> {
+  void
+  SetUp() override
+  {
+    cloud = make_shared<PointCloud<PointXYZ>>();
+
+    std::uint32_t seed = GetParam();
+    common::CloudGenerator<PointXYZ, common::UniformGenerator<float>> generator{
+        {-10., 0., seed}};
+    generator.fill(20, 20, negative_cloud);
+    generator.setParameters({0., 10., seed});
+    generator.fill(10, 10, positive_cloud);
+    *cloud = negative_cloud + positive_cloud;
+  }
+
+  shared_ptr<PointCloud<PointXYZ>> cloud;
+  PointCloud<PointXYZ> out_cloud, negative_cloud, positive_cloud;
+};
+
+TEST_P(FunctorFilterRandom, functioning)
+{
+
+  const auto lambda = [](const PointCloud<PointXYZ>& cloud, index_t idx) {
+    const auto& pt = cloud[idx];
+    return (pt.getArray3fMap() < 0).all();
+  };
+
+  for (const auto& keep_removed : {true, false}) {
+    FunctorFilter<PointXYZ, decltype(lambda)> filter{lambda, keep_removed};
+    filter.setInputCloud(cloud);
+    const auto removed_size = filter.getRemovedIndices()->size();
+
+    filter.setNegative(false);
+    filter.filter(out_cloud);
+
+    EXPECT_EQ(out_cloud.size(), negative_cloud.size());
+    if (keep_removed) {
+      EXPECT_EQ(filter.getRemovedIndices()->size(), positive_cloud.size());
+    }
+    else {
+      EXPECT_EQ(filter.getRemovedIndices()->size(), removed_size);
+    }
+
+    filter.setNegative(true);
+    filter.filter(out_cloud);
+
+    EXPECT_EQ(out_cloud.size(), positive_cloud.size());
+    if (keep_removed) {
+      EXPECT_EQ(filter.getRemovedIndices()->size(), negative_cloud.size());
+    }
+    else {
+      EXPECT_EQ(filter.getRemovedIndices()->size(), removed_size);
+    }
+  }
+}
+
+INSTANTIATE_TEST_SUITE_P(RandomSeed,
+                         FunctorFilterRandom,
+                         testing::Values(123, 456, 789));
+
+namespace type_test {
+int
+free_func(const PointCloud<PointXYZ>&, const index_t& idx)
+{
+  return idx % 2;
+}
+
+static const auto lambda_func = [](const PointCloud<PointXYZ>& cloud, index_t idx) {
+  return free_func(cloud, idx);
+};
+
+struct StaticFunctor {
+  static int
+  functor(PointCloud<PointXYZ> cloud, index_t idx)
+  {
+    return free_func(cloud, idx);
+  }
+  int
+  operator()(PointCloud<PointXYZ> cloud, index_t idx)
+  {
+    return free_func(cloud, idx);
+  }
+};
+struct StaticFunctorConst {
+  int
+  operator()(PointCloud<PointXYZ> cloud, index_t idx) const
+  {
+    return free_func(cloud, idx);
+  }
+};
+
+using LambdaT = decltype(lambda_func);
+using StdFunctorBoolT = std::function<bool(PointCloud<PointXYZ>, index_t)>;
+using StdFunctorIntT = std::function<int(PointCloud<PointXYZ>, index_t)>;
+using FreeFuncT = decltype(free_func)*;
+using StaticFunctorT = decltype(StaticFunctor::functor)*;
+using NonConstFuntorT = StaticFunctor;
+using ConstFuntorT = StaticFunctorConst;
+
+template <typename FunctorT>
+struct Helper {};
+
+#define HELPER_MACRO(TYPE, VALUE)                                                      \
+  template <>                                                                          \
+  struct Helper<TYPE> {                                                                \
+    using type = TYPE;                                                                 \
+    static type value;                                                                 \
+  };                                                                                   \
+  TYPE Helper<TYPE>::value = VALUE
+
+HELPER_MACRO(LambdaT, lambda_func);
+HELPER_MACRO(StdFunctorBoolT, lambda_func);
+HELPER_MACRO(StdFunctorIntT, lambda_func);
+HELPER_MACRO(FreeFuncT, free_func);
+HELPER_MACRO(StaticFunctorT, StaticFunctor::functor);
+HELPER_MACRO(NonConstFuntorT, {});
+HELPER_MACRO(ConstFuntorT, {});
+
+#undef HELPER_MACRO
+
+using types = ::testing::Types<LambdaT,
+                               StdFunctorBoolT,
+                               StdFunctorIntT,
+                               FreeFuncT,
+                               StaticFunctorT,
+                               NonConstFuntorT,
+                               ConstFuntorT>;
+} // namespace type_test
+
+template <typename T>
+struct FunctorFilterFunctor : public ::testing::Test {
+  void
+  SetUp() override
+  {
+    cloud.resize(2);
+  }
+  PointCloud<PointXYZ> cloud;
+};
+TYPED_TEST_SUITE_P(FunctorFilterFunctor);
+
+TYPED_TEST_P(FunctorFilterFunctor, type_check)
+{
+  using FunctorT = TypeParam;
+  const auto& functor = type_test::Helper<FunctorT>::value;
+
+  FunctorFilter<PointXYZ, FunctorT> filter_lambda{functor};
+  EXPECT_EQ(filter_lambda.getFunctor()(this->cloud, 0), 0);
+  EXPECT_EQ(filter_lambda.getFunctor()(this->cloud, 1), 1);
+}
+
+REGISTER_TYPED_TEST_SUITE_P(FunctorFilterFunctor, type_check);
+INSTANTIATE_TYPED_TEST_SUITE_P(pcl, FunctorFilterFunctor, type_test::types);
+
+int
+main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/include/pcl/test/gtest.h

@@ -54,6 +54,15 @@
   #define TYPED_TEST_SUITE TYPED_TEST_CASE
 #endif
 
+/**
+ * \brief Macro choose between TYPED_TEST_CASE_P and TYPED_TEST_SUITE_P depending on the GTest version
+ *
+ * \ingroup test
+ */
+#if !defined(TYPED_TEST_SUITE_P)
+  #define TYPED_TEST_SUITE_P TYPED_TEST_CASE_P
+#endif
+
 /**
  * \brief Macro choose between INSTANTIATE_TEST_CASE_P and INSTANTIATE_TEST_SUITE_P depending on the GTest version
  *
@@ -63,3 +72,20 @@
   #define INSTANTIATE_TEST_SUITE_P INSTANTIATE_TEST_CASE_P
 #endif
 
+/**
+ * \brief Macro choose between INSTANTIATE_TYPED_TEST_CASE_P and INSTANTIATE_TYPED_TEST_SUITE_P depending on the GTest version
+ *
+ * \ingroup test
+ */
+#if !defined(INSTANTIATE_TYPED_TEST_SUITE_P)
+  #define INSTANTIATE_TYPED_TEST_SUITE_P INSTANTIATE_TYPED_TEST_CASE_P
+#endif
+
+/**
+ * \brief Macro choose between REGISTER_TYPED_TEST_CASE_P and REGISTER_TYPED_TEST_SUITE_P depending on the GTest version
+ *
+ * \ingroup test
+ */
+#if !defined(REGISTER_TYPED_TEST_SUITE_P)
+  #define REGISTER_TYPED_TEST_SUITE_P REGISTER_TYPED_TEST_CASE_P
+#endif