Point Cloud Library (PCL)  1.7.1
sac_model_normal_parallel_plane.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_NORMALPARALLELPLANE_H_
42 #define PCL_SAMPLE_CONSENSUS_MODEL_NORMALPARALLELPLANE_H_
43 
44 #include <pcl/sample_consensus/sac_model.h>
45 #include <pcl/sample_consensus/sac_model_plane.h>
46 #include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
47 #include <pcl/sample_consensus/model_types.h>
48 
49 namespace pcl
50 {
51  /** \brief SampleConsensusModelNormalParallelPlane defines a model for 3D
52  * plane segmentation using additional surface normal constraints. Basically
53  * this means that checking for inliers will not only involve a "distance to
54  * model" criterion, but also an additional "maximum angular deviation"
55  * between the plane's normal and the inlier points normals. In addition,
56  * the plane normal must lie parallel to an user-specified axis.
57  *
58  * The model coefficients are defined as:
59  * - \b a : the X coordinate of the plane's normal (normalized)
60  * - \b b : the Y coordinate of the plane's normal (normalized)
61  * - \b c : the Z coordinate of the plane's normal (normalized)
62  * - \b d : the fourth <a href="http://mathworld.wolfram.com/HessianNormalForm.html">Hessian component</a> of the plane's equation
63  *
64  * To set the influence of the surface normals in the inlier estimation
65  * process, set the normal weight (0.0-1.0), e.g.:
66  * \code
67  * SampleConsensusModelNormalPlane<pcl::PointXYZ, pcl::Normal> sac_model;
68  * ...
69  * sac_model.setNormalDistanceWeight (0.1);
70  * ...
71  * \endcode
72  *
73  * In addition, the user can specify more constraints, such as:
74  *
75  * - an axis along which we need to search for a plane perpendicular to (\ref setAxis);
76  * - an angle \a tolerance threshold between the plane's normal and the above given axis (\ref setEpsAngle);
77  * - a distance we expect the plane to be from the origin (\ref setDistanceFromOrigin);
78  * - a distance \a tolerance as the maximum allowed deviation from the above given distance from the origin (\ref setEpsDist).
79  *
80  * \note Please remember that you need to specify an angle > 0 in order to activate the axis-angle constraint!
81  *
82  * \author Radu B. Rusu and Jared Glover and Nico Blodow
83  * \ingroup sample_consensus
84  */
85  template <typename PointT, typename PointNT>
87  {
88  public:
94 
98 
101 
102  typedef boost::shared_ptr<SampleConsensusModelNormalParallelPlane> Ptr;
103 
104  /** \brief Constructor for base SampleConsensusModelNormalParallelPlane.
105  * \param[in] cloud the input point cloud dataset
106  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
107  */
108  SampleConsensusModelNormalParallelPlane (const PointCloudConstPtr &cloud,
109  bool random = false)
110  : SampleConsensusModelPlane<PointT> (cloud, random)
112  , axis_ (Eigen::Vector4f::Zero ())
113  , distance_from_origin_ (0)
114  , eps_angle_ (-1.0)
115  , cos_angle_ (-1.0)
116  , eps_dist_ (0.0)
117  {
118  }
119 
120  /** \brief Constructor for base SampleConsensusModelNormalParallelPlane.
121  * \param[in] cloud the input point cloud dataset
122  * \param[in] indices a vector of point indices to be used from \a cloud
123  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
124  */
125  SampleConsensusModelNormalParallelPlane (const PointCloudConstPtr &cloud,
126  const std::vector<int> &indices,
127  bool random = false)
128  : SampleConsensusModelPlane<PointT> (cloud, indices, random)
130  , axis_ (Eigen::Vector4f::Zero ())
131  , distance_from_origin_ (0)
132  , eps_angle_ (-1.0)
133  , cos_angle_ (-1.0)
134  , eps_dist_ (0.0)
135  {
136  }
137 
138  /** \brief Empty destructor */
140 
141  /** \brief Set the axis along which we need to search for a plane perpendicular to.
142  * \param[in] ax the axis along which we need to search for a plane perpendicular to
143  */
144  inline void
145  setAxis (const Eigen::Vector3f &ax) { axis_.head<3> () = ax; axis_.normalize ();}
146 
147  /** \brief Get the axis along which we need to search for a plane perpendicular to. */
148  inline Eigen::Vector3f
149  getAxis () { return (axis_.head<3> ()); }
150 
151  /** \brief Set the angle epsilon (delta) threshold.
152  * \param[in] ea the maximum allowed deviation from 90 degrees between the plane normal and the given axis.
153  * \note You need to specify an angle > 0 in order to activate the axis-angle constraint!
154  */
155  inline void
156  setEpsAngle (const double ea) { eps_angle_ = ea; cos_angle_ = fabs (cos (ea));}
157 
158  /** \brief Get the angle epsilon (delta) threshold. */
159  inline double
160  getEpsAngle () { return (eps_angle_); }
161 
162  /** \brief Set the distance we expect the plane to be from the origin
163  * \param[in] d distance from the template plane to the origin
164  */
165  inline void
166  setDistanceFromOrigin (const double d) { distance_from_origin_ = d; }
167 
168  /** \brief Get the distance of the plane from the origin. */
169  inline double
170  getDistanceFromOrigin () { return (distance_from_origin_); }
171 
172  /** \brief Set the distance epsilon (delta) threshold.
173  * \param[in] delta the maximum allowed deviation from the template distance from the origin
174  */
175  inline void
176  setEpsDist (const double delta) { eps_dist_ = delta; }
177 
178  /** \brief Get the distance epsilon (delta) threshold. */
179  inline double
180  getEpsDist () { return (eps_dist_); }
181 
182  /** \brief Select all the points which respect the given model coefficients as inliers.
183  * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to
184  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
185  * \param[out] inliers the resultant model inliers
186  */
187  void
188  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
189  const double threshold,
190  std::vector<int> &inliers);
191 
192  /** \brief Count all the points which respect the given model coefficients as inliers.
193  *
194  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
195  * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
196  * \return the resultant number of inliers
197  */
198  virtual int
199  countWithinDistance (const Eigen::VectorXf &model_coefficients,
200  const double threshold);
201 
202  /** \brief Compute all distances from the cloud data to a given plane model.
203  * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to
204  * \param[out] distances the resultant estimated distances
205  */
206  void
207  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
208  std::vector<double> &distances);
209 
210  /** \brief Return an unique id for this model (SACMODEL_NORMAL_PARALLEL_PLANE). */
211  inline pcl::SacModel
213 
214  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
215 
216  protected:
217  /** \brief Check whether a model is valid given the user constraints.
218  * \param[in] model_coefficients the set of model coefficients
219  */
220  bool
221  isModelValid (const Eigen::VectorXf &model_coefficients);
222 
223  private:
224  /** \brief The axis along which we need to search for a plane perpendicular to. */
225  Eigen::Vector4f axis_;
226 
227  /** \brief The distance from the template plane to the origin. */
228  double distance_from_origin_;
229 
230  /** \brief The maximum allowed difference between the plane normal and the given axis. */
231  double eps_angle_;
232 
233  /** \brief The cosine of the angle*/
234  double cos_angle_;
235  /** \brief The maximum allowed deviation from the template distance from the origin. */
236  double eps_dist_;
237  };
238 }
239 
240 #ifdef PCL_NO_PRECOMPILE
241 #include <pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp>
242 #endif
243 
244 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_NORMALPARALLELPLANE_H_
SampleConsensusModelNormalParallelPlane(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModelNormalParallelPlane.
pcl::PointCloud< PointT >::Ptr PointCloudPtr
Definition: sac_model.h:71
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a plane perpendicular to.
double getEpsAngle()
Get the angle epsilon (delta) threshold.
SampleConsensusModel< PointT >::PointCloud PointCloud
pcl::PointCloud< PointNT >::Ptr PointCloudNPtr
Definition: sac_model.h:560
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)
Count all the points which respect the given model coefficients as inliers.
pcl::PointCloud< PointNT >::ConstPtr PointCloudNConstPtr
Definition: sac_model.h:559
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
Definition: sac_model.h:556
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)
Compute all distances from the cloud data to a given plane model.
SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
SampleConsensusModelPlane defines a model for 3D plane segmentation.
SampleConsensusModelNormalParallelPlane(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelNormalParallelPlane.
Definition: bfgs.h:10
double getEpsDist()
Get the distance epsilon (delta) threshold.
SacModel
Definition: model_types.h:48
SampleConsensusModelNormalParallelPlane defines a model for 3D plane segmentation using additional su...
bool isModelValid(const Eigen::VectorXf &model_coefficients)
Check whether a model is valid given the user constraints.
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
SampleConsensusModelFromNormals< PointT, PointNT >::PointCloudNPtr PointCloudNPtr
SampleConsensusModel represents the base model class.
Definition: sac_model.h:66
pcl::SacModel getModelType() const
Return an unique id for this model (SACMODEL_NORMAL_PARALLEL_PLANE).
SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
Eigen::Vector3f getAxis()
Get the axis along which we need to search for a plane perpendicular to.
SampleConsensusModelFromNormals< PointT, PointNT >::PointCloudNConstPtr PointCloudNConstPtr
A point structure representing Euclidean xyz coordinates, and the RGB color.
void setDistanceFromOrigin(const double d)
Set the distance we expect the plane to be from the origin.
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.
boost::shared_ptr< SampleConsensusModelNormalParallelPlane > Ptr
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
Definition: sac_model.h:70
void setEpsDist(const double delta)
Set the distance epsilon (delta) threshold.
double getDistanceFromOrigin()
Get the distance of the plane from the origin.
PointCloud represents the base class in PCL for storing collections of 3D points. ...