Point Cloud Library (PCL)  1.14.0-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 
103  using Ptr = shared_ptr<SampleConsensusModelNormalParallelPlane<PointT, PointNT> >;
104  using ConstPtr = shared_ptr<const SampleConsensusModelNormalParallelPlane<PointT, PointNT>>;
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 Indices &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
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
Definition: sac_model.h:613
typename pcl::PointCloud< PointNT >::ConstPtr PointCloudNConstPtr
Definition: sac_model.h:615
typename pcl::PointCloud< PointNT >::Ptr PointCloudNPtr
Definition: sac_model.h:616
SampleConsensusModel represents the base model class.
Definition: sac_model.h:71
shared_ptr< SampleConsensusModel< PointT > > Ptr
Definition: sac_model.h:78
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition: sac_model.h:589
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: sac_model.h:74
std::string model_name_
The model name.
Definition: sac_model.h:551
unsigned int model_size_
The number of coefficients in the model.
Definition: sac_model.h:592
typename PointCloud::Ptr PointCloudPtr
Definition: sac_model.h:75
shared_ptr< const SampleConsensusModel< PointT > > ConstPtr
Definition: sac_model.h:79
SampleConsensusModelNormalParallelPlane defines a model for 3D plane segmentation using additional su...
double getDistanceFromOrigin() const
Get the distance of the plane from the origin.
void setEpsDist(const double delta)
Set the distance epsilon (delta) threshold.
SampleConsensusModelNormalParallelPlane(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelNormalParallelPlane.
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
void setDistanceFromOrigin(const double d)
Set the distance we expect the plane to be from the origin.
SampleConsensusModelNormalParallelPlane(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelNormalParallelPlane.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_NORMAL_PARALLEL_PLANE).
~SampleConsensusModelNormalParallelPlane() override=default
Empty destructor.
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a plane perpendicular to.
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a plane perpendicular to.
double getEpsDist() const
Get the distance epsilon (delta) threshold.
SampleConsensusModelNormalPlane defines a model for 3D plane segmentation using additional surface no...
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
Defines functions, macros and traits for allocating and using memory.
Definition: bfgs.h:10
SacModel
Definition: model_types.h:46
@ SACMODEL_NORMAL_PARALLEL_PLANE
Definition: model_types.h:63
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
Defines all the PCL and non-PCL macros used.
A point structure representing Euclidean xyz coordinates, and the RGB color.