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_->getDistancesToModel (model_coefficients_, distances);
223 for (
int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii)
226 if (!
isFinite (input_->points[ (*indices_)[iii]]))
228 if (extract_removed_indices_)
229 (*removed_indices_)[rii++] = (*indices_)[iii];
234 thresh_result = threshold_function_ (distances[iii]);
237 if (!negative_ && !thresh_result)
239 if (extract_removed_indices_)
240 (*removed_indices_)[rii++] = (*indices_)[iii];
245 if (negative_ && thresh_result)
247 if (extract_removed_indices_)
248 (*removed_indices_)[rii++] = (*indices_)[iii];
253 indices[oii++] = (*indices_)[iii];
258 indices.resize (oii);
259 removed_indices_->resize (rii);
263 #define PCL_INSTANTIATE_ModelOutlierRemoval(T) template class PCL_EXPORTS pcl::ModelOutlierRemoval<T>;
265 #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.
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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...
void applyFilterIndices(std::vector< int > &indices)
Filtered results are indexed by an indices array.
void applyFilter(PointCloud &output)
Filtered results are stored in a separate point cloud.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).