Point Cloud Library (PCL)  1.9.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/pcl_macros.h>
46 
47 namespace pcl
48 {
49  /** \brief SampleConsensusModelNormalParallelPlane defines a model for 3D
50  * plane segmentation using additional surface normal constraints. Basically
51  * this means that checking for inliers will not only involve a "distance to
52  * model" criterion, but also an additional "maximum angular deviation"
53  * between the plane's normal and the inlier points normals. In addition,
54  * the plane <b>normal</b> must lie parallel to a user-specified axis.
55  * This means that the plane itself will lie perpendicular to that axis, similar to \link pcl::SampleConsensusModelPerpendicularPlane SACMODEL_PERPENDICULAR_PLANE \endlink.
56  *
57  * The model coefficients are defined as:
58  * - \b a : the X coordinate of the plane's normal (normalized)
59  * - \b b : the Y coordinate of the plane's normal (normalized)
60  * - \b c : the Z coordinate of the plane's normal (normalized)
61  * - \b d : the fourth <a href="http://mathworld.wolfram.com/HessianNormalForm.html">Hessian component</a> of the plane's equation
62  *
63  * To set the influence of the surface normals in the inlier estimation
64  * process, set the normal weight (0.0-1.0), e.g.:
65  * \code
66  * SampleConsensusModelNormalPlane<pcl::PointXYZ, pcl::Normal> sac_model;
67  * ...
68  * sac_model.setNormalDistanceWeight (0.1);
69  * ...
70  * \endcode
71  *
72  * In addition, the user can specify more constraints, such as:
73  *
74  * - an axis along which we need to search for a plane perpendicular to (\ref setAxis);
75  * - an angle \a tolerance threshold between the plane's normal and the above given axis (\ref setEpsAngle);
76  * - a distance we expect the plane to be from the origin (\ref setDistanceFromOrigin);
77  * - a distance \a tolerance as the maximum allowed deviation from the above given distance from the origin (\ref setEpsDist).
78  *
79  * \note Please remember that you need to specify an angle > 0 in order to activate the axis-angle constraint!
80  *
81  * \author Radu B. Rusu and Jared Glover and Nico Blodow
82  * \ingroup sample_consensus
83  */
84  template <typename PointT, typename PointNT>
86  {
87  public:
94 
98 
101 
102  using Ptr = boost::shared_ptr<SampleConsensusModelNormalParallelPlane<PointT, PointNT> >;
103  using ConstPtr = boost::shared_ptr<const SampleConsensusModelNormalParallelPlane<PointT, PointNT>>;
104 
105  /** \brief Constructor for base SampleConsensusModelNormalParallelPlane.
106  * \param[in] cloud the input point cloud dataset
107  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
108  */
110  bool random = false)
111  : SampleConsensusModelNormalPlane<PointT, PointNT> (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  model_name_ = "SampleConsensusModelNormalParallelPlane";
119  sample_size_ = 3;
120  model_size_ = 4;
121  }
122 
123  /** \brief Constructor for base SampleConsensusModelNormalParallelPlane.
124  * \param[in] cloud the input point cloud dataset
125  * \param[in] indices a vector of point indices to be used from \a cloud
126  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
127  */
129  const std::vector<int> &indices,
130  bool random = false)
131  : SampleConsensusModelNormalPlane<PointT, PointNT> (cloud, indices, random)
132  , axis_ (Eigen::Vector4f::Zero ())
133  , distance_from_origin_ (0)
134  , eps_angle_ (-1.0)
135  , cos_angle_ (-1.0)
136  , eps_dist_ (0.0)
137  {
138  model_name_ = "SampleConsensusModelNormalParallelPlane";
139  sample_size_ = 3;
140  model_size_ = 4;
141  }
142 
143  /** \brief Empty destructor */
145 
146  /** \brief Set the axis along which we need to search for a plane perpendicular to.
147  * \param[in] ax the axis along which we need to search for a plane perpendicular to
148  */
149  inline void
150  setAxis (const Eigen::Vector3f &ax) { axis_.head<3> () = ax; axis_.normalize ();}
151 
152  /** \brief Get the axis along which we need to search for a plane perpendicular to. */
153  inline Eigen::Vector3f
154  getAxis () const { return (axis_.head<3> ()); }
155 
156  /** \brief Set the angle epsilon (delta) threshold.
157  * \param[in] ea the maximum allowed deviation from 90 degrees between the plane normal and the given axis.
158  * \note You need to specify an angle > 0 in order to activate the axis-angle constraint!
159  */
160  inline void
161  setEpsAngle (const double ea) { eps_angle_ = ea; cos_angle_ = std::abs (std::cos (ea));}
162 
163  /** \brief Get the angle epsilon (delta) threshold. */
164  inline double
165  getEpsAngle () const { return (eps_angle_); }
166 
167  /** \brief Set the distance we expect the plane to be from the origin
168  * \param[in] d distance from the template plane to the origin
169  */
170  inline void
171  setDistanceFromOrigin (const double d) { distance_from_origin_ = d; }
172 
173  /** \brief Get the distance of the plane from the origin. */
174  inline double
175  getDistanceFromOrigin () const { return (distance_from_origin_); }
176 
177  /** \brief Set the distance epsilon (delta) threshold.
178  * \param[in] delta the maximum allowed deviation from the template distance from the origin
179  */
180  inline void
181  setEpsDist (const double delta) { eps_dist_ = delta; }
182 
183  /** \brief Get the distance epsilon (delta) threshold. */
184  inline double
185  getEpsDist () const { return (eps_dist_); }
186 
187  /** \brief Return a unique id for this model (SACMODEL_NORMAL_PARALLEL_PLANE). */
188  inline pcl::SacModel
189  getModelType () const override { return (SACMODEL_NORMAL_PARALLEL_PLANE); }
190 
192 
193  protected:
196 
197  /** \brief Check whether a model is valid given the user constraints.
198  * \param[in] model_coefficients the set of model coefficients
199  */
200  bool
201  isModelValid (const Eigen::VectorXf &model_coefficients) const override;
202 
203  private:
204  /** \brief The axis along which we need to search for a plane perpendicular to. */
205  Eigen::Vector4f axis_;
206 
207  /** \brief The distance from the template plane to the origin. */
208  double distance_from_origin_;
209 
210  /** \brief The maximum allowed difference between the plane normal and the given axis. */
211  double eps_angle_;
212 
213  /** \brief The cosine of the angle*/
214  double cos_angle_;
215  /** \brief The maximum allowed deviation from the template distance from the origin. */
216  double eps_dist_;
217  };
218 }
219 
220 #ifdef PCL_NO_PRECOMPILE
221 #include <pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp>
222 #endif
boost::shared_ptr< SampleConsensusModel< PointT > > Ptr
Definition: sac_model.h:76
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:564
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: pcl_macros.h:359
typename PointCloud::Ptr PointCloudPtr
Definition: sac_model.h:73
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:583
SampleConsensusModel represents the base model class.
Definition: sac_model.h:68
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.
boost::shared_ptr< const SampleConsensusModel< PointT > > ConstPtr
Definition: sac_model.h:77
std::string model_name_
The model name.
Definition: sac_model.h:523
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:72
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
Definition: sac_model.h:580
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
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.
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
void setEpsDist(const double delta)
Set the distance epsilon (delta) threshold.
typename pcl::PointCloud< PointNT >::Ptr PointCloudNPtr
Definition: sac_model.h:584
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:561