From 3fda1735f4e9f63b73fb22f0ab41c9dc6afd7c24 Mon Sep 17 00:00:00 2001 From: Bradley J Chambers Date: Thu, 27 Feb 2014 01:42:05 +0000 Subject: [PATCH] Add: morphological filters operate on the z dimension (replaces #521) --- filters/CMakeLists.txt | 3 + .../pcl/filters/impl/morphological_filter.hpp | 208 ++++++++++++++++++ .../pcl/filters/morphological_filter.h | 82 +++++++ filters/src/morphological_filter.cpp | 52 +++++ test/filters/CMakeLists.txt | 4 + test/filters/test_morphological.cpp | 169 ++++++++++++++ 6 files changed, 518 insertions(+) create mode 100644 filters/include/pcl/filters/impl/morphological_filter.hpp create mode 100644 filters/include/pcl/filters/morphological_filter.h create mode 100644 filters/src/morphological_filter.cpp create mode 100644 test/filters/test_morphological.cpp diff --git a/filters/CMakeLists.txt b/filters/CMakeLists.txt index e35e91a73f9..4e49f241ed0 100644 --- a/filters/CMakeLists.txt +++ b/filters/CMakeLists.txt @@ -37,6 +37,7 @@ if(build) src/voxel_grid_occlusion_estimation.cpp src/normal_refinement.cpp src/grid_minimum.cpp + src/morphological_filter.cpp ) set(incs @@ -73,6 +74,7 @@ if(build) "include/pcl/${SUBSYS_NAME}/median_filter.h" "include/pcl/${SUBSYS_NAME}/normal_refinement.h" "include/pcl/${SUBSYS_NAME}/grid_minimum.h" + "include/pcl/${SUBSYS_NAME}/morphological_filter.h" ) set(impl_incs @@ -106,6 +108,7 @@ if(build) "include/pcl/${SUBSYS_NAME}/impl/median_filter.hpp" "include/pcl/${SUBSYS_NAME}/impl/normal_refinement.hpp" "include/pcl/${SUBSYS_NAME}/impl/grid_minimum.hpp" + "include/pcl/${SUBSYS_NAME}/impl/morphological_filter.hpp" ) set(LIB_NAME "pcl_${SUBSYS_NAME}") diff --git a/filters/include/pcl/filters/impl/morphological_filter.hpp b/filters/include/pcl/filters/impl/morphological_filter.hpp new file mode 100644 index 00000000000..59a0a965f8e --- /dev/null +++ b/filters/include/pcl/filters/impl/morphological_filter.hpp @@ -0,0 +1,208 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * 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$ + * + */ + +#ifndef PCL_FILTERS_IMPL_MORPHOLOGICAL_FILTER_H_ +#define PCL_FILTERS_IMPL_MORPHOLOGICAL_FILTER_H_ + +#include +#include + +#include + +#include +#include +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::applyMorphologicalOperator (const typename pcl::PointCloud::ConstPtr &cloud_in, + float resolution, const int morphological_operator, + pcl::PointCloud &cloud_out) +{ + if (cloud_in->empty ()) + return; + + pcl::copyPointCloud (*cloud_in, cloud_out); + + pcl::octree::OctreePointCloudSearch tree (resolution); + + tree.setInputCloud (cloud_in); + tree.addPointsFromInputCloud (); + + float half_res = resolution / 2.0f; + + switch (morphological_operator) + { + case MORPH_DILATE: + case MORPH_ERODE: + { + for (size_t p_idx = 0; p_idx < cloud_in->points.size (); ++p_idx) + { + Eigen::Vector3f bbox_min, bbox_max; + std::vector pt_indices; + float minx = cloud_in->points[p_idx].x - half_res; + float miny = cloud_in->points[p_idx].y - half_res; + float minz = -std::numeric_limits::max (); + float maxx = cloud_in->points[p_idx].x + half_res; + float maxy = cloud_in->points[p_idx].y + half_res; + float maxz = std::numeric_limits::max (); + bbox_min = Eigen::Vector3f (minx, miny, minz); + bbox_max = Eigen::Vector3f (maxx, maxy, maxz); + tree.boxSearch (bbox_min, bbox_max, pt_indices); + + if (pt_indices.size () > 0) + { + Eigen::Vector4f min_pt, max_pt; + pcl::getMinMax3D (*cloud_in, pt_indices, min_pt, max_pt); + + switch (morphological_operator) + { + case MORPH_DILATE: + { + cloud_out.points[p_idx].z = max_pt.z (); + break; + } + case MORPH_ERODE: + { + cloud_out.points[p_idx].z = min_pt.z (); + break; + } + } + } + } + break; + } + case MORPH_OPEN: + case MORPH_CLOSE: + { + pcl::PointCloud cloud_temp; + + pcl::copyPointCloud (*cloud_in, cloud_temp); + + for (size_t p_idx = 0; p_idx < cloud_temp.points.size (); ++p_idx) + { + Eigen::Vector3f bbox_min, bbox_max; + std::vector pt_indices; + float minx = cloud_temp.points[p_idx].x - half_res; + float miny = cloud_temp.points[p_idx].y - half_res; + float minz = -std::numeric_limits::max (); + float maxx = cloud_temp.points[p_idx].x + half_res; + float maxy = cloud_temp.points[p_idx].y + half_res; + float maxz = std::numeric_limits::max (); + bbox_min = Eigen::Vector3f (minx, miny, minz); + bbox_max = Eigen::Vector3f (maxx, maxy, maxz); + tree.boxSearch (bbox_min, bbox_max, pt_indices); + + if (pt_indices.size () > 0) + { + Eigen::Vector4f min_pt, max_pt; + pcl::getMinMax3D (cloud_temp, pt_indices, min_pt, max_pt); + + switch (morphological_operator) + { + case MORPH_OPEN: + { + cloud_out.points[p_idx].z = min_pt.z (); + break; + } + case MORPH_CLOSE: + { + cloud_out.points[p_idx].z = max_pt.z (); + break; + } + } + } + } + + cloud_temp.swap (cloud_out); + + for (size_t p_idx = 0; p_idx < cloud_temp.points.size (); ++p_idx) + { + Eigen::Vector3f bbox_min, bbox_max; + std::vector pt_indices; + float minx = cloud_temp.points[p_idx].x - half_res; + float miny = cloud_temp.points[p_idx].y - half_res; + float minz = -std::numeric_limits::max (); + float maxx = cloud_temp.points[p_idx].x + half_res; + float maxy = cloud_temp.points[p_idx].y + half_res; + float maxz = std::numeric_limits::max (); + bbox_min = Eigen::Vector3f (minx, miny, minz); + bbox_max = Eigen::Vector3f (maxx, maxy, maxz); + tree.boxSearch (bbox_min, bbox_max, pt_indices); + + if (pt_indices.size () > 0) + { + Eigen::Vector4f min_pt, max_pt; + pcl::getMinMax3D (cloud_temp, pt_indices, min_pt, max_pt); + + switch (morphological_operator) + { + case MORPH_OPEN: + default: + { + cloud_out.points[p_idx].z = max_pt.z (); + break; + } + case MORPH_CLOSE: + { + cloud_out.points[p_idx].z = min_pt.z (); + break; + } + } + } + } + break; + } + default: + { + PCL_ERROR ("Morphological operator is not supported!\n"); + break; + } + } + + return; +} + +#define PCL_INSTANTIATE_applyMorphologicalOperator(T) template PCL_EXPORTS void pcl::applyMorphologicalOperator (const pcl::PointCloud::ConstPtr &, float, const int, pcl::PointCloud &); + +#endif //#ifndef PCL_FILTERS_IMPL_MORPHOLOGICAL_FILTER_H_ + diff --git a/filters/include/pcl/filters/morphological_filter.h b/filters/include/pcl/filters/morphological_filter.h new file mode 100644 index 00000000000..4ea8dda338f --- /dev/null +++ b/filters/include/pcl/filters/morphological_filter.h @@ -0,0 +1,82 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * 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$ + * + */ + +#ifndef PCL_FILTERS_MORPHOLOGICAL_FILTER_H_ +#define PCL_FILTERS_MORPHOLOGICAL_FILTER_H_ + +#include +#include +#include +#include +#include + +namespace pcl +{ + enum MorphologicalOperators + { + MORPH_OPEN, + MORPH_CLOSE, + MORPH_DILATE, + MORPH_ERODE + }; +} + +namespace pcl +{ + /** \brief Apply morphological operator to the z dimension of the input point cloud + * \param[in] cloud_in the input point cloud dataset + * \param[in] resolution the window size to be used for the morphological operation + * \param[in] morphological_operator the morphological operator to apply (open, close, dilate, erode) + * \param[out] cloud_out the resultant output point cloud dataset + * \ingroup filters + */ + template PCL_EXPORTS void + applyMorphologicalOperator (const typename pcl::PointCloud::ConstPtr &cloud_in, + float resolution, const int morphological_operator, + pcl::PointCloud &cloud_out); +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif //#ifndef PCL_FILTERS_MORPHOLOGICAL_FILTER_H_ + diff --git a/filters/src/morphological_filter.cpp b/filters/src/morphological_filter.cpp new file mode 100644 index 00000000000..8985881d61d --- /dev/null +++ b/filters/src/morphological_filter.cpp @@ -0,0 +1,52 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * 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$ + * + */ + +#include + +#ifndef PCL_NO_PRECOMPILE +#include +#include + +// Instantiations of specific point types +PCL_INSTANTIATE(applyMorphologicalOperator, PCL_XYZ_POINT_TYPES) + +#endif // PCL_NO_PRECOMPILE + diff --git a/test/filters/CMakeLists.txt b/test/filters/CMakeLists.txt index 4d414cd0476..6bae7778b90 100644 --- a/test/filters/CMakeLists.txt +++ b/test/filters/CMakeLists.txt @@ -18,5 +18,9 @@ PCL_ADD_TEST(filters_bilateral test_filters_bilateral PCL_ADD_TEST(filters_grid_minimum test_filters_grid_minimum FILES test_grid_minimum.cpp LINK_WITH pcl_gtest pcl_common pcl_filters) + +PCL_ADD_TEST(filters_morphological test_morphological + FILES test_morphological.cpp + LINK_WITH pcl_gtest pcl_common pcl_filters) diff --git a/test/filters/test_morphological.cpp b/test/filters/test_morphological.cpp new file mode 100644 index 00000000000..ee56eca210b --- /dev/null +++ b/test/filters/test_morphological.cpp @@ -0,0 +1,169 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, RadiantBlue Technologies, Inc. + * + * 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$ + * + */ + +#include +#include +#include + +using namespace pcl; + +PointCloud cloud; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (Morphological, Dilate) +{ + PointCloud cloud_in, cloud_out; + + cloud_in.height = 1; + cloud_in.width = 2; + cloud_in.is_dense = true; + cloud_in.resize (2); + + cloud_in[0].x = 0; cloud_in[0].y = 0; cloud_in[0].z = 0; + cloud_in[1].x = 1; cloud_in[1].y = 1; cloud_in[1].z = 1; + + float resolution = 5.0f; + + applyMorphologicalOperator (cloud_in.makeShared (), resolution, MORPH_DILATE, cloud_out); + + EXPECT_EQ (cloud_out[0].z, 1.0f); + EXPECT_EQ (cloud_out[1].z, 1.0f); + EXPECT_EQ (cloud_in.size(), cloud_out.size()); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (Morphological, Erode) +{ + PointCloud cloud_in, cloud_out; + + cloud_in.height = 1; + cloud_in.width = 2; + cloud_in.is_dense = true; + cloud_in.resize (2); + + cloud_in[0].x = 0; cloud_in[0].y = 0; cloud_in[0].z = 0; + cloud_in[1].x = 1; cloud_in[1].y = 1; cloud_in[1].z = 1; + + float resolution = 5.0f; + + applyMorphologicalOperator (cloud_in.makeShared (), resolution, MORPH_ERODE, cloud_out); + + EXPECT_EQ (cloud_out[0].z, 0.0f); + EXPECT_EQ (cloud_out[1].z, 0.0f); + EXPECT_EQ (cloud_in.size(), cloud_out.size()); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (Morphological, Open) +{ + PointCloud cloud_in, cloud_out; + + cloud_in.height = 1; + cloud_in.width = 2; + cloud_in.is_dense = true; + cloud_in.resize (2); + + cloud_in[0].x = 0; cloud_in[0].y = 0; cloud_in[0].z = 0; + cloud_in[1].x = 1; cloud_in[1].y = 1; cloud_in[1].z = 1; + + float resolution = 5.0f; + + applyMorphologicalOperator (cloud_in.makeShared (), resolution, MORPH_OPEN, cloud_out); + + EXPECT_EQ (cloud_out[0].z, 0.0f); + EXPECT_EQ (cloud_out[1].z, 0.0f); + EXPECT_EQ (cloud_in.size (), cloud_out.size ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (Morphological, Close) +{ + PointCloud cloud_in, cloud_out; + + cloud_in.height = 1; + cloud_in.width = 2; + cloud_in.is_dense = true; + cloud_in.resize (2); + + cloud_in[0].x = 0; cloud_in[0].y = 0; cloud_in[0].z = 0; + cloud_in[1].x = 1; cloud_in[1].y = 1; cloud_in[1].z = 1; + + float resolution = 5.0f; + + applyMorphologicalOperator (cloud_in.makeShared (), resolution, MORPH_CLOSE, cloud_out); + + EXPECT_EQ (cloud_out[0].z, 1.0f); + EXPECT_EQ (cloud_out[1].z, 1.0f); + EXPECT_EQ (cloud_in.size (), cloud_out.size ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (Morphological, Unsupported) +{ + PointCloud cloud_in, cloud_out; + + cloud_in.height = 1; + cloud_in.width = 2; + cloud_in.is_dense = true; + cloud_in.resize (2); + + cloud_in[0].x = 0; cloud_in[0].y = 0; cloud_in[0].z = 0; + cloud_in[1].x = 1; cloud_in[1].y = 1; cloud_in[1].z = 1; + + float resolution = 5.0f; + + applyMorphologicalOperator (cloud_in.makeShared (), resolution, 99, cloud_out); + + EXPECT_EQ (cloud_out[0].z, 0.0f); + EXPECT_EQ (cloud_out[1].z, 1.0f); + EXPECT_EQ (cloud_in.size (), cloud_out.size ()); +} + + +/* ---[ */ +int +main (int argc, char** argv) +{ + testing::InitGoogleTest (&argc, argv); + return (RUN_ALL_TESTS ()); +} +/* ]--- */ +