Point Cloud Library (PCL)  1.7.0
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  */
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  */
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_