Point Cloud Library (PCL)  1.14.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/memory.h>
50 #include <pcl/pcl_macros.h>
51 #include <pcl/point_types.h>
52 
53 namespace pcl
54 {
55 
56  namespace detail
57  {
58 
59  /* Below are several helper accumulator structures that are used by the
60  * `CentroidPoint` class. Each of them is capable of accumulating
61  * information from a particular field(s) of a point. The points are
62  * inserted via `add()` and extracted via `get()` functions. Note that the
63  * accumulators are not templated on point type, so in principle it is
64  * possible to insert and extract points of different types. It is the
65  * responsibility of the user to make sure that points have corresponding
66  * fields. */
67 
69  {
70 
71  // Requires that point type has x, y, and z fields
72  using IsCompatible = pcl::traits::has_xyz<boost::mpl::_1>;
73 
74  // Storage
75  Eigen::Vector3f xyz = Eigen::Vector3f::Zero ();
76 
77  template <typename PointT> void
78  add (const PointT& t) { xyz += t.getVector3fMap (); }
79 
80  template <typename PointT> void
81  get (PointT& t, std::size_t n) const { t.getVector3fMap () = xyz / n; }
82 
84 
85  };
86 
88  {
89 
90  // Requires that point type has normal_x, normal_y, and normal_z fields
91  using IsCompatible = pcl::traits::has_normal<boost::mpl::_1>;
92 
93  // Storage
94  Eigen::Vector4f normal = Eigen::Vector4f::Zero ();
95 
96  // Requires that the normal of the given point is normalized, otherwise it
97  // does not make sense to sum it up with the accumulated value.
98  template <typename PointT> void
99  add (const PointT& t) { normal += t.getNormalVector4fMap (); }
100 
101  template <typename PointT> void
102  get (PointT& t, std::size_t) const
103  {
104 #if EIGEN_VERSION_AT_LEAST (3, 3, 0)
105  t.getNormalVector4fMap () = normal.normalized ();
106 #else
107  if (normal.squaredNorm() > 0)
108  t.getNormalVector4fMap () = normal.normalized ();
109  else
110  t.getNormalVector4fMap () = Eigen::Vector4f::Zero ();
111 #endif
112  }
113 
115 
116  };
117 
119  {
120 
121  // Requires that point type has curvature field
122  using IsCompatible = pcl::traits::has_curvature<boost::mpl::_1>;
123 
124  // Storage
125  float curvature = 0;
126 
127  template <typename PointT> void
128  add (const PointT& t) { curvature += t.curvature; }
129 
130  template <typename PointT> void
131  get (PointT& t, std::size_t n) const { t.curvature = curvature / n; }
132 
133  };
134 
136  {
137 
138  // Requires that point type has rgb or rgba field
139  using IsCompatible = pcl::traits::has_color<boost::mpl::_1>;
140 
141  // Storage
142  float r = 0, g = 0, b = 0, a = 0;
143 
144  template <typename PointT> void
145  add (const PointT& t)
146  {
147  r += static_cast<float> (t.r);
148  g += static_cast<float> (t.g);
149  b += static_cast<float> (t.b);
150  a += static_cast<float> (t.a);
151  }
152 
153  template <typename PointT> void
154  get (PointT& t, std::size_t n) const
155  {
156  t.rgba = static_cast<std::uint32_t> (a / n) << 24 |
157  static_cast<std::uint32_t> (r / n) << 16 |
158  static_cast<std::uint32_t> (g / n) << 8 |
159  static_cast<std::uint32_t> (b / n);
160  }
161 
162  };
163 
165  {
166 
167  // Requires that point type has intensity field
168  using IsCompatible = pcl::traits::has_intensity<boost::mpl::_1>;
169 
170  // Storage
171  float intensity = 0;
172 
173  template <typename PointT> void
174  add (const PointT& t) { intensity += t.intensity; }
175 
176  template <typename PointT> void
177  get (PointT& t, std::size_t n) const { t.intensity = intensity / n; }
178 
179  };
180 
182  {
183 
184  // Requires that point type has label field
185  using IsCompatible = pcl::traits::has_label<boost::mpl::_1>;
186 
187  // Storage
188  // A better performance may be achieved with a heap structure
189  std::map<std::uint32_t, std::size_t> labels;
190 
191  template <typename PointT> void
192  add (const PointT& t)
193  {
194  auto itr = labels.find (t.label);
195  if (itr == labels.end ())
196  labels.insert (std::make_pair (t.label, 1));
197  else
198  ++itr->second;
199  }
200 
201  template <typename PointT> void
202  get (PointT& t, std::size_t) const
203  {
204  std::size_t max = 0;
205  for (const auto &label : labels)
206  if (label.second > max)
207  {
208  max = label.second;
209  t.label = label.first;
210  }
211  }
212 
213  };
214 
215  /* Meta-function that checks if an accumulator is compatible with given
216  * point type(s). */
217  template <typename Point1T, typename Point2T = Point1T>
219 
220  template <typename AccumulatorT>
221  struct apply : boost::mpl::and_<
222  boost::mpl::apply<typename AccumulatorT::IsCompatible, Point1T>,
223  boost::mpl::apply<typename AccumulatorT::IsCompatible, Point2T>
224  > {};
225  };
226 
227  /* Meta-function that creates a Fusion vector of accumulator types that are
228  * compatible with a given point type. */
229  template <typename PointT>
231  {
232  using type =
233  typename boost::fusion::result_of::as_vector<
234  typename boost::mpl::filter_view<
235  boost::mpl::vector<
242  >
244  >
245  >::type;
246  };
247 
248  /* Fusion function object to invoke point addition on every accumulator in
249  * a fusion sequence. */
250  template <typename PointT>
251  struct AddPoint
252  {
253 
254  const PointT& p;
255 
256  AddPoint (const PointT& point) : p (point) { }
257 
258  template <typename AccumulatorT> void
259  operator () (AccumulatorT& accumulator) const
260  {
261  accumulator.add (p);
262  }
263 
264  };
265 
266  /* Fusion function object to invoke get point on every accumulator in a
267  * fusion sequence. */
268  template <typename PointT>
269  struct GetPoint
270  {
271 
273  std::size_t n;
274 
275  GetPoint (PointT& point, std::size_t num) : p (point), n (num) { }
276 
277  template <typename AccumulatorT> void
278  operator () (AccumulatorT& accumulator) const
279  {
280  accumulator.get (p, n);
281  }
282 
283  };
284 
285  }
286 
287 }
288 
289 #endif /* PCL_COMMON_IMPL_DETAIL_ACCUMULATORS_HPP */
290 
Defines all the PCL implemented PointT point type structures.
#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.
Defines all the PCL and non-PCL macros used.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void get(PointT &t, std::size_t n) const
pcl::traits::has_curvature< boost::mpl::_1 > IsCompatible
pcl::traits::has_intensity< boost::mpl::_1 > IsCompatible
void get(PointT &t, std::size_t n) const
void get(PointT &t, std::size_t) const
pcl::traits::has_label< boost::mpl::_1 > IsCompatible
void add(const PointT &t)
std::map< std::uint32_t, std::size_t > labels
pcl::traits::has_normal< boost::mpl::_1 > IsCompatible
void get(PointT &t, std::size_t) const
void add(const PointT &t)
pcl::traits::has_color< boost::mpl::_1 > IsCompatible
void get(PointT &t, std::size_t n) const
void add(const PointT &t)
void get(PointT &t, std::size_t n) const
void add(const PointT &t)
pcl::traits::has_xyz< 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
void operator()(AccumulatorT &accumulator) const
AddPoint(const PointT &point)
GetPoint(PointT &point, std::size_t num)
void operator()(AccumulatorT &accumulator) const