Point Cloud Library (PCL)  1.7.1
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:
75 
79 
80  typedef boost::shared_ptr<SampleConsensusModelCylinder> Ptr;
81 
82  /** \brief Constructor for base SampleConsensusModelCylinder.
83  * \param[in] cloud the input point cloud dataset
84  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
85  */
86  SampleConsensusModelCylinder (const PointCloudConstPtr &cloud, bool random = false)
87  : SampleConsensusModel<PointT> (cloud, random)
89  , axis_ (Eigen::Vector3f::Zero ())
90  , eps_angle_ (0)
91  , tmp_inliers_ ()
92  {
93  }
94 
95  /** \brief Constructor for base SampleConsensusModelCylinder.
96  * \param[in] cloud the input point cloud dataset
97  * \param[in] indices a vector of point indices to be used from \a cloud
98  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
99  */
100  SampleConsensusModelCylinder (const PointCloudConstPtr &cloud,
101  const std::vector<int> &indices,
102  bool random = false)
103  : SampleConsensusModel<PointT> (cloud, indices, random)
105  , axis_ (Eigen::Vector3f::Zero ())
106  , eps_angle_ (0)
107  , tmp_inliers_ ()
108  {
109  }
110 
111  /** \brief Copy constructor.
112  * \param[in] source the model to copy into this
113  */
117  axis_ (Eigen::Vector3f::Zero ()),
118  eps_angle_ (0),
119  tmp_inliers_ ()
120  {
121  *this = source;
122  }
123 
124  /** \brief Empty destructor */
126 
127  /** \brief Copy constructor.
128  * \param[in] source the model to copy into this
129  */
132  {
134  axis_ = source.axis_;
135  eps_angle_ = source.eps_angle_;
136  tmp_inliers_ = source.tmp_inliers_;
137  return (*this);
138  }
139 
140  /** \brief Set the angle epsilon (delta) threshold.
141  * \param[in] ea the maximum allowed difference between the cyilinder axis and the given axis.
142  */
143  inline void
144  setEpsAngle (const double ea) { eps_angle_ = ea; }
145 
146  /** \brief Get the angle epsilon (delta) threshold. */
147  inline double
148  getEpsAngle () { return (eps_angle_); }
149 
150  /** \brief Set the axis along which we need to search for a cylinder direction.
151  * \param[in] ax the axis along which we need to search for a cylinder direction
152  */
153  inline void
154  setAxis (const Eigen::Vector3f &ax) { axis_ = ax; }
155 
156  /** \brief Get the axis along which we need to search for a cylinder direction. */
157  inline Eigen::Vector3f
158  getAxis () { return (axis_); }
159 
160  /** \brief Check whether the given index samples can form a valid cylinder model, compute the model coefficients
161  * from these samples and store them in model_coefficients. The cylinder coefficients are: point_on_axis,
162  * axis_direction, cylinder_radius_R
163  * \param[in] samples the point indices found as possible good candidates for creating a valid model
164  * \param[out] model_coefficients the resultant model coefficients
165  */
166  bool
167  computeModelCoefficients (const std::vector<int> &samples,
168  Eigen::VectorXf &model_coefficients);
169 
170  /** \brief Compute all distances from the cloud data to a given cylinder model.
171  * \param[in] model_coefficients the coefficients of a cylinder model that we need to compute distances to
172  * \param[out] distances the resultant estimated distances
173  */
174  void
175  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
176  std::vector<double> &distances);
177 
178  /** \brief Select all the points which respect the given model coefficients as inliers.
179  * \param[in] model_coefficients the coefficients of a cylinder model that we need to compute distances to
180  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
181  * \param[out] inliers the resultant model inliers
182  */
183  void
184  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
185  const double threshold,
186  std::vector<int> &inliers);
187 
188  /** \brief Count all the points which respect the given model coefficients as inliers.
189  *
190  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
191  * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
192  * \return the resultant number of inliers
193  */
194  virtual int
195  countWithinDistance (const Eigen::VectorXf &model_coefficients,
196  const double threshold);
197 
198  /** \brief Recompute the cylinder coefficients using the given inlier set and return them to the user.
199  * @note: these are the coefficients of the cylinder model after refinement (eg. after SVD)
200  * \param[in] inliers the data inliers found as supporting the model
201  * \param[in] model_coefficients the initial guess for the optimization
202  * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
203  */
204  void
205  optimizeModelCoefficients (const std::vector<int> &inliers,
206  const Eigen::VectorXf &model_coefficients,
207  Eigen::VectorXf &optimized_coefficients);
208 
209 
210  /** \brief Create a new point cloud with inliers projected onto the cylinder model.
211  * \param[in] inliers the data inliers that we want to project on the cylinder model
212  * \param[in] model_coefficients the coefficients of a cylinder model
213  * \param[out] projected_points the resultant projected points
214  * \param[in] copy_data_fields set to true if we need to copy the other data fields
215  */
216  void
217  projectPoints (const std::vector<int> &inliers,
218  const Eigen::VectorXf &model_coefficients,
219  PointCloud &projected_points,
220  bool copy_data_fields = true);
221 
222  /** \brief Verify whether a subset of indices verifies the given cylinder model coefficients.
223  * \param[in] indices the data indices that need to be tested against the cylinder model
224  * \param[in] model_coefficients the cylinder model coefficients
225  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
226  */
227  bool
228  doSamplesVerifyModel (const std::set<int> &indices,
229  const Eigen::VectorXf &model_coefficients,
230  const double threshold);
231 
232  /** \brief Return an unique id for this model (SACMODEL_CYLINDER). */
233  inline pcl::SacModel
234  getModelType () const { return (SACMODEL_CYLINDER); }
235 
236  protected:
237  /** \brief Get the distance from a point to a line (represented by a point and a direction)
238  * \param[in] pt a point
239  * \param[in] model_coefficients the line coefficients (a point on the line, line direction)
240  */
241  double
242  pointToLineDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients);
243 
244  /** \brief Project a point onto a line given by a point and a direction vector
245  * \param[in] pt the input point to project
246  * \param[in] line_pt the point on the line (make sure that line_pt[3] = 0 as there are no internal checks!)
247  * \param[in] line_dir the direction of the line (make sure that line_dir[3] = 0 as there are no internal checks!)
248  * \param[out] pt_proj the resultant projected point
249  */
250  inline void
251  projectPointToLine (const Eigen::Vector4f &pt,
252  const Eigen::Vector4f &line_pt,
253  const Eigen::Vector4f &line_dir,
254  Eigen::Vector4f &pt_proj)
255  {
256  float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
257  // Calculate the projection of the point on the line
258  pt_proj = line_pt + k * line_dir;
259  }
260 
261  /** \brief Project a point onto a cylinder given by its model coefficients (point_on_axis, axis_direction,
262  * cylinder_radius_R)
263  * \param[in] pt the input point to project
264  * \param[in] model_coefficients the coefficients of the cylinder (point_on_axis, axis_direction, cylinder_radius_R)
265  * \param[out] pt_proj the resultant projected point
266  */
267  void
268  projectPointToCylinder (const Eigen::Vector4f &pt,
269  const Eigen::VectorXf &model_coefficients,
270  Eigen::Vector4f &pt_proj);
271 
272  /** \brief Get a string representation of the name of this class. */
273  std::string
274  getName () const { return ("SampleConsensusModelCylinder"); }
275 
276  protected:
277  /** \brief Check whether a model is valid given the user constraints.
278  * \param[in] model_coefficients the set of model coefficients
279  */
280  bool
281  isModelValid (const Eigen::VectorXf &model_coefficients);
282 
283  /** \brief Check if a sample of indices results in a good sample of points
284  * indices. Pure virtual.
285  * \param[in] samples the resultant index samples
286  */
287  bool
288  isSampleGood (const std::vector<int> &samples) const;
289 
290  private:
291  /** \brief The axis along which we need to search for a plane perpendicular to. */
292  Eigen::Vector3f axis_;
293 
294  /** \brief The maximum allowed difference between the plane normal and the given axis. */
295  double eps_angle_;
296 
297  /** \brief temporary pointer to a list of given indices for optimizeModelCoefficients () */
298  const std::vector<int> *tmp_inliers_;
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] m_data_points the number of data points to evaluate
308  * \param[in] estimator pointer to the estimator object
309  * \param[in] distance distance computation function pointer
310  */
311  OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelCylinder<PointT, PointNT> *model) :
312  pcl::Functor<float> (m_data_points), model_ (model) {}
313 
314  /** Cost function to be minimized
315  * \param[in] x variables array
316  * \param[out] fvec resultant functions evaluations
317  * \return 0
318  */
319  int
320  operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
321  {
322  Eigen::Vector4f line_pt (x[0], x[1], x[2], 0);
323  Eigen::Vector4f line_dir (x[3], x[4], x[5], 0);
324 
325  for (int i = 0; i < values (); ++i)
326  {
327  // dist = f - r
328  Eigen::Vector4f pt (model_->input_->points[(*model_->tmp_inliers_)[i]].x,
329  model_->input_->points[(*model_->tmp_inliers_)[i]].y,
330  model_->input_->points[(*model_->tmp_inliers_)[i]].z, 0);
331 
332  fvec[i] = static_cast<float> (pcl::sqrPointToLineDistance (pt, line_pt, line_dir) - x[6]*x[6]);
333  }
334  return (0);
335  }
336 
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_
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.
pcl::PointCloud< PointT >::Ptr PointCloudPtr
Definition: sac_model.h:71
void projectPointToLine(const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir, Eigen::Vector4f &pt_proj)
Project a point onto a line given by a point and a direction vector.
Eigen::Vector3f getAxis()
Get the axis along which we need to search for a cylinder direction.
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 isModelValid(const Eigen::VectorXf &model_coefficients)
Check whether a model is valid given the user constraints.
boost::shared_ptr< SampleConsensusModelCylinder > Ptr
SampleConsensusModel< PointT >::PointCloud PointCloud
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, cylinder_radius_R)
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a cylinder direction.
SampleConsensusModelCylinder & operator=(const SampleConsensusModelCylinder &source)
Copy constructor.
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
Definition: sac_model.h:556
std::string getName() const
Get a string representation of the name of this class.
pcl::SacModel getModelType() const
Return an unique id for this model (SACMODEL_CYLINDER).
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)
Definition: bfgs.h:10
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.
SampleConsensusModelCylinder(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModelCylinder.
SampleConsensusModelCylinder(const SampleConsensusModelCylinder &source)
Copy constructor.
SampleConsensusModelCylinder defines a model for 3D cylinder segmentation.
SacModel
Definition: model_types.h:48
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...
SampleConsensusModel represents the base model class.
Definition: sac_model.h:66
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)
Count all the points which respect the given model coefficients as inliers.
SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
SampleConsensusModelCylinder(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelCylinder.
A point structure representing Euclidean xyz coordinates, and the RGB color.
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
SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
double getEpsAngle()
Get the angle epsilon (delta) threshold.
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
Definition: sac_model.h:70
virtual ~SampleConsensusModelCylinder()
Empty destructor.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Base functor all the models that need non linear optimization must define their own one and implement...
Definition: sac_model.h:618
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)
Compute all distances from the cloud data to a given cylinder model.
bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
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.