Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
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 #if EIGEN_VERSION_AT_LEAST (3, 3, 0)
106  t.getNormalVector4fMap () = normal.normalized ();
107 #else
108  if (normal.squaredNorm() > 0)
109  t.getNormalVector4fMap () = normal.normalized ();
110  else
111  t.getNormalVector4fMap () = Eigen::Vector4f::Zero ();
112 #endif
113  }
114 
115  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
116 
117  };
118 
120  {
121 
122  // Requires that point type has curvature field
123  typedef pcl::traits::has_curvature<boost::mpl::_1> IsCompatible;
124 
125  // Storage
126  float curvature;
127 
128  AccumulatorCurvature () : curvature (0) { }
129 
130  template <typename PointT> void
131  add (const PointT& t) { curvature += t.curvature; }
132 
133  template <typename PointT> void
134  get (PointT& t, size_t n) const { t.curvature = curvature / n; }
135 
136  };
137 
139  {
140 
141  // Requires that point type has rgb or rgba field
142  typedef pcl::traits::has_color<boost::mpl::_1> IsCompatible;
143 
144  // Storage
145  float r, g, b, a;
146 
147  AccumulatorRGBA () : r (0), g (0), b (0), a (0) { }
148 
149  template <typename PointT> void
150  add (const PointT& t)
151  {
152  r += static_cast<float> (t.r);
153  g += static_cast<float> (t.g);
154  b += static_cast<float> (t.b);
155  a += static_cast<float> (t.a);
156  }
157 
158  template <typename PointT> void
159  get (PointT& t, size_t n) const
160  {
161  t.rgba = static_cast<uint32_t> (a / n) << 24 |
162  static_cast<uint32_t> (r / n) << 16 |
163  static_cast<uint32_t> (g / n) << 8 |
164  static_cast<uint32_t> (b / n);
165  }
166 
167  };
168 
170  {
171 
172  // Requires that point type has intensity field
173  typedef pcl::traits::has_intensity<boost::mpl::_1> IsCompatible;
174 
175  // Storage
176  float intensity;
177 
178  AccumulatorIntensity () : intensity (0) { }
179 
180  template <typename PointT> void
181  add (const PointT& t) { intensity += t.intensity; }
182 
183  template <typename PointT> void
184  get (PointT& t, size_t n) const { t.intensity = intensity / n; }
185 
186  };
187 
189  {
190 
191  // Requires that point type has label field
192  typedef pcl::traits::has_label<boost::mpl::_1> IsCompatible;
193 
194  // Storage
195  // A better performance may be achieved with a heap structure
196  std::map<uint32_t, size_t> labels;
197 
199 
200  template <typename PointT> void
201  add (const PointT& t)
202  {
203  std::map<uint32_t, size_t>::iterator itr = labels.find (t.label);
204  if (itr == labels.end ())
205  labels.insert (std::make_pair (t.label, 1));
206  else
207  ++itr->second;
208  }
209 
210  template <typename PointT> void
211  get (PointT& t, size_t) const
212  {
213  size_t max = 0;
214  std::map<uint32_t, size_t>::const_iterator itr;
215  for (itr = labels.begin (); itr != labels.end (); ++itr)
216  if (itr->second > max)
217  {
218  max = itr->second;
219  t.label = itr->first;
220  }
221  }
222 
223  };
224 
225  /* This is a meta-function that may be used to create a Fusion vector of
226  * those accumulator types that are compatible with given point type(s). */
227 
228  template <typename Point1T, typename Point2T = Point1T>
230  {
231 
232  // Check if a given accumulator type is compatible with a given point type
233  template <typename AccumulatorT, typename PointT>
234  struct IsCompatible : boost::mpl::apply<typename AccumulatorT::IsCompatible, PointT> { };
235 
236  // A Fusion vector with accumulator types that are compatible with given
237  // point types
238  typedef
239  typename boost::fusion::result_of::as_vector<
240  typename boost::mpl::filter_view<
241  boost::mpl::vector<
248  >
249  , boost::mpl::and_<
252  >
253  >
254  >::type
256  };
257 
258  /* Fusion function object to invoke point addition on every accumulator in
259  * a fusion sequence. */
260 
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 
280  template <typename PointT>
281  struct GetPoint
282  {
283 
285  size_t n;
286 
287  GetPoint (PointT& point, size_t num) : p (point), n (num) { }
288 
289  template <typename AccumulatorT> void
290  operator () (AccumulatorT& accumulator) const
291  {
292  accumulator.get (p, n);
293  }
294 
295  };
296 
297  }
298 
299 }
300 
301 #endif /* PCL_COMMON_IMPL_DETAIL_ACCUMULATORS_HPP */
302 
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
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
void operator()(AccumulatorT &accumulator) const
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 operator()(AccumulatorT &accumulator) const
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)