Point Cloud Library (PCL)  1.10.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 #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 
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
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
Eigen
Definition: bfgs.h:9
pcl::SampleConsensusModelFromNormals
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
Definition: sac_model.h:580
pcl::SampleConsensusModelNormalParallelPlane::getDistanceFromOrigin
double getDistanceFromOrigin() const
Get the distance of the plane from the origin.
Definition: sac_model_normal_parallel_plane.h:175
pcl::SampleConsensusModel::sample_size_
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition: sac_model.h:561
pcl::SampleConsensusModelNormalParallelPlane::getModelType
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_NORMAL_PARALLEL_PLANE).
Definition: sac_model_normal_parallel_plane.h:189
pcl::SampleConsensusModelNormalPlane
SampleConsensusModelNormalPlane defines a model for 3D plane segmentation using additional surface no...
Definition: sac_model_normal_plane.h:76
pcl::SampleConsensusModel::model_size_
unsigned int model_size_
The number of coefficients in the model.
Definition: sac_model.h:564
pcl::SampleConsensusModelNormalParallelPlane::getEpsDist
double getEpsDist() const
Get the distance epsilon (delta) threshold.
Definition: sac_model_normal_parallel_plane.h:185
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:52
pcl::SampleConsensusModelNormalParallelPlane::SampleConsensusModelNormalParallelPlane
SampleConsensusModelNormalParallelPlane(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelNormalParallelPlane.
Definition: sac_model_normal_parallel_plane.h:109
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:620
pcl::SampleConsensusModelNormalParallelPlane::SampleConsensusModelNormalParallelPlane
SampleConsensusModelNormalParallelPlane(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModelNormalParallelPlane.
Definition: sac_model_normal_parallel_plane.h:128
pcl::SACMODEL_NORMAL_PARALLEL_PLANE
@ SACMODEL_NORMAL_PARALLEL_PLANE
Definition: model_types.h:63
pcl::SampleConsensusModelNormalParallelPlane::setEpsDist
void setEpsDist(const double delta)
Set the distance epsilon (delta) threshold.
Definition: sac_model_normal_parallel_plane.h:181
pcl::SampleConsensusModelNormalParallelPlane::getEpsAngle
double getEpsAngle() const
Get the angle epsilon (delta) threshold.
Definition: sac_model_normal_parallel_plane.h:165
pcl::SacModel
SacModel
Definition: model_types.h:45
pcl::SampleConsensusModel::ConstPtr
shared_ptr< const SampleConsensusModel< PointT > > ConstPtr
Definition: sac_model.h:77
pcl::SampleConsensusModelFromNormals::PointCloudNPtr
typename pcl::PointCloud< PointNT >::Ptr PointCloudNPtr
Definition: sac_model.h:584
pcl::SampleConsensusModelFromNormals::PointCloudNConstPtr
typename pcl::PointCloud< PointNT >::ConstPtr PointCloudNConstPtr
Definition: sac_model.h:583
pcl::SampleConsensusModelNormalParallelPlane
SampleConsensusModelNormalParallelPlane defines a model for 3D plane segmentation using additional su...
Definition: sac_model_normal_parallel_plane.h:85
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:389
pcl::SampleConsensusModel::Ptr
shared_ptr< SampleConsensusModel< PointT > > Ptr
Definition: sac_model.h:76
pcl::SampleConsensusModel::model_name_
std::string model_name_
The model name.
Definition: sac_model.h:523
pcl::SampleConsensusModel::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: sac_model.h:72
pcl::SampleConsensusModelNormalParallelPlane::~SampleConsensusModelNormalParallelPlane
~SampleConsensusModelNormalParallelPlane()
Empty destructor.
Definition: sac_model_normal_parallel_plane.h:144
pcl::SampleConsensusModelNormalParallelPlane::setEpsAngle
void setEpsAngle(const double ea)
Set the angle epsilon (delta) threshold.
Definition: sac_model_normal_parallel_plane.h:161
pcl::SampleConsensusModel::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: sac_model.h:73
pcl::SampleConsensusModel
SampleConsensusModel represents the base model class.
Definition: sac_model.h:68
pcl::SampleConsensusModelNormalParallelPlane::setDistanceFromOrigin
void setDistanceFromOrigin(const double d)
Set the distance we expect the plane to be from the origin.
Definition: sac_model_normal_parallel_plane.h:171
pcl::SampleConsensusModelNormalParallelPlane::isModelValid
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
Definition: sac_model_normal_parallel_plane.hpp:48
pcl::shared_ptr
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:108
pcl::SampleConsensusModelNormalParallelPlane::getAxis
Eigen::Vector3f getAxis() const
Get the axis along which we need to search for a plane perpendicular to.
Definition: sac_model_normal_parallel_plane.h:154
pcl::SampleConsensusModelNormalParallelPlane::setAxis
void setAxis(const Eigen::Vector3f &ax)
Set the axis along which we need to search for a plane perpendicular to.
Definition: sac_model_normal_parallel_plane.h:150