Point Cloud Library (PCL)  1.9.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 <boost/shared_ptr.hpp>
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  typedef thrust::host_vector<T> type;
68 
69 // // iterator type
70 // typedef thrust::detail::normal_iterator<T*> type;
71 //
72 // // pointer type
73 // typedef T* pointer_type;
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  typedef thrust::device_vector<T> type;
97 
98 // // iterator type
99 // typedef thrust::detail::normal_iterator<thrust::device_ptr<T> > iterator_type;
100 //
101 // // pointer type
102 // typedef thrust::device_ptr<T> pointer_type;
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  //////////////////////////////////////////////////////////////////////////////////////
139  inline PointCloudAOS& operator = (const PointCloudAOS& rhs)
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.points.size ());
155  //thrust::copy (rhs.points.begin (), rhs.points.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  else
169  return (PointXYZRGB (std::numeric_limits<float>::quiet_NaN (),
170  std::numeric_limits<float>::quiet_NaN (),
171  std::numeric_limits<float>::quiet_NaN (),
172  0));
173  // throw IsNotDenseException ("Can't use 2D indexing with a sparse point cloud");
174  }
175 
176  //////////////////////////////////////////////////////////////////////////////////////
177  inline PointXYZRGB& operator () (int u, int v)
178  {
179  return (points[v* this->width +u]);
180  }
181  inline const PointXYZRGB& operator () (int u, int v) const
182  {
183  return (points[v* this->width +u]);
184  }
185 
186  /** \brief The point data. */
187  //typename Storage<float3>::type points;
188  typename Storage<PointXYZRGB>::type points;
189 
190  typedef typename Storage<PointXYZRGB>::type::iterator iterator;
191 
192  /** \brief The point cloud width (if organized as an image-structure). */
193  unsigned int width;
194  /** \brief The point cloud height (if organized as an image-structure). */
195  unsigned int height;
196 
197  /** \brief True if no points are invalid (e.g., have NaN or Inf values). */
198  bool is_dense;
199 
200  typedef boost::shared_ptr<PointCloudAOS<Storage> > Ptr;
201  typedef boost::shared_ptr<const PointCloudAOS<Storage> > ConstPtr;
202  };
203 
204  /** @b PointCloudSOA represents a SOA (Struct of Arrays) PointCloud
205  * implementation for CUDA processing.
206  */
207  template <template <typename> class Storage>
209  {
210  public:
211  PointCloudSOA () : width (0), height (0), is_dense (true)
212  {}
213 
214  //////////////////////////////////////////////////////////////////////////////////////
215  inline PointCloudSOA& operator = (const PointCloudSOA& rhs)
216  {
217  points_x = rhs.points_x;
218  points_y = rhs.points_y;
219  points_z = rhs.points_z;
220  width = rhs.width;
221  height = rhs.height;
222  is_dense = rhs.is_dense;
223  return (*this);
224  }
225 
226  //////////////////////////////////////////////////////////////////////////////////////
227  template <typename OtherStorage>
228  inline PointCloudSOA& operator << (const OtherStorage& rhs)
229  {
230  points_x = rhs.points_x;
231  points_y = rhs.points_y;
232  points_z = rhs.points_z;
233  width = rhs.width;
234  height = rhs.height;
235  is_dense = rhs.is_dense;
236  return (*this);
237  }
238 
239  /** \brief Resize the internal point data vectors.
240  * \param newsize the new size
241  */
242  void
243  resize (size_t newsize)
244  {
245  assert (sane ());
246  points_x.resize (newsize);
247  points_y.resize (newsize);
248  points_z.resize (newsize);
249  }
250 
251  /** \brief Return the size of the internal vectors */
252  std::size_t
253  size () const
254  {
255  assert (sane ());
256  return (points_x.size ());
257  }
258 
259  /** \brief Check if the internal pooint data vectors are valid. */
260  bool
261  sane () const
262  {
263  return (points_x.size () == points_y.size () &&
264  points_x.size () == points_z.size ());
265  }
266 
267  /** \brief The point data. */
268  typename Storage<float>::type points_x;
269  typename Storage<float>::type points_y;
270  typename Storage<float>::type points_z;
271  typename Storage<int>::type rgb;
272 
273  /** \brief The point cloud width (if organized as an image-structure). */
274  unsigned int width;
275  /** \brief The point cloud height (if organized as an image-structure). */
276  unsigned int height;
277 
278  /** \brief True if no points are invalid (e.g., have NaN or Inf values). */
279  bool is_dense;
280 
281  typedef boost::shared_ptr<PointCloudSOA<Storage> > Ptr;
282  typedef boost::shared_ptr<const PointCloudSOA<Storage> > ConstPtr;
283 
284  //////////////////////////////////////////////////////////////////////////////////////
285  // Extras. Testing ZIP iterators
286  typedef thrust::tuple<float, float, float> tuple_type;
287  typedef typename Storage<float>::type::iterator float_iterator;
288  typedef thrust::tuple<float_iterator, float_iterator, float_iterator> iterator_tuple;
289  typedef thrust::zip_iterator<iterator_tuple> zip_iterator;
290 
291  zip_iterator
293  {
294  return (thrust::make_zip_iterator (thrust::make_tuple (points_x.begin (),
295  points_y.begin (),
296  points_z.begin ())));
297  }
298 
299  zip_iterator
301  {
302  return (thrust::make_zip_iterator (thrust::make_tuple (points_x.end (),
303  points_y.end (),
304  points_z.end ())));
305  }
306  };
307 
308  template <template <typename> class Storage, typename T>
310  {
311  typedef void type;
312  };
313 
314  template <typename T>
316  {
317  typedef thrust::detail::normal_iterator<thrust::device_ptr<T> > type;
318  };
319 
320  template <typename T>
321  struct PointIterator<Host,T>
322  {
323  typedef thrust::detail::normal_iterator<T*> type;
324  };
325 
326  template <template <typename> class Storage, typename T>
328  {
329  // typedef void* type;
330  };
331 
332  template <typename T>
334  {
335  typedef thrust::device_ptr<T> type;
336  template <typename U>
337  static thrust::device_ptr<U> cast (type ptr)
338  {
339  return thrust::device_ptr<U> ((U*)ptr.get());
340  }
341  template <typename U>
342  static thrust::device_ptr<U> cast (T* ptr)
343  {
344  return thrust::device_ptr<U> ((U*)ptr);
345  }
346  };
347 
348  template <typename T>
350  {
351  typedef T* type;
352  template <typename U>
353  static U* cast (type ptr)
354  {
355  return (U*)ptr;
356  }
357  };
358  template <template <typename> class Storage, typename T>
360  {
361  };
362 
363  template <typename T>
365  {
366  static thrust::device_ptr<T> alloc (int size)
367  {
368  return thrust::device_malloc<T> (size);
369  }
370  };
371 
372  template <typename T>
374  {
375  static T* alloc (int size)
376  {
377  return (T*) malloc (size);
378  }
379  };
380 
381 
382  } // namespace
383 } // namespace
Storage< PointXYZRGB >::type::iterator iterator
Definition: point_cloud.h:190
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:279
unsigned int width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:193
unsigned char r
Definition: point_cloud.h:57
Storage< float >::type::iterator float_iterator
Definition: point_cloud.h:287
zip_iterator zip_end()
Definition: point_cloud.h:300
misnamed class holding a 3x3 matrix
Definition: point_cloud.h:49
Storage< float >::type points_y
Definition: point_cloud.h:269
std::size_t size() const
Return the size of the internal vectors.
Definition: point_cloud.h:253
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:44
static thrust::device_ptr< U > cast(T *ptr)
Definition: point_cloud.h:342
Storage< PointXYZRGB >::type points
The point data.
Definition: point_cloud.h:188
thrust::detail::normal_iterator< thrust::device_ptr< T > > type
Definition: point_cloud.h:317
PointCloudAOS represents an AOS (Array of Structs) PointCloud implementation for CUDA processing...
Definition: point_cloud.h:132
thrust::device_vector< T > type
Definition: point_cloud.h:96
unsigned int height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:195
boost::shared_ptr< PointCloudSOA< Storage > > Ptr
Definition: point_cloud.h:281
thrust::tuple< float, float, float > tuple_type
Definition: point_cloud.h:286
PointCloudSOA represents a SOA (Struct of Arrays) PointCloud implementation for CUDA processing...
Definition: point_cloud.h:208
thrust::tuple< float_iterator, float_iterator, float_iterator > iterator_tuple
Definition: point_cloud.h:288
std::ostream & operator<<(std::ostream &os, const BivariatePolynomialT< real > &p)
Storage< float >::type points_x
The point data.
Definition: point_cloud.h:268
PointXYZRGB at(int u, int v) const
Definition: point_cloud.h:164
boost::shared_ptr< const PointCloudSOA< Storage > > ConstPtr
Definition: point_cloud.h:282
Host helper class.
Definition: point_cloud.h:64
thrust::host_vector< T > type
Definition: point_cloud.h:67
static thrust::device_ptr< U > cast(type ptr)
Definition: point_cloud.h:337
void resize(size_t newsize)
Resize the internal point data vectors.
Definition: point_cloud.h:243
unsigned int height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:276
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:198
boost::shared_ptr< PointCloudAOS< Storage > > Ptr
Definition: point_cloud.h:200
Storage< float >::type points_z
Definition: point_cloud.h:270
zip_iterator zip_begin()
Definition: point_cloud.h:292
unsigned int width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:274
boost::shared_ptr< const PointCloudAOS< Storage > > ConstPtr
Definition: point_cloud.h:201
Device helper class.
Definition: point_cloud.h:93
Default point xyz-rgb structure.
Definition: point_types.h:48
thrust::zip_iterator< iterator_tuple > zip_iterator
Definition: point_cloud.h:289
thrust::detail::normal_iterator< T * > type
Definition: point_cloud.h:323
bool sane() const
Check if the internal pooint data vectors are valid.
Definition: point_cloud.h:261
static thrust::device_ptr< T > alloc(int size)
Definition: point_cloud.h:366
Storage< int >::type rgb
Definition: point_cloud.h:271
Simple structure holding RGB data.
Definition: point_cloud.h:55