Point Cloud Library (PCL)  1.9.1-dev
accumulators.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2014-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_COMMON_IMPL_DETAIL_ACCUMULATORS_HPP
39 #define PCL_COMMON_IMPL_DETAIL_ACCUMULATORS_HPP
40 
41 #include <map>
42 
43 #include <boost/mpl/filter_view.hpp>
44 #include <boost/fusion/include/mpl.hpp>
45 #include <boost/fusion/include/for_each.hpp>
46 #include <boost/fusion/include/as_vector.hpp>
47 #include <boost/fusion/include/filter_if.hpp>
48 
49 #include <pcl/pcl_macros.h>
50 #include <pcl/point_types.h>
51 
52 namespace pcl
53 {
54 
55  namespace detail
56  {
57 
58  /* Below are several helper accumulator structures that are used by the
59  * `CentroidPoint` class. Each of them is capable of accumulating
60  * information from a particular field(s) of a point. The points are
61  * inserted via `add()` and extracted via `get()` functions. Note that the
62  * accumulators are not templated on point type, so in principle it is
63  * possible to insert and extract points of different types. It is the
64  * responsibility of the user to make sure that points have corresponding
65  * fields. */
66 
68  {
69 
70  // Requires that point type has x, y, and z fields
71  using IsCompatible = pcl::traits::has_xyz<boost::mpl::_1>;
72 
73  // Storage
74  Eigen::Vector3f xyz = Eigen::Vector3f::Zero ();
75 
76  template <typename PointT> void
77  add (const PointT& t) { xyz += t.getVector3fMap (); }
78 
79  template <typename PointT> void
80  get (PointT& t, std::size_t n) const { t.getVector3fMap () = xyz / n; }
81 
83 
84  };
85 
87  {
88 
89  // Requires that point type has normal_x, normal_y, and normal_z fields
90  using IsCompatible = pcl::traits::has_normal<boost::mpl::_1>;
91 
92  // Storage
93  Eigen::Vector4f normal = Eigen::Vector4f::Zero ();
94 
95  // Requires that the normal of the given point is normalized, otherwise it
96  // does not make sense to sum it up with the accumulated value.
97  template <typename PointT> void
98  add (const PointT& t) { normal += t.getNormalVector4fMap (); }
99 
100  template <typename PointT> void
101  get (PointT& t, std::size_t) const
102  {
103 #if EIGEN_VERSION_AT_LEAST (3, 3, 0)
104  t.getNormalVector4fMap () = normal.normalized ();
105 #else
106  if (normal.squaredNorm() > 0)
107  t.getNormalVector4fMap () = normal.normalized ();
108  else
109  t.getNormalVector4fMap () = Eigen::Vector4f::Zero ();
110 #endif
111  }
112 
114 
115  };
116 
118  {
119 
120  // Requires that point type has curvature field
121  using IsCompatible = pcl::traits::has_curvature<boost::mpl::_1>;
122 
123  // Storage
124  float curvature = 0;
125 
126  template <typename PointT> void
127  add (const PointT& t) { curvature += t.curvature; }
128 
129  template <typename PointT> void
130  get (PointT& t, std::size_t n) const { t.curvature = curvature / n; }
131 
132  };
133 
135  {
136 
137  // Requires that point type has rgb or rgba field
138  using IsCompatible = pcl::traits::has_color<boost::mpl::_1>;
139 
140  // Storage
141  float r = 0, g = 0, b = 0, a = 0;
142 
143  template <typename PointT> void
144  add (const PointT& t)
145  {
146  r += static_cast<float> (t.r);
147  g += static_cast<float> (t.g);
148  b += static_cast<float> (t.b);
149  a += static_cast<float> (t.a);
150  }
151 
152  template <typename PointT> void
153  get (PointT& t, std::size_t n) const
154  {
155  t.rgba = static_cast<std::uint32_t> (a / n) << 24 |
156  static_cast<std::uint32_t> (r / n) << 16 |
157  static_cast<std::uint32_t> (g / n) << 8 |
158  static_cast<std::uint32_t> (b / n);
159  }
160 
161  };
162 
164  {
165 
166  // Requires that point type has intensity field
167  using IsCompatible = pcl::traits::has_intensity<boost::mpl::_1>;
168 
169  // Storage
170  float intensity = 0;
171 
172  template <typename PointT> void
173  add (const PointT& t) { intensity += t.intensity; }
174 
175  template <typename PointT> void
176  get (PointT& t, std::size_t n) const { t.intensity = intensity / n; }
177 
178  };
179 
181  {
182 
183  // Requires that point type has label field
184  using IsCompatible = pcl::traits::has_label<boost::mpl::_1>;
185 
186  // Storage
187  // A better performance may be achieved with a heap structure
188  std::map<std::uint32_t, std::size_t> labels;
189 
190  template <typename PointT> void
191  add (const PointT& t)
192  {
193  auto itr = labels.find (t.label);
194  if (itr == labels.end ())
195  labels.insert (std::make_pair (t.label, 1));
196  else
197  ++itr->second;
198  }
199 
200  template <typename PointT> void
201  get (PointT& t, std::size_t) const
202  {
203  std::size_t max = 0;
204  for (const auto &label : labels)
205  if (label.second > max)
206  {
207  max = label.second;
208  t.label = label.first;
209  }
210  }
211 
212  };
213 
214  /* Meta-function that checks if an accumulator is compatible with given
215  * point type(s). */
216  template <typename Point1T, typename Point2T = Point1T>
218 
219  template <typename AccumulatorT>
220  struct apply : boost::mpl::and_<
221  boost::mpl::apply<typename AccumulatorT::IsCompatible, Point1T>,
222  boost::mpl::apply<typename AccumulatorT::IsCompatible, Point2T>
223  > {};
224  };
225 
226  /* Meta-function that creates a Fusion vector of accumulator types that are
227  * compatible with a given point type. */
228  template <typename PointT>
230  {
231  using type =
232  typename boost::fusion::result_of::as_vector<
233  typename boost::mpl::filter_view<
234  boost::mpl::vector<
241  >
243  >
245  };
246 
247  /* Fusion function object to invoke point addition on every accumulator in
248  * a fusion sequence. */
249  template <typename PointT>
250  struct AddPoint
251  {
252 
253  const PointT& p;
254 
255  AddPoint (const PointT& point) : p (point) { }
256 
257  template <typename AccumulatorT> void
258  operator () (AccumulatorT& accumulator) const
259  {
260  accumulator.add (p);
261  }
262 
263  };
264 
265  /* Fusion function object to invoke get point on every accumulator in a
266  * fusion sequence. */
267  template <typename PointT>
268  struct GetPoint
269  {
270 
272  std::size_t n;
273 
274  GetPoint (PointT& point, std::size_t num) : p (point), n (num) { }
275 
276  template <typename AccumulatorT> void
277  operator () (AccumulatorT& accumulator) const
278  {
279  accumulator.get (p, n);
280  }
281 
282  };
283 
284  }
285 
286 }
287 
288 #endif /* PCL_COMMON_IMPL_DETAIL_ACCUMULATORS_HPP */
289 
pcl::traits::has_label< boost::mpl::_1 > IsCompatible
void add(const PointT &t)
void add(const PointT &t)
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
AddPoint(const PointT &point)
std::map< std::uint32_t, std::size_t > labels
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:359
void add(const PointT &t)
pcl::traits::has_color< boost::mpl::_1 > IsCompatible
typename boost::fusion::result_of::as_vector< typename boost::mpl::filter_view< boost::mpl::vector< AccumulatorXYZ, AccumulatorNormal, AccumulatorCurvature, AccumulatorRGBA, AccumulatorIntensity, AccumulatorLabel >, IsAccumulatorCompatible< PointT > > >::type type
Defines all the PCL implemented PointT point type structures.
GetPoint(PointT &point, std::size_t num)
void add(const PointT &t)
pcl::traits::has_intensity< boost::mpl::_1 > IsCompatible
pcl::traits::has_normal< boost::mpl::_1 > IsCompatible
A point structure representing Euclidean xyz coordinates, and the RGB color.
pcl::traits::has_curvature< boost::mpl::_1 > IsCompatible
Defines all the PCL and non-PCL macros used.
pcl::traits::has_xyz< boost::mpl::_1 > IsCompatible