41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_
44 #include <pcl/sample_consensus/eigen.h>
45 #include <pcl/sample_consensus/sac_model_cylinder.h>
46 #include <pcl/common/concatenate.h>
49 template <
typename Po
intT,
typename Po
intNT>
bool
56 template <
typename Po
intT,
typename Po
intNT>
bool
58 const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
const
61 if (samples.size () != 2)
63 PCL_ERROR (
"[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
69 PCL_ERROR (
"[pcl::SampleConsensusModelCylinder::computeModelCoefficients] No input dataset containing normals was given!\n");
73 if (std::abs (input_->points[samples[0]].x - input_->points[samples[1]].x) <= std::numeric_limits<float>::epsilon () &&
74 std::abs (input_->points[samples[0]].y - input_->points[samples[1]].y) <= std::numeric_limits<float>::epsilon () &&
75 std::abs (input_->points[samples[0]].z - input_->points[samples[1]].z) <= std::numeric_limits<float>::epsilon ())
80 Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0);
81 Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0);
83 Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0);
84 Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0);
85 Eigen::Vector4f w = n1 + p1 - p2;
87 float a = n1.dot (n1);
88 float b = n1.dot (n2);
89 float c = n2.dot (n2);
92 float denominator = a*c - b*b;
95 if (denominator < 1e-8)
98 tc = (b > c ? d / b : e / c);
102 sc = (b*e - c*d) / denominator;
103 tc = (a*e - b*d) / denominator;
107 Eigen::Vector4f line_pt = p1 + n1 + sc * n1;
108 Eigen::Vector4f line_dir = p2 + tc * n2 - line_pt;
109 line_dir.normalize ();
111 model_coefficients.resize (7);
113 model_coefficients[0] = line_pt[0];
114 model_coefficients[1] = line_pt[1];
115 model_coefficients[2] = line_pt[2];
117 model_coefficients[3] = line_dir[0];
118 model_coefficients[4] = line_dir[1];
119 model_coefficients[5] = line_dir[2];
123 if (model_coefficients[6] > radius_max_ || model_coefficients[6] < radius_min_)
130 template <
typename Po
intT,
typename Po
intNT>
void
132 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
135 if (!isModelValid (model_coefficients))
141 distances.resize (indices_->size ());
143 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
144 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
145 float ptdotdir = line_pt.dot (line_dir);
146 float dirdotdir = 1.0f / line_dir.dot (line_dir);
148 for (std::size_t i = 0; i < indices_->size (); ++i)
153 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
154 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
156 double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
159 float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
160 Eigen::Vector4f pt_proj = line_pt + k * line_dir;
161 Eigen::Vector4f dir = pt - pt_proj;
165 double d_normal = std::abs (
getAngle3D (n, dir));
166 d_normal = (std::min) (d_normal, M_PI - d_normal);
168 distances[i] = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
173 template <
typename Po
intT,
typename Po
intNT>
void
175 const Eigen::VectorXf &model_coefficients,
const double threshold, std::vector<int> &inliers)
178 if (!isModelValid (model_coefficients))
185 inliers.resize (indices_->size ());
186 error_sqr_dists_.resize (indices_->size ());
188 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
189 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
190 float ptdotdir = line_pt.dot (line_dir);
191 float dirdotdir = 1.0f / line_dir.dot (line_dir);
193 for (std::size_t i = 0; i < indices_->size (); ++i)
197 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
198 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
199 double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
202 float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
203 Eigen::Vector4f pt_proj = line_pt + k * line_dir;
204 Eigen::Vector4f dir = pt - pt_proj;
208 double d_normal = std::abs (
getAngle3D (n, dir));
209 d_normal = (std::min) (d_normal, M_PI - d_normal);
211 double distance = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
215 inliers[nr_p] = (*indices_)[i];
220 inliers.resize (nr_p);
221 error_sqr_dists_.resize (nr_p);
225 template <
typename Po
intT,
typename Po
intNT> std::size_t
227 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
230 if (!isModelValid (model_coefficients))
233 std::size_t nr_p = 0;
235 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
236 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
237 float ptdotdir = line_pt.dot (line_dir);
238 float dirdotdir = 1.0f / line_dir.dot (line_dir);
240 for (std::size_t i = 0; i < indices_->size (); ++i)
244 Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
245 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
246 double d_euclid = std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
249 float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
250 Eigen::Vector4f pt_proj = line_pt + k * line_dir;
251 Eigen::Vector4f dir = pt - pt_proj;
255 double d_normal = std::abs (
getAngle3D (n, dir));
256 d_normal = (std::min) (d_normal, M_PI - d_normal);
258 if (std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
265 template <
typename Po
intT,
typename Po
intNT>
void
267 const std::vector<int> &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const
269 optimized_coefficients = model_coefficients;
272 if (model_coefficients.size () != 7)
274 PCL_ERROR (
"[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
278 if (inliers.empty ())
280 PCL_DEBUG (
"[pcl::SampleConsensusModelCylinder:optimizeModelCoefficients] Inliers vector empty! Returning the same coefficients.\n");
284 OptimizationFunctor functor (
this, inliers);
285 Eigen::NumericalDiff<OptimizationFunctor > num_diff (functor);
286 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>,
float> lm (num_diff);
287 int info = lm.minimize (optimized_coefficients);
290 PCL_DEBUG (
"[pcl::SampleConsensusModelCylinder::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",
291 info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
292 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]);
294 Eigen::Vector3f line_dir (optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5]);
295 line_dir.normalize ();
296 optimized_coefficients[3] = line_dir[0];
297 optimized_coefficients[4] = line_dir[1];
298 optimized_coefficients[5] = line_dir[2];
302 template <
typename Po
intT,
typename Po
intNT>
void
304 const std::vector<int> &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields)
const
307 if (model_coefficients.size () != 7)
309 PCL_ERROR (
"[pcl::SampleConsensusModelCylinder::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
313 projected_points.
header = input_->header;
314 projected_points.
is_dense = input_->is_dense;
316 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
317 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
318 float ptdotdir = line_pt.dot (line_dir);
319 float dirdotdir = 1.0f / line_dir.dot (line_dir);
322 if (copy_data_fields)
325 projected_points.
points.resize (input_->points.size ());
326 projected_points.
width = input_->width;
327 projected_points.
height = input_->height;
331 for (std::size_t i = 0; i < projected_points.
points.size (); ++i)
336 for (
const int &inlier : inliers)
338 Eigen::Vector4f p (input_->points[inlier].x,
339 input_->points[inlier].y,
340 input_->points[inlier].z,
343 float k = (p.dot (line_dir) - ptdotdir) * dirdotdir;
346 pp.matrix () = line_pt + k * line_dir;
348 Eigen::Vector4f dir = p - pp;
352 pp += dir * model_coefficients[6];
358 projected_points.
points.resize (inliers.size ());
360 projected_points.
height = 1;
364 for (std::size_t i = 0; i < inliers.size (); ++i)
369 for (std::size_t i = 0; i < inliers.size (); ++i)
374 float k = (p.dot (line_dir) - ptdotdir) * dirdotdir;
376 pp.matrix () = line_pt + k * line_dir;
378 Eigen::Vector4f dir = p - pp;
382 pp += dir * model_coefficients[6];
388 template <
typename Po
intT,
typename Po
intNT>
bool
390 const std::set<int> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const
393 if (model_coefficients.size () != 7)
395 PCL_ERROR (
"[pcl::SampleConsensusModelCylinder::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
399 for (
const int &index : indices)
404 Eigen::Vector4f pt (input_->points[index].x, input_->points[index].y, input_->points[index].z, 0);
405 if (std::abs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]) > threshold)
413 template <
typename Po
intT,
typename Po
intNT>
double
415 const Eigen::Vector4f &pt,
const Eigen::VectorXf &model_coefficients)
const
417 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
418 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
423 template <
typename Po
intT,
typename Po
intNT>
void
425 const Eigen::Vector4f &pt,
const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj)
const
427 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
428 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
430 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) * line_dir.dot (line_dir);
431 pt_proj = line_pt + k * line_dir;
433 Eigen::Vector4f dir = pt - pt_proj;
437 pt_proj += dir * model_coefficients[6];
441 template <
typename Po
intT,
typename Po
intNT>
bool
448 if (eps_angle_ > 0.0)
451 Eigen::Vector4f coeff;
452 coeff[0] = model_coefficients[3];
453 coeff[1] = model_coefficients[4];
454 coeff[2] = model_coefficients[5];
457 Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
458 double angle_diff = std::abs (
getAngle3D (axis, coeff));
459 angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
461 if (angle_diff > eps_angle_)
465 if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[6] < radius_min_)
467 if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[6] > radius_max_)
473 #define PCL_INSTANTIATE_SampleConsensusModelCylinder(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelCylinder<PointT, PointNT>;
475 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_