Point Cloud Library (PCL)  1.11.0-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 #include <vector>
47 
48 #include <Eigen/Core>
49 
50 #include <pcl/common/common.h>
51 #include <pcl/common/io.h>
52 #include <pcl/filters/morphological_filter.h>
53 #include <pcl/octree/octree_search.h>
54 
55 namespace pcl
56 {
57 template <typename PointT> void
59  float resolution, const int morphological_operator,
60  pcl::PointCloud<PointT> &cloud_out)
61 {
62  if (cloud_in->empty ())
63  return;
64 
65  pcl::copyPointCloud (*cloud_in, cloud_out);
66 
68 
69  tree.setInputCloud (cloud_in);
71 
72  float half_res = resolution / 2.0f;
73 
74  switch (morphological_operator)
75  {
76  case MORPH_DILATE:
77  case MORPH_ERODE:
78  {
79  for (std::size_t p_idx = 0; p_idx < cloud_in->points.size (); ++p_idx)
80  {
81  Eigen::Vector3f bbox_min, bbox_max;
82  std::vector<int> pt_indices;
83  float minx = cloud_in->points[p_idx].x - half_res;
84  float miny = cloud_in->points[p_idx].y - half_res;
85  float minz = -std::numeric_limits<float>::max ();
86  float maxx = cloud_in->points[p_idx].x + half_res;
87  float maxy = cloud_in->points[p_idx].y + half_res;
88  float maxz = std::numeric_limits<float>::max ();
89  bbox_min = Eigen::Vector3f (minx, miny, minz);
90  bbox_max = Eigen::Vector3f (maxx, maxy, maxz);
91  tree.boxSearch (bbox_min, bbox_max, pt_indices);
92 
93  if (!pt_indices.empty ())
94  {
95  Eigen::Vector4f min_pt, max_pt;
96  pcl::getMinMax3D<PointT> (*cloud_in, pt_indices, min_pt, max_pt);
97 
98  switch (morphological_operator)
99  {
100  case MORPH_DILATE:
101  {
102  cloud_out.points[p_idx].z = max_pt.z ();
103  break;
104  }
105  case MORPH_ERODE:
106  {
107  cloud_out.points[p_idx].z = min_pt.z ();
108  break;
109  }
110  }
111  }
112  }
113  break;
114  }
115  case MORPH_OPEN:
116  case MORPH_CLOSE:
117  {
118  pcl::PointCloud<PointT> cloud_temp;
119 
120  pcl::copyPointCloud (*cloud_in, cloud_temp);
121 
122  for (std::size_t p_idx = 0; p_idx < cloud_temp.points.size (); ++p_idx)
123  {
124  Eigen::Vector3f bbox_min, bbox_max;
125  std::vector<int> pt_indices;
126  float minx = cloud_temp.points[p_idx].x - half_res;
127  float miny = cloud_temp.points[p_idx].y - half_res;
128  float minz = -std::numeric_limits<float>::max ();
129  float maxx = cloud_temp.points[p_idx].x + half_res;
130  float maxy = cloud_temp.points[p_idx].y + half_res;
131  float maxz = std::numeric_limits<float>::max ();
132  bbox_min = Eigen::Vector3f (minx, miny, minz);
133  bbox_max = Eigen::Vector3f (maxx, maxy, maxz);
134  tree.boxSearch (bbox_min, bbox_max, pt_indices);
135 
136  if (!pt_indices.empty ())
137  {
138  Eigen::Vector4f min_pt, max_pt;
139  pcl::getMinMax3D<PointT> (cloud_temp, pt_indices, min_pt, max_pt);
140 
141  switch (morphological_operator)
142  {
143  case MORPH_OPEN:
144  {
145  cloud_out.points[p_idx].z = min_pt.z ();
146  break;
147  }
148  case MORPH_CLOSE:
149  {
150  cloud_out.points[p_idx].z = max_pt.z ();
151  break;
152  }
153  }
154  }
155  }
156 
157  cloud_temp.swap (cloud_out);
158 
159  for (std::size_t p_idx = 0; p_idx < cloud_temp.points.size (); ++p_idx)
160  {
161  Eigen::Vector3f bbox_min, bbox_max;
162  std::vector<int> pt_indices;
163  float minx = cloud_temp.points[p_idx].x - half_res;
164  float miny = cloud_temp.points[p_idx].y - half_res;
165  float minz = -std::numeric_limits<float>::max ();
166  float maxx = cloud_temp.points[p_idx].x + half_res;
167  float maxy = cloud_temp.points[p_idx].y + half_res;
168  float maxz = std::numeric_limits<float>::max ();
169  bbox_min = Eigen::Vector3f (minx, miny, minz);
170  bbox_max = Eigen::Vector3f (maxx, maxy, maxz);
171  tree.boxSearch (bbox_min, bbox_max, pt_indices);
172 
173  if (!pt_indices.empty ())
174  {
175  Eigen::Vector4f min_pt, max_pt;
176  pcl::getMinMax3D<PointT> (cloud_temp, pt_indices, min_pt, max_pt);
177 
178  switch (morphological_operator)
179  {
180  case MORPH_OPEN:
181  default:
182  {
183  cloud_out.points[p_idx].z = max_pt.z ();
184  break;
185  }
186  case MORPH_CLOSE:
187  {
188  cloud_out.points[p_idx].z = min_pt.z ();
189  break;
190  }
191  }
192  }
193  }
194  break;
195  }
196  default:
197  {
198  PCL_ERROR ("Morphological operator is not supported!\n");
199  break;
200  }
201  }
202 
203  return;
204 }
205 
206 } // namespace pcl
207 
208 #define PCL_INSTANTIATE_applyMorphologicalOperator(T) template PCL_EXPORTS void pcl::applyMorphologicalOperator<T> (const pcl::PointCloud<T>::ConstPtr &, float, const int, pcl::PointCloud<T> &);
209 
210 #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:603
pcl
Definition: convolution.h:46
pcl::MORPH_CLOSE
@ MORPH_CLOSE
Definition: morphological_filter.h:55
common.h
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
pcl::PointCloud::empty
bool empty() const
Definition: point_cloud.h:461
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndices, OctreeContainerEmpty >::addPointsFromInputCloud
void addPointsFromInputCloud()
Add points from input point cloud to octree.
Definition: octree_pointcloud.hpp:78
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:53
pcl::MORPH_DILATE
@ MORPH_DILATE
Definition: morphological_filter.h:56
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:121
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::MORPH_OPEN
@ MORPH_OPEN
Definition: morphological_filter.h:54
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:58
pcl::MORPH_ERODE
@ MORPH_ERODE
Definition: morphological_filter.h:57
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
pcl::octree::OctreePointCloudSearch
Octree pointcloud search class
Definition: octree_search.h:57
pcl::octree::OctreePointCloudSearch::boxSearch
int boxSearch(const Eigen::Vector3f &min_pt, const Eigen::Vector3f &max_pt, std::vector< int > &k_indices) const
Search for points within rectangular search area Points exactly on the edges of the search rectangle ...
Definition: octree_search.hpp:205