Point Cloud Library (PCL) 1.13.0
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<float>::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<float>::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 ();
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 }
183
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 inliers.reserve (indices_->size ());
198
199 const auto squared_threshold = threshold * threshold;
200 // Iterate through the 3d points and calculate the distances from them to the sphere
201 for (std::size_t i = 0; i < indices_->size (); ++i)
202 {
203 // what i have:
204 // P : Sample Point
205 Eigen::Vector3d P ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
206 // C : Circle Center
207 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
208 // N : Circle (Plane) Normal
209 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
210 // r : Radius
211 double r = model_coefficients[3];
212
213 Eigen::Vector3d helper_vectorPC = P - C;
214 // 1.1. get line parameter
215 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
216 // Projected Point on plane
217 Eigen::Vector3d P_proj = P + lambda * N;
218 Eigen::Vector3d helper_vectorP_projC = P_proj - C;
219
220 // K : Point on Circle
221 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
222 Eigen::Vector3d distanceVector = P - K;
223
224 if (distanceVector.squaredNorm () < squared_threshold)
225 {
226 // Returns the indices of the points whose distances are smaller than the threshold
227 inliers.push_back ((*indices_)[i]);
228 }
229 }
230}
231
232//////////////////////////////////////////////////////////////////////////
233template <typename PointT> std::size_t
235 const Eigen::VectorXf &model_coefficients, const double threshold) const
236{
237 // Check if the model is valid given the user constraints
238 if (!isModelValid (model_coefficients))
239 return (0);
240 std::size_t nr_p = 0;
241
242 const auto squared_threshold = threshold * threshold;
243 // Iterate through the 3d points and calculate the distances from them to the sphere
244 for (std::size_t i = 0; i < indices_->size (); ++i)
245 {
246 // what i have:
247 // P : Sample Point
248 Eigen::Vector3d P ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
249 // C : Circle Center
250 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
251 // N : Circle (Plane) Normal
252 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
253 // r : Radius
254 double r = model_coefficients[3];
255
256 Eigen::Vector3d helper_vectorPC = P - C;
257 // 1.1. get line parameter
258 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
259
260 // Projected Point on plane
261 Eigen::Vector3d P_proj = P + lambda * N;
262 Eigen::Vector3d helper_vectorP_projC = P_proj - C;
263
264 // K : Point on Circle
265 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
266 Eigen::Vector3d distanceVector = P - K;
267
268 if (distanceVector.squaredNorm () < squared_threshold)
269 nr_p++;
270 }
271 return (nr_p);
272}
273
274//////////////////////////////////////////////////////////////////////////
275template <typename PointT> void
277 const Indices &inliers,
278 const Eigen::VectorXf &model_coefficients,
279 Eigen::VectorXf &optimized_coefficients) const
280{
281 optimized_coefficients = model_coefficients;
282
283 // Needs a set of valid model coefficients
284 if (!isModelValid (model_coefficients))
285 {
286 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Given model is invalid!\n");
287 return;
288 }
289
290 // Need more than the minimum sample size to make a difference
291 if (inliers.size () <= sample_size_)
292 {
293 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
294 return;
295 }
296
297 OptimizationFunctor functor (this, inliers);
298 Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
299 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm (num_diff);
300 Eigen::VectorXd coeff;
301 int info = lm.minimize (coeff);
302 for (Eigen::Index i = 0; i < coeff.size (); ++i)
303 optimized_coefficients[i] = static_cast<float> (coeff[i]);
304
305 // Compute the L2 norm of the residuals
306 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",
307 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]);
308}
309
310//////////////////////////////////////////////////////////////////////////
311template <typename PointT> void
313 const Indices &inliers, const Eigen::VectorXf &model_coefficients,
314 PointCloud &projected_points, bool copy_data_fields) const
315{
316 // Needs a valid set of model coefficients
317 if (!isModelValid (model_coefficients))
318 {
319 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::projectPoints] Given model is invalid!\n");
320 return;
321 }
322
323 projected_points.header = input_->header;
324 projected_points.is_dense = input_->is_dense;
325
326 // Copy all the data fields from the input cloud to the projected one?
327 if (copy_data_fields)
328 {
329 // Allocate enough space and copy the basics
330 projected_points.resize (input_->size ());
331 projected_points.width = input_->width;
332 projected_points.height = input_->height;
333
334 using FieldList = typename pcl::traits::fieldList<PointT>::type;
335 // Iterate over each point
336 for (std::size_t i = 0; i < projected_points.size (); ++i)
337 // Iterate over each dimension
338 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[i], projected_points[i]));
339
340 // Iterate through the 3d points and calculate the distances from them to the plane
341 for (std::size_t i = 0; i < inliers.size (); ++i)
342 {
343 // what i have:
344 // P : Sample Point
345 Eigen::Vector3d P ((*input_)[inliers[i]].x, (*input_)[inliers[i]].y, (*input_)[inliers[i]].z);
346 // C : Circle Center
347 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
348 // N : Circle (Plane) Normal
349 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
350 // r : Radius
351 double r = model_coefficients[3];
352
353 Eigen::Vector3d helper_vectorPC = P - C;
354 // 1.1. get line parameter
355 //float lambda = (helper_vectorPC.dot(N)) / N.squaredNorm() ;
356 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
357 // Projected Point on plane
358 Eigen::Vector3d P_proj = P + lambda * N;
359 Eigen::Vector3d helper_vectorP_projC = P_proj - C;
360
361 // K : Point on Circle
362 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
363
364 projected_points[i].x = static_cast<float> (K[0]);
365 projected_points[i].y = static_cast<float> (K[1]);
366 projected_points[i].z = static_cast<float> (K[2]);
367 }
368 }
369 else
370 {
371 // Allocate enough space and copy the basics
372 projected_points.resize (inliers.size ());
373 projected_points.width = inliers.size ();
374 projected_points.height = 1;
375
376 using FieldList = typename pcl::traits::fieldList<PointT>::type;
377 // Iterate over each point
378 for (std::size_t i = 0; i < inliers.size (); ++i)
379 // Iterate over each dimension
380 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[inliers[i]], projected_points[i]));
381
382 // Iterate through the 3d points and calculate the distances from them to the plane
383 for (std::size_t i = 0; i < inliers.size (); ++i)
384 {
385 // what i have:
386 // P : Sample Point
387 Eigen::Vector3d P ((*input_)[inliers[i]].x, (*input_)[inliers[i]].y, (*input_)[inliers[i]].z);
388 // C : Circle Center
389 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
390 // N : Circle (Plane) Normal
391 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
392 // r : Radius
393 double r = model_coefficients[3];
394
395 Eigen::Vector3d helper_vectorPC = P - C;
396 // 1.1. get line parameter
397 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
398 // Projected Point on plane
399 Eigen::Vector3d P_proj = P + lambda * N;
400 Eigen::Vector3d helper_vectorP_projC = P_proj - C;
401
402 // K : Point on Circle
403 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
404
405 projected_points[i].x = static_cast<float> (K[0]);
406 projected_points[i].y = static_cast<float> (K[1]);
407 projected_points[i].z = static_cast<float> (K[2]);
408 }
409 }
410}
411
412//////////////////////////////////////////////////////////////////////////
413template <typename PointT> bool
415 const std::set<index_t> &indices,
416 const Eigen::VectorXf &model_coefficients,
417 const double threshold) const
418{
419 // Needs a valid model coefficients
420 if (!isModelValid (model_coefficients))
421 {
422 PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel] Given model is invalid!\n");
423 return (false);
424 }
425
426 const auto squared_threshold = threshold * threshold;
427 for (const auto &index : indices)
428 {
429 // Calculate the distance from the point to the sphere as the difference between
430 //dist(point,sphere_origin) and sphere_radius
431
432 // what i have:
433 // P : Sample Point
434 Eigen::Vector3d P ((*input_)[index].x, (*input_)[index].y, (*input_)[index].z);
435 // C : Circle Center
436 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
437 // N : Circle (Plane) Normal
438 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
439 // r : Radius
440 double r = model_coefficients[3];
441 Eigen::Vector3d helper_vectorPC = P - C;
442 // 1.1. get line parameter
443 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
444 // Projected Point on plane
445 Eigen::Vector3d P_proj = P + lambda * N;
446 Eigen::Vector3d helper_vectorP_projC = P_proj - C;
447
448 // K : Point on Circle
449 Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
450 Eigen::Vector3d distanceVector = P - K;
451
452 if (distanceVector.squaredNorm () > squared_threshold)
453 return (false);
454 }
455 return (true);
456}
457
458//////////////////////////////////////////////////////////////////////////
459template <typename PointT> bool
460pcl::SampleConsensusModelCircle3D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
461{
462 if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
463 return (false);
464
465 if (radius_min_ != std::numeric_limits<double>::lowest() && model_coefficients[3] < radius_min_)
466 {
467 PCL_DEBUG ("[pcl::SampleConsensusModelCircle3D::isModelValid] Radius of circle is too small: should be larger than %g, but is %g.\n",
468 radius_min_, model_coefficients[3]);
469 return (false);
470 }
471 if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[3] > radius_max_)
472 {
473 PCL_DEBUG ("[pcl::SampleConsensusModelCircle3D::isModelValid] Radius of circle is too big: should be smaller than %g, but is %g.\n",
474 radius_max_, model_coefficients[3]);
475 return (false);
476 }
477
478 return (true);
479}
480
481#define PCL_INSTANTIATE_SampleConsensusModelCircle3D(T) template class PCL_EXPORTS pcl::SampleConsensusModelCircle3D<T>;
482
483#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE3D_HPP_
484
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:403
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
std::size_t size() const
Definition: point_cloud.h:443
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.
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:70
@ K
Definition: norms.h:54
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
Helper functor structure for concatenate.
Definition: concatenate.h:50