Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
sac_model_circle3d.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2012-, Open Perception, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 *
37 */
38
39#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_3D_HPP_
40#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_3D_HPP_
41
42#include <limits>
43
44#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
45#include <pcl/sample_consensus/sac_model_circle3d.h>
46#include <pcl/common/concatenate.h>
47
48//////////////////////////////////////////////////////////////////////////
49template <typename PointT> bool
51 const Indices &samples) const
52{
53 if (samples.size () != sample_size_)
54 {
55 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
56 return (false);
57 }
58
59 // Double precision here follows computeModelCoefficients, which means we
60 // can't use getVector3fMap-accessor to make our lives easier.
61 Eigen::Vector3d p0 ((*input_)[samples[0]].x, (*input_)[samples[0]].y, (*input_)[samples[0]].z);
62 Eigen::Vector3d p1 ((*input_)[samples[1]].x, (*input_)[samples[1]].y, (*input_)[samples[1]].z);
63 Eigen::Vector3d p2 ((*input_)[samples[2]].x, (*input_)[samples[2]].y, (*input_)[samples[2]].z);
64
65 // Check if the squared norm of the cross-product is non-zero, otherwise
66 // common_helper_vec, which plays an important role in computeModelCoefficients,
67 // would likely be ill-formed.
68 if ((p1 - p0).cross(p1 - p2).squaredNorm() < Eigen::NumTraits<double>::dummy_precision ())
69 {
70 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::isSampleGood] Sample points too similar or collinear!\n");
71 return (false);
72 }
73
74 return (true);
75}
76
77//////////////////////////////////////////////////////////////////////////
78template <typename PointT> bool
79pcl::SampleConsensusModelCircle3D<PointT>::computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const
80{
81 if (samples.size () != sample_size_)
82 {
83 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
84 return (false);
85 }
86
87 model_coefficients.resize (model_size_); //needing 7 coefficients: centerX, centerY, centerZ, radius, normalX, normalY, normalZ
88
89 Eigen::Vector3d p0 ((*input_)[samples[0]].x, (*input_)[samples[0]].y, (*input_)[samples[0]].z);
90 Eigen::Vector3d p1 ((*input_)[samples[1]].x, (*input_)[samples[1]].y, (*input_)[samples[1]].z);
91 Eigen::Vector3d p2 ((*input_)[samples[2]].x, (*input_)[samples[2]].y, (*input_)[samples[2]].z);
92
93
94 Eigen::Vector3d helper_vec01 = p0 - p1;
95 Eigen::Vector3d helper_vec02 = p0 - p2;
96 Eigen::Vector3d helper_vec10 = p1 - p0;
97 Eigen::Vector3d helper_vec12 = p1 - p2;
98 Eigen::Vector3d helper_vec20 = p2 - p0;
99 Eigen::Vector3d helper_vec21 = p2 - p1;
100
101 Eigen::Vector3d common_helper_vec = helper_vec01.cross (helper_vec12);
102
103 // The same check is implemented in isSampleGood, so be sure to look there too
104 // if you find the need to change something here.
105 if (common_helper_vec.squaredNorm() < Eigen::NumTraits<double>::dummy_precision ())
106 {
107 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Sample points too similar or collinear!\n");
108 return (false);
109 }
110
111 double commonDividend = 2.0 * common_helper_vec.squaredNorm ();
112
113 double alpha = (helper_vec12.squaredNorm () * helper_vec01.dot (helper_vec02)) / commonDividend;
114 double beta = (helper_vec02.squaredNorm () * helper_vec10.dot (helper_vec12)) / commonDividend;
115 double gamma = (helper_vec01.squaredNorm () * helper_vec20.dot (helper_vec21)) / commonDividend;
116
117 Eigen::Vector3d circle_center = alpha * p0 + beta * p1 + gamma * p2;
118
119 Eigen::Vector3d circle_radiusVector = circle_center - p0;
120 double circle_radius = circle_radiusVector.norm ();
121 Eigen::Vector3d circle_normal = common_helper_vec.normalized ();
122
123 model_coefficients[0] = static_cast<float> (circle_center[0]);
124 model_coefficients[1] = static_cast<float> (circle_center[1]);
125 model_coefficients[2] = static_cast<float> (circle_center[2]);
126 model_coefficients[3] = static_cast<float> (circle_radius);
127 model_coefficients[4] = static_cast<float> (circle_normal[0]);
128 model_coefficients[5] = static_cast<float> (circle_normal[1]);
129 model_coefficients[6] = static_cast<float> (circle_normal[2]);
130
131 PCL_DEBUG ("[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g,%g).\n",
132 model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
133 model_coefficients[4], model_coefficients[5], model_coefficients[6]);
134 return (true);
135}
136
137//////////////////////////////////////////////////////////////////////////
138template <typename PointT> void
139pcl::SampleConsensusModelCircle3D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
140{
141 // Check if the model is valid given the user constraints
142 if (!isModelValid (model_coefficients))
143 {
144 distances.clear ();
145 return;
146 }
147 distances.resize (indices_->size ());
148
149 // Iterate through the 3d points and calculate the distances from them to the sphere
150 for (std::size_t i = 0; i < indices_->size (); ++i)
151 // Calculate the distance from the point to the circle:
152 // 1. calculate intersection point of the plane in which the circle lies and the
153 // line from the sample point with the direction of the plane normal (projected point)
154 // 2. calculate the intersection point of the line from the circle center to the projected point
155 // with the circle
156 // 3. calculate distance from corresponding point on the circle to the sample point
157 {
158 // what i have:
159 // P : Sample Point
160 Eigen::Vector3d P ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
161 // C : Circle Center
162 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
163 // N : Circle (Plane) Normal
164 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
165 // r : Radius
166 double r = model_coefficients[3];
167
168 Eigen::Vector3d helper_vectorPC = P - C;
169 // 1.1. get line parameter
170 double lambda = (helper_vectorPC.dot (N)) / N.squaredNorm ();
171
172 // Projected Point on plane
173 Eigen::Vector3d P_proj = P + lambda * N;
174 Eigen::Vector3d helper_vectorP_projC = P_proj - C;
175
176 // K : Point on Circle
177 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
178 Eigen::Vector3d distanceVector = P - K;
179
180 distances[i] = distanceVector.norm ();
181 }
182}
184//////////////////////////////////////////////////////////////////////////
185template <typename PointT> void
187 const Eigen::VectorXf &model_coefficients, const double threshold,
188 Indices &inliers)
189{
190 // Check if the model is valid given the user constraints
191 if (!isModelValid (model_coefficients))
192 {
193 inliers.clear ();
194 return;
195 }
196 inliers.clear ();
197 error_sqr_dists_.clear ();
198 inliers.reserve (indices_->size ());
199 error_sqr_dists_.reserve (indices_->size ());
200
201 const auto squared_threshold = threshold * threshold;
202 // Iterate through the 3d points and calculate the distances from them to the sphere
203 for (std::size_t i = 0; i < indices_->size (); ++i)
204 {
205 // what i have:
206 // P : Sample Point
207 Eigen::Vector3d P ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
208 // C : Circle Center
209 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
210 // N : Circle (Plane) Normal
211 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
212 // r : Radius
213 double r = model_coefficients[3];
214
215 Eigen::Vector3d helper_vectorPC = P - C;
216 // 1.1. get line parameter
217 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
218 // Projected Point on plane
219 Eigen::Vector3d P_proj = P + lambda * N;
220 Eigen::Vector3d helper_vectorP_projC = P_proj - C;
221
222 // K : Point on Circle
223 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
224 Eigen::Vector3d distanceVector = P - K;
225
226 const double sqr_dist = distanceVector.squaredNorm ();
227 if (sqr_dist < squared_threshold)
228 {
229 // Returns the indices of the points whose distances are smaller than the threshold
230 inliers.push_back ((*indices_)[i]);
231 error_sqr_dists_.push_back (sqr_dist);
232 }
233 }
234}
235
236//////////////////////////////////////////////////////////////////////////
237template <typename PointT> std::size_t
239 const Eigen::VectorXf &model_coefficients, const double threshold) const
240{
241 // Check if the model is valid given the user constraints
242 if (!isModelValid (model_coefficients))
243 return (0);
244 std::size_t nr_p = 0;
245
246 const auto squared_threshold = threshold * threshold;
247 // Iterate through the 3d points and calculate the distances from them to the sphere
248 for (std::size_t i = 0; i < indices_->size (); ++i)
249 {
250 // what i have:
251 // P : Sample Point
252 Eigen::Vector3d P ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
253 // C : Circle Center
254 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
255 // N : Circle (Plane) Normal
256 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
257 // r : Radius
258 double r = model_coefficients[3];
259
260 Eigen::Vector3d helper_vectorPC = P - C;
261 // 1.1. get line parameter
262 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
263
264 // Projected Point on plane
265 Eigen::Vector3d P_proj = P + lambda * N;
266 Eigen::Vector3d helper_vectorP_projC = P_proj - C;
267
268 // K : Point on Circle
269 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
270 Eigen::Vector3d distanceVector = P - K;
271
272 if (distanceVector.squaredNorm () < squared_threshold)
273 nr_p++;
274 }
275 return (nr_p);
276}
277
278//////////////////////////////////////////////////////////////////////////
279template <typename PointT> void
281 const Indices &inliers,
282 const Eigen::VectorXf &model_coefficients,
283 Eigen::VectorXf &optimized_coefficients) const
284{
285 optimized_coefficients = model_coefficients;
286
287 // Needs a set of valid model coefficients
288 if (!isModelValid (model_coefficients))
289 {
290 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Given model is invalid!\n");
291 return;
292 }
293
294 // Need more than the minimum sample size to make a difference
295 if (inliers.size () <= sample_size_)
296 {
297 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
298 return;
299 }
300
301 OptimizationFunctor functor (this, inliers);
302 Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
303 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm (num_diff);
304 Eigen::VectorXd coeff = optimized_coefficients.cast<double>();
305 int info = lm.minimize (coeff);
306 coeff.tail(3).normalize(); // normalize the cylinder axis
307 for (Eigen::Index i = 0; i < coeff.size (); ++i)
308 optimized_coefficients[i] = static_cast<float> (coeff[i]);
309
310 // Compute the L2 norm of the residuals
311 PCL_DEBUG ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n",
312 info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3], model_coefficients[4], model_coefficients[5], model_coefficients[6], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5], optimized_coefficients[6]);
313}
314
315//////////////////////////////////////////////////////////////////////////
316template <typename PointT> void
318 const Indices &inliers, const Eigen::VectorXf &model_coefficients,
319 PointCloud &projected_points, bool copy_data_fields) const
320{
321 // Needs a valid set of model coefficients
322 if (!isModelValid (model_coefficients))
323 {
324 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::projectPoints] Given model is invalid!\n");
325 return;
326 }
327
328 projected_points.header = input_->header;
329 projected_points.is_dense = input_->is_dense;
330
331 // Copy all the data fields from the input cloud to the projected one?
332 if (copy_data_fields)
333 {
334 // Allocate enough space and copy the basics
335 projected_points.resize (input_->size ());
336 projected_points.width = input_->width;
337 projected_points.height = input_->height;
338
339 using FieldList = typename pcl::traits::fieldList<PointT>::type;
340 // Iterate over each point
341 for (std::size_t i = 0; i < projected_points.size (); ++i)
342 // Iterate over each dimension
343 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[i], projected_points[i]));
344
345 // Iterate through the 3d points and calculate the distances from them to the plane
346 for (const auto &inlier : inliers)
347 {
348 // what i have:
349 // P : Sample Point
350 Eigen::Vector3d P ((*input_)[inlier].x, (*input_)[inlier].y, (*input_)[inlier].z);
351 // C : Circle Center
352 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
353 // N : Circle (Plane) Normal
354 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
355 // r : Radius
356 double r = model_coefficients[3];
357
358 Eigen::Vector3d helper_vectorPC = P - C;
359 // 1.1. get line parameter
360 //float lambda = (helper_vectorPC.dot(N)) / N.squaredNorm() ;
361 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
362 // Projected Point on plane
363 Eigen::Vector3d P_proj = P + lambda * N;
364 Eigen::Vector3d helper_vectorP_projC = P_proj - C;
365
366 // K : Point on Circle
367 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
368
369 projected_points[inlier].x = static_cast<float> (K[0]);
370 projected_points[inlier].y = static_cast<float> (K[1]);
371 projected_points[inlier].z = static_cast<float> (K[2]);
372 }
373 }
374 else
375 {
376 // Allocate enough space and copy the basics
377 projected_points.resize (inliers.size ());
378 projected_points.width = inliers.size ();
379 projected_points.height = 1;
380
381 using FieldList = typename pcl::traits::fieldList<PointT>::type;
382 // Iterate over each point
383 for (std::size_t i = 0; i < inliers.size (); ++i)
384 // Iterate over each dimension
385 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[inliers[i]], projected_points[i]));
386
387 // Iterate through the 3d points and calculate the distances from them to the plane
388 for (std::size_t i = 0; i < inliers.size (); ++i)
389 {
390 // what i have:
391 // P : Sample Point
392 Eigen::Vector3d P ((*input_)[inliers[i]].x, (*input_)[inliers[i]].y, (*input_)[inliers[i]].z);
393 // C : Circle Center
394 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
395 // N : Circle (Plane) Normal
396 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
397 // r : Radius
398 double r = model_coefficients[3];
399
400 Eigen::Vector3d helper_vectorPC = P - C;
401 // 1.1. get line parameter
402 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
403 // Projected Point on plane
404 Eigen::Vector3d P_proj = P + lambda * N;
405 Eigen::Vector3d helper_vectorP_projC = P_proj - C;
406
407 // K : Point on Circle
408 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
409
410 projected_points[i].x = static_cast<float> (K[0]);
411 projected_points[i].y = static_cast<float> (K[1]);
412 projected_points[i].z = static_cast<float> (K[2]);
413 }
414 }
415}
416
417//////////////////////////////////////////////////////////////////////////
418template <typename PointT> bool
420 const std::set<index_t> &indices,
421 const Eigen::VectorXf &model_coefficients,
422 const double threshold) const
423{
424 // Needs a valid model coefficients
425 if (!isModelValid (model_coefficients))
426 {
427 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel] Given model is invalid!\n");
428 return (false);
429 }
430
431 const auto squared_threshold = threshold * threshold;
432 for (const auto &index : indices)
433 {
434 // Calculate the distance from the point to the sphere as the difference between
435 //dist(point,sphere_origin) and sphere_radius
436
437 // what i have:
438 // P : Sample Point
439 Eigen::Vector3d P ((*input_)[index].x, (*input_)[index].y, (*input_)[index].z);
440 // C : Circle Center
441 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
442 // N : Circle (Plane) Normal
443 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
444 // r : Radius
445 double r = model_coefficients[3];
446 Eigen::Vector3d helper_vectorPC = P - C;
447 // 1.1. get line parameter
448 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
449 // Projected Point on plane
450 Eigen::Vector3d P_proj = P + lambda * N;
451 Eigen::Vector3d helper_vectorP_projC = P_proj - C;
452
453 // K : Point on Circle
454 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
455 Eigen::Vector3d distanceVector = P - K;
456
457 if (distanceVector.squaredNorm () > squared_threshold)
458 return (false);
459 }
460 return (true);
461}
462
463//////////////////////////////////////////////////////////////////////////
464template <typename PointT> bool
465pcl::SampleConsensusModelCircle3D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
466{
467 if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
468 return (false);
469
470 if (radius_min_ != std::numeric_limits<double>::lowest() && model_coefficients[3] < radius_min_)
471 {
472 PCL_DEBUG ("[pcl::SampleConsensusModelCircle3D::isModelValid] Radius of circle is too small: should be larger than %g, but is %g.\n",
473 radius_min_, model_coefficients[3]);
474 return (false);
475 }
476 if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[3] > radius_max_)
477 {
478 PCL_DEBUG ("[pcl::SampleConsensusModelCircle3D::isModelValid] Radius of circle is too big: should be smaller than %g, but is %g.\n",
479 radius_max_, model_coefficients[3]);
480 return (false);
481 }
482
483 return (true);
484}
485
486#define PCL_INSTANTIATE_SampleConsensusModelCircle3D(T) template class PCL_EXPORTS pcl::SampleConsensusModelCircle3D<T>;
487
488#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE3D_HPP_
489
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
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 3d circle model coefficients.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
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 3d circle model.
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.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given 3D circle model.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the 3d circle coefficients using the given inlier set and return them to the user.
typename SampleConsensusModel< PointT >::PointCloud PointCloud
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Compute all distances from the cloud data to a given 3D circle model.
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...
SampleConsensusModel represents the base model class.
Definition sac_model.h:71
@ K
Definition norms.h:54
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133