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