diff --git a/benchmarks/CMakeLists.txt b/benchmarks/CMakeLists.txt index 6fccc23f74f..62f741a9692 100644 --- a/benchmarks/CMakeLists.txt +++ b/benchmarks/CMakeLists.txt @@ -23,3 +23,9 @@ PCL_ADD_BENCHMARK(filters_voxel_grid FILES filters/voxel_grid.cpp ARGUMENTS "${PCL_SOURCE_DIR}/test/table_scene_mug_stereo_textured.pcd" "${PCL_SOURCE_DIR}/test/milk_cartoon_all_small_clorox.pcd") +PCL_ADD_BENCHMARK(filters_passthrough FILES filters/passthrough.cpp + LINK_WITH pcl_filters) + +PCL_ADD_BENCHMARK(filters_crop_box FILES filters/crop_box.cpp + LINK_WITH pcl_filters) + diff --git a/benchmarks/filters/crop_box.cpp b/benchmarks/filters/crop_box.cpp new file mode 100644 index 00000000000..b79b5b1f271 --- /dev/null +++ b/benchmarks/filters/crop_box.cpp @@ -0,0 +1,97 @@ + +#include +#include +#include + +#include + +#include +#include +#include +#include + +using namespace pcl; + +template class Filter> +static void +BM_crop_box(benchmark::State& state, const typename PointCloud::Ptr cloud) +{ + Filter filter; + typename pcl::PointCloud out_cloud; + + filter.setInputCloud(cloud); + const Eigen::Vector3f r = Eigen::Vector3f::Random(); + filter.setTransform(pcl::getTransformationFromTwoUnitVectors(r, r)); + filter.setRotation(r); + filter.setTranslation(r); + + for (auto _ : state) + filter.filter(out_cloud); +} + +template class T> +void +BM_register(const std::string& class_name, + const typename PointCloud::Ptr dataset, + const std::string& dataset_name, + const benchmark::TimeUnit unit) +{ + const std::string bm_name = dataset_name + "_" + class_name; + benchmark::RegisterBenchmark(bm_name.c_str(), &BM_crop_box, dataset) + ->Unit(unit); +}; + +template class... Ts, std::size_t ArraySize> +bool +BM_registration(const std::array& classes_name, + const std::vector::Ptr>& datasets, + const std::vector& datasets_name, + const benchmark::TimeUnit unit = benchmark::kMillisecond) +{ + static_assert(sizeof...(Ts) == ArraySize, + "Number of template classes and classes name are different.\n"); + + if (datasets.size() != datasets_name.size()) { + PCL_ERROR("[Benchmark] Number of datasets and datasets name are different.\n"); + return false; + } + + for (std::size_t i = 0; i < datasets.size(); ++i) { + typename std::array::const_iterator name_it = + classes_name.begin(); + const int res[] = {(BM_register( + *(name_it++), datasets.at(i), datasets_name.at(i), unit), + 0)...}; + (void)res; // suppress warning + } + + return true; +} + +int +main(int argc, char** argv) +{ + const std::vector datasets_name = { + "320p", "480p", "720p", "1080p", "1440p"}; + const std::vector datasets_size = { + 480 * 320, 640 * 480, 1280 * 720, 1920 * 1080, 2560 * 1440}; + + std::vector::Ptr> datasets; + for (const std::size_t dataset_size : datasets_size) { + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + cloud->resize(dataset_size); + for (auto& pt : *cloud) + pt = pcl::PointXYZ{0., 0., 0.}; + datasets.push_back(std::move(cloud)); + } + + const std::array filters_name{"CropBox", "FunctorCropBox"}; + + if (BM_registration( + filters_name, datasets, datasets_name)) { + benchmark::Initialize(&argc, argv); + benchmark::RunSpecifiedBenchmarks(); + } + + return 0; +} \ No newline at end of file diff --git a/benchmarks/filters/passthrough.cpp b/benchmarks/filters/passthrough.cpp new file mode 100644 index 00000000000..9bbdbfeb980 --- /dev/null +++ b/benchmarks/filters/passthrough.cpp @@ -0,0 +1,94 @@ + +#include +#include + +#include + +#include +#include +#include +#include + +using namespace pcl; + +template class Filter> +static void +BM_passthrough(benchmark::State& state, const typename PointCloud::Ptr cloud) +{ + Filter filter; + typename pcl::PointCloud out_cloud; + + filter.setFilterLimits(-10., 10.); + filter.setInputCloud(cloud); + filter.setFilterFieldName("x"); + + for (auto _ : state) + filter.filter(out_cloud); +} + +template class T> +void +BM_register(const std::string& class_name, + const typename PointCloud::Ptr dataset, + const std::string& dataset_name, + const benchmark::TimeUnit unit) +{ + const std::string bm_name = dataset_name + "_" + class_name; + benchmark::RegisterBenchmark(bm_name.c_str(), &BM_passthrough, dataset) + ->Unit(unit); +}; + +template class... Ts, std::size_t ArraySize> +bool +BM_registration(const std::array& classes_name, + const std::vector::Ptr>& datasets, + const std::vector& datasets_name, + const benchmark::TimeUnit unit = benchmark::kMillisecond) +{ + static_assert(sizeof...(Ts) == ArraySize, + "Number of template classes and classes name are different.\n"); + + if (datasets.size() != datasets_name.size()) { + PCL_ERROR("[Benchmark] Number of datasets and datasets name are different.\n"); + return false; + } + + for (std::size_t i = 0; i < datasets.size(); ++i) { + typename std::array::const_iterator name_it = + classes_name.begin(); + const int res[] = {(BM_register( + *(name_it++), datasets.at(i), datasets_name.at(i), unit), + 0)...}; + (void)res; // suppress warning + } + + return true; +} + +int +main(int argc, char** argv) +{ + const std::vector datasets_name = { + "320p", "480p", "720p", "1080p", "1440p"}; + const std::vector datasets_size = { + 480 * 320, 640 * 480, 1280 * 720, 1920 * 1080, 2560 * 1440}; + + std::vector::Ptr> datasets; + for (const std::size_t dataset_size : datasets_size) { + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + cloud->resize(dataset_size); + for (auto& pt : *cloud) + pt = pcl::PointXYZ{0., 0., 0.}; + datasets.push_back(std::move(cloud)); + } + + const std::array filters_name{"PassThrough", "FunctorPassThrough"}; + + if (BM_registration( + filters_name, datasets, datasets_name)) { + benchmark::Initialize(&argc, argv); + benchmark::RunSpecifiedBenchmarks(); + } + + return 0; +} diff --git a/filters/CMakeLists.txt b/filters/CMakeLists.txt index bb91d7bb179..f6c207a1620 100644 --- a/filters/CMakeLists.txt +++ b/filters/CMakeLists.txt @@ -88,6 +88,8 @@ set(incs ) set(experimental_incs "include/pcl/${SUBSYS_NAME}/experimental/functor_filter.h" + "include/pcl/${SUBSYS_NAME}/experimental/crop_box.h" + "include/pcl/${SUBSYS_NAME}/experimental/passthrough.h" ) set(impl_incs diff --git a/filters/include/pcl/filters/experimental/crop_box.h b/filters/include/pcl/filters/experimental/crop_box.h new file mode 100644 index 00000000000..32dbdb42b2d --- /dev/null +++ b/filters/include/pcl/filters/experimental/crop_box.h @@ -0,0 +1,202 @@ +/* + * SPDX-License-Identifier: BSD-3-Clause + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2021-, Open Perception + * + * All rights reserved + */ + +#pragma once + +#include // for getTransformation +#include // for isFinite +#include +#include + +#include +namespace pcl { +namespace experimental { + +/** \brief CropBox is a filter that allows the user to filter all the data + * inside of a given box. + * + * \author Justin Rosen + * \ingroup filters + */ +template +class CropBox : public FilterIndices { + using PointCloud = typename FilterIndices::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + +public: + using Ptr = shared_ptr>; + using ConstPtr = shared_ptr>; + + /** \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). + */ + CropBox(bool extract_removed_indices = false) + : FilterIndices(extract_removed_indices) + { + filter_name_ = "CropBox"; + } + + /** \brief Set the minimum point of the box + * \param[in] min_pt the minimum point of the box + */ + inline void + setMin(const Eigen::Vector4f& min_pt) + { + min_pt_ = min_pt; + } + + /** \brief Get the value of the minimum point of the box, as set by the user + * \return the value of the internal \a min_pt parameter. + */ + inline Eigen::Vector4f + getMin() const + { + return min_pt_; + } + + /** \brief Set the maximum point of the box + * \param[in] max_pt the maximum point of the box + */ + inline void + setMax(const Eigen::Vector4f& max_pt) + { + max_pt_ = max_pt; + } + + /** \brief Get the value of the maximum point of the box, as set by the user + * \return the value of the internal \a max_pt parameter. + */ + inline Eigen::Vector4f + getMax() const + { + return max_pt_; + } + + /** \brief Set a translation value for the box + * \param[in] translation the (tx,ty,tz) values that the box should be translated by + */ + inline void + setTranslation(const Eigen::Vector3f& translation) + { + translation_ = translation; + } + + /** \brief Get the value of the box translation parameter as set by the user. */ + Eigen::Vector3f + getTranslation() const + { + return translation_; + } + + /** \brief Set a rotation value for the box + * \param[in] rotation the (rx,ry,rz) values that the box should be rotated by + */ + inline void + setRotation(const Eigen::Vector3f& rotation) + { + rotation_ = rotation; + } + + /** \brief Get the value of the box rotatation parameter, as set by the user. */ + inline Eigen::Vector3f + getRotation() const + { + return rotation_; + } + + /** \brief Set a transformation that should be applied to the cloud before filtering + * \param[in] transform an affine transformation that needs to be applied to the + * cloud before filtering + */ + inline void + setTransform(const Eigen::Affine3f& transform) + { + transform_ = transform; + } + + /** \brief Get the value of the transformation parameter, as set by the user. */ + inline Eigen::Affine3f + getTransform() const + { + return transform_; + } + + /** \brief Sample of point indices + * \param[out] indices the resultant point cloud indices + */ + void + applyFilter(Indices& indices) override + { + Eigen::Affine3f box_transform; + pcl::getTransformation(translation_(0), + translation_(1), + translation_(2), + rotation_(0), + rotation_(1), + rotation_(2), + box_transform); + const Eigen::Matrix4f pt_transform = + (box_transform.inverse() * transform_).matrix(); + + std::function lambda; + if (pt_transform.isIdentity()) + lambda = [&](const PointCloud& cloud, index_t idx) { + const auto& pt = cloud.at(idx).getVector4fMap(); + return (pt.array() >= min_pt_.array()).template head<3>().all() && + (pt.array() <= max_pt_.array()).template head<3>().all(); + }; + else + lambda = [&](const PointCloud& cloud, index_t idx) { + const Eigen::Vector4f pt = + pt_transform * cloud.at(idx).getVector3fMap().homogeneous(); + return (pt.array() >= min_pt_.array()).template head<3>().all() && + (pt.array() <= max_pt_.array()).template head<3>().all(); + }; + + auto filter = advanced::FunctorFilter( + lambda, this->extract_removed_indices_); + filter.setNegative(this->getNegative()); + filter.setKeepOrganized(this->getKeepOrganized()); + filter.setIndices(this->getIndices()); + filter.setInputCloud(this->getInputCloud()); + filter.applyFilter(indices); + if (this->extract_removed_indices_) + *removed_indices_ = *filter.getRemovedIndices(); // copy + } + +protected: + using PCLBase::input_; + using PCLBase::indices_; + using Filter::filter_name_; + using FilterIndices::negative_; + using FilterIndices::keep_organized_; + using FilterIndices::user_filter_value_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::removed_indices_; + +private: + /** \brief The minimum point of the box. */ + Eigen::Vector4f min_pt_ = Eigen::Vector4f(-1, -1, -1, 1); + + /** \brief The maximum point of the box. */ + Eigen::Vector4f max_pt_ = Eigen::Vector4f(1, 1, 1, 1); + + /** \brief The 3D rotation for the box. */ + Eigen::Vector3f rotation_ = Eigen::Vector3f::Zero(); + + /** \brief The 3D translation for the box. */ + Eigen::Vector3f translation_ = Eigen::Vector3f::Zero(); + + /** \brief The affine transform applied to the cloud. */ + Eigen::Affine3f transform_ = Eigen::Affine3f::Identity(); +}; +} // namespace experimental +} // namespace pcl \ No newline at end of file diff --git a/filters/include/pcl/filters/experimental/functor_filter.h b/filters/include/pcl/filters/experimental/functor_filter.h index d03c1828697..b622f7a8fc4 100644 --- a/filters/include/pcl/filters/experimental/functor_filter.h +++ b/filters/include/pcl/filters/experimental/functor_filter.h @@ -9,6 +9,7 @@ #pragma once +#include // for isXYZFinite #include #include // for is_invocable @@ -94,13 +95,22 @@ class FunctorFilter : public FilterIndices { 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); + // function object returns true for points that should be selected + if (input_->is_dense) { + for (const auto index : *indices_) { + if (negative_ != functionObject_(*input_, index)) + indices.push_back(index); + else if (extract_removed_indices_) + removed_indices_->push_back(index); } - else if (extract_removed_indices_) { - removed_indices_->push_back(index); + } + else { + for (const auto index : *indices_) { + if (isXYZFinite(input_->at(index)) && + negative_ != functionObject_(*input_, index)) + indices.push_back(index); + else if (extract_removed_indices_) + removed_indices_->push_back(index); } } } @@ -124,7 +134,7 @@ class FunctorFilter : public FilterIndices { * filter out the indices for which it returns false */ void - setFunctionObject(FunctionObjectT function_object) const noexcept + setFunctionObject(FunctionObjectT&& function_object) noexcept { functionObject_ = std::move(function_object); } diff --git a/filters/include/pcl/filters/experimental/passthrough.h b/filters/include/pcl/filters/experimental/passthrough.h new file mode 100644 index 00000000000..f7903f79a25 --- /dev/null +++ b/filters/include/pcl/filters/experimental/passthrough.h @@ -0,0 +1,227 @@ +/* + * SPDX-License-Identifier: BSD-3-Clause + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2021-, Open Perception + * + * All rights reserved + */ + +#pragma once + +#include + +#include // for numeric_limits + +namespace pcl { +namespace experimental { + +template +struct PassThroughFunctor { + PassThroughFunctor() = default; + PassThroughFunctor(const uindex_t* field_offset, + const float* filter_limit_min, + const float* filter_limit_max, + const bool* has_filter_field, + const bool* negative) + : field_offset_(field_offset) + , filter_limit_min_(filter_limit_min) + , filter_limit_max_(filter_limit_max) + , has_filter_field_(has_filter_field) + , negative_(negative) + {} + + bool + operator()(const PointCloud& cloud, index_t idx) const + { + // always filter out points with NaN in XYZ fields, regardless of the user's choice + // of what points should be filtered (normal/inverted) + if (!*has_filter_field_) + return !*negative_; // always keep the point in FunctorFilter + + float field_value; + const std::uint8_t* pt_data = reinterpret_cast(&cloud.at(idx)); + memcpy(&field_value, pt_data + *field_offset_, sizeof(float)); + + if (!std::isfinite(field_value)) + return *negative_; // always discard the point in FunctorFilter + else + return field_value >= *filter_limit_min_ && field_value <= *filter_limit_max_; + } + +private: + const uindex_t* field_offset_ = nullptr; + const float* filter_limit_min_ = nullptr; + const float* filter_limit_max_ = nullptr; + const bool* has_filter_field_ = nullptr; + const bool* negative_ = nullptr; +}; + +template +using PassThroughFilter = advanced::FunctorFilter>; + +/** \brief @b PassThrough passes points in a cloud based on constraints for one + * particular field of the point type. + * \details Iterates through the entire input once, automatically filtering non-finite + * points and the points outside the interval specified by setFilterLimits(), which + * applies only to the field specified by setFilterFieldName(). The specified field + * should have float type. + *

+ * Usage example: + * \code + * pcl::PassThrough ptfilter (true); // Initializing with true will allow us + * to extract the removed indices + * ptfilter.setInputCloud (cloud_in); + * ptfilter.setFilterFieldName ("x"); + * ptfilter.setFilterLimits (0.0, 1000.0); + * ptfilter.filter (*indices_x); + * // The indices_x array indexes all points of cloud_in that have x between 0.0 and + * 1000.0 indices_rem = ptfilter.getRemovedIndices (); + * // The indices_rem array indexes all points of cloud_in that have x smaller than 0.0 + * or larger than 1000.0 + * // and also indexes all non-finite points of cloud_in + * ptfilter.setIndices (indices_x); + * ptfilter.setFilterFieldName ("z"); + * ptfilter.setFilterLimits (-10.0, 10.0); + * ptfilter.setNegative (true); + * ptfilter.filter (*indices_xz); + * // The indices_xz array indexes all points of cloud_in that have x between 0.0 and + * 1000.0 and z larger than 10.0 or smaller than -10.0 ptfilter.setIndices (indices_xz); + * ptfilter.setFilterFieldName ("intensity"); + * ptfilter.setFilterLimits (FLT_MIN, 0.5); + * ptfilter.setNegative (false); + * ptfilter.filter (*cloud_out); + * // The resulting cloud_out contains all points of cloud_in that are finite and have: + * // x between 0.0 and 1000.0, z larger than 10.0 or smaller than -10.0 and intensity + * smaller than 0.5. + * \endcode + * \author Radu Bogdan Rusu + * \ingroup filters + */ +template +class PassThrough : public PassThroughFilter { +protected: + using PointCloud = typename FilterIndices::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + using FieldList = typename pcl::traits::fieldList::type; + +public: + using Ptr = shared_ptr>; + using ConstPtr = shared_ptr>; + + /** \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). + */ + PassThrough(bool extract_removed_indices = false) + : PassThroughFilter(extract_removed_indices) + { + filter_name_ = "PassThrough"; + PassThroughFilter::setFunctionObject( + PassThroughFunctor(&field_offset_, + &filter_limit_min_, + &filter_limit_max_, + &has_filter_field_, + &negative_)); + } + + /** \brief Provide the name of the field to be used for filtering data. + * \details In conjunction with setFilterLimits(), points having values outside this + * interval for this field will be discarded. + * \param[in] field_name The name of the field that will be used for filtering. + */ + inline void + setFilterFieldName(const std::string& field_name) + { + filter_field_name_ = field_name; + } + + /** \brief Retrieve the name of the field to be used for filtering data. + * \return The name of the field that will be used for filtering. + */ + inline std::string const + getFilterFieldName() const + { + return filter_field_name_; + } + + /** \brief Set the numerical limits for the field for filtering data. + * \details In conjunction with setFilterFieldName(), points having values outside + * this interval for this field will be discarded. + * \param[in] limit_min The minimum allowed field value (default = FLT_MIN). + * \param[in] limit_max The maximum allowed field value (default = FLT_MAX). + */ + inline void + setFilterLimits(const float& limit_min, const float& limit_max) + { + filter_limit_min_ = limit_min; + filter_limit_max_ = limit_max; + } + + /** \brief Get the numerical limits for the field for filtering data. + * \param[out] limit_min The minimum allowed field value (default = FLT_MIN). + * \param[out] limit_max The maximum allowed field value (default = FLT_MAX). + */ + inline void + getFilterLimits(float& limit_min, float& limit_max) const + { + limit_min = filter_limit_min_; + limit_max = filter_limit_max_; + } + +protected: + using PCLBase::input_; + using PCLBase::indices_; + using Filter::filter_name_; + using Filter::getClassName; + using FilterIndices::negative_; + using FilterIndices::keep_organized_; + using FilterIndices::user_filter_value_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::removed_indices_; + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilter(Indices& indices) override + { + has_filter_field_ = !filter_field_name_.empty(); + if (has_filter_field_) { + // Attempt to get the field name's index + std::vector fields; + const int distance_idx_ = getFieldIndex(filter_field_name_, fields); + if (distance_idx_ == -1) { + PCL_WARN("[pcl::%s::applyFilter] Unable to find field name in point type.\n", + getClassName().c_str()); + indices.clear(); + removed_indices_->clear(); + return; + } + field_offset_ = fields.at(distance_idx_).offset; + } + + PassThroughFilter::applyFilter(indices); + } + +private: + /** \brief The name of the field that will be used for filtering. */ + std::string filter_field_name_; + + /** \brief The minimum allowed field value (default = FLT_MIN). */ + float filter_limit_min_ = std::numeric_limits::min(); + + /** \brief The maximum allowed field value (default = FLT_MIN). */ + float filter_limit_max_ = std::numeric_limits::min(); + + /** \brief Whether a filtering field is set. */ + bool has_filter_field_; + + /** \brief The offset of the filtering field. */ + uindex_t field_offset_; + + using PassThroughFilter::getFunctionObject; // hide method +}; +} // namespace experimental +} // namespace pcl diff --git a/test/filters/CMakeLists.txt b/test/filters/CMakeLists.txt index 1cbb324938b..62f494f4cdc 100644 --- a/test/filters/CMakeLists.txt +++ b/test/filters/CMakeLists.txt @@ -46,6 +46,11 @@ if(BUILD_io) FILES test_bilateral.cpp LINK_WITH pcl_gtest pcl_filters pcl_io pcl_kdtree ARGUMENTS "${PCL_SOURCE_DIR}/test/milk_cartoon_all_small_clorox.pcd") + + PCL_ADD_TEST(filters_experimental_filters test_filters_experimental_filters + FILES test_experimental_filters.cpp + LINK_WITH pcl_gtest pcl_common pcl_filters pcl_io + ARGUMENTS "${PCL_SOURCE_DIR}/test/bun0.pcd") if(BUILD_features) PCL_ADD_TEST(filters_sampling test_filters_sampling diff --git a/test/filters/test_clipper.cpp b/test/filters/test_clipper.cpp index 46f82e48e90..198f70e8100 100644 --- a/test/filters/test_clipper.cpp +++ b/test/filters/test_clipper.cpp @@ -134,14 +134,7 @@ TEST (CropBox, Filters) input->push_back (PointXYZ (-0.9f, -0.9f, -0.9f)); // Create indices vector ( without 0 and 4) - pcl::IndicesPtr idx (new pcl::Indices (7)); - (*idx)[0] = 1; - (*idx)[1] = 2; - (*idx)[2] = 3; - (*idx)[3] = 5; - (*idx)[4] = 6; - (*idx)[5] = 7; - (*idx)[6] = 8; + pcl::IndicesPtr idx(new pcl::Indices({1,2,3,5,6,7,8})); // Define cropBox limit Eigen::Vector4f min_pt (-1.0f, -1.0f, -1.0f, 1.0f); diff --git a/test/filters/test_experimental_filters.cpp b/test/filters/test_experimental_filters.cpp new file mode 100644 index 00000000000..2cec519d3cc --- /dev/null +++ b/test/filters/test_experimental_filters.cpp @@ -0,0 +1,521 @@ +/* + * SPDX-License-Identifier: BSD-3-Clause + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2021-, Open Perception + * + * All rights reserved + */ + +#include +#include +#include +#include // for loadPCDFile +#include +#include // for pcl::Indices +#include +#include + +using namespace pcl; +using namespace Eigen; + +PointCloud::Ptr cloud(new PointCloud); + +TEST(ExperimentalCropBox, Filters) +{ + // Create cloud with center point and corner points + PointCloud::Ptr input(new PointCloud()); + + input->push_back(PointXYZ(0.0f, 0.0f, 0.0f)); + input->push_back(PointXYZ(0.9f, 0.9f, 0.9f)); + input->push_back(PointXYZ(0.9f, 0.9f, -0.9f)); + input->push_back(PointXYZ(0.9f, -0.9f, 0.9f)); + input->push_back(PointXYZ(-0.9f, 0.9f, 0.9f)); + input->push_back(PointXYZ(0.9f, -0.9f, -0.9f)); + input->push_back(PointXYZ(-0.9f, -0.9f, 0.9f)); + input->push_back(PointXYZ(-0.9f, 0.9f, -0.9f)); + input->push_back(PointXYZ(-0.9f, -0.9f, -0.9f)); + + // Create indices vector ( without 0 and 4) + pcl::IndicesPtr idx(new pcl::Indices({1, 2, 3, 5, 6, 7, 8})); + + // Define cropBox limit + Eigen::Vector4f min_pt(-1.0f, -1.0f, -1.0f, 1.0f); + Eigen::Vector4f max_pt(1.0f, 1.0f, 1.0f, 1.0f); + + // PointCloud without indices + // ------------------------------------------------------------------------- + + // Test the PointCloud method + experimental::CropBox cropBoxFilter(true); + cropBoxFilter.setInputCloud(input); + + // Cropbox slightly bigger then bounding box of points + cropBoxFilter.setMin(min_pt); + cropBoxFilter.setMax(max_pt); + + // Indices + pcl::Indices indices; + cropBoxFilter.filter(indices); + + // Cloud + PointCloud cloud_out; + cropBoxFilter.filter(cloud_out); + + // Should contain all + EXPECT_EQ(int(indices.size()), 9); + EXPECT_EQ(int(cloud_out.size()), 9); + EXPECT_EQ(int(cloud_out.width), 9); + EXPECT_EQ(int(cloud_out.height), 1); + + IndicesConstPtr removed_indices; + removed_indices = cropBoxFilter.getRemovedIndices(); + EXPECT_EQ(int(removed_indices->size()), 0); + + // Test setNegative + PointCloud cloud_out_negative; + cropBoxFilter.setNegative(true); + cropBoxFilter.filter(cloud_out_negative); + EXPECT_EQ(int(cloud_out_negative.size()), 0); + + cropBoxFilter.filter(indices); + EXPECT_EQ(int(indices.size()), 0); + cropBoxFilter.setNegative(false); + cropBoxFilter.filter(cloud_out); + + // Translate crop box up by 1 + cropBoxFilter.setTranslation(Eigen::Vector3f(0, 1, 0)); + cropBoxFilter.filter(indices); + cropBoxFilter.filter(cloud_out); + + EXPECT_EQ(int(indices.size()), 5); + EXPECT_EQ(int(cloud_out.size()), 5); + + removed_indices = cropBoxFilter.getRemovedIndices(); + EXPECT_EQ(int(removed_indices->size()), 4); + + // Test setNegative + cropBoxFilter.setNegative(true); + cropBoxFilter.filter(cloud_out_negative); + EXPECT_EQ(int(cloud_out_negative.size()), 4); + + cropBoxFilter.filter(indices); + EXPECT_EQ(int(indices.size()), 4); + + cropBoxFilter.setNegative(false); + cropBoxFilter.filter(cloud_out); + + // Rotate crop box up by 45 + cropBoxFilter.setRotation(Eigen::Vector3f(0.0f, 45.0f * float(M_PI) / 180.0f, 0.0f)); + cropBoxFilter.filter(indices); + cropBoxFilter.filter(cloud_out); + + EXPECT_EQ(int(indices.size()), 1); + EXPECT_EQ(int(cloud_out.size()), 1); + EXPECT_EQ(int(cloud_out.width), 1); + EXPECT_EQ(int(cloud_out.height), 1); + + removed_indices = cropBoxFilter.getRemovedIndices(); + EXPECT_EQ(int(removed_indices->size()), 8); + + // Test setNegative + cropBoxFilter.setNegative(true); + cropBoxFilter.filter(cloud_out_negative); + EXPECT_EQ(int(cloud_out_negative.size()), 8); + + cropBoxFilter.filter(indices); + EXPECT_EQ(int(indices.size()), 8); + + cropBoxFilter.setNegative(false); + cropBoxFilter.filter(cloud_out); + + // Rotate point cloud by -45 + cropBoxFilter.setTransform( + getTransformation(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -45.0f * float(M_PI) / 180.0f)); + cropBoxFilter.filter(indices); + cropBoxFilter.filter(cloud_out); + + EXPECT_EQ(int(indices.size()), 3); + EXPECT_EQ(int(cloud_out.size()), 3); + EXPECT_EQ(int(cloud_out.width), 3); + EXPECT_EQ(int(cloud_out.height), 1); + + removed_indices = cropBoxFilter.getRemovedIndices(); + EXPECT_EQ(int(removed_indices->size()), 6); + + // Test setNegative + cropBoxFilter.setNegative(true); + cropBoxFilter.filter(cloud_out_negative); + EXPECT_EQ(int(cloud_out_negative.size()), 6); + + cropBoxFilter.filter(indices); + EXPECT_EQ(int(indices.size()), 6); + + cropBoxFilter.setNegative(false); + cropBoxFilter.filter(cloud_out); + + // Translate point cloud down by -1 + cropBoxFilter.setTransform( + getTransformation(0, -1, 0, 0, 0, -45.0 * float(M_PI) / 180.0)); + cropBoxFilter.filter(indices); + cropBoxFilter.filter(cloud_out); + + EXPECT_EQ(int(indices.size()), 2); + EXPECT_EQ(int(cloud_out.size()), 2); + EXPECT_EQ(int(cloud_out.width), 2); + EXPECT_EQ(int(cloud_out.height), 1); + + removed_indices = cropBoxFilter.getRemovedIndices(); + EXPECT_EQ(int(removed_indices->size()), 7); + + // Test setNegative + cropBoxFilter.setNegative(true); + cropBoxFilter.filter(cloud_out_negative); + EXPECT_EQ(int(cloud_out_negative.size()), 7); + + cropBoxFilter.filter(indices); + EXPECT_EQ(int(indices.size()), 7); + + cropBoxFilter.setNegative(false); + cropBoxFilter.filter(cloud_out); + + // Remove point cloud rotation + cropBoxFilter.setTransform(getTransformation(0, -1, 0, 0, 0, 0)); + cropBoxFilter.filter(indices); + cropBoxFilter.filter(cloud_out); + + EXPECT_EQ(int(indices.size()), 0); + EXPECT_EQ(int(cloud_out.size()), 0); + EXPECT_EQ(int(cloud_out.width), 0); + EXPECT_EQ(int(cloud_out.height), 1); + + removed_indices = cropBoxFilter.getRemovedIndices(); + EXPECT_EQ(int(removed_indices->size()), 9); + + // Test setNegative + cropBoxFilter.setNegative(true); + cropBoxFilter.filter(cloud_out_negative); + EXPECT_EQ(int(cloud_out_negative.size()), 9); + + cropBoxFilter.filter(indices); + EXPECT_EQ(int(indices.size()), 9); + + // PointCloud with indices selection + // ------------------------------------------------------------------------- + + // Reset cropBox transformation + cropBoxFilter.setNegative(false); + cropBoxFilter.setTransform(getTransformation(0, 0, 0, 0, 0, 0)); + cropBoxFilter.setRotation(Eigen::Vector3f(0.0f, 0.0f, 0.0f)); + cropBoxFilter.setTranslation(Eigen::Vector3f(0.0f, 0.0f, 0.0f)); + + // Setup input indices selection + cropBoxFilter.setIndices(idx); + + // Indices + cropBoxFilter.filter(indices); + + // Cloud + cropBoxFilter.filter(cloud_out); + + // Should contain all + EXPECT_EQ(int(indices.size()), 7); + EXPECT_VECTOR_CONTAINS_ALL(pcl::Indices({1, 2, 3, 5, 6, 7, 8}), indices); + EXPECT_EQ(int(cloud_out.size()), 7); + EXPECT_EQ(int(cloud_out.width), 7); + EXPECT_EQ(int(cloud_out.height), 1); + + removed_indices = cropBoxFilter.getRemovedIndices(); + EXPECT_EQ(int(removed_indices->size()), 0); + + // Test setNegative + cropBoxFilter.setNegative(true); + cropBoxFilter.filter(cloud_out_negative); + EXPECT_EQ(int(cloud_out_negative.size()), 0); + + cropBoxFilter.filter(indices); + EXPECT_EQ(int(indices.size()), 0); + cropBoxFilter.setNegative(false); + cropBoxFilter.filter(cloud_out); + + // Translate crop box up by 1 + cropBoxFilter.setTranslation(Eigen::Vector3f(0, 1, 0)); + cropBoxFilter.filter(indices); + cropBoxFilter.filter(cloud_out); + + EXPECT_EQ(int(indices.size()), 3); + EXPECT_VECTOR_CONTAINS_ALL(pcl::Indices({1, 2, 7}), indices); + EXPECT_EQ(int(cloud_out.size()), 3); + + removed_indices = cropBoxFilter.getRemovedIndices(); + EXPECT_EQ(int(removed_indices->size()), 4); + EXPECT_VECTOR_CONTAINS_ALL(pcl::Indices({3, 5, 6, 8}), *removed_indices); + + // Test setNegative + cropBoxFilter.setNegative(true); + cropBoxFilter.filter(cloud_out_negative); + EXPECT_EQ(int(cloud_out_negative.size()), 4); + + cropBoxFilter.filter(indices); + EXPECT_EQ(int(indices.size()), 4); + EXPECT_VECTOR_CONTAINS_ALL(pcl::Indices({3, 5, 6, 8}), indices); + + cropBoxFilter.setNegative(false); + cropBoxFilter.filter(cloud_out); + + // Rotate crop box up by 45 + cropBoxFilter.setRotation(Eigen::Vector3f(0.0f, 45.0f * float(M_PI) / 180.0f, 0.0f)); + cropBoxFilter.filter(indices); + cropBoxFilter.filter(cloud_out); + + EXPECT_EQ(int(indices.size()), 0); + EXPECT_EQ(int(cloud_out.size()), 0); + EXPECT_EQ(int(cloud_out.width), 0); + EXPECT_EQ(int(cloud_out.height), 1); + + removed_indices = cropBoxFilter.getRemovedIndices(); + EXPECT_EQ(int(removed_indices->size()), 7); + EXPECT_VECTOR_DOES_NOT_CONTAIN(pcl::Indices({0, 4}), *removed_indices); + // Test setNegative + cropBoxFilter.setNegative(true); + cropBoxFilter.filter(cloud_out_negative); + EXPECT_EQ(int(cloud_out_negative.size()), 7); + + cropBoxFilter.filter(indices); + EXPECT_EQ(int(indices.size()), 7); + EXPECT_VECTOR_DOES_NOT_CONTAIN(pcl::Indices({0, 4}), indices); + + cropBoxFilter.setNegative(false); + cropBoxFilter.filter(cloud_out); + + // Rotate point cloud by -45 + cropBoxFilter.setTransform( + getTransformation(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -45.0f * float(M_PI) / 180.0f)); + cropBoxFilter.filter(indices); + cropBoxFilter.filter(cloud_out); + + EXPECT_EQ(int(indices.size()), 1); + EXPECT_VECTOR_CONTAINS_ALL(pcl::Indices({7}), indices); + EXPECT_EQ(int(cloud_out.size()), 1); + EXPECT_EQ(int(cloud_out.width), 1); + EXPECT_EQ(int(cloud_out.height), 1); + + removed_indices = cropBoxFilter.getRemovedIndices(); + EXPECT_EQ(int(removed_indices->size()), 6); + EXPECT_VECTOR_CONTAINS_ALL(pcl::Indices({1, 2, 3, 5, 6, 8}), *removed_indices); + + // Test setNegative + cropBoxFilter.setNegative(true); + cropBoxFilter.filter(cloud_out_negative); + EXPECT_EQ(int(cloud_out_negative.size()), 6); + + cropBoxFilter.filter(indices); + EXPECT_EQ(int(indices.size()), 6); + EXPECT_VECTOR_CONTAINS_ALL(pcl::Indices({1, 2, 3, 5, 6, 8}), indices); + + cropBoxFilter.setNegative(false); + cropBoxFilter.filter(cloud_out); + + // Translate point cloud down by -1 + cropBoxFilter.setTransform( + getTransformation(0, -1, 0, 0, 0, -45.0 * float(M_PI) / 180.0)); + cropBoxFilter.filter(indices); + cropBoxFilter.filter(cloud_out); + + EXPECT_EQ(int(indices.size()), 1); + EXPECT_VECTOR_CONTAINS_ALL(indices, pcl::Indices({7})); + EXPECT_EQ(int(cloud_out.size()), 1); + EXPECT_EQ(int(cloud_out.width), 1); + EXPECT_EQ(int(cloud_out.height), 1); + + removed_indices = cropBoxFilter.getRemovedIndices(); + EXPECT_EQ(int(removed_indices->size()), 6); + EXPECT_VECTOR_CONTAINS_ALL(pcl::Indices({1, 2, 3, 5, 6, 8}), *removed_indices); + + // Test setNegative + cropBoxFilter.setNegative(true); + cropBoxFilter.filter(cloud_out_negative); + EXPECT_EQ(int(cloud_out_negative.size()), 6); + + cropBoxFilter.filter(indices); + EXPECT_EQ(int(indices.size()), 6); + EXPECT_VECTOR_CONTAINS_ALL(pcl::Indices({1, 2, 3, 5, 6, 8}), indices); + + cropBoxFilter.setNegative(false); + cropBoxFilter.filter(cloud_out); + + // Remove point cloud rotation + cropBoxFilter.setTransform(getTransformation(0, -1, 0, 0, 0, 0)); + cropBoxFilter.filter(indices); + cropBoxFilter.filter(cloud_out); + + EXPECT_EQ(int(indices.size()), 0); + EXPECT_EQ(int(cloud_out.size()), 0); + EXPECT_EQ(int(cloud_out.width), 0); + EXPECT_EQ(int(cloud_out.height), 1); + + removed_indices = cropBoxFilter.getRemovedIndices(); + EXPECT_EQ(int(removed_indices->size()), 7); + EXPECT_VECTOR_DOES_NOT_CONTAIN(pcl::Indices({0, 4}), *removed_indices); + + // Test setNegative + cropBoxFilter.setNegative(true); + cropBoxFilter.filter(cloud_out_negative); + EXPECT_EQ(int(cloud_out_negative.size()), 7); + + cropBoxFilter.filter(indices); + EXPECT_EQ(int(indices.size()), 7); + EXPECT_VECTOR_DOES_NOT_CONTAIN(pcl::Indices({0, 4}), indices); +} + +TEST(ExperimentalPassThrough, Filters) +{ + // Test the PointCloud method + PointCloud output; + experimental::PassThrough pt; + + pt.setInputCloud(cloud); + pt.filter(output); + + EXPECT_EQ(output.size(), cloud->size()); + EXPECT_EQ(output.width, cloud->width); + EXPECT_EQ(output.height, cloud->height); + + pt.setFilterFieldName("z"); + pt.setFilterLimits(0.05f, 0.1f); + pt.filter(output); + + EXPECT_EQ(output.size(), 42); + EXPECT_EQ(output.width, 42); + EXPECT_EQ(output.height, 1); + EXPECT_TRUE(output.is_dense); + + EXPECT_NEAR(output[0].x, -0.074556, 1e-5); + EXPECT_NEAR(output[0].y, 0.13415, 1e-5); + EXPECT_NEAR(output[0].z, 0.051046, 1e-5); + + EXPECT_NEAR(output[41].x, -0.030331, 1e-5); + EXPECT_NEAR(output[41].y, 0.039749, 1e-5); + EXPECT_NEAR(output[41].z, 0.052133, 1e-5); + + pt.setNegative(true); + pt.filter(output); + + EXPECT_EQ(output.size(), 355); + EXPECT_EQ(output.width, 355); + EXPECT_EQ(output.height, 1); + EXPECT_TRUE(output.is_dense); + + EXPECT_NEAR(output[0].x, 0.0054216, 1e-5); + EXPECT_NEAR(output[0].y, 0.11349, 1e-5); + EXPECT_NEAR(output[0].z, 0.040749, 1e-5); + + EXPECT_NEAR(output[354].x, -0.07793, 1e-5); + EXPECT_NEAR(output[354].y, 0.17516, 1e-5); + EXPECT_NEAR(output[354].z, -0.0444, 1e-5); + + experimental::PassThrough pt_(true); + + pt_.setInputCloud(cloud); + pt_.filter(output); + + EXPECT_EQ(pt_.getRemovedIndices()->size(), 0); + EXPECT_EQ(output.size(), cloud->size()); + EXPECT_EQ(output.width, cloud->width); + EXPECT_EQ(output.height, cloud->height); + + pt_.setFilterFieldName("z"); + pt_.setFilterLimits(0.05f, 0.1f); + pt_.filter(output); + + EXPECT_EQ(output.size(), 42); + EXPECT_EQ(output.width, 42); + EXPECT_EQ(output.height, 1); + EXPECT_TRUE(output.is_dense); + EXPECT_EQ(output.size(), cloud->size() - pt_.getRemovedIndices()->size()); + + EXPECT_NEAR(output[0].x, -0.074556, 1e-5); + EXPECT_NEAR(output[0].y, 0.13415, 1e-5); + EXPECT_NEAR(output[0].z, 0.051046, 1e-5); + + EXPECT_NEAR(output[41].x, -0.030331, 1e-5); + EXPECT_NEAR(output[41].y, 0.039749, 1e-5); + EXPECT_NEAR(output[41].z, 0.052133, 1e-5); + + pt_.setNegative(true); + pt_.filter(output); + + EXPECT_EQ(output.size(), 355); + EXPECT_EQ(output.width, 355); + EXPECT_EQ(output.height, 1); + EXPECT_TRUE(output.is_dense); + EXPECT_EQ(output.size(), cloud->size() - pt_.getRemovedIndices()->size()); + + EXPECT_NEAR(output[0].x, 0.0054216, 1e-5); + EXPECT_NEAR(output[0].y, 0.11349, 1e-5); + EXPECT_NEAR(output[0].z, 0.040749, 1e-5); + + EXPECT_NEAR(output[354].x, -0.07793, 1e-5); + EXPECT_NEAR(output[354].y, 0.17516, 1e-5); + EXPECT_NEAR(output[354].z, -0.0444, 1e-5); + + // Test the keep organized structure + pt.setUserFilterValue(std::numeric_limits::quiet_NaN()); + pt.setFilterFieldName(""); + pt.filter(output); + + EXPECT_EQ(output.size(), cloud->size()); + EXPECT_EQ(output.width, cloud->width); + EXPECT_EQ(output.height, cloud->height); + EXPECT_EQ(output.is_dense, cloud->is_dense); + EXPECT_NEAR(output[0].x, (*cloud)[0].x, 1e-5); + EXPECT_NEAR(output[output.size() - 1].x, (*cloud)[cloud->size() - 1].x, 1e-5); + + pt.setFilterFieldName("z"); + pt.setNegative(false); + pt.setKeepOrganized(true); + pt.filter(output); + + EXPECT_EQ(output.size(), cloud->size()); + EXPECT_EQ(output.width, cloud->width); + EXPECT_EQ(output.height, cloud->height); + EXPECT_FALSE(output.is_dense); // NaN was set as a user filter value + + EXPECT_TRUE(std::isnan(output[0].x)); + EXPECT_TRUE(std::isnan(output[0].y)); + EXPECT_TRUE(std::isnan(output[0].z)); + EXPECT_TRUE(std::isnan(output[41].x)); + EXPECT_TRUE(std::isnan(output[41].y)); + EXPECT_TRUE(std::isnan(output[41].z)); + + pt.setNegative(true); + pt.filter(output); + + EXPECT_EQ(output.size(), cloud->size()); + EXPECT_EQ(output.width, cloud->width); + EXPECT_EQ(output.height, cloud->height); + EXPECT_FALSE(output.is_dense); // NaN was set as a user filter value + + EXPECT_NEAR(output[0].x, (*cloud)[0].x, 1e-5); + EXPECT_NEAR(output[0].y, (*cloud)[0].y, 1e-5); + EXPECT_NEAR(output[0].z, (*cloud)[0].z, 1e-5); + + EXPECT_NEAR(output[41].x, (*cloud)[41].x, 1e-5); + EXPECT_NEAR(output[41].y, (*cloud)[41].y, 1e-5); + EXPECT_NEAR(output[41].z, (*cloud)[41].z, 1e-5); +} + +int +main(int argc, char** argv) +{ + if (argc < 2) + { + std::cerr << "No test file given. Please download `bun0.pcd` and pass the path to the test." << std::endl; + return (-1); + } + + io::loadPCDFile(argv[1], *cloud); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}