Point Cloud Library (PCL)  1.12.1-dev
cloud_iterator.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, 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 
39 #ifndef PCL_POINT_CLOUD_ITERATOR_HPP_
40 #define PCL_POINT_CLOUD_ITERATOR_HPP_
41 
42 #include <pcl/cloud_iterator.h>
43 
44 namespace pcl
45 {
46  /** \brief
47  * \author Suat Gedikli
48  */
49  template <class PointT>
50  class DefaultIterator : public CloudIterator<PointT>::Iterator
51  {
52  public:
54  : cloud_ (cloud)
55  , iterator_ (cloud.begin ())
56  {
57  }
58 
59  ~DefaultIterator () = default;
60 
61  void operator ++ ()
62  {
63  ++iterator_;
64  }
65 
66  void operator ++ (int)
67  {
68  iterator_++;
69  }
70 
71  PointT& operator* () const
72  {
73  return (*iterator_);
74  }
75 
77  {
78  return (&(*iterator_));
79  }
80 
81  unsigned getCurrentPointIndex () const
82  {
83  return (iterator_ - cloud_.begin ());
84  }
85 
86  unsigned getCurrentIndex () const
87  {
88  return (iterator_ - cloud_.begin ());
89  }
90 
91  std::size_t size () const
92  {
93  return cloud_.size ();
94  }
95 
96  void reset ()
97  {
98  iterator_ = cloud_.begin ();
99  }
100 
101  bool isValid () const
102  {
103  return (iterator_ != cloud_.end ());
104  }
105  private:
106  PointCloud<PointT>& cloud_;
107  typename PointCloud<PointT>::iterator iterator_;
108  };
109 
110  /** \brief
111  * \author Suat Gedikli
112  */
113  template <class PointT>
114  class IteratorIdx : public CloudIterator<PointT>::Iterator
115  {
116  public:
117  IteratorIdx (PointCloud<PointT>& cloud, const Indices& indices)
118  : cloud_ (cloud)
119  , indices_ (indices)
120  , iterator_ (indices_.begin ())
121  {
122  }
123 
124  IteratorIdx (PointCloud<PointT>& cloud, const PointIndices& indices)
125  : cloud_ (cloud)
126  , indices_ (indices.indices)
127  , iterator_ (indices_.begin ())
128  {
129  }
130 
131  virtual ~IteratorIdx () = default;
132 
134  {
135  ++iterator_;
136  }
137 
138  void operator ++ (int)
139  {
140  iterator_++;
141  }
142 
144  {
145  return (cloud_.points [*iterator_]);
146  }
147 
149  {
150  return (&(cloud_.points [*iterator_]));
151  }
152 
153  unsigned getCurrentPointIndex () const
154  {
155  return (*iterator_);
156  }
157 
158  unsigned getCurrentIndex () const
159  {
160  return (iterator_ - indices_.begin ());
161  }
162 
163  std::size_t size () const
164  {
165  return indices_.size ();
166  }
167 
168  void reset ()
169  {
170  iterator_ = indices_.begin ();
171  }
172 
173  bool isValid () const
174  {
175  return (iterator_ != indices_.end ());
176  }
177 
178  private:
179  PointCloud<PointT>& cloud_;
180  Indices indices_;
181  Indices::iterator iterator_;
182  };
183 
184  /** \brief
185  * \author Suat Gedikli
186  */
187  template <class PointT>
189  {
190  public:
192  : cloud_ (cloud)
193  , iterator_ (cloud.begin ())
194  {
195  }
196 
197  ~DefaultConstIterator () override = default;
198 
199  void operator ++ () override
200  {
201  ++iterator_;
202  }
203 
204  void operator ++ (int) override
205  {
206  iterator_++;
207  }
208 
209  const PointT& operator* () const override
210  {
211  return (*iterator_);
212  }
213 
214  const PointT* operator-> () const override
215  {
216  return (&(*iterator_));
217  }
218 
219  unsigned getCurrentPointIndex () const override
220  {
221  return (unsigned (iterator_ - cloud_.begin ()));
222  }
223 
224  unsigned getCurrentIndex () const override
225  {
226  return (unsigned (iterator_ - cloud_.begin ()));
227  }
228 
229  std::size_t size () const override
230  {
231  return cloud_.size ();
232  }
233 
234  void reset () override
235  {
236  iterator_ = cloud_.begin ();
237  }
238 
239  bool isValid () const override
240  {
241  return (iterator_ != cloud_.end ());
242  }
243  private:
244  const PointCloud<PointT>& cloud_;
245  typename PointCloud<PointT>::const_iterator iterator_;
246  };
247 
248  /** \brief
249  * \author Suat Gedikli
250  */
251  template <class PointT>
253  {
254  public:
256  const Indices& indices)
257  : cloud_ (cloud)
258  , indices_ (indices)
259  , iterator_ (indices_.begin ())
260  {
261  }
262 
264  const PointIndices& indices)
265  : cloud_ (cloud)
266  , indices_ (indices.indices)
267  , iterator_ (indices_.begin ())
268  {
269  }
270 
271  ~ConstIteratorIdx () override = default;
272 
273  void operator ++ () override
274  {
275  ++iterator_;
276  }
277 
278  void operator ++ (int) override
279  {
280  iterator_++;
281  }
282 
283  const PointT& operator* () const override
284  {
285  return (cloud_[*iterator_]);
286  }
287 
288  const PointT* operator-> () const override
289  {
290  return (&(cloud_.points [*iterator_]));
291  }
292 
293  unsigned getCurrentPointIndex () const override
294  {
295  return (unsigned (*iterator_));
296  }
297 
298  unsigned getCurrentIndex () const override
299  {
300  return (unsigned (iterator_ - indices_.begin ()));
301  }
302 
303  std::size_t size () const override
304  {
305  return indices_.size ();
306  }
307 
308  void reset () override
309  {
310  iterator_ = indices_.begin ();
311  }
312 
313  bool isValid () const override
314  {
315  return (iterator_ != indices_.end ());
316  }
317 
318  private:
319  const PointCloud<PointT>& cloud_;
320  Indices indices_;
321  Indices::iterator iterator_;
322  };
323 } // namespace pcl
324 
325 //////////////////////////////////////////////////////////////////////////////
326 template <class PointT>
328  : iterator_ (new DefaultIterator<PointT> (cloud))
329 {
330 }
331 
332 //////////////////////////////////////////////////////////////////////////////
333 template <class PointT>
335  PointCloud<PointT>& cloud, const Indices& indices)
336  : iterator_ (new IteratorIdx<PointT> (cloud, indices))
337 {
338 }
339 
340 //////////////////////////////////////////////////////////////////////////////
341 template <class PointT>
343  PointCloud<PointT>& cloud, const PointIndices& indices)
344  : iterator_ (new IteratorIdx<PointT> (cloud, indices))
345 {
346 }
347 
348 //////////////////////////////////////////////////////////////////////////////
349 template <class PointT>
351  PointCloud<PointT>& cloud, const Correspondences& corrs, bool source)
352 {
353  Indices indices;
354  indices.reserve (corrs.size ());
355  if (source)
356  {
357  for (const auto &corr : corrs)
358  indices.push_back (corr.index_query);
359  }
360  else
361  {
362  for (const auto &corr : corrs)
363  indices.push_back (corr.index_match);
364  }
365  iterator_ = new IteratorIdx<PointT> (cloud, indices);
366 }
367 
368 //////////////////////////////////////////////////////////////////////////////
369 template <class PointT>
371 {
372  delete iterator_;
373 }
374 
375 //////////////////////////////////////////////////////////////////////////////
376 template <class PointT> void
378 {
379  iterator_->operator++ ();
380 }
381 
382 //////////////////////////////////////////////////////////////////////////////
383 template <class PointT> void
385 {
386  iterator_->operator++ (0);
387 }
388 
389 //////////////////////////////////////////////////////////////////////////////
390 template <class PointT> PointT&
392 {
393  return (iterator_->operator * ());
394 }
395 
396 //////////////////////////////////////////////////////////////////////////////
397 template <class PointT> PointT*
399 {
400  return (iterator_->operator-> ());
401 }
402 
403 //////////////////////////////////////////////////////////////////////////////
404 template <class PointT> unsigned
406 {
407  return (iterator_->getCurrentPointIndex ());
408 }
409 
410 //////////////////////////////////////////////////////////////////////////////
411 template <class PointT> unsigned
413 {
414  return (iterator_->getCurrentIndex ());
415 }
416 
417 //////////////////////////////////////////////////////////////////////////////
418 template <class PointT> std::size_t
420 {
421  return (iterator_->size ());
422 }
423 
424 //////////////////////////////////////////////////////////////////////////////
425 template <class PointT> void
427 {
428  iterator_->reset ();
429 }
430 
431 //////////////////////////////////////////////////////////////////////////////
432 template <class PointT> bool
434 {
435  return (iterator_->isValid ());
436 }
437 
438 
439 //////////////////////////////////////////////////////////////////////////////
440 template <class PointT>
442  : iterator_ (new typename pcl::ConstCloudIterator<PointT>::DefaultConstIterator (cloud))
443 {
444 }
445 
446 //////////////////////////////////////////////////////////////////////////////
447 template <class PointT>
449  const PointCloud<PointT>& cloud, const Indices& indices)
450  : iterator_ (new typename pcl::ConstCloudIterator<PointT>::ConstIteratorIdx (cloud, indices))
451 {
452 }
453 
454 //////////////////////////////////////////////////////////////////////////////
455 template <class PointT>
457  const PointCloud<PointT>& cloud, const PointIndices& indices)
458  : iterator_ (new typename pcl::ConstCloudIterator<PointT>::ConstIteratorIdx (cloud, indices))
459 {
460 }
461 
462 //////////////////////////////////////////////////////////////////////////////
463 template <class PointT>
465  const PointCloud<PointT>& cloud, const Correspondences& corrs, bool source)
466 {
467  Indices indices;
468  indices.reserve (corrs.size ());
469  if (source)
470  {
471  for (const auto &corr : corrs)
472  indices.push_back (corr.index_query);
473  }
474  else
475  {
476  for (const auto &corr : corrs)
477  indices.push_back (corr.index_match);
478  }
479  iterator_ = new typename pcl::ConstCloudIterator<PointT>::ConstIteratorIdx (cloud, indices);
480 }
481 
482 //////////////////////////////////////////////////////////////////////////////
483 template <class PointT>
485 {
486  delete iterator_;
487 }
488 
489 //////////////////////////////////////////////////////////////////////////////
490 template <class PointT> void
492 {
493  iterator_->operator++ ();
494 }
495 
496 //////////////////////////////////////////////////////////////////////////////
497 template <class PointT> void
499 {
500  iterator_->operator++ (0);
501 }
502 
503 //////////////////////////////////////////////////////////////////////////////
504 template <class PointT> const PointT&
506 {
507  return (iterator_->operator * ());
508 }
509 
510 //////////////////////////////////////////////////////////////////////////////
511 template <class PointT> const PointT*
513 {
514  return (iterator_->operator-> ());
515 }
516 
517 //////////////////////////////////////////////////////////////////////////////
518 template <class PointT> unsigned
520 {
521  return (iterator_->getCurrentPointIndex ());
522 }
523 
524 //////////////////////////////////////////////////////////////////////////////
525 template <class PointT> unsigned
527 {
528  return (iterator_->getCurrentIndex ());
529 }
530 
531 //////////////////////////////////////////////////////////////////////////////
532 template <class PointT> std::size_t
534 {
535  return (iterator_->size ());
536 }
537 
538 //////////////////////////////////////////////////////////////////////////////
539 template <class PointT> void
541 {
542  iterator_->reset ();
543 }
544 
545 //////////////////////////////////////////////////////////////////////////////
546 template <class PointT> bool
548 {
549  return (iterator_->isValid ());
550 }
551 
552 #endif // PCL_POINT_CLOUD_ITERATOR_HPP_
553 
Iterator class for point clouds with or without given indices.
CloudIterator(PointCloud< PointT > &cloud)
unsigned getCurrentIndex() const
unsigned getCurrentPointIndex() const
std::size_t size() const
Size of the range the iterator is going through.
PointT * operator->() const
PointT & operator*() const
unsigned getCurrentPointIndex() const override
ConstIteratorIdx(const PointCloud< PointT > &cloud, const PointIndices &indices)
ConstIteratorIdx(const PointCloud< PointT > &cloud, const Indices &indices)
DefaultConstIterator(const PointCloud< PointT > &cloud)
Iterator class for point clouds with or without given indices.
const PointT * operator->() const
unsigned getCurrentIndex() const
ConstCloudIterator(const PointCloud< PointT > &cloud)
unsigned getCurrentPointIndex() const
const PointT & operator*() const
std::size_t size() const
Size of the range the iterator is going through.
std::size_t size() const
~DefaultIterator()=default
unsigned getCurrentPointIndex() const
PointT & operator*() const
unsigned getCurrentIndex() const
DefaultIterator(PointCloud< PointT > &cloud)
IteratorIdx(PointCloud< PointT > &cloud, const Indices &indices)
std::size_t size() const
unsigned getCurrentIndex() const
PointT & operator*() const
unsigned getCurrentPointIndex() const
IteratorIdx(PointCloud< PointT > &cloud, const PointIndices &indices)
bool isValid() const
virtual ~IteratorIdx()=default
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
typename VectorType::iterator iterator
Definition: point_cloud.h:425
typename VectorType::const_iterator const_iterator
Definition: point_cloud.h:426
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.