Point Cloud Library (PCL)  1.10.1-dev
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 #pragma once
42 
43 #include <pcl/sample_consensus/sac_model_normal_plane.h>
44 #include <pcl/sample_consensus/model_types.h>
45 #include <pcl/memory.h>
46 #include <pcl/pcl_macros.h>
47 
48 namespace pcl
49 {
50  /** \brief SampleConsensusModelNormalParallelPlane defines a model for 3D
51  * plane segmentation using additional surface normal constraints. Basically
52  * this means that checking for inliers will not only involve a "distance to
53  * model" criterion, but also an additional "maximum angular deviation"
54  * between the plane's normal and the inlier points normals. In addition,
55  * the plane <b>normal</b> must lie parallel to a user-specified axis.
56  * This means that the plane itself will lie perpendicular to that axis, similar to \link pcl::SampleConsensusModelPerpendicularPlane SACMODEL_PERPENDICULAR_PLANE \endlink.
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:
95 
99 
102 
105 
106  /** \brief Constructor for base SampleConsensusModelNormalParallelPlane.
107  * \param[in] cloud the input point cloud dataset
108  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
109  */
111  bool random = false)
112  : SampleConsensusModelNormalPlane<PointT, PointNT> (cloud, random)
113  , axis_ (Eigen::Vector4f::Zero ())
114  , distance_from_origin_ (0)
115  , eps_angle_ (-1.0)
116  , cos_angle_ (-1.0)
117  , eps_dist_ (0.0)
118  {
119  model_name_ = "SampleConsensusModelNormalParallelPlane";
120  sample_size_ = 3;
121  model_size_ = 4;
122  }
123 
124  /** \brief Constructor for base SampleConsensusModelNormalParallelPlane.
125  * \param[in] cloud the input point cloud dataset
126  * \param[in] indices a vector of point indices to be used from \a cloud
127  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
128  */
130  const std::vector<int> &indices,
131  bool random = false)
132  : SampleConsensusModelNormalPlane<PointT, PointNT> (cloud, indices, random)
133  , axis_ (Eigen::Vector4f::Zero ())
134  , distance_from_origin_ (0)
135  , eps_angle_ (-1.0)
136  , cos_angle_ (-1.0)
137  , eps_dist_ (0.0)
138  {
139  model_name_ = "SampleConsensusModelNormalParallelPlane";
140  sample_size_ = 3;
141  model_size_ = 4;
142  }
143 
144  /** \brief Empty destructor */
146 
147  /** \brief Set the axis along which we need to search for a plane perpendicular to.
148  * \param[in] ax the axis along which we need to search for a plane perpendicular to
149  */
150  inline void
151  setAxis (const Eigen::Vector3f &ax) { axis_.head<3> () = ax; axis_.normalize ();}
152 
153  /** \brief Get the axis along which we need to search for a plane perpendicular to. */
154  inline Eigen::Vector3f
155  getAxis () const { return (axis_.head<3> ()); }
156 
157  /** \brief Set the angle epsilon (delta) threshold.
158  * \param[in] ea the maximum allowed deviation from 90 degrees between the plane normal and the given axis.
159  * \note You need to specify an angle > 0 in order to activate the axis-angle constraint!
160  */
161  inline void
162  setEpsAngle (const double ea) { eps_angle_ = ea; cos_angle_ = std::abs (std::cos (ea));}
163 
164  /** \brief Get the angle epsilon (delta) threshold. */
165  inline double
166  getEpsAngle () const { return (eps_angle_); }
167 
168  /** \brief Set the distance we expect the plane to be from the origin
169  * \param[in] d distance from the template plane to the origin
170  */
171  inline void
172  setDistanceFromOrigin (const double d) { distance_from_origin_ = d; }
173 
174  /** \brief Get the distance of the plane from the origin. */
175  inline double
176  getDistanceFromOrigin () const { return (distance_from_origin_); }
177 
178  /** \brief Set the distance epsilon (delta) threshold.
179  * \param[in] delta the maximum allowed deviation from the template distance from the origin
180  */
181  inline void
182  setEpsDist (const double delta) { eps_dist_ = delta; }
183 
184  /** \brief Get the distance epsilon (delta) threshold. */
185  inline double
186  getEpsDist () const { return (eps_dist_); }
187 
188  /** \brief Return a unique id for this model (SACMODEL_NORMAL_PARALLEL_PLANE). */
189  inline pcl::SacModel
190  getModelType () const override { return (SACMODEL_NORMAL_PARALLEL_PLANE); }
191 
193 
194  protected:
197 
198  /** \brief Check whether a model is valid given the user constraints.
199  * \param[in] model_coefficients the set of model coefficients
200  */
201  bool
202  isModelValid (const Eigen::VectorXf &model_coefficients) const override;
203 
204  private:
205  /** \brief The axis along which we need to search for a plane perpendicular to. */
206  Eigen::Vector4f axis_;
207 
208  /** \brief The distance from the template plane to the origin. */
209  double distance_from_origin_;
210 
211  /** \brief The maximum allowed difference between the plane normal and the given axis. */
212  double eps_angle_;
213 
214  /** \brief The cosine of the angle*/
215  double cos_angle_;
216  /** \brief The maximum allowed deviation from the template distance from the origin. */
217  double eps_dist_;
218  };
219 }
220 
221 #ifdef PCL_NO_PRECOMPILE
222 #include <pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp>
223 #endif
Defines functions, macros and traits for allocating and using memory.
unsigned int model_size_
The number of coefficients in the model.
Definition: sac_model.h:565
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:65
typename PointCloud::Ptr PointCloudPtr
Definition: sac_model.h:74
Definition: bfgs.h:9
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a plane perpendicular to.
typename pcl::PointCloud< PointNT >::ConstPtr PointCloudNConstPtr
Definition: sac_model.h:584
SampleConsensusModel represents the base model class.
Definition: sac_model.h:69
void setDistanceFromOrigin(const double d)
Set the distance we expect the plane to be from the origin.
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a plane perpendicular to.
std::string model_name_
The model name.
Definition: sac_model.h:524
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: sac_model.h:73
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
Definition: sac_model.h:581
SampleConsensusModelNormalPlane defines a model for 3D plane segmentation using additional surface no...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
SacModel
Definition: model_types.h:45
shared_ptr< const SampleConsensusModel< PointT > > ConstPtr
Definition: sac_model.h:78
double getDistanceFromOrigin() const
Get the distance of the plane from the origin.
double getEpsDist() const
Get the distance epsilon (delta) threshold.
SampleConsensusModelNormalParallelPlane(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelNormalParallelPlane.
SampleConsensusModelNormalParallelPlane(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModelNormalParallelPlane.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_NORMAL_PARALLEL_PLANE).
A point structure representing Euclidean xyz coordinates, and the RGB color.
shared_ptr< SampleConsensusModel< PointT > > Ptr
Definition: sac_model.h:77
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
void setEpsDist(const double delta)
Set the distance epsilon (delta) threshold.
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: memory.h:81
typename pcl::PointCloud< PointNT >::Ptr PointCloudNPtr
Definition: sac_model.h:585
Defines all the PCL and non-PCL macros used.
SampleConsensusModelNormalParallelPlane defines a model for 3D plane segmentation using additional su...
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition: sac_model.h:562