Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
53namespace 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