Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
sac_model_circle.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009, 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#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_
42#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_
43
44#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
45#include <pcl/sample_consensus/sac_model_circle.h>
46#include <pcl/common/concatenate.h>
47
48//////////////////////////////////////////////////////////////////////////
49template <typename PointT> bool
51{
52 if (samples.size () != sample_size_)
53 {
54 PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
55 return (false);
56 }
57
58 // Double precision here follows computeModelCoefficients, which means we
59 // can't use getVector3fMap-accessor to make our lives easier.
60 Eigen::Array2d p0 ((*input_)[samples[0]].x, (*input_)[samples[0]].y);
61 Eigen::Array2d p1 ((*input_)[samples[1]].x, (*input_)[samples[1]].y);
62 Eigen::Array2d p2 ((*input_)[samples[2]].x, (*input_)[samples[2]].y);
63
64 // Compute the segment values (in 2d) between p1 and p0
65 p1 -= p0;
66 // Compute the segment values (in 2d) between p2 and p0
67 p2 -= p0;
68
69 // Check if the triangle area spanned by the three sample points is greater than 0
70 // https://en.wikipedia.org/wiki/Triangle#Using_vectors
71 if (std::abs (p1.x()*p2.y() - p2.x()*p1.y()) < Eigen::NumTraits<double>::dummy_precision ()) {
72 PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::isSampleGood] Sample points too similar or collinear!\n");
73 return (false);
74 }
75
76 return (true);
77}
78
79//////////////////////////////////////////////////////////////////////////
80template <typename PointT> bool
81pcl::SampleConsensusModelCircle2D<PointT>::computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const
82{
83 if (!isSampleGood (samples))
84 {
85 PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Invalid set of samples given!\n");
86 return (false);
87 }
88
89 model_coefficients.resize (model_size_);
90
91 Eigen::Vector2d p0 ((*input_)[samples[0]].x, (*input_)[samples[0]].y);
92 Eigen::Vector2d p1 ((*input_)[samples[1]].x, (*input_)[samples[1]].y);
93 Eigen::Vector2d p2 ((*input_)[samples[2]].x, (*input_)[samples[2]].y);
94
95 Eigen::Vector2d u = (p0 + p1) / 2.0;
96 Eigen::Vector2d v = (p1 + p2) / 2.0;
97
98 Eigen::Vector2d p1p0dif = p1 - p0;
99 Eigen::Vector2d p2p1dif = p2 - p1;
100 Eigen::Vector2d uvdif = u - v;
101
102 Eigen::Vector2d m (- p1p0dif[0] / p1p0dif[1], - p2p1dif[0] / p2p1dif[1]);
103
104 // Center (x, y)
105 model_coefficients[0] = static_cast<float> ((m[0] * u[0] - m[1] * v[0] - uvdif[1] ) / (m[0] - m[1]));
106 model_coefficients[1] = static_cast<float> ((m[0] * m[1] * uvdif[0] + m[0] * v[1] - m[1] * u[1]) / (m[0] - m[1]));
107
108 // Radius
109 model_coefficients[2] = static_cast<float> (sqrt ((model_coefficients[0] - p0[0]) * (model_coefficients[0] - p0[0]) +
110 (model_coefficients[1] - p0[1]) * (model_coefficients[1] - p0[1])));
111 PCL_DEBUG ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Model is (%g,%g,%g).\n",
112 model_coefficients[0], model_coefficients[1], model_coefficients[2]);
113 return (true);
114}
115
116#define AT(POS) ((*input_)[(*indices_)[(POS)]])
117
118#ifdef __AVX__
119// This function computes the squared distances (i.e. the distances without the square root) of 8 points to the center of the circle
120template <typename PointT> inline __m256 pcl::SampleConsensusModelCircle2D<PointT>::sqr_dist8 (const std::size_t i, const __m256 a_vec, const __m256 b_vec) const
121{
122 const __m256 tmp1 = _mm256_sub_ps (_mm256_set_ps (AT(i ).x, AT(i+1).x, AT(i+2).x, AT(i+3).x, AT(i+4).x, AT(i+5).x, AT(i+6).x, AT(i+7).x), a_vec);
123 const __m256 tmp2 = _mm256_sub_ps (_mm256_set_ps (AT(i ).y, AT(i+1).y, AT(i+2).y, AT(i+3).y, AT(i+4).y, AT(i+5).y, AT(i+6).y, AT(i+7).y), b_vec);
124 return _mm256_add_ps (_mm256_mul_ps (tmp1, tmp1), _mm256_mul_ps (tmp2, tmp2));
125}
126#endif // ifdef __AVX__
127
128#ifdef __SSE__
129// This function computes the squared distances (i.e. the distances without the square root) of 4 points to the center of the circle
130template <typename PointT> inline __m128 pcl::SampleConsensusModelCircle2D<PointT>::sqr_dist4 (const std::size_t i, const __m128 a_vec, const __m128 b_vec) const
131{
132 const __m128 tmp1 = _mm_sub_ps (_mm_set_ps (AT(i ).x, AT(i+1).x, AT(i+2).x, AT(i+3).x), a_vec);
133 const __m128 tmp2 = _mm_sub_ps (_mm_set_ps (AT(i ).y, AT(i+1).y, AT(i+2).y, AT(i+3).y), b_vec);
134 return _mm_add_ps (_mm_mul_ps (tmp1, tmp1), _mm_mul_ps (tmp2, tmp2));
135}
136#endif // ifdef __SSE__
137
138#undef AT
140//////////////////////////////////////////////////////////////////////////
141template <typename PointT> void
142pcl::SampleConsensusModelCircle2D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
143{
144 // Check if the model is valid given the user constraints
145 if (!isModelValid (model_coefficients))
146 {
147 distances.clear ();
148 return;
149 }
150 distances.resize (indices_->size ());
151
152 // Iterate through the 3d points and calculate the distances from them to the circle
153 for (std::size_t i = 0; i < indices_->size (); ++i)
154 // Calculate the distance from the point to the circle as the difference between
155 // dist(point,circle_origin) and circle_radius
156 distances[i] = std::abs (std::sqrt (
157 ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) *
158 ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) +
159
160 ( (*input_)[(*indices_)[i]].y - model_coefficients[1] ) *
161 ( (*input_)[(*indices_)[i]].y - model_coefficients[1] )
162 ) - model_coefficients[2]);
163}
164
165//////////////////////////////////////////////////////////////////////////
166template <typename PointT> void
168 const Eigen::VectorXf &model_coefficients, const double threshold,
169 Indices &inliers)
170{
171 // Check if the model is valid given the user constraints
172 if (!isModelValid (model_coefficients))
173 {
174 inliers.clear ();
175 return;
176 }
177 inliers.clear ();
178 error_sqr_dists_.clear ();
179 inliers.reserve (indices_->size ());
180 error_sqr_dists_.reserve (indices_->size ());
181
182 const float sqr_inner_radius = (model_coefficients[2] <= threshold ? 0.0f : (model_coefficients[2] - threshold) * (model_coefficients[2] - threshold));
183 const float sqr_outer_radius = (model_coefficients[2] + threshold) * (model_coefficients[2] + threshold);
184 // Iterate through the 3d points and calculate the distances from them to the circle
185 for (std::size_t i = 0; i < indices_->size (); ++i)
186 {
187 // To avoid sqrt computation: consider one larger circle (radius + threshold) and one smaller circle (radius - threshold).
188 // Valid if point is in larger circle, but not in smaller circle.
189 const float sqr_dist = ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) *
190 ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) +
191 ( (*input_)[(*indices_)[i]].y - model_coefficients[1] ) *
192 ( (*input_)[(*indices_)[i]].y - model_coefficients[1] );
193 if ((sqr_dist <= sqr_outer_radius) && (sqr_dist >= sqr_inner_radius))
194 {
195 // Returns the indices of the points whose distances are smaller than the threshold
196 inliers.push_back ((*indices_)[i]);
197 // Only compute exact distance if necessary (if point is inlier)
198 error_sqr_dists_.push_back (static_cast<double> (std::abs (std::sqrt (sqr_dist) - model_coefficients[2])));
200 }
201}
202
203//////////////////////////////////////////////////////////////////////////
204template <typename PointT> std::size_t
206 const Eigen::VectorXf &model_coefficients, const double threshold) const
207{
208 // Check if the model is valid given the user constraints
209 if (!isModelValid (model_coefficients))
210 return (0);
211
212#if defined (__AVX__) && defined (__AVX2__)
213 return countWithinDistanceAVX (model_coefficients, threshold);
214#elif defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
215 return countWithinDistanceSSE (model_coefficients, threshold);
216#else
217 return countWithinDistanceStandard (model_coefficients, threshold);
218#endif
219}
220
221//////////////////////////////////////////////////////////////////////////
222template <typename PointT> std::size_t
224 const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
225{
226 std::size_t nr_p = 0;
227 const float sqr_inner_radius = (model_coefficients[2] <= threshold ? 0.0f : (model_coefficients[2] - threshold) * (model_coefficients[2] - threshold));
228 const float sqr_outer_radius = (model_coefficients[2] + threshold) * (model_coefficients[2] + threshold);
229 // Iterate through the 3d points and calculate the distances from them to the circle
230 for (; i < indices_->size (); ++i)
231 {
232 // To avoid sqrt computation: consider one larger circle (radius + threshold) and one smaller circle (radius - threshold).
233 // Valid if point is in larger circle, but not in smaller circle.
234 const float sqr_dist = ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) *
235 ( (*input_)[(*indices_)[i]].x - model_coefficients[0] ) +
236 ( (*input_)[(*indices_)[i]].y - model_coefficients[1] ) *
237 ( (*input_)[(*indices_)[i]].y - model_coefficients[1] );
238 if ((sqr_dist <= sqr_outer_radius) && (sqr_dist >= sqr_inner_radius))
239 nr_p++;
240 }
241 return (nr_p);
242}
243
244//////////////////////////////////////////////////////////////////////////
245#if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
246template <typename PointT> std::size_t
248 const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
249{
250 std::size_t nr_p = 0;
251 const __m128 a_vec = _mm_set1_ps (model_coefficients[0]);
252 const __m128 b_vec = _mm_set1_ps (model_coefficients[1]);
253 // To avoid sqrt computation: consider one larger circle (radius + threshold) and one smaller circle (radius - threshold). Valid if point is in larger circle, but not in smaller circle.
254 const __m128 sqr_inner_radius = _mm_set1_ps ((model_coefficients[2] <= threshold ? 0.0 : (model_coefficients[2]-threshold)*(model_coefficients[2]-threshold)));
255 const __m128 sqr_outer_radius = _mm_set1_ps ((model_coefficients[2]+threshold)*(model_coefficients[2]+threshold));
256 __m128i res = _mm_set1_epi32(0); // This corresponds to nr_p: 4 32bit integers that, summed together, hold the number of inliers
257 for (; (i + 4) <= indices_->size (); i += 4)
258 {
259 const __m128 sqr_dist = sqr_dist4 (i, a_vec, b_vec);
260 const __m128 mask = _mm_and_ps (_mm_cmplt_ps (sqr_inner_radius, sqr_dist), _mm_cmplt_ps (sqr_dist, sqr_outer_radius)); // The mask contains 1 bits if the corresponding points are inliers, else 0 bits
261 res = _mm_add_epi32 (res, _mm_and_si128 (_mm_set1_epi32 (1), _mm_castps_si128 (mask))); // The latter part creates a vector with ones (as 32bit integers) where the points are inliers
262 //const int res = _mm_movemask_ps (mask);
263 //if (res & 1) nr_p++;
264 //if (res & 2) nr_p++;
265 //if (res & 4) nr_p++;
266 //if (res & 8) nr_p++;
267 }
268 nr_p += _mm_extract_epi32 (res, 0);
269 nr_p += _mm_extract_epi32 (res, 1);
270 nr_p += _mm_extract_epi32 (res, 2);
271 nr_p += _mm_extract_epi32 (res, 3);
272
273 // Process the remaining points (at most 3)
274 nr_p += countWithinDistanceStandard (model_coefficients, threshold, i);
275 return (nr_p);
276}
277#endif
278
279//////////////////////////////////////////////////////////////////////////
280#if defined (__AVX__) && defined (__AVX2__)
281template <typename PointT> std::size_t
283 const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i) const
284{
285 std::size_t nr_p = 0;
286 const __m256 a_vec = _mm256_set1_ps (model_coefficients[0]);
287 const __m256 b_vec = _mm256_set1_ps (model_coefficients[1]);
288 // To avoid sqrt computation: consider one larger circle (radius + threshold) and one smaller circle (radius - threshold). Valid if point is in larger circle, but not in smaller circle.
289 const __m256 sqr_inner_radius = _mm256_set1_ps ((model_coefficients[2] <= threshold ? 0.0 : (model_coefficients[2]-threshold)*(model_coefficients[2]-threshold)));
290 const __m256 sqr_outer_radius = _mm256_set1_ps ((model_coefficients[2]+threshold)*(model_coefficients[2]+threshold));
291 __m256i res = _mm256_set1_epi32(0); // This corresponds to nr_p: 8 32bit integers that, summed together, hold the number of inliers
292 for (; (i + 8) <= indices_->size (); i += 8)
293 {
294 const __m256 sqr_dist = sqr_dist8 (i, a_vec, b_vec);
295 const __m256 mask = _mm256_and_ps (_mm256_cmp_ps (sqr_inner_radius, sqr_dist, _CMP_LT_OQ), _mm256_cmp_ps (sqr_dist, sqr_outer_radius, _CMP_LT_OQ)); // The mask contains 1 bits if the corresponding points are inliers, else 0 bits
296 res = _mm256_add_epi32 (res, _mm256_and_si256 (_mm256_set1_epi32 (1), _mm256_castps_si256 (mask))); // The latter part creates a vector with ones (as 32bit integers) where the points are inliers
297 //const int res = _mm256_movemask_ps (mask);
298 //if (res & 1) nr_p++;
299 //if (res & 2) nr_p++;
300 //if (res & 4) nr_p++;
301 //if (res & 8) nr_p++;
302 //if (res & 16) nr_p++;
303 //if (res & 32) nr_p++;
304 //if (res & 64) nr_p++;
305 //if (res & 128) nr_p++;
306 }
307 nr_p += _mm256_extract_epi32 (res, 0);
308 nr_p += _mm256_extract_epi32 (res, 1);
309 nr_p += _mm256_extract_epi32 (res, 2);
310 nr_p += _mm256_extract_epi32 (res, 3);
311 nr_p += _mm256_extract_epi32 (res, 4);
312 nr_p += _mm256_extract_epi32 (res, 5);
313 nr_p += _mm256_extract_epi32 (res, 6);
314 nr_p += _mm256_extract_epi32 (res, 7);
315
316 // Process the remaining points (at most 7)
317 nr_p += countWithinDistanceStandard (model_coefficients, threshold, i);
318 return (nr_p);
319}
320#endif
321
322//////////////////////////////////////////////////////////////////////////
323template <typename PointT> void
325 const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
326{
327 optimized_coefficients = model_coefficients;
328
329 // Needs a set of valid model coefficients
330 if (!isModelValid (model_coefficients))
331 {
332 PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Given model is invalid!\n");
333 return;
334 }
335
336 // Need more than the minimum sample size to make a difference
337 if (inliers.size () <= sample_size_)
338 {
339 PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
340 return;
341 }
342
343 OptimizationFunctor functor (this, inliers);
344 Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
345 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, float> lm (num_diff);
346 int info = lm.minimize (optimized_coefficients);
347
348 // Compute the L2 norm of the residuals
349 PCL_DEBUG ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g \nFinal solution: %g %g %g\n",
350 info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2]);
351}
352
353//////////////////////////////////////////////////////////////////////////
354template <typename PointT> void
356 const Indices &inliers, const Eigen::VectorXf &model_coefficients,
357 PointCloud &projected_points, bool copy_data_fields) const
358{
359 // Needs a valid set of model coefficients
360 if (!isModelValid (model_coefficients))
361 {
362 PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Given model is invalid!\n");
363 return;
364 }
365
366 projected_points.header = input_->header;
367 projected_points.is_dense = input_->is_dense;
368
369 // Copy all the data fields from the input cloud to the projected one?
370 if (copy_data_fields)
371 {
372 // Allocate enough space and copy the basics
373 projected_points.resize (input_->size ());
374 projected_points.width = input_->width;
375 projected_points.height = input_->height;
376
377 using FieldList = typename pcl::traits::fieldList<PointT>::type;
378 // Iterate over each point
379 for (std::size_t i = 0; i < projected_points.size (); ++i)
380 // Iterate over each dimension
381 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[i], projected_points[i]));
382
383 // Iterate through the points and project them to the circle
384 for (const auto &inlier : inliers)
385 {
386 float dx = (*input_)[inlier].x - model_coefficients[0];
387 float dy = (*input_)[inlier].y - model_coefficients[1];
388 float a = std::sqrt ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );
389
390 projected_points[inlier].x = a * dx + model_coefficients[0];
391 projected_points[inlier].y = a * dy + model_coefficients[1];
392 }
393 }
394 else
395 {
396 // Allocate enough space and copy the basics
397 projected_points.resize (inliers.size ());
398 projected_points.width = inliers.size ();
399 projected_points.height = 1;
400
401 using FieldList = typename pcl::traits::fieldList<PointT>::type;
402 // Iterate over each point
403 for (std::size_t i = 0; i < inliers.size (); ++i)
404 // Iterate over each dimension
405 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[inliers[i]], projected_points[i]));
406
407 // Iterate through the points and project them to the circle
408 for (std::size_t i = 0; i < inliers.size (); ++i)
409 {
410 float dx = (*input_)[inliers[i]].x - model_coefficients[0];
411 float dy = (*input_)[inliers[i]].y - model_coefficients[1];
412 float a = std::sqrt ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );
413
414 projected_points[i].x = a * dx + model_coefficients[0];
415 projected_points[i].y = a * dy + model_coefficients[1];
416 }
417 }
418}
419
420//////////////////////////////////////////////////////////////////////////
421template <typename PointT> bool
423 const std::set<index_t> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
424{
425 // Needs a valid model coefficients
426 if (!isModelValid (model_coefficients))
427 {
428 PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel] Given model is invalid!\n");
429 return (false);
430 }
431
432 const float sqr_inner_radius = (model_coefficients[2] <= threshold ? 0.0f : (model_coefficients[2] - threshold) * (model_coefficients[2] - threshold));
433 const float sqr_outer_radius = (model_coefficients[2] + threshold) * (model_coefficients[2] + threshold);
434 for (const auto &index : indices)
435 {
436 // To avoid sqrt computation: consider one larger circle (radius + threshold) and one smaller circle (radius - threshold).
437 // Valid if point is in larger circle, but not in smaller circle.
438 const float sqr_dist = ( (*input_)[index].x - model_coefficients[0] ) *
439 ( (*input_)[index].x - model_coefficients[0] ) +
440 ( (*input_)[index].y - model_coefficients[1] ) *
441 ( (*input_)[index].y - model_coefficients[1] );
442 if ((sqr_dist > sqr_outer_radius) || (sqr_dist < sqr_inner_radius))
443 return (false);
444 }
445 return (true);
446}
447
448//////////////////////////////////////////////////////////////////////////
449template <typename PointT> bool
450pcl::SampleConsensusModelCircle2D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
451{
452 if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
453 return (false);
454
455 if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[2] < radius_min_)
456 {
457 PCL_DEBUG ("[pcl::SampleConsensusModelCircle2D::isModelValid] Radius of circle is too small: should be larger than %g, but is %g.\n",
458 radius_min_, model_coefficients[2]);
459 return (false);
460 }
461 if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[2] > radius_max_)
462 {
463 PCL_DEBUG ("[pcl::SampleConsensusModelCircle2D::isModelValid] Radius of circle is too big: should be smaller than %g, but is %g.\n",
464 radius_max_, model_coefficients[2]);
465 return (false);
466 }
467
468 return (true);
469}
470
471#define PCL_INSTANTIATE_SampleConsensusModelCircle2D(T) template class PCL_EXPORTS pcl::SampleConsensusModelCircle2D<T>;
472
473#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_H_
474
SampleConsensusModelCircle2D defines a model for 2D circle segmentation on the X-Y plane.
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.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid 2D circle model, compute the model coefficient...
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 2d circle model.
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Compute all distances from the cloud data to a given 2D circle model.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given 2D circle model.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the 2d circle coefficients using the given inlier set and return them to the user.
typename SampleConsensusModel< PointT >::PointCloud PointCloud
std::size_t countWithinDistanceStandard(const Eigen::VectorXf &model_coefficients, const double threshold, std::size_t i=0) const
This implementation uses no SIMD instructions.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
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 2d circle model coefficients.
SampleConsensusModel represents the base model class.
Definition sac_model.h:71
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133