Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
sac_model_cylinder.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, 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_MODEL_CYLINDER_H_
42 #define PCL_SAMPLE_CONSENSUS_MODEL_CYLINDER_H_
43 
44 #include <pcl/sample_consensus/sac_model.h>
45 #include <pcl/sample_consensus/model_types.h>
46 #include <pcl/common/common.h>
47 #include <pcl/common/distances.h>
48 
49 namespace pcl
50 {
51  /** \brief @b SampleConsensusModelCylinder defines a model for 3D cylinder segmentation.
52  * The model coefficients are defined as:
53  * - \b point_on_axis.x : the X coordinate of a point located on the cylinder axis
54  * - \b point_on_axis.y : the Y coordinate of a point located on the cylinder axis
55  * - \b point_on_axis.z : the Z coordinate of a point located on the cylinder axis
56  * - \b axis_direction.x : the X coordinate of the cylinder's axis direction
57  * - \b axis_direction.y : the Y coordinate of the cylinder's axis direction
58  * - \b axis_direction.z : the Z coordinate of the cylinder's axis direction
59  * - \b radius : the cylinder's radius
60  *
61  * \author Radu Bogdan Rusu
62  * \ingroup sample_consensus
63  */
64  template <typename PointT, typename PointNT>
66  {
67  public:
76 
80 
81  typedef boost::shared_ptr<SampleConsensusModelCylinder> Ptr;
82 
83  /** \brief Constructor for base SampleConsensusModelCylinder.
84  * \param[in] cloud the input point cloud dataset
85  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
86  */
87  SampleConsensusModelCylinder (const PointCloudConstPtr &cloud, bool random = false)
88  : SampleConsensusModel<PointT> (cloud, random)
90  , axis_ (Eigen::Vector3f::Zero ())
91  , eps_angle_ (0)
92  {
93  model_name_ = "SampleConsensusModelCylinder";
94  sample_size_ = 2;
95  model_size_ = 7;
96  }
97 
98  /** \brief Constructor for base SampleConsensusModelCylinder.
99  * \param[in] cloud the input point cloud dataset
100  * \param[in] indices a vector of point indices to be used from \a cloud
101  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
102  */
103  SampleConsensusModelCylinder (const PointCloudConstPtr &cloud,
104  const std::vector<int> &indices,
105  bool random = false)
106  : SampleConsensusModel<PointT> (cloud, indices, random)
108  , axis_ (Eigen::Vector3f::Zero ())
109  , eps_angle_ (0)
110  {
111  model_name_ = "SampleConsensusModelCylinder";
112  sample_size_ = 2;
113  model_size_ = 7;
114  }
115 
116  /** \brief Copy constructor.
117  * \param[in] source the model to copy into this
118  */
122  axis_ (Eigen::Vector3f::Zero ()),
123  eps_angle_ (0)
124  {
125  *this = source;
126  model_name_ = "SampleConsensusModelCylinder";
127  }
128 
129  /** \brief Empty destructor */
131 
132  /** \brief Copy constructor.
133  * \param[in] source the model to copy into this
134  */
137  {
140  axis_ = source.axis_;
141  eps_angle_ = source.eps_angle_;
142  return (*this);
143  }
144 
145  /** \brief Set the angle epsilon (delta) threshold.
146  * \param[in] ea the maximum allowed difference between the cylinder axis and the given axis.
147  */
148  inline void
149  setEpsAngle (const double ea) { eps_angle_ = ea; }
150 
151  /** \brief Get the angle epsilon (delta) threshold. */
152  inline double
153  getEpsAngle () { return (eps_angle_); }
154 
155  /** \brief Set the axis along which we need to search for a cylinder direction.
156  * \param[in] ax the axis along which we need to search for a cylinder direction
157  */
158  inline void
159  setAxis (const Eigen::Vector3f &ax) { axis_ = ax; }
160 
161  /** \brief Get the axis along which we need to search for a cylinder direction. */
162  inline Eigen::Vector3f
163  getAxis () { return (axis_); }
164 
165  /** \brief Check whether the given index samples can form a valid cylinder model, compute the model coefficients
166  * from these samples and store them in model_coefficients. The cylinder coefficients are: point_on_axis,
167  * axis_direction, cylinder_radius_R
168  * \param[in] samples the point indices found as possible good candidates for creating a valid model
169  * \param[out] model_coefficients the resultant model coefficients
170  */
171  bool
172  computeModelCoefficients (const std::vector<int> &samples,
173  Eigen::VectorXf &model_coefficients) const;
174 
175  /** \brief Compute all distances from the cloud data to a given cylinder model.
176  * \param[in] model_coefficients the coefficients of a cylinder model that we need to compute distances to
177  * \param[out] distances the resultant estimated distances
178  */
179  void
180  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
181  std::vector<double> &distances) const;
182 
183  /** \brief Select all the points which respect the given model coefficients as inliers.
184  * \param[in] model_coefficients the coefficients of a cylinder model that we need to compute distances to
185  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
186  * \param[out] inliers the resultant model inliers
187  */
188  void
189  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
190  const double threshold,
191  std::vector<int> &inliers);
192 
193  /** \brief Count all the points which respect the given model coefficients as inliers.
194  *
195  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
196  * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
197  * \return the resultant number of inliers
198  */
199  virtual int
200  countWithinDistance (const Eigen::VectorXf &model_coefficients,
201  const double threshold) const;
202 
203  /** \brief Recompute the cylinder coefficients using the given inlier set and return them to the user.
204  * @note: these are the coefficients of the cylinder model after refinement (e.g. after SVD)
205  * \param[in] inliers the data inliers found as supporting the model
206  * \param[in] model_coefficients the initial guess for the optimization
207  * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
208  */
209  void
210  optimizeModelCoefficients (const std::vector<int> &inliers,
211  const Eigen::VectorXf &model_coefficients,
212  Eigen::VectorXf &optimized_coefficients) const;
213 
214 
215  /** \brief Create a new point cloud with inliers projected onto the cylinder model.
216  * \param[in] inliers the data inliers that we want to project on the cylinder model
217  * \param[in] model_coefficients the coefficients of a cylinder model
218  * \param[out] projected_points the resultant projected points
219  * \param[in] copy_data_fields set to true if we need to copy the other data fields
220  */
221  void
222  projectPoints (const std::vector<int> &inliers,
223  const Eigen::VectorXf &model_coefficients,
224  PointCloud &projected_points,
225  bool copy_data_fields = true) const;
226 
227  /** \brief Verify whether a subset of indices verifies the given cylinder model coefficients.
228  * \param[in] indices the data indices that need to be tested against the cylinder model
229  * \param[in] model_coefficients the cylinder model coefficients
230  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
231  */
232  bool
233  doSamplesVerifyModel (const std::set<int> &indices,
234  const Eigen::VectorXf &model_coefficients,
235  const double threshold) const;
236 
237  /** \brief Return an unique id for this model (SACMODEL_CYLINDER). */
238  inline pcl::SacModel
239  getModelType () const { return (SACMODEL_CYLINDER); }
240 
241  protected:
244 
245  /** \brief Get the distance from a point to a line (represented by a point and a direction)
246  * \param[in] pt a point
247  * \param[in] model_coefficients the line coefficients (a point on the line, line direction)
248  */
249  double
250  pointToLineDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const;
251 
252  /** \brief Project a point onto a line given by a point and a direction vector
253  * \param[in] pt the input point to project
254  * \param[in] line_pt the point on the line (make sure that line_pt[3] = 0 as there are no internal checks!)
255  * \param[in] line_dir the direction of the line (make sure that line_dir[3] = 0 as there are no internal checks!)
256  * \param[out] pt_proj the resultant projected point
257  */
258  inline void
259  projectPointToLine (const Eigen::Vector4f &pt,
260  const Eigen::Vector4f &line_pt,
261  const Eigen::Vector4f &line_dir,
262  Eigen::Vector4f &pt_proj) const
263  {
264  float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
265  // Calculate the projection of the point on the line
266  pt_proj = line_pt + k * line_dir;
267  }
268 
269  /** \brief Project a point onto a cylinder given by its model coefficients (point_on_axis, axis_direction,
270  * cylinder_radius_R)
271  * \param[in] pt the input point to project
272  * \param[in] model_coefficients the coefficients of the cylinder (point_on_axis, axis_direction, cylinder_radius_R)
273  * \param[out] pt_proj the resultant projected point
274  */
275  void
276  projectPointToCylinder (const Eigen::Vector4f &pt,
277  const Eigen::VectorXf &model_coefficients,
278  Eigen::Vector4f &pt_proj) const;
279 
280  /** \brief Check whether a model is valid given the user constraints.
281  * \param[in] model_coefficients the set of model coefficients
282  */
283  virtual bool
284  isModelValid (const Eigen::VectorXf &model_coefficients) const;
285 
286  /** \brief Check if a sample of indices results in a good sample of points
287  * indices. Pure virtual.
288  * \param[in] samples the resultant index samples
289  */
290  bool
291  isSampleGood (const std::vector<int> &samples) const;
292 
293  private:
294  /** \brief The axis along which we need to search for a plane perpendicular to. */
295  Eigen::Vector3f axis_;
296 
297  /** \brief The maximum allowed difference between the plane normal and the given axis. */
298  double eps_angle_;
299 
300 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
301 #pragma GCC diagnostic ignored "-Weffc++"
302 #endif
303  /** \brief Functor for the optimization function */
304  struct OptimizationFunctor : pcl::Functor<float>
305  {
306  /** Functor constructor
307  * \param[in] indices the indices of data points to evaluate
308  * \param[in] estimator pointer to the estimator object
309  */
310  OptimizationFunctor (const pcl::SampleConsensusModelCylinder<PointT, PointNT> *model, const std::vector<int>& indices) :
311  pcl::Functor<float> (indices.size ()), model_ (model), indices_ (indices) {}
312 
313  /** Cost function to be minimized
314  * \param[in] x variables array
315  * \param[out] fvec resultant functions evaluations
316  * \return 0
317  */
318  int
319  operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
320  {
321  Eigen::Vector4f line_pt (x[0], x[1], x[2], 0);
322  Eigen::Vector4f line_dir (x[3], x[4], x[5], 0);
323 
324  for (int i = 0; i < values (); ++i)
325  {
326  // dist = f - r
327  Eigen::Vector4f pt (model_->input_->points[indices_[i]].x,
328  model_->input_->points[indices_[i]].y,
329  model_->input_->points[indices_[i]].z, 0);
330 
331  fvec[i] = static_cast<float> (pcl::sqrPointToLineDistance (pt, line_pt, line_dir) - x[6]*x[6]);
332  }
333  return (0);
334  }
335 
337  const std::vector<int> &indices_;
338  };
339 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
340 #pragma GCC diagnostic warning "-Weffc++"
341 #endif
342  };
343 }
344 
345 #ifdef PCL_NO_PRECOMPILE
346 #include <pcl/sample_consensus/impl/sac_model_cylinder.hpp>
347 #endif
348 
349 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CYLINDER_H_
SampleConsensusModel< PointT >::PointCloud PointCloud
SampleConsensusModelCylinder defines a model for 3D cylinder segmentation.
Eigen::Vector3f getAxis()
Get the axis along which we need to search for a cylinder direction.
SampleConsensusModelCylinder(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModelCylinder.
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
unsigned int model_size_
The number of coefficients in the model.
Definition: sac_model.h:575
void projectPointToLine(const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir, Eigen::Vector4f &pt_proj) const
Project a point onto a line given by a point and a direction vector.
double pointToLineDistance(const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const
Get the distance from a point to a line (represented by a point and a direction)
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients) const
Check whether the given index samples can form a valid cylinder model, compute the model coefficients...
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const
Compute all distances from the cloud data to a given cylinder model.
Base functor all the models that need non linear optimization must define their own one and implement...
Definition: sac_model.h:653
SampleConsensusModelCylinder & operator=(const SampleConsensusModelCylinder &source)
Copy constructor.
boost::shared_ptr< std::vector< int > > indices_
A pointer to the vector of point indices to use.
Definition: sac_model.h:540
boost::shared_ptr< SampleConsensusModelCylinder > Ptr
Definition: bfgs.h:10
bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
Verify whether a subset of indices verifies the given cylinder model coefficients.
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
Recompute the cylinder coefficients using the given inlier set and return them to the user...
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a cylinder direction.
pcl::SacModel getModelType() const
Return an unique id for this model (SACMODEL_CYLINDER).
SampleConsensusModel represents the base model class.
Definition: sac_model.h:66
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) ...
Definition: distances.h:69
std::string model_name_
The model name.
Definition: sac_model.h:534
pcl::PointCloud< PointT >::Ptr PointCloudPtr
Definition: sac_model.h:71
SampleConsensusModelCylinder(const SampleConsensusModelCylinder &source)
Copy constructor.
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const
Count all the points which respect the given model coefficients as inliers.
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
Definition: sac_model.h:591
SacModel
Definition: model_types.h:46
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
Definition: sac_model.h:70
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void projectPoints(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const
Create a new point cloud with inliers projected onto the cylinder model.
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.
void projectPointToCylinder(const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj) const
Project a point onto a cylinder given by its model coefficients (point_on_axis, axis_direction, cylinder_radius_R)
A point structure representing Euclidean xyz coordinates, and the RGB color.
virtual ~SampleConsensusModelCylinder()
Empty destructor.
double getEpsAngle()
Get the angle epsilon (delta) threshold.
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients) const
Check whether a model is valid given the user constraints.
SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
SampleConsensusModelCylinder(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCylinder.
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition: sac_model.h:572