Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
project_inliers.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the copyright holder(s) nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *
34 * $Id$
35 *
36 */
37
38#ifndef PCL_FILTERS_IMPL_PROJECT_INLIERS_H_
39#define PCL_FILTERS_IMPL_PROJECT_INLIERS_H_
40
41#include <pcl/filters/project_inliers.h>
42#include <pcl/sample_consensus/sac_model_circle.h>
43#include <pcl/sample_consensus/sac_model_cylinder.h>
44#include <pcl/sample_consensus/sac_model_cone.h>
45#include <pcl/sample_consensus/sac_model_line.h>
46#include <pcl/sample_consensus/sac_model_normal_plane.h>
47#include <pcl/sample_consensus/sac_model_normal_sphere.h>
48#include <pcl/sample_consensus/sac_model_parallel_plane.h>
49#include <pcl/sample_consensus/sac_model_normal_parallel_plane.h>
50#include <pcl/sample_consensus/sac_model_parallel_line.h>
51#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
52#include <pcl/sample_consensus/sac_model_plane.h>
53#include <pcl/sample_consensus/sac_model_sphere.h>
54
55//////////////////////////////////////////////////////////////////////////////////////////////
56template <typename PointT> void
58{
59 if (indices_->empty ())
60 {
61 PCL_WARN ("[pcl::%s::applyFilter] No indices given or empty indices!\n", getClassName ().c_str ());
62 output.width = output.height = 0;
63 output.clear ();
64 return;
65 }
66
67 //Eigen::Map<Eigen::VectorXf, Eigen::Aligned> model_coefficients (&model_->values[0], model_->values.size ());
68 // More expensive than a map but safer (32bit architectures seem to complain)
69 Eigen::VectorXf model_coefficients (model_->values.size ());
70 for (std::size_t i = 0; i < model_->values.size (); ++i)
71 model_coefficients[i] = model_->values[i];
72
73 // Initialize the Sample Consensus model and set its parameters
74 if (!initSACModel (model_type_))
75 {
76 PCL_ERROR ("[pcl::%s::applyFilter] Error initializing the SAC model!\n", getClassName ().c_str ());
77 output.width = output.height = 0;
78 output.clear ();
79 return;
80 }
81 if (copy_all_data_)
82 sacmodel_->projectPoints (*indices_, model_coefficients, output, true);
83 else
84 sacmodel_->projectPoints (*indices_, model_coefficients, output, false);
85}
86
87//////////////////////////////////////////////////////////////////////////////////////////////
88template <typename PointT> bool
90{
91 // Build the model
92 switch (model_type)
93 {
94 case SACMODEL_PLANE:
95 {
96 //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PLANE\n", getClassName ().c_str ());
97 sacmodel_.reset (new SampleConsensusModelPlane<PointT> (input_));
98 break;
99 }
100 case SACMODEL_LINE:
101 {
102 //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ());
103 sacmodel_.reset (new SampleConsensusModelLine<PointT> (input_));
104 break;
105 }
107 {
108 //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ());
109 sacmodel_.reset (new SampleConsensusModelCircle2D<PointT> (input_));
110 break;
111 }
112 case SACMODEL_SPHERE:
113 {
114 //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ());
115 sacmodel_.reset (new SampleConsensusModelSphere<PointT> (input_));
116 break;
117 }
119 {
120 //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ());
121 sacmodel_.reset (new SampleConsensusModelParallelLine<PointT> (input_));
122 break;
123 }
125 {
126 //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ());
127 sacmodel_.reset (new SampleConsensusModelPerpendicularPlane<PointT> (input_));
128 break;
129 }
131 {
132 //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ());
133 sacmodel_.reset (new SampleConsensusModelCylinder<PointT, pcl::Normal> (input_));
134 break;
135 }
137 {
138 //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ());
139 sacmodel_.reset (new SampleConsensusModelNormalPlane<PointT, pcl::Normal> (input_));
140 break;
141 }
142 case SACMODEL_CONE:
143 {
144 //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ());
145 sacmodel_.reset (new SampleConsensusModelCone<PointT, pcl::Normal> (input_));
146 break;
147 }
149 {
150 //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ());
151 sacmodel_.reset (new SampleConsensusModelNormalSphere<PointT, pcl::Normal> (input_));
152 break;
153 }
155 {
156 //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
157 sacmodel_.reset (new SampleConsensusModelNormalParallelPlane<PointT, pcl::Normal> (input_));
158 break;
159 }
161 {
162 //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ());
163 sacmodel_.reset (new SampleConsensusModelParallelPlane<PointT> (input_));
164 break;
165 }
166 default:
167 {
168 PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
169 return (false);
170 }
171 }
172 return (true);
173}
174
175#define PCL_INSTANTIATE_ProjectInliers(T) template class PCL_EXPORTS pcl::ProjectInliers<T>;
176
177#endif // PCL_FILTERS_IMPL_PROJECT_INLIERS_H_
178
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
ProjectInliers uses a model and a set of inlier indices from a PointCloud to project them into a sepa...
void applyFilter(PointCloud &output) override
Project point indices into a separate PointCloud.
@ SACMODEL_CYLINDER
Definition model_types.h:52
@ SACMODEL_PLANE
Definition model_types.h:47
@ SACMODEL_PARALLEL_PLANE
Definition model_types.h:62
@ SACMODEL_SPHERE
Definition model_types.h:51
@ SACMODEL_PARALLEL_LINE
Definition model_types.h:55
@ SACMODEL_NORMAL_PARALLEL_PLANE
Definition model_types.h:63
@ SACMODEL_PERPENDICULAR_PLANE
Definition model_types.h:56
@ SACMODEL_NORMAL_SPHERE
Definition model_types.h:59
@ SACMODEL_CIRCLE2D
Definition model_types.h:49
@ SACMODEL_NORMAL_PLANE
Definition model_types.h:58
@ SACMODEL_CONE
Definition model_types.h:53
@ SACMODEL_LINE
Definition model_types.h:48