Point Cloud Library (PCL)  1.9.1-dev
model_outlier_removal.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  */
37 
38 #ifndef PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_
39 #define PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_
40 
41 #include <pcl/filters/model_outlier_removal.h>
42 #include <pcl/common/io.h>
43 #include <pcl/sample_consensus/sac_model_circle.h>
44 #include <pcl/sample_consensus/sac_model_cylinder.h>
45 #include <pcl/sample_consensus/sac_model_cone.h>
46 #include <pcl/sample_consensus/sac_model_line.h>
47 #include <pcl/sample_consensus/sac_model_normal_plane.h>
48 #include <pcl/sample_consensus/sac_model_normal_sphere.h>
49 #include <pcl/sample_consensus/sac_model_parallel_plane.h>
50 #include <pcl/sample_consensus/sac_model_normal_parallel_plane.h>
51 #include <pcl/sample_consensus/sac_model_parallel_line.h>
52 #include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
53 #include <pcl/sample_consensus/sac_model_plane.h>
54 #include <pcl/sample_consensus/sac_model_sphere.h>
55 
56 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
57 template <typename PointT> bool
59 {
60  // Build the model
61  switch (model_type)
62  {
63  case SACMODEL_PLANE:
64  {
65  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPLANE\n", getClassName ().c_str ());
66  model_.reset (new SampleConsensusModelPlane<PointT> (input_));
67  break;
68  }
69  case SACMODEL_LINE:
70  {
71  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelLINE\n", getClassName ().c_str ());
72  model_.reset (new SampleConsensusModelLine<PointT> (input_));
73  break;
74  }
75  case SACMODEL_CIRCLE2D:
76  {
77  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelCIRCLE2D\n", getClassName ().c_str ());
78  model_.reset (new SampleConsensusModelCircle2D<PointT> (input_));
79  break;
80  }
81  case SACMODEL_SPHERE:
82  {
83  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelSPHERE\n", getClassName ().c_str ());
84  model_.reset (new SampleConsensusModelSphere<PointT> (input_));
85  break;
86  }
88  {
89  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPARALLEL_LINE\n", getClassName ().c_str ());
90  model_.reset (new SampleConsensusModelParallelLine<PointT> (input_));
91  break;
92  }
94  {
95  PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPERPENDICULAR_PLANE\n", getClassName ().c_str ());
96  model_.reset (new SampleConsensusModelPerpendicularPlane<PointT> (input_));
97  break;
98  }
99  case SACMODEL_CYLINDER:
100  {
101  PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCYLINDER\n", getClassName ().c_str ());
102  model_.reset (new SampleConsensusModelCylinder<PointT, pcl::Normal> (input_));
103  break;
104  }
106  {
107  PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PLANE\n", getClassName ().c_str ());
108  model_.reset (new SampleConsensusModelNormalPlane<PointT, pcl::Normal> (input_));
109  break;
110  }
111  case SACMODEL_CONE:
112  {
113  PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCONE\n", getClassName ().c_str ());
114  model_.reset (new SampleConsensusModelCone<PointT, pcl::Normal> (input_));
115  break;
116  }
118  {
119  PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_SPHERE\n", getClassName ().c_str ());
120  model_.reset (new SampleConsensusModelNormalSphere<PointT, pcl::Normal> (input_));
121  break;
122  }
124  {
125  PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
126  model_.reset (new SampleConsensusModelNormalParallelPlane<PointT, pcl::Normal> (input_));
127  break;
128  }
130  {
131  PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelPARALLEL_PLANE\n", getClassName ().c_str ());
132  model_.reset (new SampleConsensusModelParallelPlane<PointT> (input_));
133  break;
134  }
135  default:
136  {
137  PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
138  return (false);
139  }
140  }
141  return (true);
142 }
143 
144 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
145 template <typename PointT> void
147 {
148  std::vector<int> indices;
149  if (keep_organized_)
150  {
151  bool temp = extract_removed_indices_;
152  extract_removed_indices_ = true;
153  applyFilterIndices (indices);
154  extract_removed_indices_ = temp;
155 
156  output = *input_;
157  for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii) // rii = removed indices iterator
158  output.points[ (*removed_indices_)[rii]].x = output.points[ (*removed_indices_)[rii]].y = output.points[ (*removed_indices_)[rii]].z = user_filter_value_;
159  if (!std::isfinite (user_filter_value_))
160  output.is_dense = false;
161  }
162  else
163  {
164  applyFilterIndices (indices);
165  copyPointCloud (*input_, indices, output);
166  }
167 }
168 
169 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
170 template <typename PointT> void
172 {
173  //The arrays to be used
174  indices.resize (indices_->size ());
175  removed_indices_->resize (indices_->size ());
176  int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator
177  //is the filtersetup correct?
178  bool valid_setup = true;
179 
180  valid_setup &= initSACModel (model_type_);
181 
182  using SACModelFromNormals = SampleConsensusModelFromNormals<PointT, pcl::Normal>;
183  // Returns NULL if cast isn't possible
184  SACModelFromNormals *model_from_normals = dynamic_cast<SACModelFromNormals *> (& (*model_));
185 
186  if (model_from_normals)
187  {
188  if (!cloud_normals_)
189  {
190  valid_setup = false;
191  PCL_ERROR ("[pcl::ModelOutlierRemoval::applyFilterIndices]: no normals cloud set.\n");
192  }
193  else
194  {
195  model_from_normals->setNormalDistanceWeight (normals_distance_weight_);
196  model_from_normals->setInputNormals (cloud_normals_);
197  }
198  }
199 
200  //if the filter setup is invalid filter for nan and return;
201  if (!valid_setup)
202  {
203  for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator
204  {
205  // Non-finite entries are always passed to removed indices
206  if (!isFinite (input_->points[ (*indices_)[iii]]))
207  {
208  if (extract_removed_indices_)
209  (*removed_indices_)[rii++] = (*indices_)[iii];
210  continue;
211  }
212  indices[oii++] = (*indices_)[iii];
213  }
214  return;
215  }
216  // check distance of pointcloud to model
217  std::vector<double> distances;
218  //TODO: get signed distances !
219  model_->setIndices(indices_); // added to reduce computation and arrange distances with indices
220  model_->getDistancesToModel (model_coefficients_, distances);
221 
222  bool thresh_result;
223 
224  // Filter for non-finite entries and the specified field limits
225  for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator
226  {
227  // Non-finite entries are always passed to removed indices
228  if (!isFinite (input_->points[ (*indices_)[iii]]))
229  {
230  if (extract_removed_indices_)
231  (*removed_indices_)[rii++] = (*indices_)[iii];
232  continue;
233  }
234 
235  // use threshold function to separate outliers from inliers:
236  thresh_result = threshold_function_ (distances[iii]);
237 
238  // in normal mode: define outliers as false thresh_result
239  if (!negative_ && !thresh_result)
240  {
241  if (extract_removed_indices_)
242  (*removed_indices_)[rii++] = (*indices_)[iii];
243  continue;
244  }
245 
246  // in negative_ mode: define outliers as true thresh_result
247  if (negative_ && thresh_result)
248  {
249  if (extract_removed_indices_)
250  (*removed_indices_)[rii++] = (*indices_)[iii];
251  continue;
252  }
253 
254  // Otherwise it was a normal point for output (inlier)
255  indices[oii++] = (*indices_)[iii];
256 
257  }
258 
259  // Resize the output arrays
260  indices.resize (oii);
261  removed_indices_->resize (rii);
262 
263 }
264 
265 #define PCL_INSTANTIATE_ModelOutlierRemoval(T) template class PCL_EXPORTS pcl::ModelOutlierRemoval<T>;
266 
267 #endif // PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:53
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
ModelOutlierRemoval filters points in a cloud based on the distance between model and point...
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
Definition: sac_model.h:578
PointCloud represents the base class in PCL for storing collections of 3D points. ...
SacModel
Definition: model_types.h:45
void applyFilterIndices(std::vector< int > &indices)
Filtered results are indexed by an indices array.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:419
void applyFilter(PointCloud &output) override
Filtered results are stored in a separate point cloud.