Gazebo Math

API Reference

6.15.1
gz/math/MovingWindowFilter.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef GZ_MATH_MOVINGWINDOWFILTER_HH_
18 #define GZ_MATH_MOVINGWINDOWFILTER_HH_
19 
20 #include <memory>
21 #include <vector>
22 #include "gz/math/Export.hh"
23 
24 namespace ignition
25 {
26  namespace math
27  {
28  // Inline bracket to help doxygen filtering.
29  inline namespace IGNITION_MATH_VERSION_NAMESPACE {
30  //
31 
35  template< typename T>
36  class MovingWindowFilterPrivate
37  {
38  // \brief Constructor
39  public: MovingWindowFilterPrivate();
40 
42  public: unsigned int valWindowSize = 4;
43 
45  public: std::vector<T> valHistory;
46 
48  public: typename std::vector<T>::iterator valIter;
49 
51  public: T sum;
52 
54  public: unsigned int samples = 0;
55  };
56 
58  template<typename T>
59  MovingWindowFilterPrivate<T>::MovingWindowFilterPrivate()
60  {
62  this->valHistory.resize(this->valWindowSize);
63  this->valIter = this->valHistory.begin();
64  this->sum = T();
65  }
67 
72  template< typename T>
74  {
76  public: MovingWindowFilter();
77 
79  public: virtual ~MovingWindowFilter();
80 
83  public: void Update(const T _val);
84 
87  public: void SetWindowSize(const unsigned int _n);
88 
91  public: unsigned int WindowSize() const;
92 
95  public: bool WindowFilled() const;
96 
99  public: T Value() const;
100 
103  };
104 
106  template<typename T>
108  : dataPtr(new MovingWindowFilterPrivate<T>())
109  {
110  }
111 
113  template<typename T>
115  {
116  this->dataPtr->valHistory.clear();
117  }
118 
120  template<typename T>
121  void MovingWindowFilter<T>::Update(const T _val)
122  {
123  // update sum and sample size with incoming _val
124 
125  // keep running sum
126  this->dataPtr->sum += _val;
127 
128  // shift pointer, wrap around if end has been reached.
129  ++this->dataPtr->valIter;
130  if (this->dataPtr->valIter == this->dataPtr->valHistory.end())
131  {
132  // reset iterator to beginning of queue
133  this->dataPtr->valIter = this->dataPtr->valHistory.begin();
134  }
135 
136  // increment sample size
137  ++this->dataPtr->samples;
138 
139  if (this->dataPtr->samples > this->dataPtr->valWindowSize)
140  {
141  // subtract old value if buffer already filled
142  this->dataPtr->sum -= (*this->dataPtr->valIter);
143  // put new value into queue
144  (*this->dataPtr->valIter) = _val;
145  // reduce sample size
146  --this->dataPtr->samples;
147  }
148  else
149  {
150  // put new value into queue
151  (*this->dataPtr->valIter) = _val;
152  }
153  }
154 
156  template<typename T>
157  void MovingWindowFilter<T>::SetWindowSize(const unsigned int _n)
158  {
159  this->dataPtr->valWindowSize = _n;
160  this->dataPtr->valHistory.clear();
161  this->dataPtr->valHistory.resize(this->dataPtr->valWindowSize);
162  this->dataPtr->valIter = this->dataPtr->valHistory.begin();
163  this->dataPtr->sum = T();
164  this->dataPtr->samples = 0;
165  }
166 
168  template<typename T>
170  {
171  return this->dataPtr->valWindowSize;
172  }
173 
175  template<typename T>
177  {
178  return this->dataPtr->samples == this->dataPtr->valWindowSize;
179  }
180 
182  template<typename T>
184  {
185  return this->dataPtr->sum / static_cast<double>(this->dataPtr->samples);
186  }
187  }
188  }
189 }
190 #endif
Base class for MovingWindowFilter. This replaces the version of MovingWindowFilter in the Ignition Co...
Definition: gz/math/MovingWindowFilter.hh:73
T resize(T... args)
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
T Value() const
Get filtered result.
Definition: gz/math/MovingWindowFilter.hh:183
MovingWindowFilter()
Constructor.
Definition: gz/math/MovingWindowFilter.hh:107
STL class.
void Update(const T _val)
Update value of filter.
Definition: gz/math/MovingWindowFilter.hh:121
void SetWindowSize(const unsigned int _n)
Set window size.
Definition: gz/math/MovingWindowFilter.hh:157
unsigned int WindowSize() const
Get the window size.
Definition: gz/math/MovingWindowFilter.hh:169
bool WindowFilled() const
Get whether the window has been filled.
Definition: gz/math/MovingWindowFilter.hh:176
virtual ~MovingWindowFilter()
Destructor.
Definition: gz/math/MovingWindowFilter.hh:114
STL class.