Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
sac_model_sphere.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, 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 * $Id$
38 *
39 */
40
41#pragma once
42
43#ifdef __SSE__
44#include <xmmintrin.h> // for __m128
45#endif // ifdef __SSE__
46#ifdef __AVX__
47#include <immintrin.h> // for __m256
48#endif // ifdef __AVX__
49
50#include <pcl/sample_consensus/sac_model.h>
51#include <pcl/sample_consensus/model_types.h>
52
53namespace pcl
54{
55 /** \brief SampleConsensusModelSphere defines a model for 3D sphere segmentation.
56 * The model coefficients are defined as:
57 * - \b center.x : the X coordinate of the sphere's center
58 * - \b center.y : the Y coordinate of the sphere's center
59 * - \b center.z : the Z coordinate of the sphere's center
60 * - \b radius : the sphere's radius
61 *
62 * \author Radu B. Rusu
63 * \ingroup sample_consensus
64 */
65 template <typename PointT>
67 {
68 public:
75
79
82
83 /** \brief Constructor for base SampleConsensusModelSphere.
84 * \param[in] cloud the input point cloud dataset
85 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
86 */
88 bool random = false)
90 {
91 model_name_ = "SampleConsensusModelSphere";
92 sample_size_ = 4;
93 model_size_ = 4;
94 }
95
96 /** \brief Constructor for base SampleConsensusModelSphere.
97 * \param[in] cloud the input point cloud dataset
98 * \param[in] indices a vector of point indices to be used from \a cloud
99 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
100 */
102 const Indices &indices,
103 bool random = false)
104 : SampleConsensusModel<PointT> (cloud, indices, random)
105 {
106 model_name_ = "SampleConsensusModelSphere";
107 sample_size_ = 4;
108 model_size_ = 4;
109 }
110
111 /** \brief Empty destructor */
113
114 /** \brief Copy constructor.
115 * \param[in] source the model to copy into this
116 */
119 {
120 *this = source;
121 model_name_ = "SampleConsensusModelSphere";
122 }
123
124 /** \brief Copy constructor.
125 * \param[in] source the model to copy into this
126 */
133
134 /** \brief Check whether the given index samples can form a valid sphere model, compute the model
135 * coefficients from these samples and store them internally in model_coefficients.
136 * The sphere coefficients are: x, y, z, R.
137 * \param[in] samples the point indices found as possible good candidates for creating a valid model
138 * \param[out] model_coefficients the resultant model coefficients
139 */
140 bool
142 Eigen::VectorXf &model_coefficients) const override;
143
144 /** \brief Compute all distances from the cloud data to a given sphere model.
145 * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to
146 * \param[out] distances the resultant estimated distances
147 */
148 void
149 getDistancesToModel (const Eigen::VectorXf &model_coefficients,
150 std::vector<double> &distances) const override;
151
152 /** \brief Select all the points which respect the given model coefficients as inliers.
153 * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to
154 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
155 * \param[out] inliers the resultant model inliers
156 */
157 void
158 selectWithinDistance (const Eigen::VectorXf &model_coefficients,
159 const double threshold,
160 Indices &inliers) override;
161
162 /** \brief Count all the points which respect the given model coefficients as inliers.
163 *
164 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
165 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
166 * \return the resultant number of inliers
167 */
168 std::size_t
169 countWithinDistance (const Eigen::VectorXf &model_coefficients,
170 const double threshold) const override;
171
172 /** \brief Recompute the sphere coefficients using the given inlier set and return them to the user.
173 * @note: these are the coefficients of the sphere model after refinement (e.g. after SVD)
174 * \param[in] inliers the data inliers found as supporting the model
175 * \param[in] model_coefficients the initial guess for the optimization
176 * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
177 */
178 void
180 const Eigen::VectorXf &model_coefficients,
181 Eigen::VectorXf &optimized_coefficients) const override;
182
183 /** \brief Create a new point cloud with inliers projected onto the sphere model.
184 * \param[in] inliers the data inliers that we want to project on the sphere model
185 * \param[in] model_coefficients the coefficients of a sphere model
186 * \param[out] projected_points the resultant projected points
187 * \param[in] copy_data_fields set to true if we need to copy the other data fields
188 * \todo implement this.
189 */
190 void
192 const Eigen::VectorXf &model_coefficients,
194 bool copy_data_fields = true) const override;
195
196 /** \brief Verify whether a subset of indices verifies the given sphere model coefficients.
197 * \param[in] indices the data indices that need to be tested against the sphere model
198 * \param[in] model_coefficients the sphere model coefficients
199 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
200 */
201 bool
202 doSamplesVerifyModel (const std::set<index_t> &indices,
203 const Eigen::VectorXf &model_coefficients,
204 const double threshold) const override;
205
206 /** \brief Return a unique id for this model (SACMODEL_SPHERE). */
207 inline pcl::SacModel getModelType () const override { return (SACMODEL_SPHERE); }
208
209 protected:
212
213 /** \brief Check whether a model is valid given the user constraints.
214 * \param[in] model_coefficients the set of model coefficients
215 */
216 bool
217 isModelValid (const Eigen::VectorXf &model_coefficients) const override
218 {
220 return (false);
221
222 if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[3] < radius_min_)
223 return (false);
224 if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[3] > radius_max_)
225 return (false);
226
227 return (true);
228 }
229
230 /** \brief Check if a sample of indices results in a good sample of points
231 * indices.
232 * \param[in] samples the resultant index samples
233 */
234 bool
235 isSampleGood(const Indices &samples) const override;
236
237 /** This implementation uses no SIMD instructions. It is not intended for normal use.
238 * See countWithinDistance which automatically uses the fastest implementation.
239 */
240 std::size_t
241 countWithinDistanceStandard (const Eigen::VectorXf &model_coefficients,
242 const double threshold,
243 std::size_t i = 0) const;
244
245#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
246 /** This implementation uses SSE, SSE2, and SSE4.1 instructions. It is not intended for normal use.
247 * See countWithinDistance which automatically uses the fastest implementation.
248 */
249 std::size_t
250 countWithinDistanceSSE (const Eigen::VectorXf &model_coefficients,
251 const double threshold,
252 std::size_t i = 0) const;
253#endif
254
255#if defined (__AVX__) && defined (__AVX2__)
256 /** This implementation uses AVX and AVX2 instructions. It is not intended for normal use.
257 * See countWithinDistance which automatically uses the fastest implementation.
258 */
259 std::size_t
260 countWithinDistanceAVX (const Eigen::VectorXf &model_coefficients,
261 const double threshold,
262 std::size_t i = 0) const;
263#endif
264
265 private:
266 struct OptimizationFunctor : pcl::Functor<float>
267 {
268 /** Functor constructor
269 * \param[in] indices the indices of data points to evaluate
270 * \param[in] estimator pointer to the estimator object
271 */
272 OptimizationFunctor (const pcl::SampleConsensusModelSphere<PointT> *model, const Indices& indices) :
273 pcl::Functor<float> (indices.size ()), model_ (model), indices_ (indices) {}
274
275 /** Cost function to be minimized
276 * \param[in] x the variables array
277 * \param[out] fvec the resultant functions evaluations
278 * \return 0
279 */
280 int
281 operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
282 {
283 Eigen::Vector4f cen_t;
284 cen_t[3] = 0;
285 for (int i = 0; i < values (); ++i)
286 {
287 // Compute the difference between the center of the sphere and the datapoint X_i
288 cen_t.head<3>() = (*model_->input_)[indices_[i]].getVector3fMap() - x.head<3>();
289
290 // g = sqrt ((x-a)^2 + (y-b)^2 + (z-c)^2) - R
291 fvec[i] = std::sqrt (cen_t.dot (cen_t)) - x[3];
292 }
293 return (0);
294 }
295
297 const Indices &indices_;
298 };
299
300#ifdef __AVX__
301 inline __m256 sqr_dist8 (const std::size_t i, const __m256 a_vec, const __m256 b_vec, const __m256 c_vec) const;
302#endif
303
304#ifdef __SSE__
305 inline __m128 sqr_dist4 (const std::size_t i, const __m128 a_vec, const __m128 b_vec, const __m128 c_vec) const;
306#endif
307 };
308}
309
310#ifdef PCL_NO_PRECOMPILE
311#include <pcl/sample_consensus/impl/sac_model_sphere.hpp>
312#endif
Iterator class for point clouds with or without given indices.
PointCloud represents the base class in PCL for storing collections of 3D points.
SampleConsensusModel represents the base model class.
Definition sac_model.h:70
double radius_min_
The minimum and maximum radius limits for the model.
Definition sac_model.h:564
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition sac_model.h:588
typename PointCloud::ConstPtr PointCloudConstPtr
Definition sac_model.h:73
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition sac_model.h:556
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
Definition sac_model.h:553
std::string model_name_
The model name.
Definition sac_model.h:550
unsigned int model_size_
The number of coefficients in the model.
Definition sac_model.h:591
typename PointCloud::Ptr PointCloudPtr
Definition sac_model.h:74
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
Definition sac_model.h:585
SampleConsensusModelSphere defines a model for 3D sphere segmentation.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
typename SampleConsensusModel< PointT >::PointCloud PointCloud
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_SPHERE).
SampleConsensusModelSphere(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelSphere.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given sphere model.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the sphere coefficients using the given inlier set and return them to the user.
typename SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
SampleConsensusModelSphere(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelSphere.
std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const override
Count all the points which respect the given model coefficients as inliers.
typename SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
SampleConsensusModelSphere & operator=(const SampleConsensusModelSphere &source)
Copy constructor.
bool doSamplesVerifyModel(const std::set< index_t > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override
Verify whether a subset of indices verifies the given sphere model coefficients.
std::size_t countWithinDistanceStandard(const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i=0) const
This implementation uses no SIMD instructions.
void projectPoints(const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const override
Create a new point cloud with inliers projected onto the sphere model.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid sphere model, compute the model coefficients f...
SampleConsensusModelSphere(const SampleConsensusModelSphere &source)
Copy constructor.
~SampleConsensusModelSphere()
Empty destructor.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Select all the points which respect the given model coefficients as inliers.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
@ SACMODEL_SPHERE
Definition model_types.h:51
Base functor all the models that need non linear optimization must define their own one and implement...
Definition sac_model.h:679
int values() const
Get the number of values.
Definition sac_model.h:703
A point structure representing Euclidean xyz coordinates, and the RGB color.