Point Cloud Library (PCL)  1.8.0
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 #ifndef PCL_CUDA_POINT_CLOUD_H_
39 #define PCL_CUDA_POINT_CLOUD_H_
40 
41 #include <pcl/cuda/point_types.h>
42 #include <pcl/cuda/thrust.h>
43 #include <boost/shared_ptr.hpp>
44 
45 namespace pcl
46 {
47  namespace cuda
48  {
49  /** \brief misnamed class holding a 3x3 matrix */
51  {
52  float3 data[3];
53  };
54 
55  /** \brief Simple structure holding RGB data. */
56  struct OpenNIRGB
57  {
58  unsigned char r, g, b;
59  };
60 
61  /** \brief Host helper class. Contains several typedefs and some static
62  * functions to help writing portable code (that runs both on host
63  * and device) */
64  template <typename T>
65  struct Host
66  {
67  // vector type
68  typedef typename thrust::host_vector<T> type;
69 
70 // // iterator type
71 // typedef thrust::detail::normal_iterator<T*> type;
72 //
73 // // pointer type
74 // typedef T* pointer_type;
75 //
76 // // allocator
77 // static T* alloc (int size)
78 // {
79 // return (T*) malloc (size);
80 // }
81 //
82 // // cast to different pointer
83 // template <typename U>
84 // static U* cast (type ptr)
85 // {
86 // return (U*)ptr;
87 // }
88  };
89 
90  /** \brief Device helper class. Contains several typedefs and some static
91  * functions to help writing portable code (that runs both on host
92  * and device) */
93  template <typename T>
94  struct Device
95  {
96  // vector type
97  typedef typename thrust::device_vector<T> type;
98 
99 // // iterator type
100 // typedef thrust::detail::normal_iterator<thrust::device_ptr<T> > iterator_type;
101 //
102 // // pointer type
103 // typedef thrust::device_ptr<T> pointer_type;
104 //
105 // // allocator
106 // static thrust::device_ptr<T> alloc (int size)
107 // {
108 // return thrust::device_malloc<T> (size);
109 // }
110 //
111 // // cast to different pointer
112 // template <typename U>
113 // static thrust::device_ptr<U> cast (type ptr)
114 // {
115 // return thrust::device_ptr<U> ((U*)ptr.get());
116 // }
117 //
118 // // cast raw pointer to different pointer
119 // template <typename U>
120 // static thrust::device_ptr<U> cast (T* ptr)
121 // {
122 // return thrust::device_ptr<U> ((U*)ptr);
123 // }
124  };
125 
126  /** @b PointCloudAOS represents an AOS (Array of Structs) PointCloud
127  * implementation for CUDA processing.
128  *
129  * This is the most efficient way to perform operations on x86 architectures
130  * (using SSE alignment).
131  */
132  template <template <typename> class Storage>
134  {
135  public:
136  PointCloudAOS () : width (0), height (0), is_dense (true)
137  {}
138 
139  //////////////////////////////////////////////////////////////////////////////////////
140  inline PointCloudAOS& operator = (const PointCloudAOS& rhs)
141  {
142  points = rhs.points;
143  width = rhs.width;
144  height = rhs.height;
145  is_dense = rhs.is_dense;
146  return (*this);
147  }
148 
149  //////////////////////////////////////////////////////////////////////////////////////
150  template <typename OtherStorage>
151  inline PointCloudAOS& operator << (const OtherStorage& rhs)
152  {
153  points = rhs.points;
154  // TODO: Test speed on operator () = vs resize+copy
155  //points.resize (rhs.points.size ());
156  //thrust::copy (rhs.points.begin (), rhs.points.end (), points.begin ());
157  width = rhs.width;
158  height = rhs.height;
159  is_dense = rhs.is_dense;
160  return (*this);
161  }
162 
163  //////////////////////////////////////////////////////////////////////////////////////
164  inline PointXYZRGB
165  at (int u, int v) const
166  {
167  if (this->height > 1)
168  return (points[v * this->width + u]);
169  else
170  return (PointXYZRGB (std::numeric_limits<float>::quiet_NaN (),
171  std::numeric_limits<float>::quiet_NaN (),
172  std::numeric_limits<float>::quiet_NaN (),
173  0));
174  // throw IsNotDenseException ("Can't use 2D indexing with a sparse point cloud");
175  }
176 
177  //////////////////////////////////////////////////////////////////////////////////////
178  inline PointXYZRGB& operator () (int u, int v)
179  {
180  return (points[v* this->width +u]);
181  }
182  inline const PointXYZRGB& operator () (int u, int v) const
183  {
184  return (points[v* this->width +u]);
185  }
186 
187  /** \brief The point data. */
188  //typename Storage<float3>::type points;
189  typename Storage<PointXYZRGB>::type points;
190 
191  typedef typename Storage<PointXYZRGB>::type::iterator iterator;
192 
193  /** \brief The point cloud width (if organized as an image-structure). */
194  unsigned int width;
195  /** \brief The point cloud height (if organized as an image-structure). */
196  unsigned int height;
197 
198  /** \brief True if no points are invalid (e.g., have NaN or Inf values). */
199  bool is_dense;
200 
201  typedef boost::shared_ptr<PointCloudAOS<Storage> > Ptr;
202  typedef boost::shared_ptr<const PointCloudAOS<Storage> > ConstPtr;
203  };
204 
205  /** @b PointCloudSOA represents a SOA (Struct of Arrays) PointCloud
206  * implementation for CUDA processing.
207  */
208  template <template <typename> class Storage>
210  {
211  public:
212  PointCloudSOA () : width (0), height (0), is_dense (true)
213  {}
214 
215  //////////////////////////////////////////////////////////////////////////////////////
216  inline PointCloudSOA& operator = (const PointCloudSOA& rhs)
217  {
218  points_x = rhs.points_x;
219  points_y = rhs.points_y;
220  points_z = rhs.points_z;
221  width = rhs.width;
222  height = rhs.height;
223  is_dense = rhs.is_dense;
224  return (*this);
225  }
226 
227  //////////////////////////////////////////////////////////////////////////////////////
228  template <typename OtherStorage>
229  inline PointCloudSOA& operator << (const OtherStorage& rhs)
230  {
231  points_x = rhs.points_x;
232  points_y = rhs.points_y;
233  points_z = rhs.points_z;
234  width = rhs.width;
235  height = rhs.height;
236  is_dense = rhs.is_dense;
237  return (*this);
238  }
239 
240  /** \brief Resize the internal point data vectors.
241  * \param newsize the new size
242  */
243  void
244  resize (size_t newsize)
245  {
246  assert (sane ());
247  points_x.resize (newsize);
248  points_y.resize (newsize);
249  points_z.resize (newsize);
250  }
251 
252  /** \brief Return the size of the internal vectors */
253  std::size_t
254  size () const
255  {
256  assert (sane ());
257  return (points_x.size ());
258  }
259 
260  /** \brief Check if the internal pooint data vectors are valid. */
261  bool
262  sane () const
263  {
264  return (points_x.size () == points_y.size () &&
265  points_x.size () == points_z.size ());
266  }
267 
268  /** \brief The point data. */
269  typename Storage<float>::type points_x;
270  typename Storage<float>::type points_y;
271  typename Storage<float>::type points_z;
272  typename Storage<int>::type rgb;
273 
274  /** \brief The point cloud width (if organized as an image-structure). */
275  unsigned int width;
276  /** \brief The point cloud height (if organized as an image-structure). */
277  unsigned int height;
278 
279  /** \brief True if no points are invalid (e.g., have NaN or Inf values). */
280  bool is_dense;
281 
282  typedef boost::shared_ptr<PointCloudSOA<Storage> > Ptr;
283  typedef boost::shared_ptr<const PointCloudSOA<Storage> > ConstPtr;
284 
285  //////////////////////////////////////////////////////////////////////////////////////
286  // Extras. Testing ZIP iterators
287  typedef thrust::tuple<float, float, float> tuple_type;
288  typedef typename Storage<float>::type::iterator float_iterator;
289  typedef thrust::tuple<float_iterator, float_iterator, float_iterator> iterator_tuple;
290  typedef thrust::zip_iterator<iterator_tuple> zip_iterator;
291 
292  zip_iterator
294  {
295  return (thrust::make_zip_iterator (thrust::make_tuple (points_x.begin (),
296  points_y.begin (),
297  points_z.begin ())));
298  }
299 
300  zip_iterator
302  {
303  return (thrust::make_zip_iterator (thrust::make_tuple (points_x.end (),
304  points_y.end (),
305  points_z.end ())));
306  }
307  };
308 
309  template <template <typename> class Storage, typename T>
311  {
312  typedef void type;
313  };
314 
315  template <typename T>
317  {
318  typedef thrust::detail::normal_iterator<thrust::device_ptr<T> > type;
319  };
320 
321  template <typename T>
322  struct PointIterator<Host,T>
323  {
324  typedef thrust::detail::normal_iterator<T*> type;
325  };
326 
327  template <template <typename> class Storage, typename T>
329  {
330  // typedef void* type;
331  };
332 
333  template <typename T>
335  {
336  typedef thrust::device_ptr<T> type;
337  template <typename U>
338  static thrust::device_ptr<U> cast (type ptr)
339  {
340  return thrust::device_ptr<U> ((U*)ptr.get());
341  }
342  template <typename U>
343  static thrust::device_ptr<U> cast (T* ptr)
344  {
345  return thrust::device_ptr<U> ((U*)ptr);
346  }
347  };
348 
349  template <typename T>
351  {
352  typedef T* type;
353  template <typename U>
354  static U* cast (type ptr)
355  {
356  return (U*)ptr;
357  }
358  };
359  template <template <typename> class Storage, typename T>
361  {
362  };
363 
364  template <typename T>
366  {
367  static thrust::device_ptr<T> alloc (int size)
368  {
369  return thrust::device_malloc<T> (size);
370  }
371  };
372 
373  template <typename T>
375  {
376  static T* alloc (int size)
377  {
378  return (T*) malloc (size);
379  }
380  };
381 
382 
383  } // namespace
384 } // namespace
385 
386 #endif //#ifndef PCL_CUDA_POINT_CLOUD_H_
Storage< PointXYZRGB >::type::iterator iterator
Definition: point_cloud.h:191
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:280
unsigned int width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:194
unsigned char r
Definition: point_cloud.h:58
Storage< float >::type::iterator float_iterator
Definition: point_cloud.h:288
zip_iterator zip_end()
Definition: point_cloud.h:301
misnamed class holding a 3x3 matrix
Definition: point_cloud.h:50
Storage< float >::type points_y
Definition: point_cloud.h:270
std::size_t size() const
Return the size of the internal vectors.
Definition: point_cloud.h:254
static thrust::device_ptr< U > cast(T *ptr)
Definition: point_cloud.h:343
Storage< PointXYZRGB >::type points
The point data.
Definition: point_cloud.h:189
thrust::detail::normal_iterator< thrust::device_ptr< T > > type
Definition: point_cloud.h:318
PointCloudAOS represents an AOS (Array of Structs) PointCloud implementation for CUDA processing...
Definition: point_cloud.h:133
thrust::device_vector< T > type
Definition: point_cloud.h:97
unsigned int height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:196
boost::shared_ptr< PointCloudSOA< Storage > > Ptr
Definition: point_cloud.h:282
PointXYZRGB at(int u, int v) const
Definition: point_cloud.h:165
thrust::tuple< float, float, float > tuple_type
Definition: point_cloud.h:287
PointCloudSOA represents a SOA (Struct of Arrays) PointCloud implementation for CUDA processing...
Definition: point_cloud.h:209
thrust::tuple< float_iterator, float_iterator, float_iterator > iterator_tuple
Definition: point_cloud.h:289
std::ostream & operator<<(std::ostream &os, const BivariatePolynomialT< real > &p)
Storage< float >::type points_x
The point data.
Definition: point_cloud.h:269
bool sane() const
Check if the internal pooint data vectors are valid.
Definition: point_cloud.h:262
boost::shared_ptr< const PointCloudSOA< Storage > > ConstPtr
Definition: point_cloud.h:283
Host helper class.
Definition: point_cloud.h:65
thrust::host_vector< T > type
Definition: point_cloud.h:68
static thrust::device_ptr< U > cast(type ptr)
Definition: point_cloud.h:338
void resize(size_t newsize)
Resize the internal point data vectors.
Definition: point_cloud.h:244
unsigned int height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:277
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:199
boost::shared_ptr< PointCloudAOS< Storage > > Ptr
Definition: point_cloud.h:201
Storage< float >::type points_z
Definition: point_cloud.h:271
zip_iterator zip_begin()
Definition: point_cloud.h:293
unsigned int width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:275
boost::shared_ptr< const PointCloudAOS< Storage > > ConstPtr
Definition: point_cloud.h:202
Device helper class.
Definition: point_cloud.h:94
Default point xyz-rgb structure.
Definition: point_types.h:49
thrust::zip_iterator< iterator_tuple > zip_iterator
Definition: point_cloud.h:290
thrust::detail::normal_iterator< T * > type
Definition: point_cloud.h:324
static thrust::device_ptr< T > alloc(int size)
Definition: point_cloud.h:367
Storage< int >::type rgb
Definition: point_cloud.h:272
Simple structure holding RGB data.
Definition: point_cloud.h:56