38 #ifndef PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_ 39 #define PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_ 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> 57 template <
typename Po
intT>
bool 65 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: modelPLANE\n", getClassName ().c_str ());
66 model_.reset (
new SampleConsensusModelPlane<PointT> (input_));
71 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: modelLINE\n", getClassName ().c_str ());
72 model_.reset (
new SampleConsensusModelLine<PointT> (input_));
77 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: modelCIRCLE2D\n", getClassName ().c_str ());
78 model_.reset (
new SampleConsensusModelCircle2D<PointT> (input_));
83 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: modelSPHERE\n", getClassName ().c_str ());
84 model_.reset (
new SampleConsensusModelSphere<PointT> (input_));
89 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: modelPARALLEL_LINE\n", getClassName ().c_str ());
90 model_.reset (
new SampleConsensusModelParallelLine<PointT> (input_));
95 PCL_DEBUG (
"[pcl::%s::initSACModel] Using a model of type: modelPERPENDICULAR_PLANE\n", getClassName ().c_str ());
96 model_.reset (
new SampleConsensusModelPerpendicularPlane<PointT> (input_));
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_));
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_));
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_));
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_));
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_));
131 PCL_DEBUG (
"[pcl::%s::segment] Using a model of type: modelPARALLEL_PLANE\n", getClassName ().c_str ());
132 model_.reset (
new SampleConsensusModelParallelPlane<PointT> (input_));
137 PCL_ERROR (
"[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
145 template <
typename Po
intT>
void 148 std::vector<int> indices;
151 bool temp = extract_removed_indices_;
152 extract_removed_indices_ =
true;
153 applyFilterIndices (indices);
154 extract_removed_indices_ = temp;
157 for (
int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii)
158 output.
points[ (*removed_indices_)[rii]].x = output.
points[ (*removed_indices_)[rii]].y = output.
points[ (*removed_indices_)[rii]].z = user_filter_value_;
159 if (!pcl_isfinite (user_filter_value_))
164 applyFilterIndices (indices);
170 template <
typename Po
intT>
void 174 indices.resize (indices_->size ());
175 removed_indices_->resize (indices_->size ());
176 int oii = 0, rii = 0;
178 bool valid_setup =
true;
180 valid_setup &= initSACModel (model_type_);
184 SACModelFromNormals *model_from_normals =
dynamic_cast<SACModelFromNormals *
> (& (*model_));
186 if (model_from_normals)
191 PCL_ERROR (
"[pcl::ModelOutlierRemoval::applyFilterIndices]: no normals cloud set.\n");
195 model_from_normals->setNormalDistanceWeight (normals_distance_weight_);
196 model_from_normals->setInputNormals (cloud_normals_);
203 for (
int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
206 if (!
isFinite (input_->points[ (*indices_)[iii]]))
208 if (extract_removed_indices_)
209 (*removed_indices_)[rii++] = (*indices_)[iii];
212 indices[oii++] = (*indices_)[iii];
217 std::vector<double> distances;
219 model_->setIndices(indices_);
220 model_->getDistancesToModel (model_coefficients_, distances);
225 for (
int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
228 if (!
isFinite (input_->points[ (*indices_)[iii]]))
230 if (extract_removed_indices_)
231 (*removed_indices_)[rii++] = (*indices_)[iii];
236 thresh_result = threshold_function_ (distances[iii]);
239 if (!negative_ && !thresh_result)
241 if (extract_removed_indices_)
242 (*removed_indices_)[rii++] = (*indices_)[iii];
247 if (negative_ && thresh_result)
249 if (extract_removed_indices_)
250 (*removed_indices_)[rii++] = (*indices_)[iii];
255 indices[oii++] = (*indices_)[iii];
260 indices.resize (oii);
261 removed_indices_->resize (rii);
265 #define PCL_INSTANTIATE_ModelOutlierRemoval(T) template class PCL_EXPORTS pcl::ModelOutlierRemoval<T>; 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...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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)...
void applyFilter(PointCloud &output)
Filtered results are stored in a separate point cloud.