Point Cloud Library (PCL)  1.11.1-dev
common.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder(s) nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id$
35  *
36  */
37 
38 #pragma once
39 
40 #ifdef __SSE__
41 #include <xmmintrin.h> // for __m128
42 #endif // ifdef __SSE__
43 #ifdef __AVX__
44 #include <immintrin.h> // for __m256
45 #endif // ifdef __AVX__
46 
47 #include <pcl/point_cloud.h> // for PointCloud
48 #include <pcl/PointIndices.h> // for PointIndices
49 namespace pcl { struct PCLPointCloud2; }
50 
51 /**
52  * \file pcl/common/common.h
53  * Define standard C methods and C++ classes that are common to all methods
54  * \ingroup common
55  */
56 
57 /*@{*/
58 namespace pcl
59 {
60  /** \brief Compute the smallest angle between two 3D vectors in radians (default) or degree.
61  * \param v1 the first 3D vector (represented as a \a Eigen::Vector4f)
62  * \param v2 the second 3D vector (represented as a \a Eigen::Vector4f)
63  * \param in_degree determine if angle should be in radians or degrees
64  * \return the angle between v1 and v2 in radians or degrees
65  * \note Handles rounding error for parallel and anti-parallel vectors
66  * \ingroup common
67  */
68  inline double
69  getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree = false);
70 
71  /** \brief Compute the smallest angle between two 3D vectors in radians (default) or degree.
72  * \param v1 the first 3D vector (represented as a \a Eigen::Vector3f)
73  * \param v2 the second 3D vector (represented as a \a Eigen::Vector3f)
74  * \param in_degree determine if angle should be in radians or degrees
75  * \return the angle between v1 and v2 in radians or degrees
76  * \ingroup common
77  */
78  inline double
79  getAngle3D (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const bool in_degree = false);
80 
81 #ifdef __SSE__
82  /** \brief Compute the approximate arccosine of four values at once using SSE instructions.
83  *
84  * The approximation used is \f$ (1.59121552+x*(-0.15461442+x*0.05354897))*\sqrt{0.89286965-0.89282669*x}+0.06681017+x*(-0.09402311+x*0.02708663) \f$.
85  * The average error is 0.00012 rad. This approximation is more accurate than other approximations of acos, but also uses a few more operations.
86  * \param x four floats, each should be in [0; 1]. They must not be greater than 1 since acos is undefined there.
87  * They should not be less than 0 because there the approximation is less precise
88  * \return the four arccosines, each in [0; pi/2]
89  * \ingroup common
90  */
91  inline __m128
92  acos_SSE (const __m128 &x);
93 
94  /** \brief Similar to getAngle3D, but four times in parallel using SSE instructions.
95  *
96  * This behaves like \f$ min(getAngle3D(dot_product), \pi-getAngle3D(dot_product)) \f$.
97  * All vectors must be normalized (length is 1.0).
98  * Since an approximate acos is used, the results may be slightly imprecise.
99  * \param[in] the x components of the first four vectors
100  * \param[in] the y components of the first four vectors
101  * \param[in] the z components of the first four vectors
102  * \param[in] the x components of the second four vectors
103  * \param[in] the y components of the second four vectors
104  * \param[in] the z components of the second four vectors
105  * \return the four angles in radians in [0; pi/2]
106  * \ingroup common
107  */
108  inline __m128
109  getAcuteAngle3DSSE (const __m128 &x1, const __m128 &y1, const __m128 &z1, const __m128 &x2, const __m128 &y2, const __m128 &z2);
110 #endif // ifdef __SSE__
111 
112 #ifdef __AVX__
113  /** \brief Compute the approximate arccosine of eight values at once using AVX instructions.
114  *
115  * The approximation used is \f$ (1.59121552+x*(-0.15461442+x*0.05354897))*\sqrt{0.89286965-0.89282669*x}+0.06681017+x*(-0.09402311+x*0.02708663) \f$.
116  * The average error is 0.00012 rad. This approximation is more accurate than other approximations of acos, but also uses a few more operations.
117  * \param x eight floats, each should be in [0; 1]. They must not be greater than 1 since acos is undefined there.
118  * They should not be less than 0 because there the approximation is less precise
119  * \return the eight arccosines, each in [0; pi/2]
120  * \ingroup common
121  */
122  inline __m256
123  acos_AVX (const __m256 &x);
124 
125  /** \brief Similar to getAngle3D, but eight times in parallel using AVX instructions.
126  *
127  * This behaves like \f$ min(getAngle3D(dot_product), \pi-getAngle3D(dot_product)) \f$.
128  * All vectors must be normalized (length is 1.0).
129  * Since an approximate acos is used, the results may be slightly imprecise.
130  * \param[in] the x components of the first eight vectors
131  * \param[in] the y components of the first eight vectors
132  * \param[in] the z components of the first eight vectors
133  * \param[in] the x components of the second eight vectors
134  * \param[in] the y components of the second eight vectors
135  * \param[in] the z components of the second eight vectors
136  * \return the eight angles in radians in [0; pi/2]
137  * \ingroup common
138  */
139  inline __m256
140  getAcuteAngle3DAVX (const __m256 &x1, const __m256 &y1, const __m256 &z1, const __m256 &x2, const __m256 &y2, const __m256 &z2);
141 #endif // ifdef __AVX__
142 
143  /** \brief Compute both the mean and the standard deviation of an array of values
144  * \param values the array of values
145  * \param mean the resultant mean of the distribution
146  * \param stddev the resultant standard deviation of the distribution
147  * \ingroup common
148  */
149  inline void
150  getMeanStd (const std::vector<float> &values, double &mean, double &stddev);
151 
152  /** \brief Get a set of points residing in a box given its bounds
153  * \param cloud the point cloud data message
154  * \param min_pt the minimum bounds
155  * \param max_pt the maximum bounds
156  * \param indices the resultant set of point indices residing in the box
157  * \ingroup common
158  */
159  template <typename PointT> inline void
160  getPointsInBox (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &min_pt,
161  Eigen::Vector4f &max_pt, Indices &indices);
162 
163  /** \brief Get the point at maximum distance from a given point and a given pointcloud
164  * \param cloud the point cloud data message
165  * \param pivot_pt the point from where to compute the distance
166  * \param max_pt the point in cloud that is the farthest point away from pivot_pt
167  * \ingroup common
168  */
169  template<typename PointT> inline void
170  getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt);
171 
172  /** \brief Get the point at maximum distance from a given point and a given pointcloud
173  * \param cloud the point cloud data message
174  * \param indices the vector of point indices to use from \a cloud
175  * \param pivot_pt the point from where to compute the distance
176  * \param max_pt the point in cloud that is the farthest point away from pivot_pt
177  * \ingroup common
178  */
179  template<typename PointT> inline void
180  getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Indices &indices,
181  const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt);
182 
183  /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud
184  * \param[in] cloud the point cloud data message
185  * \param[out] min_pt the resultant minimum bounds
186  * \param[out] max_pt the resultant maximum bounds
187  * \ingroup common
188  */
189  template <typename PointT> inline void
190  getMinMax3D (const pcl::PointCloud<PointT> &cloud, PointT &min_pt, PointT &max_pt);
191 
192  /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud
193  * \param[in] cloud the point cloud data message
194  * \param[out] min_pt the resultant minimum bounds
195  * \param[out] max_pt the resultant maximum bounds
196  * \ingroup common
197  */
198  template <typename PointT> inline void
199  getMinMax3D (const pcl::PointCloud<PointT> &cloud,
200  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
201 
202  /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud
203  * \param[in] cloud the point cloud data message
204  * \param[in] indices the vector of point indices to use from \a cloud
205  * \param[out] min_pt the resultant minimum bounds
206  * \param[out] max_pt the resultant maximum bounds
207  * \ingroup common
208  */
209  template <typename PointT> inline void
210  getMinMax3D (const pcl::PointCloud<PointT> &cloud, const Indices &indices,
211  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
212 
213  /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud
214  * \param[in] cloud the point cloud data message
215  * \param[in] indices the vector of point indices to use from \a cloud
216  * \param[out] min_pt the resultant minimum bounds
217  * \param[out] max_pt the resultant maximum bounds
218  * \ingroup common
219  */
220  template <typename PointT> inline void
221  getMinMax3D (const pcl::PointCloud<PointT> &cloud, const pcl::PointIndices &indices,
222  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
223 
224  /** \brief Compute the radius of a circumscribed circle for a triangle formed of three points pa, pb, and pc
225  * \param pa the first point
226  * \param pb the second point
227  * \param pc the third point
228  * \return the radius of the circumscribed circle
229  * \ingroup common
230  */
231  template <typename PointT> inline double
232  getCircumcircleRadius (const PointT &pa, const PointT &pb, const PointT &pc);
233 
234  /** \brief Get the minimum and maximum values on a point histogram
235  * \param histogram the point representing a multi-dimensional histogram
236  * \param len the length of the histogram
237  * \param min_p the resultant minimum
238  * \param max_p the resultant maximum
239  * \ingroup common
240  */
241  template <typename PointT> inline void
242  getMinMax (const PointT &histogram, int len, float &min_p, float &max_p);
243 
244  /** \brief Calculate the area of a polygon given a point cloud that defines the polygon
245  * \param polygon point cloud that contains those vertices that comprises the polygon. Vertices are stored in counterclockwise.
246  * \return the polygon area
247  * \ingroup common
248  */
249  template<typename PointT> inline float
251 
252  /** \brief Get the minimum and maximum values on a point histogram
253  * \param cloud the cloud containing multi-dimensional histograms
254  * \param idx point index representing the histogram that we need to compute min/max for
255  * \param field_name the field name containing the multi-dimensional histogram
256  * \param min_p the resultant minimum
257  * \param max_p the resultant maximum
258  * \ingroup common
259  */
260  PCL_EXPORTS void
261  getMinMax (const pcl::PCLPointCloud2 &cloud, int idx, const std::string &field_name,
262  float &min_p, float &max_p);
263 
264  /** \brief Compute both the mean and the standard deviation of an array of values
265  * \param values the array of values
266  * \param mean the resultant mean of the distribution
267  * \param stddev the resultant standard deviation of the distribution
268  * \ingroup common
269  */
270  PCL_EXPORTS void
271  getMeanStdDev (const std::vector<float> &values, double &mean, double &stddev);
272 
273  /** \brief Compute the median of a list of values (fast). If the number of values is even, take the mean of the two middle values.
274  * This function can be used like this:
275  * \code{.cpp}
276  * std::vector<double> vector{1.0, 25.0, 9.0, 4.0, 16.0};
277  * const double median = pcl::computeMedian (vector.begin (), vector.end (), static_cast<double(*)(double)>(std::sqrt)); // = 3
278  * \endcode
279  * \param[in,out] begin,end Iterators that mark the beginning and end of the value range. These values will be reordered!
280  * \param[in] f A lamda, function pointer, or similar that is implicitly applied to all values before median computation. In reality, it will be applied lazily (i.e. at most twice) and thus may not change the sorting order (e.g. monotonic functions like sqrt are allowed)
281  * \return the median
282  * \ingroup common
283  */
284  template<typename IteratorT, typename Functor> inline auto
285  computeMedian (IteratorT begin, IteratorT end, Functor f) noexcept -> typename std::result_of<Functor(decltype(*begin))>::type
286  {
287  const std::size_t size = std::distance(begin, end);
288  const std::size_t mid = size/2;
289  if (size%2==0)
290  { // Even number of values
291  std::nth_element (begin, begin + (mid-1), end);
292  return (f(begin[mid-1]) + f(*(std::min_element (begin + mid, end)))) / 2.0;
293  }
294  else
295  { // Odd number of values
296  std::nth_element (begin, begin + mid, end);
297  return f(begin[mid]);
298  }
299  }
300 
301  /** \brief Compute the median of a list of values (fast). See the other overloaded function for more information.
302  */
303  template<typename IteratorT> inline auto
304  computeMedian (IteratorT begin, IteratorT end) noexcept -> typename std::iterator_traits<IteratorT>::value_type
305  {
306  return computeMedian (begin, end, [](const auto& x){return x;});
307  }
308 }
309 /*@}*/
310 #include <pcl/common/impl/common.hpp>
pcl
Definition: convolution.h:46
pcl::geometry::distance
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
pcl::getPointsInBox
void getPointsInBox(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, Indices &indices)
Get a set of points residing in a box given its bounds.
Definition: common.hpp:154
pcl::computeMedian
auto computeMedian(IteratorT begin, IteratorT end, Functor f) noexcept -> typename std::result_of< Functor(decltype(*begin))>::type
Compute the median of a list of values (fast).
Definition: common.h:285
pcl::getMeanStd
void getMeanStd(const std::vector< float > &values, double &mean, double &stddev)
Compute both the mean and the standard deviation of an array of values.
Definition: common.hpp:124
pcl::getCircumcircleRadius
double getCircumcircleRadius(const PointT &pa, const PointT &pb, const PointT &pc)
Compute the radius of a circumscribed circle for a triangle formed of three points pa,...
Definition: common.hpp:383
pcl::getMeanStdDev
PCL_EXPORTS void getMeanStdDev(const std::vector< float > &values, double &mean, double &stddev)
Compute both the mean and the standard deviation of an array of values.
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:630
pcl::getAngle3D
double getAngle3D(const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree=false)
Compute the smallest angle between two 3D vectors in radians (default) or degree.
Definition: common.hpp:47
pcl::calculatePolygonArea
float calculatePolygonArea(const pcl::PointCloud< PointT > &polygon)
Calculate the area of a polygon given a point cloud that defines the polygon.
Definition: common.hpp:414
pcl::getMaxDistance
void getMaxDistance(const pcl::PointCloud< PointT > &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
Get the point at maximum distance from a given point and a given pointcloud.
Definition: common.hpp:197
pcl::Functor
Base functor all the models that need non linear optimization must define their own one and implement...
Definition: sac_model.h:678
pcl::PointIndices
Definition: PointIndices.h:11
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
pcl::PCLPointCloud2
Definition: PCLPointCloud2.h:16
pcl::getMinMax3D
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud.
Definition: common.hpp:295
pcl::getMinMax
void getMinMax(const PointT &histogram, int len, float &min_p, float &max_p)
Get the minimum and maximum values on a point histogram.
Definition: common.hpp:400
PCL_EXPORTS
#define PCL_EXPORTS
Definition: pcl_macros.h:323