iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
PassThroughImpl.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 #ifndef INCLUDE_NRT_POINTCLOUD2_FILTER_DETAILS_PASSTHROUGHFILTERIMPL_H
36 #define INCLUDE_NRT_POINTCLOUD2_FILTER_DETAILS_PASSTHROUGHFILTERIMPL_H
37 
38 template <class Field> inline
39 auto nrt::PassThroughFilter::filter( PointCloud2 const input,
40  std::function<bool(Field const &)> && func,
41  Field const & replaceValue ) -> PointCloud2
42 {
43  PointCloud2 output = input;
44 
45  output.quickRemove( filterImpl( input.template size<Field>(),
46  output.template end<Field>(),
47  std::move( func ),
48  replaceValue ), true );
49 
50  return output;
51 }
52 
53 template <class Field> inline
54 auto nrt::PassThroughFilter::filter( PointCloud2 const input,
55  Indices const indices,
56  std::function<bool(Field const &)> && func,
57  Field const & replaceValue ) -> PointCloud2
58 {
59  PointCloud2 output = input;
60 
61  output.quickRemove( filterImpl( indices.size(), // no need to template this, subset will conform or throw
62  output.template subset_end<Field>(),
63  std::move( func ),
64  replaceValue ), true );
65 
66  return output;
67 }
68 
69 template <class Field> inline
70 auto nrt::PassThroughFilter::filterImpl( size_t const inputSize,
71  PointCloud2::Iterator<Field> outputEnd,
72  std::function<bool(Field const &)> && func,
73  Field const & replaceValue ) -> Indices
74 {
75  // things we'll end up removing
76  Indices toRemove;
77  toRemove.reserve( inputSize );
78 
79  // Adjust iterator to point to last element
80  --outputEnd;
81 
82  // Iterate in reverse order so that when we perform quickRemove
83  for( int i = inputSize - 1; i >= 0; --i, --outputEnd )
84  {
85  // F I Output
86  // 0 0 0 We'll flip whenever invert is set to true,
87  // 0 1 1 so this ends up looking just like xor
88  // 1 0 1
89  // 1 1 0
90  const bool result = func( outputEnd->template get<Field>() ) xor itsInvertResult;
91 
92  if( !result )
93  {
94  if( itsReplaceValue )
95  outputEnd->template get<Field>() = replaceValue;
96  else
97  toRemove.push_back( outputEnd.index() ); // pull index directly since we might not be traversing in order
98  }
99  }
100 
101  return toRemove;
102 }
103 
104 #endif // INCLUDE_NRT_POINTCLOUD_FILTER_DETAILS_PASSTHROUGHFILTERIMPL_H