Point Cloud Library (PCL)  1.14.1-dev
sac_model_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 #ifdef __SSE__
44 #include <xmmintrin.h> // for __m128
45 #endif // ifdef __SSE__
46 #ifdef __AVX__
47 #include <immintrin.h> // for __m256
48 #endif // ifdef __AVX__
49 
50 #include <pcl/sample_consensus/sac_model.h>
51 #include <pcl/sample_consensus/model_types.h>
52 
53 namespace pcl
54 {
55 
56  /** \brief Project a point on a planar model given by a set of normalized coefficients
57  * \param[in] p the input point to project
58  * \param[in] model_coefficients the coefficients of the plane (a, b, c, d) that satisfy ax+by+cz+d=0
59  * \param[out] q the resultant projected point
60  */
61  template <typename Point> inline void
62  projectPoint (const Point &p, const Eigen::Vector4f &model_coefficients, Point &q)
63  {
64  // Calculate the distance from the point to the plane
65  Eigen::Vector4f pp (p.x, p.y, p.z, 1);
66  // use normalized coefficients to calculate the scalar projection
67  float distance_to_plane = pp.dot(model_coefficients);
68 
69 
70  //TODO: Why doesn't getVector4Map work here?
71  //Eigen::Vector4f q_e = q.getVector4fMap ();
72  //q_e = pp - model_coefficients * distance_to_plane;
73 
74  Eigen::Vector4f q_e = pp - distance_to_plane * model_coefficients;
75  q.x = q_e[0];
76  q.y = q_e[1];
77  q.z = q_e[2];
78  }
79 
80  /** \brief Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0
81  * \param p a point
82  * \param a the normalized <i>a</i> coefficient of a plane
83  * \param b the normalized <i>b</i> coefficient of a plane
84  * \param c the normalized <i>c</i> coefficient of a plane
85  * \param d the normalized <i>d</i> coefficient of a plane
86  * \ingroup sample_consensus
87  */
88  template <typename Point> inline double
89  pointToPlaneDistanceSigned (const Point &p, double a, double b, double c, double d)
90  {
91  return (a * p.x + b * p.y + c * p.z + d);
92  }
93 
94  /** \brief Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0
95  * \param p a point
96  * \param plane_coefficients the normalized coefficients (a, b, c, d) of a plane
97  * \ingroup sample_consensus
98  */
99  template <typename Point> inline double
100  pointToPlaneDistanceSigned (const Point &p, const Eigen::Vector4f &plane_coefficients)
101  {
102  return ( plane_coefficients[0] * p.x + plane_coefficients[1] * p.y + plane_coefficients[2] * p.z + plane_coefficients[3] );
103  }
104 
105  /** \brief Get the distance from a point to a plane (unsigned) defined by ax+by+cz+d=0
106  * \param p a point
107  * \param a the normalized <i>a</i> coefficient of a plane
108  * \param b the normalized <i>b</i> coefficient of a plane
109  * \param c the normalized <i>c</i> coefficient of a plane
110  * \param d the normalized <i>d</i> coefficient of a plane
111  * \ingroup sample_consensus
112  */
113  template <typename Point> inline double
114  pointToPlaneDistance (const Point &p, double a, double b, double c, double d)
115  {
116  return (std::abs (pointToPlaneDistanceSigned (p, a, b, c, d)) );
117  }
118 
119  /** \brief Get the distance from a point to a plane (unsigned) defined by ax+by+cz+d=0
120  * \param p a point
121  * \param plane_coefficients the normalized coefficients (a, b, c, d) of a plane
122  * \ingroup sample_consensus
123  */
124  template <typename Point> inline double
125  pointToPlaneDistance (const Point &p, const Eigen::Vector4f &plane_coefficients)
126  {
127  return ( std::abs (pointToPlaneDistanceSigned (p, plane_coefficients)) );
128  }
129 
130  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
131  /** \brief SampleConsensusModelPlane defines a model for 3D plane segmentation.
132  * The model coefficients are defined as:
133  * - \b a : the X coordinate of the plane's normal (normalized)
134  * - \b b : the Y coordinate of the plane's normal (normalized)
135  * - \b c : the Z coordinate of the plane's normal (normalized)
136  * - \b d : the fourth <a href="http://mathworld.wolfram.com/HessianNormalForm.html">Hessian component</a> of the plane's equation
137  *
138  * \author Radu B. Rusu
139  * \ingroup sample_consensus
140  */
141  template <typename PointT>
143  {
144  public:
150 
154 
155  using Ptr = shared_ptr<SampleConsensusModelPlane<PointT> >;
156  using ConstPtr = shared_ptr<const SampleConsensusModelPlane<PointT>>;
157 
158  /** \brief Constructor for base SampleConsensusModelPlane.
159  * \param[in] cloud the input point cloud dataset
160  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
161  */
162  SampleConsensusModelPlane (const PointCloudConstPtr &cloud, bool random = false)
163  : SampleConsensusModel<PointT> (cloud, random)
164  {
165  model_name_ = "SampleConsensusModelPlane";
166  sample_size_ = 3;
167  model_size_ = 4;
168  }
169 
170  /** \brief Constructor for base SampleConsensusModelPlane.
171  * \param[in] cloud the input point cloud dataset
172  * \param[in] indices a vector of point indices to be used from \a cloud
173  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
174  */
176  const Indices &indices,
177  bool random = false)
178  : SampleConsensusModel<PointT> (cloud, indices, random)
179  {
180  model_name_ = "SampleConsensusModelPlane";
181  sample_size_ = 3;
182  model_size_ = 4;
183  }
184 
185  /** \brief Empty destructor */
186  ~SampleConsensusModelPlane () override = default;
187 
188  /** \brief Check whether the given index samples can form a valid plane model, compute the model coefficients from
189  * these samples and store them internally in model_coefficients_. The plane coefficients are:
190  * a, b, c, d (ax+by+cz+d=0)
191  * \param[in] samples the point indices found as possible good candidates for creating a valid model
192  * \param[out] model_coefficients the resultant model coefficients
193  */
194  bool
195  computeModelCoefficients (const Indices &samples,
196  Eigen::VectorXf &model_coefficients) const override;
197 
198  /** \brief Compute all distances from the cloud data to a given plane model.
199  * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to
200  * \param[out] distances the resultant estimated distances
201  */
202  void
203  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
204  std::vector<double> &distances) const override;
205 
206  /** \brief Select all the points which respect the given model coefficients as inliers.
207  * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to
208  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
209  * \param[out] inliers the resultant model inliers
210  */
211  void
212  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
213  const double threshold,
214  Indices &inliers) override;
215 
216  /** \brief Count all the points which respect the given model coefficients as inliers.
217  *
218  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
219  * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
220  * \return the resultant number of inliers
221  */
222  std::size_t
223  countWithinDistance (const Eigen::VectorXf &model_coefficients,
224  const double threshold) const override;
225 
226  /** \brief Recompute the plane coefficients using the given inlier set and return them to the user.
227  * @note: these are the coefficients of the plane model after refinement (e.g. after SVD)
228  * \param[in] inliers the data inliers found as supporting the model
229  * \param[in] model_coefficients the initial guess for the model coefficients
230  * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
231  */
232  void
233  optimizeModelCoefficients (const Indices &inliers,
234  const Eigen::VectorXf &model_coefficients,
235  Eigen::VectorXf &optimized_coefficients) const override;
236 
237  /** \brief Create a new point cloud with inliers projected onto the plane model.
238  * \param[in] inliers the data inliers that we want to project on the plane model
239  * \param[in] model_coefficients the *normalized* coefficients of a plane model
240  * \param[out] projected_points the resultant projected points
241  * \param[in] copy_data_fields set to true if we need to copy the other data fields
242  */
243  void
244  projectPoints (const Indices &inliers,
245  const Eigen::VectorXf &model_coefficients,
246  PointCloud &projected_points,
247  bool copy_data_fields = true) const override;
248 
249  /** \brief Verify whether a subset of indices verifies the given plane model coefficients.
250  * \param[in] indices the data indices that need to be tested against the plane model
251  * \param[in] model_coefficients the plane model coefficients
252  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
253  */
254  bool
255  doSamplesVerifyModel (const std::set<index_t> &indices,
256  const Eigen::VectorXf &model_coefficients,
257  const double threshold) const override;
258 
259  /** \brief Return a unique id for this model (SACMODEL_PLANE). */
260  inline pcl::SacModel
261  getModelType () const override { return (SACMODEL_PLANE); }
262 
263  protected:
266 
267  /** This implementation uses no SIMD instructions. It is not intended for normal use.
268  * See countWithinDistance which automatically uses the fastest implementation.
269  */
270  std::size_t
271  countWithinDistanceStandard (const Eigen::VectorXf &model_coefficients,
272  const double threshold,
273  std::size_t i = 0) const;
274 
275 #if defined (__SSE__) && defined (__SSE2__) && defined (__SSE4_1__)
276  /** This implementation uses SSE, SSE2, and SSE4.1 instructions. It is not intended for normal use.
277  * See countWithinDistance which automatically uses the fastest implementation.
278  */
279  std::size_t
280  countWithinDistanceSSE (const Eigen::VectorXf &model_coefficients,
281  const double threshold,
282  std::size_t i = 0) const;
283 #endif
284 
285 #if defined (__AVX__) && defined (__AVX2__)
286  /** This implementation uses AVX and AVX2 instructions. It is not intended for normal use.
287  * See countWithinDistance which automatically uses the fastest implementation.
288  */
289  std::size_t
290  countWithinDistanceAVX (const Eigen::VectorXf &model_coefficients,
291  const double threshold,
292  std::size_t i = 0) const;
293 #endif
294 
295 #define PCLAT(POS) ((*input_)[(*indices_)[(POS)]])
296 
297 #ifdef __AVX__
298 // This function computes the distances of 8 points to the plane
299 inline __m256 dist8 (const std::size_t i, const __m256 &a_vec, const __m256 &b_vec, const __m256 &c_vec, const __m256 &d_vec, const __m256 &abs_help) const
300 {
301  // The andnot-function realizes an abs-operation: the sign bit is removed
302  return _mm256_andnot_ps (abs_help,
303  _mm256_add_ps (_mm256_add_ps (_mm256_mul_ps (a_vec, _mm256_set_ps (PCLAT(i ).x, PCLAT(i+1).x, PCLAT(i+2).x, PCLAT(i+3).x, PCLAT(i+4).x, PCLAT(i+5).x, PCLAT(i+6).x, PCLAT(i+7).x)),
304  _mm256_mul_ps (b_vec, _mm256_set_ps (PCLAT(i ).y, PCLAT(i+1).y, PCLAT(i+2).y, PCLAT(i+3).y, PCLAT(i+4).y, PCLAT(i+5).y, PCLAT(i+6).y, PCLAT(i+7).y))),
305  _mm256_add_ps (_mm256_mul_ps (c_vec, _mm256_set_ps (PCLAT(i ).z, PCLAT(i+1).z, PCLAT(i+2).z, PCLAT(i+3).z, PCLAT(i+4).z, PCLAT(i+5).z, PCLAT(i+6).z, PCLAT(i+7).z)),
306  d_vec))); // TODO this could be replaced by three fmadd-instructions (if available), but the speed gain would probably be minimal
307 }
308 #endif // ifdef __AVX__
309 
310 #ifdef __SSE__
311 // This function computes the distances of 4 points to the plane
312 inline __m128 dist4 (const std::size_t i, const __m128 &a_vec, const __m128 &b_vec, const __m128 &c_vec, const __m128 &d_vec, const __m128 &abs_help) const
313 {
314  // The andnot-function realizes an abs-operation: the sign bit is removed
315  return _mm_andnot_ps (abs_help,
316  _mm_add_ps (_mm_add_ps (_mm_mul_ps (a_vec, _mm_set_ps (PCLAT(i ).x, PCLAT(i+1).x, PCLAT(i+2).x, PCLAT(i+3).x)),
317  _mm_mul_ps (b_vec, _mm_set_ps (PCLAT(i ).y, PCLAT(i+1).y, PCLAT(i+2).y, PCLAT(i+3).y))),
318  _mm_add_ps (_mm_mul_ps (c_vec, _mm_set_ps (PCLAT(i ).z, PCLAT(i+1).z, PCLAT(i+2).z, PCLAT(i+3).z)),
319  d_vec))); // TODO this could be replaced by three fmadd-instructions (if available), but the speed gain would probably be minimal
320 }
321 #endif // ifdef __SSE__
322 
323 #undef PCLAT
324 
325  private:
326  /** \brief Check if a sample of indices results in a good sample of points
327  * indices.
328  * \param[in] samples the resultant index samples
329  */
330  bool
331  isSampleGood (const Indices &samples) const override;
332  };
333 }
334 
335 #ifdef PCL_NO_PRECOMPILE
336 #include <pcl/sample_consensus/impl/sac_model_plane.hpp>
337 #endif
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
SampleConsensusModel represents the base model class.
Definition: sac_model.h:71
shared_ptr< SampleConsensusModel< PointT > > Ptr
Definition: sac_model.h:78
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: sac_model.h:74
typename PointCloud::Ptr PointCloudPtr
Definition: sac_model.h:75
shared_ptr< const SampleConsensusModel< PointT > > ConstPtr
Definition: sac_model.h:79
SampleConsensusModelPlane defines a model for 3D plane segmentation.
~SampleConsensusModelPlane() override=default
Empty destructor.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_PLANE).
SampleConsensusModelPlane(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelPlane.
SampleConsensusModelPlane(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelPlane.
double pointToPlaneDistanceSigned(const Point &p, double a, double b, double c, double d)
Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0.
double pointToPlaneDistance(const Point &p, double a, double b, double c, double d)
Get the distance from a point to a plane (unsigned) defined by ax+by+cz+d=0.
void projectPoint(const Point &p, const Eigen::Vector4f &model_coefficients, Point &q)
Project a point on a planar model given by a set of normalized coefficients.
SacModel
Definition: model_types.h:46
@ SACMODEL_PLANE
Definition: model_types.h:47
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define PCL_EXPORTS
Definition: pcl_macros.h:323
A point structure representing Euclidean xyz coordinates, and the RGB color.