38 #ifndef PCL_IO_IMPL_BUFFERS_HPP 39 #define PCL_IO_IMPL_BUFFERS_HPP 44 #include <pcl/pcl_macros.h> 56 static float invalid () {
return std::numeric_limits<float>::quiet_NaN (); };
57 static bool is_invalid (
float value) {
return pcl_isnan (value); };
63 static double invalid () {
return std::numeric_limits<double>::quiet_NaN (); };
64 static bool is_invalid (
double value) {
return pcl_isnan (value); };
90 template <
typename T> T
97 template <
typename T>
void 100 assert (data.size () == size_);
101 boost::mutex::scoped_lock lock (data_mutex_);
106 template <
typename T>
108 unsigned char window_size)
110 , window_size_ (window_size)
111 , midpoint_ (window_size_ / 2)
112 , data_current_idx_ (window_size_ - 1)
115 assert (window_size_ > 0);
117 data_.resize (window_size_);
118 for (
size_t i = 0; i < window_size_; ++i)
121 data_argsort_indices_.resize (
size_);
122 for (
size_t i = 0; i <
size_; ++i)
124 data_argsort_indices_[i].resize (window_size_);
125 for (
size_t j = 0; j < window_size_; ++j)
126 data_argsort_indices_[i][j] = j;
129 data_invalid_count_.resize (
size_, window_size_);
132 template <
typename T>
137 template <
typename T> T
140 assert (idx < size_);
141 int midpoint = (window_size_ - data_invalid_count_[idx]) / 2;
142 return (data_[data_argsort_indices_[idx][midpoint]][idx]);
145 template <
typename T>
void 148 assert (data.size () == size_);
149 boost::mutex::scoped_lock lock (data_mutex_);
151 if (++data_current_idx_ >= window_size_)
152 data_current_idx_ = 0;
157 for (
size_t i = 0; i < size_; ++i)
159 const T& new_value = data[i];
160 const T& old_value = data_[data_current_idx_][i];
163 if (compare (new_value, old_value) == 0)
165 std::vector<unsigned char>& argsort_indices = data_argsort_indices_[i];
168 if (compare (new_value, old_value) == 1)
170 for (
int j = 0; j < window_size_; ++j)
171 if (argsort_indices[j] == data_current_idx_)
174 while (k < window_size_ && compare (new_value, data_[argsort_indices[k]][i]) == 1)
176 std::swap (argsort_indices[k - 1], argsort_indices[k]);
184 for (
int j = window_size_ - 1; j >= 0; --j)
185 if (argsort_indices[j] == data_current_idx_)
188 while (k >= 0 && compare (new_value, data_[argsort_indices[k]][i]) == -1)
190 std::swap (argsort_indices[k], argsort_indices[k + 1]);
197 if (new_is_invalid && !old_is_invalid)
198 ++data_invalid_count_[i];
199 else if (!new_is_invalid && old_is_invalid)
200 --data_invalid_count_[i];
204 data_[data_current_idx_].swap (data);
208 template <
typename T>
int 213 if (a_is_invalid && b_is_invalid)
221 return a > b ? 1 : -1;
224 template <
typename T>
226 unsigned char window_size)
228 , window_size_ (window_size)
229 , data_current_idx_ (window_size_ - 1)
232 assert (window_size_ > 0);
234 data_.resize (window_size_);
235 for (
size_t i = 0; i < window_size_; ++i)
238 data_sum_.resize (
size_, 0);
239 data_invalid_count_.resize (
size_, window_size_);
242 template <
typename T>
247 template <
typename T> T
250 assert (idx < size_);
251 if (data_invalid_count_[idx] == window_size_)
254 return (data_sum_[idx] / static_cast<T> (window_size_ - data_invalid_count_[idx]));
257 template <
typename T>
void 260 assert (data.size () == size_);
261 boost::mutex::scoped_lock lock (data_mutex_);
263 if (++data_current_idx_ >= window_size_)
264 data_current_idx_ = 0;
269 for (
size_t i = 0; i < size_; ++i)
271 const float& new_value = data[i];
272 const float& old_value = data_[data_current_idx_][i];
277 data_sum_[i] -= old_value;
279 data_sum_[i] += new_value;
281 if (new_is_invalid && !old_is_invalid)
282 ++data_invalid_count_[i];
283 else if (!new_is_invalid && old_is_invalid)
284 --data_invalid_count_[i];
288 data_[data_current_idx_].swap (data);
AverageBuffer(size_t size, unsigned char window_size)
Construct a buffer of given size with given running window size.
static bool is_invalid(T value)
SingleBuffer(size_t size)
Construct a buffer of given size.
virtual T operator[](size_t idx) const
Access an element at a given index.
virtual void push(std::vector< T > &data)
Insert a new chunk of data into the buffer.
static bool is_invalid(float value)
static bool is_invalid(double value)
virtual void push(std::vector< T > &data)
Insert a new chunk of data into the buffer.
An abstract base class for fixed-size data buffers.
virtual T operator[](size_t idx) const
Access an element at a given index.