iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
PointCloud2Impl.H
Go to the documentation of this file.
1 /*! @file
2  @author Shane Grant
3  @copyright GNU Public License (GPL v3)
4  @section License
5  @verbatim
6  // ////////////////////////////////////////////////////////////////////////
7  // The iLab Neuromorphic Robotics Toolkit (NRT) //
8  // Copyright 2010-2012 by the University of Southern California (USC) //
9  // and the iLab at USC. //
10  // //
11  // iLab - University of Southern California //
12  // Hedco Neurociences Building, Room HNB-10 //
13  // Los Angeles, Ca 90089-2520 - USA //
14  // //
15  // See http://ilab.usc.edu for information about this project. //
16  // ////////////////////////////////////////////////////////////////////////
17  // This file is part of The iLab Neuromorphic Robotics Toolkit. //
18  // //
19  // The iLab Neuromorphic Robotics Toolkit is free software: you can //
20  // redistribute it and/or modify it under the terms of the GNU General //
21  // Public License as published by the Free Software Foundation, either //
22  // version 3 of the License, or (at your option) any later version. //
23  // //
24  // The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
25  // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
26  // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
27  // PURPOSE. See the GNU General Public License for more details. //
28  // //
29  // You should have received a copy of the GNU General Public License //
30  // along with The iLab Neuromorphic Robotics Toolkit. If not, see //
31  // <http://www.gnu.org/licenses/>. //
32  // ////////////////////////////////////////////////////////////////////////
33  @endverbatim */
34 
35 
36 #ifdef NRT_HAVE_CLOUD
37 #ifndef INCLUDE_NRT_POINTCLOUD2_DETAILS_POINTCLOUD2IMPL_H
38 #define INCLUDE_NRT_POINTCLOUD2_DETAILS_POINTCLOUD2IMPL_H
39 
40 #include <nrt/Core/Debugging/Log.H>
43 #include <nrt/External/cereal/types/map.hpp>
44 
45 #include <unordered_map>
46 #include <map>
47 #include <atomic>
48 #include <list>
49 
50 // detailed declaration of memory class
51 class nrt::PointCloud2::Memory
52 {
53  public:
54  typedef std::map<std::size_t, DenseData> DenseMap;
55  typedef std::map<std::size_t, SparseData> SparseMap;
56  //typedef std::unordered_map<unsigned int, DenseData> DenseMap;
57  //typedef std::unordered_map<unsigned int, SparseData> SparseMap;
58 
59  // Constructs a new memory with a geometry field already present
60  // in the DenseMap. This field can be directly accessed
61  // through the itsGeometry pointer
62  Memory();
63  Memory( Memory const & other );
64 
65  DenseMap itsDenseData;
66  SparseMap itsSparseData;
67 
68  // Will always point to the geometry
69  DenseData * itsGeometry;
70 
71  private:
72  friend class cereal::access;
73 
74  template <class Archive> inline
75  void save( Archive & ar ) const
76  {
77  ar( itsDenseData, itsSparseData );
78  }
79 
80  template <class Archive> inline
81  void load( Archive & ar )
82  {
83  ar( itsDenseData, itsSparseData );
84 
85  static auto const hash( cloudHash<typename StripSparse<Geometry>::type>() );
86 
87  itsGeometry = &( itsDenseData.operator[]( hash ) );
88  }
89 };
90 
91 //! @cond PRIVATE_NEVERDEFINED
92 namespace nrt
93 {
94  // Add field base case
95  template<>
96  struct PointCloud2::addFieldHelper<>
97  {
98  static void add( PointCloud2 * const cloud )
99  {
100  cloud->deepCopyIfNotUnique();
101  }
102  };
103 
104  // Add field normal case
105  template <class FirstT, class ... OtherT>
106  struct PointCloud2::addFieldHelper<FirstT, OtherT...>
107  {
108  static void add( PointCloud2 * const cloud )
109  {
110  addFieldHelper<OtherT...>::add( cloud );
111  cloud->addDataField<FirstT>();
112  }
113  };
114 
115  // remove field base case
116  template<>
117  struct PointCloud2::removeFieldHelper<>
118  {
119  static void remove( PointCloud2 * const cloud )
120  {
121  cloud->deepCopyIfNotUnique();
122  }
123  };
124 
125  // remove field normal case
126  template <class FirstT, class ... OtherT>
127  struct PointCloud2::removeFieldHelper<FirstT, OtherT...>
128  {
129  static void remove( PointCloud2 * const cloud )
130  {
131  removeFieldHelper<OtherT...>::remove( cloud );
132  cloud->removeDataField<FirstT>();
133  }
134  };
135 
136  // insert data base
137  template <>
138  struct PointCloud2::insertDataHelper<0>
139  {
140  template <class ... Fields>
141  static void insert( PointCloud2 * const cloud, size_t index, PointCloud2Data<Fields...> const & point )
142  {
143  // PointCloud2Data<> will hit this template, which has no actual data inside of it
144  // so sizeof...(Fields) will return 1 when we have one item, and we'll adjust the index
145  // accordingly when we pull it out in the other specialization
146  }
147  };
148 
149  // insert data recursive
150  template <size_t Index>
151  struct PointCloud2::insertDataHelper
152  {
153  template <class ... Fields>
154  static void insert( PointCloud2 * const cloud, size_t index, PointCloud2Data<Fields...> const & point )
155  {
156  cloud->insertFieldData( index, point.template get<Index - 1>() );
157  insertDataHelper<Index - 1>::insert( cloud, index, point );
158  }
159  };
160 
161  // lookup data base case
162  template <class RetT>
163  struct PointCloud2::lookupHelper<RetT>
164  {
165  template <class CloudT, class ... Args>
166  inline static RetT lookup( CloudT * const cloud, size_t const index, Args && ... args )
167  {
168  // lookup geometry as a final step
169  return RetT( cloud->itsData->itsGeometry->template get<Geometry>( index ), std::forward<Args>( args )... );
170  }
171 
172  template <class CloudT, class ... Args>
173  inline static RetT lookupSparseReferences( CloudT * const cloud, size_t const index, Args && ... args )
174  {
175  // omit geometry in final step
176  return RetT( std::forward<Args>( args )... );
177  }
178  };
179 
180  // lookup data normal case
181  template <class RetT, class FirstT, class ... OtherT>
182  struct PointCloud2::lookupHelper<RetT, FirstT, OtherT...>
183  {
184  template <class CloudT, class ... Args>
185  inline static RetT lookup( CloudT * const cloud, size_t const index, Args && ... args )
186  {
187  return lookupHelper<RetT, OtherT...>::template lookup( cloud, index, std::forward<Args>( args )..., cloud->template getFieldData<FirstT>( index ) );
188  }
189 
190  template <class CloudT, class ... Args>
191  inline static RetT lookupSparseReferences( CloudT * const cloud, size_t const index, Args && ... args )
192  {
193  return lookupHelper<RetT, OtherT...>::template lookupSparseReferences( cloud, index, std::forward<Args>( args )..., cloud->template getFieldData<FirstT>( index ) );
194  }
195  };
196 
197  // base case
198  template <>
199  struct PointCloud2::insertionChecker<>
200  {
201  static size_t inline check( PointCloud2 const * const cloud )
202  {
203  return 1;
204  }
205  };
206 
207  // standard case
208  template <class FirstT, class ... OtherT>
209  struct PointCloud2::insertionChecker<FirstT, OtherT...>
210  {
211  static size_t inline check( PointCloud2 const * const cloud )
212  {
213  static auto const hash( cloudHash<typename StripSparse<FirstT>::type>() );
214  static std::string const name( demangledName<typename StripSparse<FirstT>::type>() );
215 
216  size_t const count = cloud->itsData->itsDenseData.count( hash );
217 
218  if( count == 0 && cloud->itsData->itsSparseData.count( hash ) == 0 )
219  throw exception::PointCloud2FieldsException( name );
220  else
221  return count + insertionChecker<OtherT...>::check( cloud );
222  }
223  };
224 
225  // base case
226  template <class RetT>
227  struct PointCloud2::pointerHelper<RetT>
228  {
229  template <class CloudT, class ... Ptrs> inline
230  static RetT apply( CloudT * const cloud, Ptrs && ... ptrs )
231  {
232  return RetT( std::forward<Ptrs>( ptrs )... );
233  }
234  };
235 
236  // standard case
237  template <class RetT, class FirstT, class ... OtherT>
238  struct PointCloud2::pointerHelper<RetT, FirstT, OtherT...>
239  {
240  typedef typename StripSparse<FirstT>::type ActualType;
241 
242  template <class CloudT, class ... Ptrs> inline
243  static RetT apply( CloudT * const cloud, Ptrs && ... ptrs )
244  {
245  static auto const hash( cloudHash<typename StripSparse<FirstT>::type>() );
246  static const std::string name( nrt::demangledName<ActualType>() );
247 
248  auto data = cloud->itsData->itsDenseData.find( hash );
249 
250  if( data != cloud->itsData->itsDenseData.end() )
251  return pointerHelper<RetT, OtherT...>::apply( cloud, std::forward<Ptrs>( ptrs )...,
252  data->second.template begin<ActualType>() );
253  else
254  throw exception::PointCloud2FieldsException( name );
255  }
256  };
257 
258  // base case
259  template <>
260  struct PointCloud2::sparseExistHelper<>
261  {
262  typedef PointCloud2::Memory::SparseMap SparseMap;
263 
264  static inline bool apply( SparseMap const & data )
265  {
266  return false;
267  }
268 
269  static inline bool check( SparseMap const & data )
270  {
271  return false;
272  }
273  };
274 
275  // standard case
276  template <class FirstT, class ... OtherT>
277  struct PointCloud2::sparseExistHelper<FirstT, OtherT...>
278  {
279  typedef PointCloud2::Memory::SparseMap SparseMap;
280 
281  static inline bool apply( SparseMap const & data )
282  {
283  if( data.empty() )
284  return false;
285  else
286  return check( data );
287  }
288 
289  static inline bool check( SparseMap const & data )
290  {
291  static auto const hash( cloudHash<typename StripSparse<FirstT>::type>() );
292 
293  for( auto d = data.cbegin(), e = data.cend(); d != e; ++d )
294  if( d->first == hash )
295  return true;
296 
297  return sparseExistHelper<OtherT...>::check( data );
298  }
299 
300  template <class SparseT> inline
301  static bool check2( SparseT & data )
302  {
303  static auto const hash( cloudHash<typename StripSparse<FirstT>::type>() );
304 
305  for( auto d = data.cbegin(), e = data.cend(); d != e; ++d )
306  if( d->first == hash )
307  return true;
308 
309  return sparseExistHelper<OtherT...>::check2( data );
310  }
311  };
312 
313  // base case
314  template <>
315  struct PointCloud2::sparseOnlyExistHelper<>
316  {
317  template <class CloudT> inline
318  static size_t check( CloudT * const cloud )
319  {
320  return 0;
321  }
322  };
323 
324  // standard case
325  template <class FirstT, class ... OtherT>
326  struct PointCloud2::sparseOnlyExistHelper<FirstT, OtherT...>
327  {
328  typedef typename StripSparse<FirstT>::type ActualType;
329 
330  template <class CloudT> inline
331  static size_t check( CloudT * const cloud )
332  {
333  static auto const hash( cloudHash<typename StripSparse<FirstT>::type>() );
334  static const std::string name( nrt::demangledName<ActualType>() );
335 
336  if( cloud->itsData->itsDenseData.count( hash ) )
337  throw exception::PointCloud2FieldsException( name, "field is not sparse" );
338 
339  if( cloud->itsData->itsSparseData.count( hash ) )
340  return 1 + sparseExistHelper<OtherT...>::check( cloud->itsData->itsSparseData );
341  else
342  return sparseExistHelper<OtherT...>::check( cloud->itsData->itsSparseData );
343  }
344  };
345 
346  // base case
347  template <>
348  struct PointCloud2::sparseInsertHelper<>
349  {
350  template <class CloudT> inline
351  static void insert( CloudT * const cloud, size_t const index )
352  {
353  }
354  };
355 
356  // standard case
357  template <class FirstT, class ... OtherT>
358  struct PointCloud2::sparseInsertHelper<FirstT, OtherT...>
359  {
360  typedef typename StripSparse<FirstT>::type ActualType;
361 
362  template <class CloudT> inline
363  static void insert( CloudT * const cloud, size_t const index )
364  {
365  static auto const hash( cloudHash<typename StripSparse<FirstT>::type>() );
366 
367  auto & data = cloud->itsData->itsSparseData.operator[]( hash );
368 
369  if( not data.hasData( index ) )
370  data.insert( index );
371 
372  sparseInsertHelper<OtherT...>::insert( cloud, index );
373  }
374  };
375 
376  // base case
377  template<>
378  struct PointCloud2::subsetHelper<>
379  {
380  inline static bool strictestSubset( nrt::PointCloud2 const * const cloud, std::list<size_t> & subset )
381  {
382  return false;
383  }
384  };
385 
386  // standard case
387  template <class FirstT, class ... OtherT>
388  struct PointCloud2::subsetHelper<FirstT, OtherT...>
389  {
390  typedef typename StripSparse<FirstT>::type ActualType;
391 
392  inline static bool strictestSubset( nrt::PointCloud2 const * const cloud, std::list<size_t> & subset )
393  {
394  // Get the list computed from the lower level
395  std::list<size_t> lower;
396  bool lowerValid = subsetHelper<OtherT...>::strictestSubset( cloud, lower );
397 
398  // get the list from the current level
399  std::list<size_t> current;
400  bool currentValid = true;
401 
402  static auto const hash( cloudHash<typename StripSparse<FirstT>::type>() );
403 
404  auto data = cloud->itsData->itsSparseData.find( hash );
405  if( data != cloud->itsData->itsSparseData.end() )
406  {
407  for( auto & i : data->second )
408  current.push_back( i.first );
409  }
410  else
411  currentValid = false;
412 
413  // If lower levels were not valid, return results from
414  // this level and whether it was valid or not
415  if( !lowerValid )
416  {
417  subset = current;
418  return currentValid;
419  }
420 
421  // If current was not valid, return lower level
422  if( !currentValid )
423  {
424  subset = lower;
425  return lowerValid;
426  }
427 
428  // otherwise, both were valid and we need to compare the lower
429  // level results to this level and remove things that aren't
430  // in both of them
431  auto lowerIter = lower.begin();
432  auto currentIter = current.begin();
433 
434  // remove items that aren't in both lists
435  while( lowerIter != lower.end() && currentIter!= current.end() )
436  {
437  size_t curVal = *currentIter;
438  size_t subVal = *lowerIter;
439 
440  if( curVal > subVal )
441  {
442  // remove from sub
443  lowerIter = lower.erase( lowerIter );
444  }
445  else if( curVal < subVal )
446  {
447  // increment map
448  ++currentIter;
449  }
450  else // found, continue on
451  {
452  ++currentIter;
453  ++lowerIter;
454  }
455  } // end while
456 
457  // if we left the map before sub, drop the rest of the items in sub
458  if( lowerIter != lower.end() )
459  lower.erase( lowerIter, lower.end() );
460 
461  // return the stricter set
462  subset = lower;
463  return true;
464  }
465  };
466 } // end namespace nrt (end helper structs)
467 //! @endcond
468 
469 ////////////////////////////////////////////////////////////////
470 //! @cond PRIVATE_NEVERDEFINED
471 template <class ... Fields> inline
472 nrt::PointCloud2 nrt::PointCloud2::create( size_t initialSize )
473 {
474  PointCloud2 cloud( initialSize );
475  cloud.addField<Fields...>();
476  return cloud;
477 }
478 //! @endcond
479 
480 template <class ... Fields> inline
482 {
483  addFieldHelper<Fields...>::add( this );
484  invalidateCache();
485 }
486 
487 template <class ... Fields> inline
489 {
490  removeFieldHelper<Fields...>::remove( this );
491  invalidateCache();
492 }
493 
494 template <class T> inline
496 {
497  static auto const hash( cloudHash<typename StripSparse<T>::type>() );
498 
499  return itsData->itsDenseData.count( hash ) || itsData->itsSparseData.count( hash );
500 }
501 
502 template <class T> inline
504 {
505  static auto const hash( cloudHash<typename StripSparse<T>::type>() );
506 
507  return itsData->itsDenseData.count( hash );
508 }
509 
510 template <class T> inline
512 {
513  static auto const hash( cloudHash<typename StripSparse<T>::type>() );
514 
515  return itsData->itsSparseData.count( hash );
516 }
517 
518 template <class T> inline
519 bool nrt::PointCloud2::hasSparseData( size_t const index ) const
520 {
521  static auto const hash( cloudHash<typename StripSparse<T>::type>() );
522 
523  auto data = itsData->itsSparseData.find( hash );
524 
525  if( data != itsData->itsSparseData.end() )
526  return data->second.hasData( index );
527  else
528  return false;
529 }
530 
531 //! @cond PRIVATE_NEVERDEFINED
532 template <class T> inline
534 {
535  static std::string const name( demangledName<typename StripSparse<T>::type>() );
536 
537  if( !hasSparseField<T>() )
539 
540  std::list<size_t> subset;
541  subsetHelper<T>::strictestSubset( this, subset );
542 
543  return {subset.begin(), subset.end()};
544 }
545 //! @endcond
546 
547 inline size_t nrt::PointCloud2::size() const
548 {
549  return itsSize;
550 }
551 
552 template <class T> inline
553 size_t nrt::PointCloud2::size() const
554 {
555  static std::string const name( demangledName<typename StripSparse<T>::type>() );
556  static auto const hash( cloudHash<typename StripSparse<T>::type>() );
557 
558  auto dense = itsData->itsDenseData.find( hash );
559 
560  if( dense != itsData->itsDenseData.end() )
561  return dense->second.size();
562 
563  auto sparse = itsData->itsSparseData.find( hash );
564 
565  if( sparse != itsData->itsSparseData.end() )
566  return sparse->second.size();
567  else
569 }
570 
571 inline bool nrt::PointCloud2::empty() const
572 {
573  return itsSize == 0;
574 }
575 
576 template <class ... Fields> inline
577 auto nrt::PointCloud2::get( size_t index ) ->
578 nrt::PointCloud2DataRef<typename StripSparse<Fields>::type...>
579 {
580  return lookupHelper<PointCloud2DataRef<typename StripSparse<Fields>::type...>, Fields...>::lookup( this, index );
581 }
582 
583 
584 template <class ... Fields> inline
585 auto nrt::PointCloud2::at( size_t index ) const ->
586 nrt::PointCloud2ConstDataRef<typename StripSparse<Fields>::type...>
587 {
588  return lookupHelper<PointCloud2ConstDataRef<typename StripSparse<Fields>::type...>, Fields...>::lookup( const_cast<PointCloud2*const>( this ), index );
589 }
590 
591 template <class T, class ... OtherT> inline
593 {
594  deepCopyIfNotUnique();
595 
596  typedef sparseExistHelper<T, OtherT...> Functor;
597 
598  if( itsSparseExistCache.template lookup<Functor, bool>( itsData->itsSparseData ) )
599  return {this, 0, true};
600  else
601  return {this, 0};
602 }
603 
604 template <class T, class ... OtherT> inline
605 auto nrt::PointCloud2::begin() const -> ConstIterator<typename StripSparse<T>::type, typename StripSparse<OtherT>::type...> {
606  typedef sparseExistHelper<T, OtherT...> Functor;
607 
608  if( itsSparseExistCache.template lookup<Functor, bool>( itsData->itsSparseData ) )
609  return {this, 0, true};
610  else
611  return {this, 0};
612 }
613 
614 template <class T, class ... OtherT> inline
615 auto nrt::PointCloud2::const_begin() const -> ConstIterator<typename StripSparse<T>::type, typename StripSparse<OtherT>::type...>
616 {
617  return this->template begin<T, OtherT...>();
618 }
619 
620 template <class T, class ... OtherT> inline
622 {
623  deepCopyIfNotUnique();
624 
625  typedef sparseExistHelper<T, OtherT...> Functor;
626  if( itsSparseExistCache.template lookup<Functor, bool>( itsData->itsSparseData ) )
627  return {this, size(), true};
628  else
629  return {this, size()};
630 }
631 
632 template <class T, class ... OtherT> inline
633 auto nrt::PointCloud2::end() const -> ConstIterator<typename StripSparse<T>::type, typename StripSparse<OtherT>::type...>
634 {
635  typedef sparseExistHelper<T, OtherT...> Functor;
636  if( itsSparseExistCache.template lookup<Functor, bool>( itsData->itsSparseData ) )
637  return {this, size(), true};
638  else
639  return {this, size()};
640 }
641 
642 template <class T, class ... OtherT> inline
643 auto nrt::PointCloud2::const_end() const -> ConstIterator<typename StripSparse<T>::type, typename StripSparse<OtherT>::type...>
644 {
645  return this->template end<T, OtherT...>();
646 }
647 
648 template <class T, class ... OtherT> inline
650 {
651  deepCopyIfNotUnique();
652 
653  typedef sparseExistHelper<T, OtherT...> Functor;
654  if( itsSparseExistCache.template lookup<Functor, bool>( itsData->itsSparseData ) )
655  return {this, 0, indices, true};
656  else
657  return {this, 0, indices};
658 }
659 
660 template <class T, class ... OtherT> inline
662 {
663  typedef sparseExistHelper<T, OtherT...> Functor;
664  if( itsSparseExistCache.template lookup<Functor, bool>( itsData->itsSparseData ) )
665  return {this, 0, indices, true};
666  else
667  return {this, 0, indices};
668 }
669 
670 template <class T, class ... OtherT> inline
672 {
673  return this->template subset_begin<T, OtherT...>( indices );
674 }
675 
676 template <class T, class ... OtherT> inline
678 {
679  deepCopyIfNotUnique();
680 
681  typedef sparseExistHelper<T, OtherT...> Functor;
682  if( itsSparseExistCache.template lookup<Functor, bool>( itsData->itsSparseData ) )
683  return {this, indices.size(), indices, true};
684  else
685  return {this, indices.size(), indices};
686 }
687 
688 template <class T, class ... OtherT> inline
690 {
691  typedef sparseExistHelper<T, OtherT...> Functor;
692  if( itsSparseExistCache.template lookup<Functor, bool>( itsData->itsSparseData ) )
693  return {this, indices.size(), indices, true};
694  else
695  return {this, indices.size(), indices};
696 }
697 
698 template <class T, class ... OtherT> inline
700 {
701  return subset_end<T, OtherT...>( indices );
702 }
703 
704 template <class T, class ... OtherT> inline
706 {
707  return {this->template begin<T, OtherT...>(), this->template end<T, OtherT...>()};
708 }
709 
710 template <class T, class ... OtherT> inline
711 auto nrt::PointCloud2::range() const -> IteratorRange<ConstIterator<typename StripSparse<T>::type, typename StripSparse<OtherT>::type...>>
712 {
713  return {this->template begin<T, OtherT...>(), this->template end<T, OtherT...>()};
714 }
715 
716 template <class T, class ... OtherT> inline
717 auto nrt::PointCloud2::const_range() const -> IteratorRange<ConstIterator<typename StripSparse<T>::type, typename StripSparse<OtherT>::type...>>
718 {
719  return {this->template begin<T, OtherT...>(), this->template end<T, OtherT...>()};
720 }
721 
722 template <class T, class ... OtherT> inline
724 {
725  return {this->template subset_begin<T, OtherT...>( indices ), this->template subset_end<T, OtherT...>( indices )};
726 }
727 
728 template <class T, class ... OtherT> inline
730 {
731  return {this->template subset_begin<T, OtherT...>( indices ), this->template subset_end<T, OtherT...>( indices )};
732 }
733 
734 template <class T, class ... OtherT> inline
736 {
737  return {this->template subset_begin<T, OtherT...>( indices ), this->template subset_end<T, OtherT...>( indices )};
738 }
739 
740 template <class ... Fields> inline
742 {
743  deepCopyIfNotUnique();
744 
745  // Do error checking first - will throw an exception if a field does not exist
746  const size_t count = insertionChecker<Fields...>::check( this );
747  const size_t expected = itsData->itsDenseData.size();
748 
749  // First insert geometric data, we do this without a string comparison
750  // so that just standard data is very fas
751  size_t index = itsData->itsGeometry->insert( point.geometry() );
752 
753  // this will only expand if we actually had fields to insert
754  insertDataHelper<sizeof...(Fields)>::insert( this, index, point );
755 
756  // equalize our dense field sizes
757  if( count != expected )
758  insertEqualize( index + 1 );
759 
760  invalidateCache();
761  ++itsSize;
762 }
763 
764 template <class ... Fields> inline
765 void nrt::PointCloud2::insertSparse( size_t const index, StreamableGenericBag<Fields...> const & data )
766 {
767  deepCopyIfNotUnique();
768 
769  // make sure all of the sparse fields listed exist
770  const size_t count = sparseOnlyExistHelper<Fields...>::check( this ); // will throw if something doesn't exist
771 
772  // check that count is ok - we'll only do an insertion if all of the fields provided were both sparse and exist
773  if( count != sizeof...(Fields) )
775 
776  // don't insert if this would be out of bounds
777  // TODO: actual out of bounds exception
778  if( index >= size() )
779  throw exception::Exception( "Index out of bounds" );
780 
781  // insert junk data if needed
782  sparseInsertHelper<Fields...>::insert( this, index );
783 
784  // finally, do assignment
785  lookupHelper<PointCloud2DataRef<typename StripSparse<Fields>::type...>, Fields...>::lookup( this, index ) = data;
786 
787  invalidateCache();
788 }
789 
790 template <class Field> inline
791 void nrt::PointCloud2::remove( size_t index )
792 {
793  deepCopyIfNotUnique();
794 
795  static std::string const name( demangledName<typename StripSparse<Field>::type>() );
796  static auto const hash( cloudHash<typename StripSparse<Field>::type>() );
797 
798  auto data = itsData->itsSparseData.find( hash );
799 
800  if( data != itsData->itsSparseData.end() )
801  {
802  data->second.remove( index );
803  invalidateCache();
804  }
805  else
807 }
808 
809 // Adds a dense field to the point cloud
810 //! @cond PRIVATE_NEVERDEFINED
811 template <class T> inline
812 typename std::enable_if<!std::is_base_of<nrt::SparseFieldCore, T>::value, void>::type
813 nrt::PointCloud2::addDataField()
814 {
815  static std::string const name( demangledName<T>() );
816  static auto const hash( cloudHash<typename StripSparse<T>::type>() );
817 
818  // insert into point cloud, same size as current dense data
819  if( !itsData->itsDenseData.count( hash ) && !itsData->itsSparseData.count( hash ) )
820  itsData->itsDenseData.operator[]( hash ) = DenseData( sizeof(T), size(), name );
821 }
822 //! @endcond
823 
824 // Adds a sparse field to the point cloud
825 //! @cond PRIVATE_NEVERDEFINED
826 template <class T> inline
827 typename std::enable_if<std::is_base_of<nrt::SparseFieldCore, T>::value, void>::type
828 nrt::PointCloud2::addDataField()
829 {
830  static std::string const name( demangledName<typename T::Actual>() );
831  static auto const hash( cloudHash<typename StripSparse<T>::type>() );
832 
833  // create sparse data struct
834  SparseData newData( sizeof(typename T::Actual), name );
835 
836  // insert into point cloud
837  if( !itsData->itsSparseData.count( hash ) && !itsData->itsDenseData.count( hash ) )
838  itsData->itsSparseData.operator[]( hash ) = newData;
839 }
840 //! @endcond
841 
842 //Removes a field from the point cloud
843 template <class T> inline
844 void nrt::PointCloud2::removeDataField()
845 {
846  static auto const hash( cloudHash<typename StripSparse<T>::type>() );
847 
848  itsData->itsDenseData.erase( hash );
849  itsData->itsSparseData.erase( hash );
850 }
851 
852 template <class T>
853 void nrt::PointCloud2::insertFieldData( size_t index, T const & data )
854 {
855  static auto const hash( cloudHash<typename StripSparse<T>::type>() );
856 
857  auto dense = itsData->itsDenseData.find( hash );
858 
859  if( dense != itsData->itsDenseData.end() )
860  dense->second.insert( data );
861  else
862  {
863  auto sparse = itsData->itsSparseData.find( hash );
864 
865  if( sparse != itsData->itsSparseData.end() )
866  sparse->second.insert( index, data );
867  }
868 }
869 
870 //! @cond PRIVATE_NEVERDEFINED
871 template <class T> inline
872 auto nrt::PointCloud2::getFieldData( size_t index ) -> T &
873 {
874  typedef typename StripSparse<T>::type ActualType;
875  static std::string const name( demangledName<ActualType>() );
876  static auto const hash( cloudHash<typename StripSparse<T>::type>() );
877 
878  auto dense = itsData->itsDenseData.find( hash );
879 
880  if( dense != itsData->itsDenseData.end() )
881  return dense->second.template get<ActualType>( index );
882 
883  auto sparse = itsData->itsSparseData.find( hash );
884 
885  if( sparse != itsData->itsSparseData.end() )
886  return sparse->second.template get<ActualType>( index );
887 
888  // else couldn't find the field requested
889  throw exception::PointCloud2FieldsException( name );
890 }
891 //! @endcond
892 
893 //! @cond PRIVATE_NEVERDEFINED
894 template <class T> inline
895 auto nrt::PointCloud2::getFieldData( size_t index ) const -> T const &
896 {
897  typedef typename StripSparse<T>::type ActualType;
898  static std::string const name( demangledName<ActualType>() );
899  static auto const hash( cloudHash<typename StripSparse<T>::type>() );
900 
901  auto dense = itsData->itsDenseData.find( hash );
902 
903  if( dense != itsData->itsDenseData.end() )
904  return dense->second.template get<ActualType>( index );
905 
906  auto sparse = itsData->itsSparseData.find( hash );
907 
908  if( sparse != itsData->itsSparseData.end() )
909  return sparse->second.template get<ActualType>( index );
910 
911  // else couldn't find the field requested
912  throw exception::PointCloud2FieldsException( name );
913 }
914 //! @endcond
915 
916 inline void nrt::PointCloud2::deepCopyIfNotUnique()
917 {
918  if( !itsData.unique() )
919  deepCopy();
920 }
921 
922 // anon namespace for various iterator related helpers
923 namespace
924 {
925  // used to dereference a bag of pointers
926  struct DereferenceFunctor
927  {
928  template <class T> inline
929  T & operator()( T * ptr ) const
930  {
931  return *ptr;
932  }
933  };
934 
935  // dereference a specific index
936  struct DereferenceIndexFunctor
937  {
938  DereferenceIndexFunctor( const size_t index ) : itsIndex( index ) {}
939 
940  template <class T> inline
941  T & operator()( T * ptr ) const
942  {
943  return ptr[itsIndex];
944  }
945 
946  const size_t itsIndex;
947  };
948 
949  // used to adjust pointers
950  template <class T>
951  struct ArithmeticFunctor
952  {
953  inline ArithmeticFunctor( T value ) : itsValue( value ) {}
954 
955  template <class U> inline
956  void operator()( U & u ) const
957  {
958  u += itsValue;
959  };
960 
961  const T itsValue;
962  };
963 } // end anon namespace
964 
965 namespace nrt
966 {
967  // Specialization for no additional fields
968  // We can make a few optimizations here:
969  // - no need to keep around dense pointers
970  // - can never get sparse data, no need for that either
971  //! Template specialization for Iterator with no parameters
972  /*! @anchor IteratorBase
973  @relates Iteartor */
974  template <>
976  public boost::iterator_facade<Iterator<>,
977  PointCloud2DataRef<>,
978  boost::random_access_traversal_tag, PointCloud2DataRef<>>
979  {
980  public:
981  //! Creates an iterator located at a certain index in the cloud
982  /*! @param cloud The point cloud
983  @param index The starting index */
984  inline Iterator( PointCloud2 * const cloud, size_t index );
985 
986  //! Creates an iterator over a specific subset of points
987  /*! @param cloud The point cloud
988  @param index The starting index in the set of indices
989  @param indices The indices that define a subset */
990  inline Iterator( PointCloud2 * const cloud, size_t index, Indices const indices );
991 
992  //! Copy construction
993  inline Iterator( Iterator const & other ) = default;
994 
995  //! Get whether we contain sparse fields
996  inline bool sparse() const;
997 
998  //! Get the current index in the point cloud of the iterator
999  /*! The value returned is not guaranteed to be valid if the iterator
1000  is not within the bounds of the cloud */
1001  inline size_t index() const;
1002 
1003  //! To keep boost happy
1004  typedef typename boost::iterator_facade<Iterator<>, PointCloud2DataRef<>,
1005  boost::random_access_traversal_tag, PointCloud2DataRef<>>::
1007 
1008  private:
1009  friend class boost::iterator_core_access;
1010 
1011  inline void increment();
1012 
1013  inline void decrement();
1014 
1015  inline void advance( difference_type n );
1016 
1017  inline difference_type distance_to( Iterator const & other ) const;
1018 
1019  inline bool equal( Iterator const & other ) const;
1020 
1021  inline PointCloud2DataRef<> dereference() const;
1022 
1023  private:
1024  size_t itsIndex; //!< position in either our sparse indices or our dense cloud
1025 
1026  Optional<Indices> itsIndices; //!< Indices we are restricted to visit
1027 
1028  Geometry * itsGeometryPtr; //!< used to get geometry in the dense case
1029 
1030  bool itsUseIndices; //!< whether we use our indices to get our index
1031  }; // class Iterator
1032 } // namespace nrt
1033 
1034 //! @cond PRIVATE_NEVERDEFINED
1035 nrt::PointCloud2::Iterator<>::Iterator( PointCloud2 * const cloud, size_t index ) :
1036  itsIndex( index ), itsIndices(),
1037  itsGeometryPtr( cloud->itsData->itsGeometry->begin<Geometry>() ),
1038  itsUseIndices( false )
1039 {
1040 }
1041 
1042 nrt::PointCloud2::Iterator<>::Iterator( PointCloud2 * const cloud, size_t index, Indices const indices ) :
1043  itsIndex( index ), itsIndices( indices ),
1044  itsGeometryPtr( cloud->itsData->itsGeometry->begin<Geometry>() ),
1045  itsUseIndices( true )
1046 {
1047 }
1048 
1050 {
1051  return false;
1052 }
1053 
1055 {
1056  if( itsUseIndices )
1057  return (*itsIndices)[itsIndex];
1058  else
1059  return itsIndex;
1060 }
1061 
1063 {
1064  ++itsIndex;
1065 }
1066 
1068 {
1069  --itsIndex;
1070 }
1071 
1072 void nrt::PointCloud2::Iterator<>::advance( difference_type n )
1073 {
1074  itsIndex += n;
1075 }
1076 
1077 auto nrt::PointCloud2::Iterator<>::distance_to( Iterator const & other ) const -> difference_type
1078 {
1079  return other.itsIndex - itsIndex;
1080 }
1081 
1082 auto nrt::PointCloud2::Iterator<>::equal( Iterator const & other ) const -> bool
1083 {
1084  return other.itsIndex == itsIndex;
1085 }
1086 
1088 {
1089  return {itsGeometryPtr[index()]};
1090 }
1091 //! @endcond
1092 
1093 namespace nrt
1094 {
1095  //! Template specialization for Iteartor with variadic templates
1096  /*! Specialization for having specific fields
1097  @anchor IteratorVariadic
1098  @relates Iterator */
1099  template <class FirstT, class ... OtherT>
1100  class PointCloud2::Iterator<FirstT, OtherT...> :
1101  public boost::iterator_facade<Iterator<FirstT, OtherT...>,
1102  PointCloud2DataRef<typename StripSparse<FirstT>::type, typename StripSparse<OtherT>::type...>,
1103  boost::random_access_traversal_tag, PointCloud2DataRef<typename StripSparse<FirstT>::type, typename StripSparse<OtherT>::type...>>
1104  {
1105  public:
1106  //! Creates a dense iterator located at a certain index in the cloud
1107  /*! @param cloud The point cloud
1108  @param index The starting index */
1109  Iterator( PointCloud2 * const cloud, size_t index );
1110 
1111  //! Creates a sparse iterator located at a certain index in the cloud
1112  /*! @param cloud The point cloud
1113  @param index The starting index
1114  @param sparse value ignored */
1115  Iterator( PointCloud2 * const cloud, size_t index, bool sparse );
1116 
1117  //! Creates an iterator over a specific subset of dense points
1118  /*! @param cloud The point cloud
1119  @param index The starting index in the set of indices
1120  @param indices The indices that define a subset */
1121  Iterator( PointCloud2 * const cloud, size_t index, Indices const indices );
1122 
1123  //! Creates an iterator over a specific subset of sparse points
1124  /*! @param cloud The point cloud
1125  @param index The starting index in the set of indices
1126  @param indices The indices that define a subset
1127  @param sparse value ignored */
1128  Iterator( PointCloud2 * const cloud, size_t index, Indices const indices, bool sparse );
1129 
1130  //! Copy construction
1131  Iterator( Iterator const & other ) = default;
1132 
1133  //! Get whether we contain sparse fields
1134  bool sparse() const;
1135 
1136  //! Get the current index in the point cloud of the iterator
1137  /*! The value returned is not guaranteed to be valid if the iterator
1138  is not within the bounds of the cloud */
1139  size_t index() const;
1140 
1141  //! Used internally by boost
1142  typedef typename boost::iterator_facade<Iterator<FirstT, OtherT...>, PointCloud2DataRef<typename StripSparse<FirstT>::type, typename StripSparse<OtherT>::type...>,
1143  boost::random_access_traversal_tag, PointCloud2DataRef<typename StripSparse<FirstT>::type, typename StripSparse<OtherT>::type...>>::
1145 
1146  private:
1147  friend class boost::iterator_core_access;
1148 
1149  void increment();
1150 
1151  void decrement();
1152 
1153  void advance( difference_type n );
1154 
1155  difference_type distance_to( Iterator const & other ) const;
1156 
1157  bool equal( Iterator const & other ) const;
1158 
1160 
1161  private:
1162  size_t itsIndex; //!< position in either our sparse indices or our dense cloud
1163 
1164  Optional<Indices> itsIndices; //!< Indices we are restricted to visit
1165 
1166  //! A set of pointers to the beginning of the various dense fields, dereferenced to get our data
1167  GenericBag<typename StripSparse<FirstT>::type*, typename StripSparse<OtherT>::type*...> itsDensePointers;
1168 
1169  //! A set of references to each specific element in our sparse intersection, including any dense fields
1170  std::vector<GenericBag<typename StripSparse<FirstT>::type &, typename StripSparse<OtherT>::type &...>> itsSparseReferences;
1171 
1172  //! Pointer to the beginning of the geometry; used in either sparse or dense case to get the data
1173  Geometry * itsGeometryPtr;
1174 
1175  bool itsOnlyDense; //!< whether we only consist of dense data
1176 
1177  bool itsUseIndices; //!< whether we use our indices to get our index
1178 
1179  PointCloud2 * const itsCloud; //!< reference back to the cloud we came from
1180  }; // class Iterator
1181 } // namespace nrt
1182 
1183 //! @cond PRIVATE_NEVERDEFINED
1184 template <class FirstT, class ... OtherT> inline
1186  itsIndex( index ),
1187  itsDensePointers( cloud->itsPointerCache.template lookup<pointerHelper<decltype(itsDensePointers), FirstT, OtherT...>, decltype(itsDensePointers)>( cloud ) ),
1188  itsGeometryPtr( cloud->itsData->itsGeometry->template begin<Geometry>() ),
1189  itsOnlyDense( true ),
1190  itsUseIndices( false ),
1191  itsCloud( cloud )
1192 {
1193 }
1194 
1195 template <class FirstT, class ... OtherT> inline
1196 nrt::PointCloud2::Iterator<FirstT, OtherT...>::Iterator( PointCloud2 * const cloud, size_t index, bool /*sparse*/ ) :
1197  itsIndex( index ), itsDensePointers(),
1198  itsGeometryPtr( cloud->itsData->itsGeometry->template begin<Geometry>() ),
1199  itsOnlyDense( false ),
1200  itsUseIndices( true ),
1201  itsCloud( cloud )
1202 {
1203  // Establish the strictest set of sparse indices
1204  // (this is the intersection of all indices for each sparse type specified)
1205  std::list<size_t> subset;
1206  bool sparseValid = subsetHelper<FirstT, OtherT...>::strictestSubset( itsCloud, subset ); // TODO: may benefit from caching
1207 
1208  if( not sparseValid )
1209  {
1210  // We couldn't make a valid subset out of these requirements, throw an exception
1211  throw exception::PointCloud2FieldsException();
1212  }
1213 
1214  // copy results to local indices set
1215  itsIndices = Indices();
1216  itsIndices->resize( subset.size() );
1217  std::copy( subset.begin(), subset.end(), itsIndices->begin() );
1218 
1219  // Populate our sparse references
1220  itsSparseReferences.reserve( itsIndices->size() );
1221  for( size_t i = 0, end = itsIndices->size(); i < end; ++i )
1222  {
1223  itsSparseReferences.emplace_back(
1224  lookupHelper<GenericBag<typename StripSparse<FirstT>::type &, typename StripSparse<OtherT>::type &...>, FirstT, OtherT...>::lookupSparseReferences(
1225  const_cast<PointCloud2* const>( itsCloud ), (*itsIndices)[i] ) );
1226  }
1227 
1228  if( index != 0 ) // adjust to fit size of sparse indices
1229  itsIndex = itsIndices->size();
1230 }
1231 
1232 template <class FirstT, class ... OtherT> inline
1233 nrt::PointCloud2::Iterator<FirstT, OtherT...>::Iterator( PointCloud2 * const cloud, size_t index, Indices const indices ) :
1234  itsIndex( index ),
1235  itsIndices( indices ),
1236  itsDensePointers( cloud->itsPointerCache.template lookup<pointerHelper<decltype(itsDensePointers), FirstT, OtherT...>, decltype(itsDensePointers)>( cloud ) ),
1237  itsGeometryPtr( cloud->itsData->itsGeometry->template begin<Geometry>() ),
1238  itsOnlyDense( true ),
1239  itsUseIndices( true ),
1240  itsCloud( cloud )
1241 {
1242 }
1243 
1244 template <class FirstT, class ... OtherT> inline
1245 nrt::PointCloud2::Iterator<FirstT, OtherT...>::Iterator( PointCloud2 * const cloud, size_t index, Indices const indices, bool /*sparse*/ ) :
1246  itsIndex( index ),
1247  itsIndices( indices ),
1248  itsDensePointers(),
1249  itsGeometryPtr( cloud->itsData->itsGeometry->template begin<Geometry>() ),
1250  itsOnlyDense( false ),
1251  itsUseIndices( true ),
1252  itsCloud( cloud )
1253 {
1254  // Set up all of our sparse references
1255  itsSparseReferences.reserve( itsIndices->size() );
1256  for( size_t i = 0, end = itsIndices->size(); i < end; ++i )
1257  {
1258  itsSparseReferences.emplace_back(
1259  lookupHelper<GenericBag<typename StripSparse<FirstT>::type&, typename StripSparse<OtherT>::type&...>, FirstT, OtherT...>::lookupSparseReferences(
1260  const_cast<PointCloud2* const>( itsCloud ), (*itsIndices)[i] ) );
1261  }
1262 }
1263 
1264 template <class FirstT, class ... OtherT> inline
1266 {
1267  return !itsOnlyDense;
1268 }
1269 
1270 template <class FirstT, class ... OtherT> inline
1272 {
1273  if( itsUseIndices )
1274  return (*itsIndices)[itsIndex];
1275  else
1276  return itsIndex;
1277 }
1278 
1279 template <class FirstT, class ... OtherT> inline
1281 {
1282  ++itsIndex;
1283 }
1284 
1285 template <class FirstT, class ... OtherT> inline
1287 {
1288  --itsIndex;
1289 }
1290 
1291 template <class FirstT, class ... OtherT> inline
1293 {
1294  itsIndex += n;
1295 }
1296 
1297 template <class FirstT, class ... OtherT> inline
1298 auto nrt::PointCloud2::Iterator<FirstT, OtherT...>::distance_to( Iterator const & other ) const -> difference_type
1299 {
1300  return other.itsIndex - itsIndex;
1301 }
1302 
1303 template <class FirstT, class ... OtherT> inline
1304 auto nrt::PointCloud2::Iterator<FirstT, OtherT...>::equal( Iterator const & other ) const -> bool
1305 {
1306  return other.itsIndex == itsIndex;
1307 }
1308 
1309 template <class FirstT, class ... OtherT> inline
1311  PointCloud2DataRef<typename StripSparse<FirstT>::type, typename StripSparse<OtherT>::type...>
1312 {
1313  const size_t idx = index();
1314 
1315  if( itsOnlyDense )
1316  return {itsGeometryPtr[idx], itsDensePointers.template applyFunction<typename StripSparse<FirstT>::type&, typename StripSparse<OtherT>::type&...>( DereferenceIndexFunctor( idx ) )};
1317  else
1318  return {itsGeometryPtr[idx], itsSparseReferences[itsIndex]};
1319 }
1320 //! @endcond
1321 
1322 namespace nrt
1323 {
1324  // Specialization for no additional fields
1325  // We can make a few optimizations here:
1326  // - no need to keep around dense pointers
1327  // - can never get sparse data, no need for that either
1328  //! Template specialization for ConstIterator with no template parameters
1329  /*! @anchor ConstIteratorBase
1330  @relates ConstIterator */
1331  template <>
1333  public boost::iterator_facade<ConstIterator<>,
1334  PointCloud2ConstDataRef<>,
1335  boost::random_access_traversal_tag, PointCloud2ConstDataRef<>>
1336  {
1337  public:
1338  //! Creates an iterator located at a certain index in the cloud
1339  /*! @param cloud The point cloud
1340  @param index The starting index */
1341  inline ConstIterator( PointCloud2 const * const cloud, size_t index );
1342 
1343  //! Creates an iterator over a specific subset of points
1344  /*! @param cloud The point cloud
1345  @param index The starting index in the set of indices
1346  @param indices The indices that define a subset */
1347  inline ConstIterator( PointCloud2 const * const cloud, size_t index, Indices const indices );
1348 
1349  //! Copy construction
1350  inline ConstIterator( ConstIterator const & other ) = default;
1351 
1352  //! Get whether we contain sparse fields
1353  inline bool sparse() const;
1354 
1355  //! Get the current index in the point cloud of the iterator
1356  /*! The value returned is not guaranteed to be valid if the iterator
1357  is not within the bounds of the cloud */
1358  inline size_t index() const;
1359 
1360  //! To keep boost happy
1361  typedef typename boost::iterator_facade<ConstIterator<>, PointCloud2ConstDataRef<>,
1362  boost::random_access_traversal_tag, PointCloud2ConstDataRef<>>::
1364 
1365  private:
1366  friend class boost::iterator_core_access;
1367 
1368  inline void increment();
1369 
1370  inline void decrement();
1371 
1372  inline void advance( difference_type n );
1373 
1374  inline difference_type distance_to( ConstIterator const & other ) const;
1375 
1376  inline bool equal( ConstIterator const & other ) const;
1377 
1378  inline PointCloud2ConstDataRef<> dereference() const;
1379 
1380  private:
1381  size_t itsIndex; //!< position in either our sparse indices or our dense cloud
1382 
1383  Optional<Indices> itsIndices; //!< Indices we are restricted to visit
1384 
1385  Geometry * itsGeometryPtr; //!< used to get geometry in the dense case
1386 
1387  bool itsUseIndices; //!< whether we use our indices to get our index
1388  }; // class ConstIterator
1389 } // namespace nrt
1390 
1391 
1392 //! @cond PRIVATE_NEVERDEFINED
1393 nrt::PointCloud2::ConstIterator<>::ConstIterator( PointCloud2 const * const cloud, size_t index ) :
1394  itsIndex( index ), itsIndices(),
1395  itsGeometryPtr( cloud->itsData->itsGeometry->begin<Geometry>() ),
1396  itsUseIndices( false )
1397 {
1398 }
1399 
1400 nrt::PointCloud2::ConstIterator<>::ConstIterator( PointCloud2 const * const cloud, size_t index, Indices const indices ) :
1401  itsIndex( index ), itsIndices( indices ),
1402  itsGeometryPtr( cloud->itsData->itsGeometry->begin<Geometry>() ),
1403  itsUseIndices( true )
1404 {
1405 }
1406 
1408 {
1409  return false;
1410 }
1411 
1413 {
1414  if( itsUseIndices )
1415  return (*itsIndices)[itsIndex];
1416  else
1417  return itsIndex;
1418 }
1419 
1421 {
1422  ++itsIndex;
1423 }
1424 
1426 {
1427  --itsIndex;
1428 }
1429 
1430 void nrt::PointCloud2::ConstIterator<>::advance( difference_type n )
1431 {
1432  itsIndex += n;
1433 }
1434 
1435 auto nrt::PointCloud2::ConstIterator<>::distance_to( ConstIterator const & other ) const -> difference_type
1436 {
1437  return other.itsIndex - itsIndex;
1438 }
1439 
1440 auto nrt::PointCloud2::ConstIterator<>::equal( ConstIterator const & other ) const -> bool
1441 {
1442  return other.itsIndex == itsIndex;
1443 }
1444 
1446 {
1447  return {itsGeometryPtr[index()]};
1448 }
1449 //! @endcond
1450 
1451 namespace nrt
1452 {
1453  //! Template specialization for ConstIterator with variadic template parameters
1454  /*! Specialization for having specific fields
1455  @anchor ConstIteratorVariadic
1456  @relates ConstIterator */
1457  template <class FirstT, class ... OtherT>
1458  class PointCloud2::ConstIterator<FirstT, OtherT...> :
1459  public boost::iterator_facade<ConstIterator<FirstT, OtherT...>,
1460  PointCloud2ConstDataRef<typename StripSparse<FirstT>::type, typename StripSparse<OtherT>::type...>,
1461  boost::random_access_traversal_tag, PointCloud2ConstDataRef<typename StripSparse<FirstT>::type, typename StripSparse<OtherT>::type...>>
1462  {
1463  public:
1464  //! Creates a dense iterator located at a certain index in the cloud
1465  /*! @param cloud The point cloud
1466  @param index The starting index */
1467  ConstIterator( PointCloud2 const * const cloud, size_t index );
1468 
1469  //! Creates a sparse iterator located at a certain index in the cloud
1470  /*! @param cloud The point cloud
1471  @param index The starting index
1472  @param sparse value ignored */
1473  ConstIterator( PointCloud2 const * const cloud, size_t index, bool sparse );
1474 
1475  //! Creates an iterator over a specific subset of dense points
1476  /*! @param cloud The point cloud
1477  @param index The starting index in the set of indices
1478  @param indices The indices that define a subset */
1479  ConstIterator( PointCloud2 const * const cloud, size_t index, Indices const indices );
1480 
1481  //! Creates an iterator over a specific subset of sparse points
1482  /*! @param cloud The point cloud
1483  @param index The starting index in the set of indices
1484  @param indices The indices that define a subset
1485  @param sparse value ignored */
1486  ConstIterator( PointCloud2 const * const cloud, size_t index, Indices const indices, bool sparse );
1487 
1488  //! Copy construction
1489  ConstIterator( ConstIterator const & other ) = default;
1490 
1491  //! Get whether we contain sparse fields
1492  bool sparse() const;
1493 
1494  //! Get the current index in the point cloud of the iterator
1495  /*! The value returned is not guaranteed to be valid if the iterator
1496  is not within the bounds of the cloud */
1497  size_t index() const;
1498 
1499  //! Used internally by boost
1500  typedef typename boost::iterator_facade<ConstIterator<FirstT, OtherT...>, PointCloud2ConstDataRef<typename StripSparse<FirstT>::type, typename StripSparse<OtherT>::type...>,
1501  boost::random_access_traversal_tag, PointCloud2ConstDataRef<typename StripSparse<FirstT>::type, typename StripSparse<OtherT>::type...>>::
1503 
1504  private:
1505  friend class boost::iterator_core_access;
1506 
1507  void increment();
1508 
1509  void decrement();
1510 
1511  void advance( difference_type n );
1512 
1513  difference_type distance_to( ConstIterator const & other ) const;
1514 
1515  bool equal( ConstIterator const & other ) const;
1516 
1518 
1519  private:
1520  size_t itsIndex; //!< position in either our sparse indices or our dense cloud
1521 
1522  Optional<Indices> itsIndices; //!< Indices we are restricted to visit
1523 
1524  //! A set of pointers to the beginning of the various dense fields, dereferenced to get our data
1525  GenericBag<typename StripSparse<FirstT>::type const *, typename StripSparse<OtherT>::type const *...> itsDensePointers;
1526 
1527  //! A set of references to each specific element in our sparse intersection, including any dense fields
1528  std::vector<GenericBag<typename StripSparse<FirstT>::type const &, typename StripSparse<OtherT>::type const &...>> itsSparseReferences;
1529 
1530  //! Pointer to the beginning of the geometry; used in either sparse or dense case to get the data
1531  Geometry const * itsGeometryPtr;
1532 
1533  bool itsOnlyDense; //!< whether we only consist of dense data
1534 
1535  bool itsUseIndices; //!< whether we use our indices to get our index
1536 
1537  PointCloud2 const * const itsCloud; //!< reference back to the cloud we came from
1538  }; // class ConstIterator
1539 } // namespace nrt
1540 
1541 //! @cond PRIVATE_NEVERDEFINED
1542 template <class FirstT, class ... OtherT> inline
1544  itsIndex( index ),
1545  itsDensePointers( cloud->itsPointerCache.template lookup<pointerHelper<decltype(itsDensePointers), FirstT, OtherT...>, decltype(itsDensePointers)>( cloud ) ),
1546  itsGeometryPtr( cloud->itsData->itsGeometry->template begin<Geometry>() ),
1547  itsOnlyDense( true ),
1548  itsUseIndices( false ),
1549  itsCloud( cloud )
1550 {
1551 }
1552 
1553 template <class FirstT, class ... OtherT> inline
1554 nrt::PointCloud2::ConstIterator<FirstT, OtherT...>::ConstIterator( PointCloud2 const * const cloud, size_t index, bool /*sparse*/ ) :
1555  itsIndex( index ), itsDensePointers(),
1556  itsGeometryPtr( cloud->itsData->itsGeometry->template begin<Geometry>() ),
1557  itsOnlyDense( false ),
1558  itsUseIndices( true ),
1559  itsCloud( cloud )
1560 {
1561  // Establish the strictest set of sparse indices
1562  // (this is the intersection of all indices for each sparse type specified)
1563  std::list<size_t> subset;
1564  bool sparseValid = subsetHelper<FirstT, OtherT...>::strictestSubset( itsCloud, subset ); // TODO: may benefit from caching
1565 
1566  if( not sparseValid )
1567  {
1568  // We couldn't make a valid subset out of these requirements, throw an exception
1569  throw exception::PointCloud2FieldsException();
1570  }
1571 
1572  // copy results to local indices set
1573  itsIndices = Indices();
1574  itsIndices->resize( subset.size() );
1575  std::copy( subset.begin(), subset.end(), itsIndices->begin() );
1576 
1577  // Populate our sparse references
1578  itsSparseReferences.reserve( itsIndices->size() );
1579  for( size_t i = 0, end = itsIndices->size(); i < end; ++i )
1580  {
1581  itsSparseReferences.emplace_back(
1582  lookupHelper<GenericBag<typename StripSparse<FirstT>::type&, typename StripSparse<OtherT>::type&...>, FirstT, OtherT...>::lookupSparseReferences(
1583  const_cast<PointCloud2* const>( itsCloud ), (*itsIndices)[i] ) );
1584  }
1585 
1586  if( index != 0 ) // adjust to fit size of sparse indices
1587  itsIndex = itsIndices->size();
1588 }
1589 
1590 template <class FirstT, class ... OtherT> inline
1591 nrt::PointCloud2::ConstIterator<FirstT, OtherT...>::ConstIterator( PointCloud2 const * const cloud, size_t index, Indices const indices ) :
1592  itsIndex( index ),
1593  itsIndices( indices ),
1594  itsDensePointers( cloud->itsPointerCache.template lookup<pointerHelper<decltype(itsDensePointers), FirstT, OtherT...>, decltype(itsDensePointers)>( cloud ) ),
1595  itsGeometryPtr( cloud->itsData->itsGeometry->template begin<Geometry>() ),
1596  itsOnlyDense( true ),
1597  itsUseIndices( true ),
1598  itsCloud( cloud )
1599 {
1600 }
1601 
1602 template <class FirstT, class ... OtherT> inline
1603 nrt::PointCloud2::ConstIterator<FirstT, OtherT...>::ConstIterator( PointCloud2 const * const cloud, size_t index, Indices const indices, bool /*sparse*/ ) :
1604  itsIndex( index ),
1605  itsIndices( indices ),
1606  itsDensePointers(),
1607  itsGeometryPtr( cloud->itsData->itsGeometry->template begin<Geometry>() ),
1608  itsOnlyDense( false ),
1609  itsUseIndices( true ),
1610  itsCloud( cloud )
1611 {
1612  // Set up all of our sparse references
1613  itsSparseReferences.reserve( itsIndices->size() );
1614  for( size_t i = 0, end = itsIndices->size(); i < end; ++i )
1615  {
1616  itsSparseReferences.emplace_back(
1617  lookupHelper<GenericBag<typename StripSparse<FirstT>::type const &, typename StripSparse<OtherT>::type const &...>, FirstT, OtherT...>::lookupSparseReferences(
1618  const_cast<PointCloud2* const>( itsCloud ), (*itsIndices)[i] ) );
1619  }
1620 }
1621 
1622 template <class FirstT, class ... OtherT> inline
1624 {
1625  return !itsOnlyDense;
1626 }
1627 
1628 template <class FirstT, class ... OtherT> inline
1630 {
1631  if( itsUseIndices )
1632  return (*itsIndices)[itsIndex];
1633  else
1634  return itsIndex;
1635 }
1636 
1637 template <class FirstT, class ... OtherT> inline
1639 {
1640  ++itsIndex;
1641 }
1642 
1643 template <class FirstT, class ... OtherT> inline
1645 {
1646  --itsIndex;
1647 }
1648 
1649 template <class FirstT, class ... OtherT> inline
1651 {
1652  itsIndex += n;
1653 }
1654 
1655 template <class FirstT, class ... OtherT> inline
1656 auto nrt::PointCloud2::ConstIterator<FirstT, OtherT...>::distance_to( ConstIterator const & other ) const -> difference_type
1657 {
1658  return other.itsIndex - itsIndex;
1659 }
1660 
1661 template <class FirstT, class ... OtherT> inline
1662 auto nrt::PointCloud2::ConstIterator<FirstT, OtherT...>::equal( ConstIterator const & other ) const -> bool
1663 {
1664  return other.itsIndex == itsIndex;
1665 }
1666 
1667 template <class FirstT, class ... OtherT> inline
1669  PointCloud2ConstDataRef<typename StripSparse<FirstT>::type, typename StripSparse<OtherT>::type...>
1670 {
1671  const size_t idx = index();
1672 
1673  if( itsOnlyDense )
1674  return {itsGeometryPtr[idx], itsDensePointers.template applyFunction<typename StripSparse<FirstT>::type const &, typename StripSparse<OtherT>::type const &...>( DereferenceIndexFunctor( idx ) )};
1675  else
1676  return {itsGeometryPtr[idx], itsSparseReferences[itsIndex]};
1677 }
1678 //! @endcond
1679 
1680 #endif // INCLUDE_NRT_POINTCLOUD2_DETAILS_POINTCLOUD2IMPL_H
1681 #endif // NRT_HAVE_CLOUD