Point Cloud Library (PCL)  1.14.1-dev
point_cloud.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id$
35  *
36  */
37 
38 #pragma once
39 
40 #include <pcl/cuda/point_types.h>
41 #include <pcl/cuda/thrust.h>
42 #include <pcl/memory.h>
43 
44 namespace pcl
45 {
46  namespace cuda
47  {
48  /** \brief misnamed class holding a 3x3 matrix */
50  {
51  float3 data[3];
52  };
53 
54  /** \brief Simple structure holding RGB data. */
55  struct OpenNIRGB
56  {
57  unsigned char r, g, b;
58  };
59 
60  /** \brief Host helper class. Contains several typedefs and some static
61  * functions to help writing portable code (that runs both on host
62  * and device) */
63  template <typename T>
64  struct Host
65  {
66  // vector type
67  using type = thrust::host_vector<T>;
68 
69 // // iterator type
70 // using type = thrust::detail::normal_iterator<T*>;
71 //
72 // // pointer type
73 // using pointer_type = T*;
74 //
75 // // allocator
76 // static T* alloc (int size)
77 // {
78 // return (T*) malloc (size);
79 // }
80 //
81 // // cast to different pointer
82 // template <typename U>
83 // static U* cast (type ptr)
84 // {
85 // return (U*)ptr;
86 // }
87  };
88 
89  /** \brief Device helper class. Contains several typedefs and some static
90  * functions to help writing portable code (that runs both on host
91  * and device) */
92  template <typename T>
93  struct Device
94  {
95  // vector type
96  using type = thrust::device_vector<T>;
97 
98 // // iterator type
99 // using iterator_type = thrust::detail::normal_iterator<thrust::device_ptr<T> >;
100 //
101 // // pointer type
102 // using pointer_type = thrust::device_ptr<T>;
103 //
104 // // allocator
105 // static thrust::device_ptr<T> alloc (int size)
106 // {
107 // return thrust::device_malloc<T> (size);
108 // }
109 //
110 // // cast to different pointer
111 // template <typename U>
112 // static thrust::device_ptr<U> cast (type ptr)
113 // {
114 // return thrust::device_ptr<U> ((U*)ptr.get());
115 // }
116 //
117 // // cast raw pointer to different pointer
118 // template <typename U>
119 // static thrust::device_ptr<U> cast (T* ptr)
120 // {
121 // return thrust::device_ptr<U> ((U*)ptr);
122 // }
123  };
124 
125  /** @b PointCloudAOS represents an AOS (Array of Structs) PointCloud
126  * implementation for CUDA processing.
127  *
128  * This is the most efficient way to perform operations on x86 architectures
129  * (using SSE alignment).
130  */
131  template <template <typename> class Storage>
133  {
134  public:
135  PointCloudAOS () : width (0), height (0), is_dense (true)
136  {}
137 
138  //////////////////////////////////////////////////////////////////////////////////////
140  {
141  points = rhs.points;
142  width = rhs.width;
143  height = rhs.height;
144  is_dense = rhs.is_dense;
145  return (*this);
146  }
147 
148  //////////////////////////////////////////////////////////////////////////////////////
149  template <typename OtherStorage>
150  inline PointCloudAOS& operator << (const OtherStorage& rhs)
151  {
152  points = rhs.points;
153  // TODO: Test speed on operator () = vs resize+copy
154  //points.resize (rhs.size ());
155  //thrust::copy (rhs.begin (), rhs.end (), points.begin ());
156  width = rhs.width;
157  height = rhs.height;
158  is_dense = rhs.is_dense;
159  return (*this);
160  }
161 
162  //////////////////////////////////////////////////////////////////////////////////////
163  inline PointXYZRGB
164  at (int u, int v) const
165  {
166  if (this->height > 1)
167  return (points[v * this->width + u]);
168  return (PointXYZRGB (std::numeric_limits<float>::quiet_NaN (),
169  std::numeric_limits<float>::quiet_NaN (),
170  std::numeric_limits<float>::quiet_NaN (),
171  0));
172  // throw IsNotDenseException ("Can't use 2D indexing with a sparse point cloud");
173  }
174 
175  //////////////////////////////////////////////////////////////////////////////////////
176  inline PointXYZRGB& operator () (int u, int v)
177  {
178  return (points[v* this->width +u]);
179  }
180  inline const PointXYZRGB& operator () (int u, int v) const
181  {
182  return (points[v* this->width +u]);
183  }
184 
185  /** \brief The point data. */
186  //typename Storage<float3>::type points;
187  typename Storage<PointXYZRGB>::type points;
188 
189  using iterator = typename Storage<PointXYZRGB>::type::iterator;
190 
191  /** \brief The point cloud width (if organized as an image-structure). */
192  unsigned int width;
193  /** \brief The point cloud height (if organized as an image-structure). */
194  unsigned int height;
195 
196  /** \brief True if no points are invalid (e.g., have NaN or Inf values). */
197  bool is_dense;
198 
199  using Ptr = shared_ptr<PointCloudAOS<Storage> >;
200  using ConstPtr = shared_ptr<const PointCloudAOS<Storage> >;
201  };
202 
203  /** @b PointCloudSOA represents a SOA (Struct of Arrays) PointCloud
204  * implementation for CUDA processing.
205  */
206  template <template <typename> class Storage>
208  {
209  public:
210  PointCloudSOA () : width (0), height (0), is_dense (true)
211  {}
212 
213  //////////////////////////////////////////////////////////////////////////////////////
215  {
216  points_x = rhs.points_x;
217  points_y = rhs.points_y;
218  points_z = rhs.points_z;
219  width = rhs.width;
220  height = rhs.height;
221  is_dense = rhs.is_dense;
222  return (*this);
223  }
224 
225  //////////////////////////////////////////////////////////////////////////////////////
226  template <typename OtherStorage>
227  inline PointCloudSOA& operator << (const OtherStorage& rhs)
228  {
229  points_x = rhs.points_x;
230  points_y = rhs.points_y;
231  points_z = rhs.points_z;
232  width = rhs.width;
233  height = rhs.height;
234  is_dense = rhs.is_dense;
235  return (*this);
236  }
237 
238  /** \brief Resize the internal point data vectors.
239  * \param newsize the new size
240  */
241  void
242  resize (std::size_t newsize)
243  {
244  assert (sane ());
245  points_x.resize (newsize);
246  points_y.resize (newsize);
247  points_z.resize (newsize);
248  }
249 
250  /** \brief Return the size of the internal vectors */
251  std::size_t
252  size () const
253  {
254  assert (sane ());
255  return (points_x.size ());
256  }
257 
258  /** \brief Check if the internal point data vectors are valid. */
259  bool
260  sane () const
261  {
262  return (points_x.size () == points_y.size () &&
263  points_x.size () == points_z.size ());
264  }
265 
266  /** \brief The point data. */
267  typename Storage<float>::type points_x;
268  typename Storage<float>::type points_y;
269  typename Storage<float>::type points_z;
270  typename Storage<int>::type rgb;
271 
272  /** \brief The point cloud width (if organized as an image-structure). */
273  unsigned int width;
274  /** \brief The point cloud height (if organized as an image-structure). */
275  unsigned int height;
276 
277  /** \brief True if no points are invalid (e.g., have NaN or Inf values). */
278  bool is_dense;
279 
280  using Ptr = shared_ptr<PointCloudSOA<Storage> >;
281  using ConstPtr = shared_ptr<const PointCloudSOA<Storage> >;
282 
283  //////////////////////////////////////////////////////////////////////////////////////
284  // Extras. Testing ZIP iterators
285  using tuple_type = thrust::tuple<float, float, float>;
286  using float_iterator = typename Storage<float>::type::iterator;
287  using iterator_tuple = thrust::tuple<float_iterator, float_iterator, float_iterator>;
288  using zip_iterator = thrust::zip_iterator<iterator_tuple>;
289 
290  zip_iterator
292  {
293  return (thrust::make_zip_iterator (thrust::make_tuple (points_x.begin (),
294  points_y.begin (),
295  points_z.begin ())));
296  }
297 
300  {
301  return (thrust::make_zip_iterator (thrust::make_tuple (points_x.end (),
302  points_y.end (),
303  points_z.end ())));
304  }
305  };
306 
307  template <template <typename> class Storage, typename T>
309  {
310  using type = void;
311  };
312 
313  template <typename T>
315  {
316  using type = thrust::detail::normal_iterator<thrust::device_ptr<T> >;
317  };
318 
319  template <typename T>
320  struct PointIterator<Host,T>
321  {
322  using type = thrust::detail::normal_iterator<T *>;
323  };
324 
325  template <template <typename> class Storage, typename T>
327  {
328  // using type = void*;
329  };
330 
331  template <typename T>
333  {
334  using type = thrust::device_ptr<T>;
335  template <typename U>
336  static thrust::device_ptr<U> cast (type ptr)
337  {
338  return thrust::device_ptr<U> ((U*)ptr.get());
339  }
340  template <typename U>
341  static thrust::device_ptr<U> cast (T* ptr)
342  {
343  return thrust::device_ptr<U> ((U*)ptr);
344  }
345  };
346 
347  template <typename T>
349  {
350  using type = T *;
351  template <typename U>
352  static U* cast (type ptr)
353  {
354  return (U*)ptr;
355  }
356  };
357  template <template <typename> class Storage, typename T>
359  {
360  };
361 
362  template <typename T>
364  {
365  static thrust::device_ptr<T> alloc (int size)
366  {
367  return thrust::device_malloc<T> (size);
368  }
369  };
370 
371  template <typename T>
373  {
374  static T* alloc (int size)
375  {
376  return (T*) malloc (size);
377  }
378  };
379 
380 
381  } // namespace
382 } // namespace
PointCloudAOS represents an AOS (Array of Structs) PointCloud implementation for CUDA processing.
Definition: point_cloud.h:133
PointCloudAOS & operator<<(const OtherStorage &rhs)
Definition: point_cloud.h:150
unsigned int height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:194
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:197
unsigned int width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:192
shared_ptr< const PointCloudAOS< Storage > > ConstPtr
Definition: point_cloud.h:200
typename Storage< PointXYZRGB >::type::iterator iterator
Definition: point_cloud.h:189
shared_ptr< PointCloudAOS< Storage > > Ptr
Definition: point_cloud.h:199
PointCloudAOS & operator=(const PointCloudAOS &rhs)
Definition: point_cloud.h:139
PointXYZRGB & operator()(int u, int v)
Definition: point_cloud.h:176
PointXYZRGB at(int u, int v) const
Definition: point_cloud.h:164
Storage< PointXYZRGB >::type points
The point data.
Definition: point_cloud.h:187
PointCloudSOA represents a SOA (Struct of Arrays) PointCloud implementation for CUDA processing.
Definition: point_cloud.h:208
unsigned int height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:275
shared_ptr< PointCloudSOA< Storage > > Ptr
Definition: point_cloud.h:280
shared_ptr< const PointCloudSOA< Storage > > ConstPtr
Definition: point_cloud.h:281
zip_iterator zip_begin()
Definition: point_cloud.h:291
unsigned int width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:273
thrust::zip_iterator< iterator_tuple > zip_iterator
Definition: point_cloud.h:288
bool sane() const
Check if the internal point data vectors are valid.
Definition: point_cloud.h:260
typename Storage< float >::type::iterator float_iterator
Definition: point_cloud.h:286
void resize(std::size_t newsize)
Resize the internal point data vectors.
Definition: point_cloud.h:242
PointCloudSOA & operator=(const PointCloudSOA &rhs)
Definition: point_cloud.h:214
thrust::tuple< float, float, float > tuple_type
Definition: point_cloud.h:285
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:278
Storage< int >::type rgb
Definition: point_cloud.h:270
Storage< float >::type points_z
Definition: point_cloud.h:269
Storage< float >::type points_x
The point data.
Definition: point_cloud.h:267
zip_iterator zip_end()
Definition: point_cloud.h:299
thrust::tuple< float_iterator, float_iterator, float_iterator > iterator_tuple
Definition: point_cloud.h:287
Storage< float >::type points_y
Definition: point_cloud.h:268
PointCloudSOA & operator<<(const OtherStorage &rhs)
Definition: point_cloud.h:227
std::size_t size() const
Return the size of the internal vectors.
Definition: point_cloud.h:252
Defines functions, macros and traits for allocating and using memory.
misnamed class holding a 3x3 matrix
Definition: point_cloud.h:50
Device helper class.
Definition: point_cloud.h:94
thrust::device_vector< T > type
Definition: point_cloud.h:96
Host helper class.
Definition: point_cloud.h:65
thrust::host_vector< T > type
Definition: point_cloud.h:67
Simple structure holding RGB data.
Definition: point_cloud.h:56
unsigned char b
Definition: point_cloud.h:57
unsigned char r
Definition: point_cloud.h:57
unsigned char g
Definition: point_cloud.h:57
thrust::detail::normal_iterator< thrust::device_ptr< T > > type
Definition: point_cloud.h:316
thrust::detail::normal_iterator< T * > type
Definition: point_cloud.h:322
Default point xyz-rgb structure.
Definition: point_types.h:49
static thrust::device_ptr< T > alloc(int size)
Definition: point_cloud.h:365
static thrust::device_ptr< U > cast(type ptr)
Definition: point_cloud.h:336
static thrust::device_ptr< U > cast(T *ptr)
Definition: point_cloud.h:341