239 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
242 if (!isModelValid (model_coefficients))
244 std::size_t nr_p = 0;
246 const auto squared_threshold = threshold * threshold;
248 for (std::size_t i = 0; i < indices_->size (); ++i)
252 Eigen::Vector3d P ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
254 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
256 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
258 double r = model_coefficients[3];
260 Eigen::Vector3d helper_vectorPC = P - C;
262 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
265 Eigen::Vector3d P_proj = P + lambda * N;
266 Eigen::Vector3d helper_vectorP_projC = P_proj - C;
269 Eigen::Vector3d
K = C + r * helper_vectorP_projC.normalized ();
270 Eigen::Vector3d distanceVector = P -
K;
272 if (distanceVector.squaredNorm () < squared_threshold)
282 const Eigen::VectorXf &model_coefficients,
283 Eigen::VectorXf &optimized_coefficients)
const
285 optimized_coefficients = model_coefficients;
288 if (!isModelValid (model_coefficients))
290 PCL_ERROR (
"[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Given model is invalid!\n");
295 if (inliers.size () <= sample_size_)
297 PCL_ERROR (
"[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
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();
307 for (Eigen::Index i = 0; i < coeff.size (); ++i)
308 optimized_coefficients[i] =
static_cast<float> (coeff[i]);
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]);
318 const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
319 PointCloud &projected_points,
bool copy_data_fields)
const
322 if (!isModelValid (model_coefficients))
324 PCL_ERROR (
"[pcl::SampleConsensusModelCircle3D::projectPoints] Given model is invalid!\n");
328 projected_points.header = input_->header;
329 projected_points.is_dense = input_->is_dense;
332 if (copy_data_fields)
335 projected_points.resize (input_->size ());
336 projected_points.width = input_->width;
337 projected_points.height = input_->height;
339 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
341 for (std::size_t i = 0; i < projected_points.size (); ++i)
343 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[i], projected_points[i]));
346 for (
const auto &inlier : inliers)
350 Eigen::Vector3d P ((*input_)[inlier].x, (*input_)[inlier].y, (*input_)[inlier].z);
352 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
354 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
356 double r = model_coefficients[3];
358 Eigen::Vector3d helper_vectorPC = P - C;
361 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
363 Eigen::Vector3d P_proj = P + lambda * N;
364 Eigen::Vector3d helper_vectorP_projC = P_proj - C;
367 Eigen::Vector3d
K = C + r * helper_vectorP_projC.normalized ();
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]);
377 projected_points.resize (inliers.size ());
378 projected_points.width = inliers.size ();
379 projected_points.height = 1;
381 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
383 for (std::size_t i = 0; i < inliers.size (); ++i)
385 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[inliers[i]], projected_points[i]));
388 for (std::size_t i = 0; i < inliers.size (); ++i)
392 Eigen::Vector3d P ((*input_)[inliers[i]].x, (*input_)[inliers[i]].y, (*input_)[inliers[i]].z);
394 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
396 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
398 double r = model_coefficients[3];
400 Eigen::Vector3d helper_vectorPC = P - C;
402 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
404 Eigen::Vector3d P_proj = P + lambda * N;
405 Eigen::Vector3d helper_vectorP_projC = P_proj - C;
408 Eigen::Vector3d
K = C + r * helper_vectorP_projC.normalized ();
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]);
420 const std::set<index_t> &indices,
421 const Eigen::VectorXf &model_coefficients,
422 const double threshold)
const
425 if (!isModelValid (model_coefficients))
427 PCL_ERROR (
"[pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel] Given model is invalid!\n");
431 const auto squared_threshold = threshold * threshold;
432 for (
const auto &index : indices)
439 Eigen::Vector3d P ((*input_)[index].x, (*input_)[index].y, (*input_)[index].z);
441 Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
443 Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
445 double r = model_coefficients[3];
446 Eigen::Vector3d helper_vectorPC = P - C;
448 double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
450 Eigen::Vector3d P_proj = P + lambda * N;
451 Eigen::Vector3d helper_vectorP_projC = P_proj - C;
454 Eigen::Vector3d
K = C + r * helper_vectorP_projC.normalized ();
455 Eigen::Vector3d distanceVector = P -
K;
457 if (distanceVector.squaredNorm () > squared_threshold)