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)
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 (fabs (input_->points[samples[0]].x - input_->points[samples[1]].x) <= std::numeric_limits<float>::epsilon () &&
74 fabs (input_->points[samples[0]].y - input_->points[samples[1]].y) <= std::numeric_limits<float>::epsilon () &&
75 fabs (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)
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 (
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 = fabs (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;
166 d_normal = (std::min) (d_normal, M_PI - d_normal);
168 distances[i] = fabs (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 (
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 = fabs (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;
209 d_normal = (std::min) (d_normal, M_PI - d_normal);
211 double distance = fabs (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>
int 227 const Eigen::VectorXf &model_coefficients,
const double threshold)
230 if (!isModelValid (model_coefficients))
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 (
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 = fabs (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;
256 d_normal = (std::min) (d_normal, M_PI - d_normal);
258 if (fabs (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)
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 tmp_inliers_ = &inliers;
286 OptimizationFunctor functor (static_cast<int> (inliers.size ()),
this);
287 Eigen::NumericalDiff<OptimizationFunctor > num_diff (functor);
288 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>,
float> lm (num_diff);
289 int info = lm.minimize (optimized_coefficients);
292 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",
293 info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
294 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]);
296 Eigen::Vector3f line_dir (optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5]);
297 line_dir.normalize ();
298 optimized_coefficients[3] = line_dir[0];
299 optimized_coefficients[4] = line_dir[1];
300 optimized_coefficients[5] = line_dir[2];
304 template <
typename Po
intT,
typename Po
intNT>
void 306 const std::vector<int> &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields)
309 if (model_coefficients.size () != 7)
311 PCL_ERROR (
"[pcl::SampleConsensusModelCylinder::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
315 projected_points.
header = input_->header;
316 projected_points.
is_dense = input_->is_dense;
318 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
319 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
320 float ptdotdir = line_pt.dot (line_dir);
321 float dirdotdir = 1.0f / line_dir.dot (line_dir);
324 if (copy_data_fields)
327 projected_points.
points.resize (input_->points.size ());
328 projected_points.
width = input_->width;
329 projected_points.
height = input_->height;
333 for (
size_t i = 0; i < projected_points.
points.size (); ++i)
338 for (
size_t i = 0; i < inliers.size (); ++i)
340 Eigen::Vector4f p (input_->points[inliers[i]].x,
341 input_->points[inliers[i]].y,
342 input_->points[inliers[i]].z,
345 float k = (p.dot (line_dir) - ptdotdir) * dirdotdir;
348 pp.matrix () = line_pt + k * line_dir;
350 Eigen::Vector4f dir = p - pp;
354 pp += dir * model_coefficients[6];
360 projected_points.
points.resize (inliers.size ());
361 projected_points.
width = static_cast<uint32_t> (inliers.size ());
362 projected_points.
height = 1;
366 for (
size_t i = 0; i < inliers.size (); ++i)
371 for (
size_t i = 0; i < inliers.size (); ++i)
376 float k = (p.dot (line_dir) - ptdotdir) * dirdotdir;
378 pp.matrix () = line_pt + k * line_dir;
380 Eigen::Vector4f dir = p - pp;
384 pp += dir * model_coefficients[6];
390 template <
typename Po
intT,
typename Po
intNT>
bool 392 const std::set<int> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
395 if (model_coefficients.size () != 7)
397 PCL_ERROR (
"[pcl::SampleConsensusModelCylinder::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
401 for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
406 Eigen::Vector4f pt (input_->points[*it].x, input_->points[*it].y, input_->points[*it].z, 0);
407 if (fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]) > threshold)
415 template <
typename Po
intT,
typename Po
intNT>
double 417 const Eigen::Vector4f &pt,
const Eigen::VectorXf &model_coefficients)
419 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
420 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
425 template <
typename Po
intT,
typename Po
intNT>
void 427 const Eigen::Vector4f &pt,
const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj)
429 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
430 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
432 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) * line_dir.dot (line_dir);
433 pt_proj = line_pt + k * line_dir;
435 Eigen::Vector4f dir = pt - pt_proj;
439 pt_proj += dir * model_coefficients[6];
443 template <
typename Po
intT,
typename Po
intNT>
bool 450 if (eps_angle_ > 0.0)
453 Eigen::Vector4f coeff;
454 coeff[0] = model_coefficients[3];
455 coeff[1] = model_coefficients[4];
456 coeff[2] = model_coefficients[5];
459 Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
460 double angle_diff = fabs (
getAngle3D (axis, coeff));
461 angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
463 if (angle_diff > eps_angle_)
467 if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[6] < radius_min_)
469 if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[6] > radius_max_)
475 #define PCL_INSTANTIATE_SampleConsensusModelCylinder(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelCylinder<PointT, PointNT>; 477 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_ void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)
Compute all distances from the cloud data to a given cylinder model.
double getAngle3D(const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree=false)
Compute the smallest angle between two 3D vectors in radians (default) or degree.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
Recompute the cylinder coefficients using the given inlier set and return them to the user.
bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
Verify whether a subset of indices verifies the given cylinder model coefficients.
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients)
Check whether the given index samples can form a valid cylinder model, compute the model coefficients...
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst
uint32_t height
The point cloud height (if organized as an image-structure).
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients)
Check whether a model is valid given the user constraints.
double pointToLineDistance(const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients)
Get the distance from a point to a line (represented by a point and a direction)
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)
Count all the points which respect the given model coefficients as inliers.
uint32_t width
The point cloud width (if organized as an image-structure).
SampleConsensusModel represents the base model class.
double sqrPointToLineDistance(const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir)
Get the square distance from a point to a line (represented by a point and a direction)
bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
float distance(const PointT &p1, const PointT &p2)
pcl::PCLHeader header
The point cloud header.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, std::vector< int > &inliers)
Select all the points which respect the given model coefficients as inliers.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void projectPointToCylinder(const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj)
Project a point onto a cylinder given by its model coefficients (point_on_axis, axis_direction,...
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
Helper functor structure for concatenate.
void projectPoints(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true)
Create a new point cloud with inliers projected onto the cylinder model.