functor_filter.h

Go to the documentation of this file.

/*
  * 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/point_traits.h> // for is_invocable
 #include <limits>
 #include "types.h"
  
 namespace pcl {
 namespace experimental {
 
 template <typename PointT, typename Function>
 constexpr static bool is_function_object_for_filter_v =
     std::is_invocable_r_v<bool, Function, const PointCloud<PointT>&, index_t>;
  
 namespace advanced {
 template <typename PointT, typename FunctionObject>
 class FunctorFilter : public FilterIndices<PointT> {
   using Base = FilterIndices<PointT>;
   using PCL_Base = PCLBase<PointT>;
  
 public:
   using FunctionObjectT = FunctionObject;
   // using in type would complicate signature
   static_assert(is_function_object_for_filter_v<PointT, FunctionObjectT>,
                 "Function object 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 PCL_Base::indices_;
   using PCL_Base::input_;
   bool keep_organized_ = false;
   float user_filter_value_ = std::numeric_limits<float>::quiet_NaN();
  
 private:
   // need to hold a value because lambdas can only be copy or move constructed in C++14
   FunctionObjectT functionObject_;
  
 public:
   FunctorFilter(FunctionObjectT function_object, bool extract_removed_indices = false)
   : Base(extract_removed_indices), functionObject_(std::move(function_object))
   {
     filter_name_ = "functor_filter";
   }
  
   const FunctionObjectT&
   getFunctionObject() const noexcept
   {
     return functionObject_;
   }
  
   FunctionObjectT&
   getFunctionObject() noexcept
   {
     return functionObject_;
   }
  
   void
   applyFilter(Indices& indices) override
   {
     indices.clear();
     indices.reserve(indices_->size());
     if (extract_removed_indices_) {
       removed_indices_->clear();
       removed_indices_->reserve(indices_->size());
     }
  
     for (const auto index : *indices_) {
       // function object returns true for points that should be selected
       if (negative_ != functionObject_(*input_, index)) {
         indices.push_back(index);
       }
       else if (extract_removed_indices_) {
         removed_indices_->push_back(index);
       }
     }
   }
 
   void applyFilter (PointCloud<PointT> &output) override
    {
        Indices indices;
        if (keep_organized_)
        {
            if (!extract_removed_indices_)
            {
                PCL_WARN ("[pcl::FilterIndices<PointT>::applyFilter] extract_removed_indices_ was set to 'true' to keep the point cloud organized.\n");
                extract_removed_indices_ = true;
            }
            applyFilter (indices);
 
            output = *input_;
 
            // To preserve legacy behavior, only coordinates xyz are filtered.
            // Copying a PointXYZ initialized with the user_filter_value_ into a generic
            // PointT, ensures only the xyz coordinates, if they exist at destination,
            // are overwritten.
            const PointXYZ ufv (user_filter_value_, user_filter_value_, user_filter_value_);
            for (const auto ri : *removed_indices_)  // ri = removed index
            copyPoint(ufv, output[ri]);
            if (!std::isfinite (user_filter_value_))
            output.is_dense = false;
        }
        else
        {
            output.is_dense = true;
            applyFilter (indices);
            pcl::copyPointCloud (*input_, indices, output);
        }
    }
  
 protected:
   FunctorFilter(bool extract_removed_indices = false) : Base(extract_removed_indices)
   {
     filter_name_ = "functor_filter";
   }
  
   void
   setFunctionObject(FunctionObjectT function_object) const noexcept
   {
     functionObject_ = std::move(function_object);
   }
 };
 } // namespace advanced
  
 template <class PointT>
 using FilterFunction = std::function<bool(const PointCloud<PointT>&, index_t)>;
  
 template <class PointT>
 using FunctionFilter = advanced::FunctorFilter<PointT, FilterFunction<PointT>>;
 } // namespace experimental
 } // namespace pcl