Point Cloud Library (PCL)  1.11.1-dev
morphological_filter.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2012, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  * Copyright (c) 2014, RadiantBlue Technologies, Inc.
8  *
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * * Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above
18  * copyright notice, this list of conditions and the following
19  * disclaimer in the documentation and/or other materials provided
20  * with the distribution.
21  * * Neither the name of the copyright holder(s) nor the names of its
22  * contributors may be used to endorse or promote products derived
23  * from this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  *
38  * $Id$
39  *
40  */
41 
42 #ifndef PCL_FILTERS_IMPL_MORPHOLOGICAL_FILTER_H_
43 #define PCL_FILTERS_IMPL_MORPHOLOGICAL_FILTER_H_
44 
45 #include <limits>
46 
47 #include <Eigen/Core>
48 
49 #include <pcl/common/common.h>
50 #include <pcl/common/io.h>
51 #include <pcl/filters/morphological_filter.h>
52 #include <pcl/octree/octree_search.h>
53 
54 namespace pcl
55 {
56 template <typename PointT> void
57 applyMorphologicalOperator (const typename pcl::PointCloud<PointT>::ConstPtr &cloud_in,
58  float resolution, const int morphological_operator,
59  pcl::PointCloud<PointT> &cloud_out)
60 {
61  if (cloud_in->empty ())
62  return;
63 
64  pcl::copyPointCloud (*cloud_in, cloud_out);
65 
66  pcl::octree::OctreePointCloudSearch<PointT> tree (resolution);
67 
68  tree.setInputCloud (cloud_in);
69  tree.addPointsFromInputCloud ();
70 
71  float half_res = resolution / 2.0f;
72 
73  switch (morphological_operator)
74  {
75  case MORPH_DILATE:
76  case MORPH_ERODE:
77  {
78  for (std::size_t p_idx = 0; p_idx < cloud_in->size (); ++p_idx)
79  {
80  Eigen::Vector3f bbox_min, bbox_max;
81  Indices pt_indices;
82  float minx = (*cloud_in)[p_idx].x - half_res;
83  float miny = (*cloud_in)[p_idx].y - half_res;
84  float minz = -std::numeric_limits<float>::max ();
85  float maxx = (*cloud_in)[p_idx].x + half_res;
86  float maxy = (*cloud_in)[p_idx].y + half_res;
87  float maxz = std::numeric_limits<float>::max ();
88  bbox_min = Eigen::Vector3f (minx, miny, minz);
89  bbox_max = Eigen::Vector3f (maxx, maxy, maxz);
90  tree.boxSearch (bbox_min, bbox_max, pt_indices);
91 
92  if (!pt_indices.empty ())
93  {
94  Eigen::Vector4f min_pt, max_pt;
95  pcl::getMinMax3D<PointT> (*cloud_in, pt_indices, min_pt, max_pt);
96 
97  switch (morphological_operator)
98  {
99  case MORPH_DILATE:
100  {
101  cloud_out[p_idx].z = max_pt.z ();
102  break;
103  }
104  case MORPH_ERODE:
105  {
106  cloud_out[p_idx].z = min_pt.z ();
107  break;
108  }
109  }
110  }
111  }
112  break;
113  }
114  case MORPH_OPEN:
115  case MORPH_CLOSE:
116  {
117  pcl::PointCloud<PointT> cloud_temp;
118 
119  pcl::copyPointCloud (*cloud_in, cloud_temp);
120 
121  for (std::size_t p_idx = 0; p_idx < cloud_temp.size (); ++p_idx)
122  {
123  Eigen::Vector3f bbox_min, bbox_max;
124  Indices pt_indices;
125  float minx = cloud_temp[p_idx].x - half_res;
126  float miny = cloud_temp[p_idx].y - half_res;
127  float minz = -std::numeric_limits<float>::max ();
128  float maxx = cloud_temp[p_idx].x + half_res;
129  float maxy = cloud_temp[p_idx].y + half_res;
130  float maxz = std::numeric_limits<float>::max ();
131  bbox_min = Eigen::Vector3f (minx, miny, minz);
132  bbox_max = Eigen::Vector3f (maxx, maxy, maxz);
133  tree.boxSearch (bbox_min, bbox_max, pt_indices);
134 
135  if (!pt_indices.empty ())
136  {
137  Eigen::Vector4f min_pt, max_pt;
138  pcl::getMinMax3D<PointT> (cloud_temp, pt_indices, min_pt, max_pt);
139 
140  switch (morphological_operator)
141  {
142  case MORPH_OPEN:
143  {
144  cloud_out[p_idx].z = min_pt.z ();
145  break;
146  }
147  case MORPH_CLOSE:
148  {
149  cloud_out[p_idx].z = max_pt.z ();
150  break;
151  }
152  }
153  }
154  }
155 
156  cloud_temp.swap (cloud_out);
157 
158  for (std::size_t p_idx = 0; p_idx < cloud_temp.size (); ++p_idx)
159  {
160  Eigen::Vector3f bbox_min, bbox_max;
161  Indices pt_indices;
162  float minx = cloud_temp[p_idx].x - half_res;
163  float miny = cloud_temp[p_idx].y - half_res;
164  float minz = -std::numeric_limits<float>::max ();
165  float maxx = cloud_temp[p_idx].x + half_res;
166  float maxy = cloud_temp[p_idx].y + half_res;
167  float maxz = std::numeric_limits<float>::max ();
168  bbox_min = Eigen::Vector3f (minx, miny, minz);
169  bbox_max = Eigen::Vector3f (maxx, maxy, maxz);
170  tree.boxSearch (bbox_min, bbox_max, pt_indices);
171 
172  if (!pt_indices.empty ())
173  {
174  Eigen::Vector4f min_pt, max_pt;
175  pcl::getMinMax3D<PointT> (cloud_temp, pt_indices, min_pt, max_pt);
176 
177  switch (morphological_operator)
178  {
179  case MORPH_OPEN:
180  default:
181  {
182  cloud_out[p_idx].z = max_pt.z ();
183  break;
184  }
185  case MORPH_CLOSE:
186  {
187  cloud_out[p_idx].z = min_pt.z ();
188  break;
189  }
190  }
191  }
192  }
193  break;
194  }
195  default:
196  {
197  PCL_ERROR ("Morphological operator is not supported!\n");
198  break;
199  }
200  }
201 
202  return;
203 }
204 
205 } // namespace pcl
206 
207 #define PCL_INSTANTIATE_applyMorphologicalOperator(T) template PCL_EXPORTS void pcl::applyMorphologicalOperator<T> (const pcl::PointCloud<T>::ConstPtr &, float, const int, pcl::PointCloud<T> &);
208 
209 #endif //#ifndef PCL_FILTERS_IMPL_MORPHOLOGICAL_FILTER_H_
pcl::PointCloud::swap
void swap(PointCloud< PointT > &rhs)
Swap a point cloud with another cloud.
Definition: point_cloud.h:855
pcl
Definition: convolution.h:46
pcl::MORPH_CLOSE
@ MORPH_CLOSE
Definition: morphological_filter.h:52
common.h
pcl::PointCloud::empty
bool empty() const
Definition: point_cloud.h:440
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::addPointsFromInputCloud
void addPointsFromInputCloud()
Add points from input point cloud to octree.
Definition: octree_pointcloud.hpp:79
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::MORPH_DILATE
@ MORPH_DILATE
Definition: morphological_filter.h:53
pcl::copyPointCloud
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
Definition: io.hpp:122
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud_arg, const IndicesConstPtr &indices_arg=IndicesConstPtr())
Provide a pointer to the input data set.
Definition: octree_pointcloud.h:117
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:131
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:437
pcl::MORPH_OPEN
@ MORPH_OPEN
Definition: morphological_filter.h:51
pcl::applyMorphologicalOperator
void applyMorphologicalOperator(const typename pcl::PointCloud< PointT >::ConstPtr &cloud_in, float resolution, const int morphological_operator, pcl::PointCloud< PointT > &cloud_out)
Apply morphological operator to the z dimension of the input point cloud.
Definition: morphological_filter.hpp:57
pcl::MORPH_ERODE
@ MORPH_ERODE
Definition: morphological_filter.h:54
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:408
pcl::octree::OctreePointCloudSearch::boxSearch
uindex_t boxSearch(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, Indices &k_indices) const
Search for points within rectangular search area Points exactly on the edges of the search rectangle ...
Definition: octree_search.hpp:205
pcl::octree::OctreePointCloudSearch
Octree pointcloud search class
Definition: octree_search.h:57