Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
sac_model_torus.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009-2010, 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_TORUS_H_
42#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_TORUS_H_
43
44// clang-format off
45#include <pcl/sample_consensus/sac_model_torus.h>
46#include <pcl/common/concatenate.h>
47// clang-format on
48
49#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
50//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51template <typename PointT, typename PointNT>
52bool
54 const Indices& samples) const
55{
56 if (samples.size() != sample_size_) {
57 PCL_ERROR("[pcl::SampleConsensusTorus::isSampleGood] Wrong number of samples (is "
58 "%lu, should be %lu)!\n",
59 samples.size(),
60 sample_size_);
61 return (false);
62 }
63
64 Eigen::Vector3f n0 = Eigen::Vector3f((*normals_)[samples[0]].getNormalVector3fMap());
65 Eigen::Vector3f n1 = Eigen::Vector3f((*normals_)[samples[1]].getNormalVector3fMap());
66 Eigen::Vector3f n2 = Eigen::Vector3f((*normals_)[samples[2]].getNormalVector3fMap());
67 Eigen::Vector3f n3 = Eigen::Vector3f((*normals_)[samples[3]].getNormalVector3fMap());
68
69 // Required for numeric stability on computeModelCoefficients
70 if (std::abs((n0).cross(n1).squaredNorm()) <
71 Eigen::NumTraits<float>::dummy_precision() ||
72 std::abs((n0).cross(n2).squaredNorm()) <
73 Eigen::NumTraits<float>::dummy_precision() ||
74 std::abs((n0).cross(n3).squaredNorm()) <
75 Eigen::NumTraits<float>::dummy_precision() ||
76 std::abs((n1).cross(n2).squaredNorm()) <
77 Eigen::NumTraits<float>::dummy_precision() ||
78 std::abs((n1).cross(n3).squaredNorm()) <
79 Eigen::NumTraits<float>::dummy_precision()) {
80 PCL_ERROR("[pcl::SampleConsensusModelEllipse3D::isSampleGood] Sample points "
81 "normals too similar or collinear!\n");
82 return (false);
83 }
84 return (true);
85}
86
87//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
88float
89crossDot(const Eigen::Vector3f& v1, const Eigen::Vector3f& v2, const Eigen::Vector3f& v3)
90{
91 return v1.cross(v2).dot(v3);
92}
93
94//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
95template <typename PointT, typename PointNT>
96bool
98 const Indices& samples, Eigen::VectorXf& model_coefficients) const
99{
100
101 // Make sure that the samples are valid
102 if (!isSampleGood(samples)) {
103 PCL_ERROR("[pcl::SampleConsensusModelTorus::computeModelCoefficients] Invalid set "
104 "of samples given!\n");
105 return (false);
106 }
107
108 if (!normals_) {
109 PCL_ERROR("[pcl::SampleConsensusModelTorus::computeModelCoefficients] No input "
110 "dataset containing normals was given!\n");
111 return (false);
112 }
113 // Find axis using:
114
115 // @article{article,
116 // author = {Lukacs, G. and Marshall, David and Martin, R.},
117 // year = {2001},
118 // month = {09},
119 // pages = {},
120 // title = {Geometric Least-Squares Fitting of Spheres, Cylinders, Cones and Tori}
121 //}
122
123 const Eigen::Vector3f n0 = Eigen::Vector3f((*normals_)[samples[0]].getNormalVector3fMap());
124 const Eigen::Vector3f n1 = Eigen::Vector3f((*normals_)[samples[1]].getNormalVector3fMap());
125 const Eigen::Vector3f n2 = Eigen::Vector3f((*normals_)[samples[2]].getNormalVector3fMap());
126 const Eigen::Vector3f n3 = Eigen::Vector3f((*normals_)[samples[3]].getNormalVector3fMap());
127
128 const Eigen::Vector3f p0 = Eigen::Vector3f((*input_)[samples[0]].getVector3fMap());
129 const Eigen::Vector3f p1 = Eigen::Vector3f((*input_)[samples[1]].getVector3fMap());
130 const Eigen::Vector3f p2 = Eigen::Vector3f((*input_)[samples[2]].getVector3fMap());
131 const Eigen::Vector3f p3 = Eigen::Vector3f((*input_)[samples[3]].getVector3fMap());
132
133 const float a01 = crossDot(n0, n1, n2);
134 const float b01 = crossDot(n0, n1, n3);
135 const float a0 = crossDot(p2 - p1, n0, n2);
136 const float a1 = crossDot(p0 - p2, n1, n2);
137 const float b0 = crossDot(p3 - p1, n0, n3);
138 const float b1 = crossDot(p0 - p3, n1, n3);
139 const float a = crossDot(p0 - p2, p1 - p0, n2);
140 const float b = crossDot(p0 - p3, p1 - p0, n3);
141
142 // a10*t0*t1 + a0*t0 + a1*t1 + a = 0
143 // b10*t0*t1 + b0*t0 + b1*t1 + b = 0
144 //
145 // (a0 - b0*a10/b10)* t0 + (a1-b1*a10/b10) *t1 + a - b*a10/b10
146 // t0 = k * t1 + p
147
148 const float k = -(a1 - b1 * a01 / b01) / (a0 - b0 * a01 / b01);
149 const float p = -(a - b * a01 / b01) / (a0 - b0 * a01 / b01);
150
151 // Second deg eqn.
152 //
153 // b10*k*t1*t1 + b10*p*t1 | + b0*k *t1 + b0*p | + b1*t1 | + b = 0
154 //
155 // (b10*k) * t1*t1 + (b10*p + b0*k + b1) * t1 + (b0*p + b)
156
157 const float _a = (b01 * k);
158 const float _b = (b01 * p + b0 * k + b1);
159 const float _c = (b0 * p + b);
160
161 const float eps = Eigen::NumTraits<float>::dummy_precision();
162
163 // Check for imaginary solutions, or small denominators.
164 if ((_b * _b - 4 * _a * _c) < 0 || std::abs(a0 - b0 * a01) < eps ||
165 std::abs(b01) < eps || std::abs(_a) < eps) {
166 PCL_DEBUG("[pcl::SampleConsensusModelTorus::computeModelCoefficients] Can't "
167 "compute model coefficients with this method!\n");
168 return (false);
169 }
170
171 const float s0 = (-_b + std::sqrt(_b * _b - 4 * _a * _c)) / (2 * _a);
172 const float s1 = (-_b - std::sqrt(_b * _b - 4 * _a * _c)) / (2 * _a);
173
174 float r_maj_stddev_cycle1 = std::numeric_limits<float>::max();
175
176 for (float s : {s0, s1}) {
177
178 const float t1 = s;
179 const float t0 = k * t1 + p;
180
181 // Direction vector
182 Eigen::Vector3f d = ((p1 + n1 * t1) - (p0 + n0 * t0));
183 d.normalize();
184 // Flip direction, so that the first element of the direction vector is
185 // positive, for consistency.
186 if (d[0] < 0) {
187 d *= -1;
188 }
189
190 // Flip normals if required. Note |d| = 1
191 // d
192 // if (n0.dot(d) / n0.norm() < M_PI / 2 ) n0 = -n0;
193 // if (n1.dot(d) / n1.norm() < M_PI / 2 ) n1 = -n1;
194 // if (n2.dot(d) / n2.norm() < M_PI / 2 ) n2 = -n2;
195 // if (n3.dot(d) / n3.norm() < M_PI / 2 ) n3 = -n3;
197 // We fit the points to the plane of the torus.
198 // Ax + By + Cz + D = 0
199 // We know that all for each point plus its normal
200 // times the minor radius will give us a point
201 // in that plane
202 // Pplane_i = P_i + n_i * r
203 // we substitute A,x,B,y,C,z
204 // dx *( P_i_x + n_i_x * r ) + dy *( P_i_y + n_i_y * r ) +dz *( P_i_z + n_i_z * r )
205 // + D = 0 and finally (dx*P_i_x + dy*P_i_y + dz*P_i_z) + (dx*n_i_x + dy*n_i_y +
206 // dz*n_i_z ) * r + D = 0 We can set up a linear least squares system of two
207 // variables r and D
208 //
209 Eigen::MatrixXf A(4, 2);
210 A << d.dot(n0), 1, d.dot(n1), 1, d.dot(n2), 1, d.dot(n3), 1;
211
212 Eigen::Matrix<float, -1, -1> B(4, 1);
213 B << -d.dot(p0), -d.dot(p1), -d.dot(p2), -d.dot(p3);
214
215 Eigen::Matrix<float, -1, -1> sol;
216 sol = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);
217
218 const float r_min = -sol(0);
219 const float D = sol(1);
220
221 // Axis line and plane intersect to find the centroid of the torus
222 // We take a random point on the line. We find P_rand + lambda * d belongs in the
223 // plane
224
225 const Eigen::Vector3f Pany = (p1 + n1 * t1);
226
227 const float lambda = (-d.dot(Pany) - D) / d.dot(d);
228
229 const Eigen::Vector3f centroid = Pany + d * lambda;
230
231 // Finally, the major radius. The least square solution will be
232 // the average in this case.
233 const float r_maj = std::sqrt(((p0 - r_min * n0 - centroid).squaredNorm() +
234 (p1 - r_min * n1 - centroid).squaredNorm() +
235 (p2 - r_min * n2 - centroid).squaredNorm() +
236 (p3 - r_min * n3 - centroid).squaredNorm()) /
237 4.f);
238
239 const float r_maj_stddev =
240 std::sqrt((std::pow(r_maj - (p0 - r_min * n0 - centroid).norm(), 2) +
241 std::pow(r_maj - (p1 - r_min * n1 - centroid).norm(), 2) +
242 std::pow(r_maj - (p2 - r_min * n2 - centroid).norm(), 2) +
243 std::pow(r_maj - (p3 - r_min * n3 - centroid).norm(), 2)) /
244 4.f);
245 // We select the minimum stddev cycle
246 if (r_maj_stddev < r_maj_stddev_cycle1) {
247 r_maj_stddev_cycle1 = r_maj_stddev;
248 }
249 else {
250 break;
251 }
252
253 model_coefficients.resize(model_size_);
254 model_coefficients[0] = r_maj;
255 model_coefficients[1] = r_min;
256
257 model_coefficients[2] = centroid[0];
258 model_coefficients[3] = centroid[1];
259 model_coefficients[4] = centroid[2];
260
261 model_coefficients[5] = d[0];
262 model_coefficients[6] = d[1];
263 model_coefficients[7] = d[2];
264 }
265 return true;
266}
267
268//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
269template <typename PointT, typename PointNT>
270void
272 const Eigen::VectorXf& model_coefficients, std::vector<double>& distances) const
273{
274
275 if (!isModelValid(model_coefficients)) {
276 distances.clear();
277 return;
278 }
279
280 distances.resize(indices_->size());
281
282 // Iterate through the 3d points and calculate the distances to the closest torus
283 // point
284 for (std::size_t i = 0; i < indices_->size(); ++i) {
285 const Eigen::Vector3f pt = (*input_)[(*indices_)[i]].getVector3fMap();
286 const Eigen::Vector3f pt_n = (*normals_)[(*indices_)[i]].getNormalVector3fMap();
287
288 Eigen::Vector3f torus_closest;
289 projectPointToTorus(pt, pt_n, model_coefficients, torus_closest);
290
291 distances[i] = (torus_closest - pt).norm();
292 }
293}
294
295//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
296template <typename PointT, typename PointNT>
297void
299 const Eigen::VectorXf& model_coefficients, const double threshold, Indices& inliers)
300{
301 // Check if the model is valid given the user constraints
302 if (!isModelValid(model_coefficients)) {
303 inliers.clear();
304 return;
305 }
306 inliers.clear();
307 error_sqr_dists_.clear();
308 inliers.reserve(indices_->size());
309 error_sqr_dists_.reserve(indices_->size());
310
311 for (std::size_t i = 0; i < indices_->size(); ++i) {
312 const Eigen::Vector3f pt = (*input_)[(*indices_)[i]].getVector3fMap();
313 const Eigen::Vector3f pt_n = (*normals_)[(*indices_)[i]].getNormalVector3fMap();
314
315 Eigen::Vector3f torus_closest;
316 projectPointToTorus(pt, pt_n, model_coefficients, torus_closest);
317
318 const float distance = (torus_closest - pt).norm();
319
320 if (distance < threshold) {
321 // Returns the indices of the points whose distances are smaller than the
322 // threshold
323 inliers.push_back((*indices_)[i]);
324 error_sqr_dists_.push_back(distance);
325 }
326 }
327}
328
329//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
330template <typename PointT, typename PointNT>
331std::size_t
333 const Eigen::VectorXf& model_coefficients, const double threshold) const
334{
335 if (!isModelValid(model_coefficients))
336 return (0);
337
338 std::size_t nr_p = 0;
339
340 for (std::size_t i = 0; i < indices_->size(); ++i) {
341 const Eigen::Vector3f pt = (*input_)[(*indices_)[i]].getVector3fMap();
342 const Eigen::Vector3f pt_n = (*normals_)[(*indices_)[i]].getNormalVector3fMap();
343
344 Eigen::Vector3f torus_closest;
345 projectPointToTorus(pt, pt_n, model_coefficients, torus_closest);
346
347 const float distance = (torus_closest - pt).norm();
348
349 if (distance < threshold) {
350 nr_p++;
351 }
352 }
353 return (nr_p);
354}
355
356//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
357template <typename PointT, typename PointNT>
358void
360 const Indices& inliers,
361 const Eigen::VectorXf& model_coefficients,
362 Eigen::VectorXf& optimized_coefficients) const
363{
364
365 optimized_coefficients = model_coefficients;
366
367 // Needs a set of valid model coefficients
368 if (!isModelValid(model_coefficients)) {
369 PCL_ERROR("[pcl::SampleConsensusModelTorus::optimizeModelCoefficients] Given model "
370 "is invalid!\n");
371 return;
372 }
373
374 // Need more than the minimum sample size to make a difference
375 if (inliers.size() <= sample_size_) {
376 PCL_ERROR("[pcl::SampleConsensusModelTorus::optimizeModelCoefficients] Not enough "
377 "inliers to refine/optimize the model's coefficients (%lu)! Returning "
378 "the same coefficients.\n",
379 inliers.size());
380 return;
381 }
382
383 OptimizationFunctor functor(this, inliers);
384 Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor);
385 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm(
386 num_diff);
387
388 Eigen::VectorXd coeff = model_coefficients.cast<double>();
389 int info = lm.minimize(coeff);
390
391 if (!info) {
392 PCL_ERROR(
393 "[pcl::SampleConsensusModelTorus::optimizeModelCoefficients] Optimizer returned"
394 "with error (%i)! Returning ",
395 info);
396 return;
397 }
398
399 // Normalize direction vector
400 coeff.tail<3>().normalize();
401 optimized_coefficients = coeff.cast<float>();
402}
403
404//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
405template <typename PointT, typename PointNT>
406void
408 const Eigen::Vector3f& p_in,
409 const Eigen::Vector3f& p_n,
410 const Eigen::VectorXf& model_coefficients,
411 Eigen::Vector3f& pt_out) const
412{
413
414 // Fetch optimization parameters
415 const float& R = model_coefficients[0];
416 const float& r = model_coefficients[1];
417
418 const float& x0 = model_coefficients[2];
419 const float& y0 = model_coefficients[3];
420 const float& z0 = model_coefficients[4];
421
422 const float& nx = model_coefficients[5];
423 const float& ny = model_coefficients[6];
424 const float& nz = model_coefficients[7];
425
426 // Normal of the plane where the torus circle lies
427 Eigen::Vector3f n{nx, ny, nz};
428 n.normalize();
429
430 Eigen::Vector3f pt0{x0, y0, z0};
431
432 // Ax + By + Cz + D = 0
433 const float D = -n.dot(pt0);
434
435 // Project to the torus circle plane folling the point normal
436 // we want to find lambda such that p + pn_n*lambda lies on the
437 // torus plane.
438 // A*(pt_x + lambda*pn_x) + B *(pt_y + lambda*pn_y) + ... + D = 0
439 // given that: n = [A,B,C]
440 // n.dot(P) + lambda*n.dot(pn) + D = 0
441 //
442
443 Eigen::Vector3f pt_proj;
444 // If the point lies in the torus plane, we just use it as projected
445
446 // C++20 -> [[likely]]
447 if (std::abs(n.dot(p_n)) > Eigen::NumTraits<float>::dummy_precision()) {
448 float lambda = (-D - n.dot(p_in)) / n.dot(p_n);
449 pt_proj = p_in + lambda * p_n;
450 }
451 else {
452 pt_proj = p_in;
453 }
454
455 // Closest point from the inner circle to the current point
456 const Eigen::Vector3f circle_closest = (pt_proj - pt0).normalized() * R + pt0;
457
458 // From the that closest point we move towards the goal point until we
459 // meet the surface of the torus
460 pt_out = (p_in - circle_closest).normalized() * r + circle_closest;
461}
462
463//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
464template <typename PointT, typename PointNT>
465void
467 const Indices& inliers,
468 const Eigen::VectorXf& model_coefficients,
469 PointCloud& projected_points,
470 bool copy_data_fields) const
471{
472 // Needs a valid set of model coefficients
473 if (!isModelValid(model_coefficients)) {
474 PCL_ERROR(
475 "[pcl::SampleConsensusModelCylinder::projectPoints] Given model is invalid!\n");
476 return;
477 }
478
479 // Copy all the data fields from the input cloud to the projected one?
480 if (copy_data_fields) {
481 // Allocate enough space and copy the basics
482 projected_points.resize(input_->size());
483 projected_points.width = input_->width;
484 projected_points.height = input_->height;
485
486 using FieldList = typename pcl::traits::fieldList<PointT>::type;
487 // Iterate over each point
488 for (std::size_t i = 0; i < input_->size(); ++i)
489 // Iterate over each dimension
490 pcl::for_each_type<FieldList>(
491 NdConcatenateFunctor<PointT, PointT>((*input_)[i], projected_points[i]));
492
493 // Iterate through the 3d points and calculate the distances from them to the plane
494 for (const auto& inlier : inliers) {
495 Eigen::Vector3f q;
496 const Eigen::Vector3f pt_n = (*normals_)[inlier].getNormalVector3fMap();
497 projectPointToTorus(
498 (*input_)[inlier].getVector3fMap(), pt_n, model_coefficients, q);
499 projected_points[inlier].getVector3fMap() = q;
500 }
501 }
502 else {
503 // Allocate enough space and copy the basics
504 projected_points.resize(inliers.size());
505 projected_points.width = inliers.size();
506 projected_points.height = 1;
507
508 using FieldList = typename pcl::traits::fieldList<PointT>::type;
509 // Iterate over each point
510 for (std::size_t i = 0; i < inliers.size(); ++i) {
511 // Iterate over each dimension
512 pcl::for_each_type<FieldList>(NdConcatenateFunctor<PointT, PointT>(
513 (*input_)[inliers[i]], projected_points[i]));
514 }
515
516 for (const auto& inlier : inliers) {
517 Eigen::Vector3f q;
518 const Eigen::Vector3f pt_n = (*normals_)[inlier].getNormalVector3fMap();
519 projectPointToTorus(
520 (*input_)[inlier].getVector3fMap(), pt_n, model_coefficients, q);
521 projected_points[inlier].getVector3fMap() = q;
522 }
523 }
524}
525
526//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
527template <typename PointT, typename PointNT>
528bool
530 const std::set<index_t>& indices,
531 const Eigen::VectorXf& model_coefficients,
532 const double threshold) const
533{
534
535 for (const auto& index : indices) {
536 const Eigen::Vector3f pt_n = (*normals_)[index].getNormalVector3fMap();
537 Eigen::Vector3f torus_closest;
538 projectPointToTorus((*input_)[index].getVector3fMap(), pt_n, model_coefficients, torus_closest);
539
540 if (((*input_)[index].getVector3fMap() - torus_closest).squaredNorm() > threshold * threshold)
541 return (false);
542 }
543 return true;
544}
545
546//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
547template <typename PointT, typename PointNT>
548bool
550 const Eigen::VectorXf& model_coefficients) const
551{
552 if (!SampleConsensusModel<PointT>::isModelValid(model_coefficients))
553 return (false);
554
555 if (radius_min_ != std::numeric_limits<double>::lowest() &&
556 (model_coefficients[0] < radius_min_ || model_coefficients[1] < radius_min_)) {
557 PCL_DEBUG(
558 "[pcl::SampleConsensusModelTorus::isModelValid] Major radius OR minor radius "
559 "of torus is/are too small: should be larger than %g, but are {%g, %g}.\n",
560 radius_min_,
561 model_coefficients[0],
562 model_coefficients[1]);
563 return (false);
564 }
565 if (radius_max_ != std::numeric_limits<double>::max() &&
566 (model_coefficients[0] > radius_max_ || model_coefficients[1] > radius_max_)) {
567 PCL_DEBUG(
568 "[pcl::SampleConsensusModelTorus::isModelValid] Major radius OR minor radius "
569 "of torus is/are too big: should be smaller than %g, but are {%g, %g}.\n",
570 radius_max_,
571 model_coefficients[0],
572 model_coefficients[1]);
573 return (false);
574 }
575 return (true);
576}
577
578#define PCL_INSTANTIATE_SampleConsensusModelTorus(PointT, PointNT) \
579 template class PCL_EXPORTS pcl::SampleConsensusModelTorus<PointT, PointNT>;
580
581#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_TORUS_H_
SampleConsensusModel represents the base model class.
Definition sac_model.h:71
void projectPointToTorus(const Eigen::Vector3f &pt, const Eigen::Vector3f &pt_n, const Eigen::VectorXf &model_coefficients, Eigen::Vector3f &pt_proj) const
Project a point onto a torus given by its model coefficients (radii, torus_center_point,...
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.
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 torus model coefficients.
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid torus model, compute the model coefficients fr...
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the torus coefficients using the given inlier set and return them to the user.
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 isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given torus model.
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 torus model.
@ B
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