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;
75 
76  AccumulatorXYZ () : xyz (Eigen::Vector3f::Zero ()) { }
77 
78  template <typename PointT> void
79  add (const PointT& t) { xyz += t.getVector3fMap (); }
80 
81  template <typename PointT> void
82  get (PointT& t, size_t n) const { t.getVector3fMap () = xyz / n; }
83 
85 
86  };
87 
89  {
90 
91  // Requires that point type has normal_x, normal_y, and normal_z fields
92  using IsCompatible = pcl::traits::has_normal<boost::mpl::_1>;
93 
94  // Storage
95  Eigen::Vector4f normal;
96 
97  AccumulatorNormal () : normal (Eigen::Vector4f::Zero ()) { }
98 
99  // Requires that the normal of the given point is normalized, otherwise it
100  // does not make sense to sum it up with the accumulated value.
101  template <typename PointT> void
102  add (const PointT& t) { normal += t.getNormalVector4fMap (); }
103 
104  template <typename PointT> void
105  get (PointT& t, size_t) const
106  {
107 #if EIGEN_VERSION_AT_LEAST (3, 3, 0)
108  t.getNormalVector4fMap () = normal.normalized ();
109 #else
110  if (normal.squaredNorm() > 0)
111  t.getNormalVector4fMap () = normal.normalized ();
112  else
113  t.getNormalVector4fMap () = Eigen::Vector4f::Zero ();
114 #endif
115  }
116 
118 
119  };
120 
122  {
123 
124  // Requires that point type has curvature field
125  using IsCompatible = pcl::traits::has_curvature<boost::mpl::_1>;
126 
127  // Storage
128  float curvature;
129 
130  AccumulatorCurvature () : curvature (0) { }
131 
132  template <typename PointT> void
133  add (const PointT& t) { curvature += t.curvature; }
134 
135  template <typename PointT> void
136  get (PointT& t, size_t n) const { t.curvature = curvature / n; }
137 
138  };
139 
141  {
142 
143  // Requires that point type has rgb or rgba field
144  using IsCompatible = pcl::traits::has_color<boost::mpl::_1>;
145 
146  // Storage
147  float r, g, b, a;
148 
149  AccumulatorRGBA () : r (0), g (0), b (0), a (0) { }
150 
151  template <typename PointT> void
152  add (const PointT& t)
153  {
154  r += static_cast<float> (t.r);
155  g += static_cast<float> (t.g);
156  b += static_cast<float> (t.b);
157  a += static_cast<float> (t.a);
158  }
159 
160  template <typename PointT> void
161  get (PointT& t, size_t n) const
162  {
163  t.rgba = static_cast<uint32_t> (a / n) << 24 |
164  static_cast<uint32_t> (r / n) << 16 |
165  static_cast<uint32_t> (g / n) << 8 |
166  static_cast<uint32_t> (b / n);
167  }
168 
169  };
170 
172  {
173 
174  // Requires that point type has intensity field
175  using IsCompatible = pcl::traits::has_intensity<boost::mpl::_1>;
176 
177  // Storage
178  float intensity;
179 
180  AccumulatorIntensity () : intensity (0) { }
181 
182  template <typename PointT> void
183  add (const PointT& t) { intensity += t.intensity; }
184 
185  template <typename PointT> void
186  get (PointT& t, size_t n) const { t.intensity = intensity / n; }
187 
188  };
189 
191  {
192 
193  // Requires that point type has label field
194  using IsCompatible = pcl::traits::has_label<boost::mpl::_1>;
195 
196  // Storage
197  // A better performance may be achieved with a heap structure
198  std::map<uint32_t, size_t> labels;
199 
201 
202  template <typename PointT> void
203  add (const PointT& t)
204  {
205  auto itr = labels.find (t.label);
206  if (itr == labels.end ())
207  labels.insert (std::make_pair (t.label, 1));
208  else
209  ++itr->second;
210  }
211 
212  template <typename PointT> void
213  get (PointT& t, size_t) const
214  {
215  size_t max = 0;
216  for (const auto &label : labels)
217  if (label.second > max)
218  {
219  max = label.second;
220  t.label = label.first;
221  }
222  }
223 
224  };
225 
226  /* Meta-function that checks if an accumulator is compatible with given
227  * point type(s). */
228  template <typename Point1T, typename Point2T = Point1T>
230 
231  template <typename AccumulatorT>
232  struct apply : boost::mpl::and_<
233  boost::mpl::apply<typename AccumulatorT::IsCompatible, Point1T>,
234  boost::mpl::apply<typename AccumulatorT::IsCompatible, Point2T>
235  > {};
236  };
237 
238  /* Meta-function that creates a Fusion vector of accumulator types that are
239  * compatible with a given point type. */
240  template <typename PointT>
242  {
243  using type =
244  typename boost::fusion::result_of::as_vector<
245  typename boost::mpl::filter_view<
246  boost::mpl::vector<
253  >
255  >
257  };
258 
259  /* Fusion function object to invoke point addition on every accumulator in
260  * a fusion sequence. */
261  template <typename PointT>
262  struct AddPoint
263  {
264 
265  const PointT& p;
266 
267  AddPoint (const PointT& point) : p (point) { }
268 
269  template <typename AccumulatorT> void
270  operator () (AccumulatorT& accumulator) const
271  {
272  accumulator.add (p);
273  }
274 
275  };
276 
277  /* Fusion function object to invoke get point on every accumulator in a
278  * fusion sequence. */
279  template <typename PointT>
280  struct GetPoint
281  {
282 
284  size_t n;
285 
286  GetPoint (PointT& point, size_t num) : p (point), n (num) { }
287 
288  template <typename AccumulatorT> void
289  operator () (AccumulatorT& accumulator) const
290  {
291  accumulator.get (p, n);
292  }
293 
294  };
295 
296  }
297 
298 }
299 
300 #endif /* PCL_COMMON_IMPL_DETAIL_ACCUMULATORS_HPP */
301 
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)
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:344
Definition: bfgs.h:9
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
void add(const PointT &t)
pcl::traits::has_intensity< boost::mpl::_1 > IsCompatible
std::map< uint32_t, size_t > labels
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
GetPoint(PointT &point, size_t num)
pcl::traits::has_xyz< boost::mpl::_1 > IsCompatible