Point Cloud Library (PCL) 1.12.0
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
54namespace pcl
55{
56template <typename PointT> void
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
67
68 tree.setInputCloud (cloud_in);
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_
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
bool empty() const
Definition: point_cloud.h:446
std::size_t size() const
Definition: point_cloud.h:443
void swap(PointCloud< PointT > &rhs)
Swap a point cloud with another cloud.
Definition: point_cloud.h:861
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
void setInputCloud(const PointCloudConstPtr &cloud_arg, const IndicesConstPtr &indices_arg=IndicesConstPtr())
Provide a pointer to the input data set.
void addPointsFromInputCloud()
Add points from input point cloud to octree.
Octree pointcloud search class
Definition: octree_search.h:58
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 ...
Define standard C methods and C++ classes that are common to all methods.
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:144
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.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133