Point Cloud Library (PCL)  1.11.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
register_point_struct.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #pragma once
42 
43 #ifdef __GNUC__
44 #pragma GCC system_header
45 #endif
46 
47 #if defined _MSC_VER
48  #pragma warning (push, 2)
49  // 4244 : conversion from 'type1' to 'type2', possible loss of data
50  #pragma warning (disable: 4244)
51 #endif
52 
53 //https://bugreports.qt-project.org/browse/QTBUG-22829
54 #ifndef Q_MOC_RUN
55 #include <pcl/pcl_macros.h>
56 #include <pcl/type_traits.h>
57 #include <boost/mpl/vector.hpp>
58 #include <boost/preprocessor/seq/enum.hpp>
59 #include <boost/preprocessor/seq/for_each.hpp>
60 #include <boost/preprocessor/seq/transform.hpp>
61 #include <boost/preprocessor/cat.hpp>
62 #include <boost/preprocessor/comparison.hpp>
63 #endif
64 
65 #include <cstddef> //offsetof
66 #include <type_traits>
67 
68 // Must be used in global namespace with name fully qualified
69 #define POINT_CLOUD_REGISTER_POINT_STRUCT(name, fseq) \
70  POINT_CLOUD_REGISTER_POINT_STRUCT_I(name, \
71  BOOST_PP_CAT(POINT_CLOUD_REGISTER_POINT_STRUCT_X fseq, 0))
72  /***/
73 
74 #define POINT_CLOUD_REGISTER_POINT_WRAPPER(wrapper, pod) \
75  BOOST_MPL_ASSERT_MSG(sizeof(wrapper) == sizeof(pod), POINT_WRAPPER_AND_POD_TYPES_HAVE_DIFFERENT_SIZES, (wrapper&, pod&)); \
76  namespace pcl { \
77  namespace traits { \
78  template<> struct POD<wrapper> { using type = pod; }; \
79  } \
80  }
81  /***/
82 
83 // These macros help transform the unusual data structure (type, name, tag)(type, name, tag)...
84 // into a proper preprocessor sequence of 3-tuples ((type, name, tag))((type, name, tag))...
85 #define POINT_CLOUD_REGISTER_POINT_STRUCT_X(type, name, tag) \
86  ((type, name, tag)) POINT_CLOUD_REGISTER_POINT_STRUCT_Y
87 #define POINT_CLOUD_REGISTER_POINT_STRUCT_Y(type, name, tag) \
88  ((type, name, tag)) POINT_CLOUD_REGISTER_POINT_STRUCT_X
89 #define POINT_CLOUD_REGISTER_POINT_STRUCT_X0
90 #define POINT_CLOUD_REGISTER_POINT_STRUCT_Y0
91 
92 namespace pcl
93 {
94  namespace traits
95  {
96  template<typename T> inline
97  std::enable_if_t<!std::is_array<T>::value>
98  plus (T &l, const T &r)
99  {
100  l += r;
101  }
102 
103  template<typename T> inline
104  std::enable_if_t<std::is_array<T>::value>
105  plus (std::remove_const_t<T> &l, const T &r)
106  {
107  using type = std::remove_all_extents_t<T>;
108  static const std::uint32_t count = sizeof (T) / sizeof (type);
109  for (std::uint32_t i = 0; i < count; ++i)
110  l[i] += r[i];
111  }
112 
113  template<typename T1, typename T2> inline
114  std::enable_if_t<!std::is_array<T1>::value>
115  plusscalar (T1 &p, const T2 &scalar)
116  {
117  p += scalar;
118  }
119 
120  template<typename T1, typename T2> inline
121  std::enable_if_t<std::is_array<T1>::value>
122  plusscalar (T1 &p, const T2 &scalar)
123  {
124  using type = std::remove_all_extents_t<T1>;
125  static const std::uint32_t count = sizeof (T1) / sizeof (type);
126  for (std::uint32_t i = 0; i < count; ++i)
127  p[i] += scalar;
128  }
129 
130  template<typename T> inline
131  std::enable_if_t<!std::is_array<T>::value>
132  minus (T &l, const T &r)
133  {
134  l -= r;
135  }
136 
137  template<typename T> inline
138  std::enable_if_t<std::is_array<T>::value>
139  minus (std::remove_const_t<T> &l, const T &r)
140  {
141  using type = std::remove_all_extents_t<T>;
142  static const std::uint32_t count = sizeof (T) / sizeof (type);
143  for (std::uint32_t i = 0; i < count; ++i)
144  l[i] -= r[i];
145  }
146 
147  template<typename T1, typename T2> inline
148  std::enable_if_t<!std::is_array<T1>::value>
149  minusscalar (T1 &p, const T2 &scalar)
150  {
151  p -= scalar;
152  }
153 
154  template<typename T1, typename T2> inline
155  std::enable_if_t<std::is_array<T1>::value>
156  minusscalar (T1 &p, const T2 &scalar)
157  {
158  using type = std::remove_all_extents_t<T1>;
159  static const std::uint32_t count = sizeof (T1) / sizeof (type);
160  for (std::uint32_t i = 0; i < count; ++i)
161  p[i] -= scalar;
162  }
163 
164  template<typename T1, typename T2> inline
165  std::enable_if_t<!std::is_array<T1>::value>
166  mulscalar (T1 &p, const T2 &scalar)
167  {
168  p *= scalar;
169  }
170 
171  template<typename T1, typename T2> inline
172  std::enable_if_t<std::is_array<T1>::value>
173  mulscalar (T1 &p, const T2 &scalar)
174  {
175  using type = std::remove_all_extents_t<T1>;
176  static const std::uint32_t count = sizeof (T1) / sizeof (type);
177  for (std::uint32_t i = 0; i < count; ++i)
178  p[i] *= scalar;
179  }
180 
181  template<typename T1, typename T2> inline
182  std::enable_if_t<!std::is_array<T1>::value>
183  divscalar (T1 &p, const T2 &scalar)
184  {
185  p /= scalar;
186  }
187 
188  template<typename T1, typename T2> inline
189  std::enable_if_t<std::is_array<T1>::value>
190  divscalar (T1 &p, const T2 &scalar)
191  {
192  using type = std::remove_all_extents_t<T1>;
193  static const std::uint32_t count = sizeof (T1) / sizeof (type);
194  for (std::uint32_t i = 0; i < count; ++i)
195  p[i] /= scalar;
196  }
197  }
198 }
199 
200 // Point operators
201 #define PCL_PLUSEQ_POINT_TAG(r, data, elem) \
202  pcl::traits::plus (lhs.BOOST_PP_TUPLE_ELEM(3, 1, elem), \
203  rhs.BOOST_PP_TUPLE_ELEM(3, 1, elem));
204  /***/
205 
206 #define PCL_PLUSEQSC_POINT_TAG(r, data, elem) \
207  pcl::traits::plusscalar (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \
208  scalar);
209  /***/
210  //p.BOOST_PP_TUPLE_ELEM(3, 1, elem) += scalar;
211 
212 #define PCL_MINUSEQ_POINT_TAG(r, data, elem) \
213  pcl::traits::minus (lhs.BOOST_PP_TUPLE_ELEM(3, 1, elem), \
214  rhs.BOOST_PP_TUPLE_ELEM(3, 1, elem));
215  /***/
216 
217 #define PCL_MINUSEQSC_POINT_TAG(r, data, elem) \
218  pcl::traits::minusscalar (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \
219  scalar);
220  /***/
221  //p.BOOST_PP_TUPLE_ELEM(3, 1, elem) -= scalar;
222 
223 #define PCL_MULEQSC_POINT_TAG(r, data, elem) \
224  pcl::traits::mulscalar (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \
225  scalar);
226  /***/
227 
228 #define PCL_DIVEQSC_POINT_TAG(r, data, elem) \
229  pcl::traits::divscalar (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \
230  scalar);
231  /***/
232 
233 // Construct type traits given full sequence of (type, name, tag) triples
234 // BOOST_MPL_ASSERT_MSG(std::is_pod<name>::value,
235 // REGISTERED_POINT_TYPE_MUST_BE_PLAIN_OLD_DATA, (name));
236 #define POINT_CLOUD_REGISTER_POINT_STRUCT_I(name, seq) \
237  namespace pcl \
238  { \
239  namespace fields \
240  { \
241  BOOST_PP_SEQ_FOR_EACH(POINT_CLOUD_REGISTER_FIELD_TAG, name, seq) \
242  } \
243  namespace traits \
244  { \
245  BOOST_PP_SEQ_FOR_EACH(POINT_CLOUD_REGISTER_FIELD_NAME, name, seq) \
246  BOOST_PP_SEQ_FOR_EACH(POINT_CLOUD_REGISTER_FIELD_OFFSET, name, seq) \
247  BOOST_PP_SEQ_FOR_EACH(POINT_CLOUD_REGISTER_FIELD_DATATYPE, name, seq) \
248  POINT_CLOUD_REGISTER_POINT_FIELD_LIST(name, POINT_CLOUD_EXTRACT_TAGS(seq)) \
249  } \
250  namespace common \
251  { \
252  inline const name& \
253  operator+= (name& lhs, const name& rhs) \
254  { \
255  BOOST_PP_SEQ_FOR_EACH(PCL_PLUSEQ_POINT_TAG, _, seq) \
256  return (lhs); \
257  } \
258  inline const name& \
259  operator+= (name& p, const float& scalar) \
260  { \
261  BOOST_PP_SEQ_FOR_EACH(PCL_PLUSEQSC_POINT_TAG, _, seq) \
262  return (p); \
263  } \
264  inline const name operator+ (const name& lhs, const name& rhs) \
265  { name result = lhs; result += rhs; return (result); } \
266  inline const name operator+ (const float& scalar, const name& p) \
267  { name result = p; result += scalar; return (result); } \
268  inline const name operator+ (const name& p, const float& scalar) \
269  { name result = p; result += scalar; return (result); } \
270  inline const name& \
271  operator-= (name& lhs, const name& rhs) \
272  { \
273  BOOST_PP_SEQ_FOR_EACH(PCL_MINUSEQ_POINT_TAG, _, seq) \
274  return (lhs); \
275  } \
276  inline const name& \
277  operator-= (name& p, const float& scalar) \
278  { \
279  BOOST_PP_SEQ_FOR_EACH(PCL_MINUSEQSC_POINT_TAG, _, seq) \
280  return (p); \
281  } \
282  inline const name operator- (const name& lhs, const name& rhs) \
283  { name result = lhs; result -= rhs; return (result); } \
284  inline const name operator- (const float& scalar, const name& p) \
285  { name result = p; result -= scalar; return (result); } \
286  inline const name operator- (const name& p, const float& scalar) \
287  { name result = p; result -= scalar; return (result); } \
288  inline const name& \
289  operator*= (name& p, const float& scalar) \
290  { \
291  BOOST_PP_SEQ_FOR_EACH(PCL_MULEQSC_POINT_TAG, _, seq) \
292  return (p); \
293  } \
294  inline const name operator* (const float& scalar, const name& p) \
295  { name result = p; result *= scalar; return (result); } \
296  inline const name operator* (const name& p, const float& scalar) \
297  { name result = p; result *= scalar; return (result); } \
298  inline const name& \
299  operator/= (name& p, const float& scalar) \
300  { \
301  BOOST_PP_SEQ_FOR_EACH(PCL_DIVEQSC_POINT_TAG, _, seq) \
302  return (p); \
303  } \
304  inline const name operator/ (const float& scalar, const name& p) \
305  { name result = p; result /= scalar; return (result); } \
306  inline const name operator/ (const name& p, const float& scalar) \
307  { name result = p; result /= scalar; return (result); } \
308  } \
309  }
310  /***/
311 
312 #define POINT_CLOUD_REGISTER_FIELD_TAG(r, name, elem) \
313  struct BOOST_PP_TUPLE_ELEM(3, 2, elem); \
314  /***/
315 
316 #define POINT_CLOUD_REGISTER_FIELD_NAME(r, point, elem) \
317  template<int dummy> \
318  struct name<point, pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem), dummy> \
319  { \
320  static const char value[]; \
321  }; \
322  \
323  template<int dummy> \
324  const char name<point, \
325  pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem), \
326  dummy>::value[] = \
327  BOOST_PP_STRINGIZE(BOOST_PP_TUPLE_ELEM(3, 2, elem));
328  /***/
329 
330 #define POINT_CLOUD_REGISTER_FIELD_OFFSET(r, name, elem) \
331  template<> struct offset<name, pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem)> \
332  { \
333  static const std::size_t value = offsetof(name, BOOST_PP_TUPLE_ELEM(3, 1, elem)); \
334  };
335  /***/
336 
337 #define POINT_CLOUD_REGISTER_FIELD_DATATYPE(r, name, elem) \
338  template<> struct datatype<name, pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem)> \
339  { \
340  using type = boost::mpl::identity<BOOST_PP_TUPLE_ELEM(3, 0, elem)>::type; \
341  using decomposed = decomposeArray<type>; \
342  static const std::uint8_t value = asEnum<decomposed::type>::value; \
343  static const std::uint32_t size = decomposed::value; \
344  };
345  /***/
346 
347 #define POINT_CLOUD_TAG_OP(s, data, elem) pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem)
348 
349 #define POINT_CLOUD_EXTRACT_TAGS(seq) BOOST_PP_SEQ_TRANSFORM(POINT_CLOUD_TAG_OP, _, seq)
350 
351 #define POINT_CLOUD_REGISTER_POINT_FIELD_LIST(name, seq) \
352  template<> struct fieldList<name> \
353  { \
354  using type = boost::mpl::vector<BOOST_PP_SEQ_ENUM(seq)>; \
355  };
356  /***/
357 
358 #if defined _MSC_VER
359  #pragma warning (pop)
360 #endif
std::enable_if_t<!std::is_array< T >::value > plus(T &l, const T &r)
std::enable_if_t<!std::is_array< T1 >::value > mulscalar(T1 &p, const T2 &scalar)
std::enable_if_t<!std::is_array< T1 >::value > plusscalar(T1 &p, const T2 &scalar)
std::enable_if_t<!std::is_array< T >::value > minus(T &l, const T &r)
std::enable_if_t<!std::is_array< T1 >::value > minusscalar(T1 &p, const T2 &scalar)
std::enable_if_t<!std::is_array< T1 >::value > divscalar(T1 &p, const T2 &scalar)