38 #ifndef PCL_IO_IMPL_BUFFERS_HPP 39 #define PCL_IO_IMPL_BUFFERS_HPP 57 static float invalid () {
return std::numeric_limits<float>::quiet_NaN (); };
58 static bool is_invalid (
float value) {
return std::isnan (value); };
64 static double invalid () {
return std::numeric_limits<double>::quiet_NaN (); };
65 static bool is_invalid (
double value) {
return std::isnan (value); };
98 template <
typename T> T
101 assert (idx < size_);
105 template <
typename T>
void 108 assert (data.size () == size_);
109 std::lock_guard<std::mutex> lock (data_mutex_);
114 template <
typename T>
116 unsigned char window_size)
118 , window_size_ (window_size)
119 , midpoint_ (window_size_ / 2)
120 , data_current_idx_ (window_size_ - 1)
123 assert (window_size_ > 0);
125 data_.resize (window_size_);
126 for (std::size_t i = 0; i < window_size_; ++i)
129 data_argsort_indices_.resize (
size_);
130 for (std::size_t i = 0; i <
size_; ++i)
132 data_argsort_indices_[i].resize (window_size_);
133 for (std::size_t j = 0; j < window_size_; ++j)
134 data_argsort_indices_[i][j] = j;
137 data_invalid_count_.resize (
size_, window_size_);
140 template <
typename T>
145 template <
typename T> T
148 assert (idx < size_);
149 int midpoint = (window_size_ - data_invalid_count_[idx]) / 2;
150 return (data_[data_argsort_indices_[idx][midpoint]][idx]);
153 template <
typename T>
void 156 assert (data.size () == size_);
157 std::lock_guard<std::mutex> lock (data_mutex_);
159 if (++data_current_idx_ >= window_size_)
160 data_current_idx_ = 0;
165 for (std::size_t i = 0; i < size_; ++i)
167 const T& new_value = data[i];
168 const T& old_value = data_[data_current_idx_][i];
171 if (compare (new_value, old_value) == 0)
173 std::vector<unsigned char>& argsort_indices = data_argsort_indices_[i];
176 if (compare (new_value, old_value) == 1)
178 for (
int j = 0; j < window_size_; ++j)
179 if (argsort_indices[j] == data_current_idx_)
182 while (k < window_size_ && compare (new_value, data_[argsort_indices[k]][i]) == 1)
184 std::swap (argsort_indices[k - 1], argsort_indices[k]);
192 for (
int j = window_size_ - 1; j >= 0; --j)
193 if (argsort_indices[j] == data_current_idx_)
196 while (k >= 0 && compare (new_value, data_[argsort_indices[k]][i]) == -1)
198 std::swap (argsort_indices[k], argsort_indices[k + 1]);
205 if (new_is_invalid && !old_is_invalid)
206 ++data_invalid_count_[i];
207 else if (!new_is_invalid && old_is_invalid)
208 --data_invalid_count_[i];
212 data_[data_current_idx_].swap (data);
216 template <
typename T>
int 221 if (a_is_invalid && b_is_invalid)
229 return a > b ? 1 : -1;
232 template <
typename T>
234 unsigned char window_size)
236 , window_size_ (window_size)
237 , data_current_idx_ (window_size_ - 1)
240 assert (window_size_ > 0);
242 data_.resize (window_size_);
243 for (std::size_t i = 0; i < window_size_; ++i)
246 data_sum_.resize (
size_, 0);
247 data_invalid_count_.resize (
size_, window_size_);
250 template <
typename T>
255 template <
typename T> T
258 assert (idx < size_);
259 if (data_invalid_count_[idx] == window_size_)
261 return (data_sum_[idx] / static_cast<T> (window_size_ - data_invalid_count_[idx]));
264 template <
typename T>
void 267 assert (data.size () == size_);
268 std::lock_guard<std::mutex> lock (data_mutex_);
270 if (++data_current_idx_ >= window_size_)
271 data_current_idx_ = 0;
276 for (std::size_t i = 0; i < size_; ++i)
278 const float& new_value = data[i];
279 const float& old_value = data_[data_current_idx_][i];
284 data_sum_[i] -= old_value;
286 data_sum_[i] += new_value;
288 if (new_is_invalid && !old_is_invalid)
289 ++data_invalid_count_[i];
290 else if (!new_is_invalid && old_is_invalid)
291 --data_invalid_count_[i];
295 data_[data_current_idx_].swap (data);
AverageBuffer(std::size_t size, unsigned char window_size)
Construct a buffer of given size with given running window size.
void push(std::vector< T > &data) override
Insert a new chunk of data into the buffer.
static bool is_invalid(T value)
SingleBuffer(std::size_t size)
Construct a buffer of given size.
static bool is_invalid(float value)
T operator[](std::size_t idx) const override
Access an element at a given index.
void push(std::vector< T > &data) override
Insert a new chunk of data into the buffer.
static bool is_invalid(double value)
T operator[](std::size_t idx) const override
Access an element at a given index.
An abstract base class for fixed-size data buffers.
Defines all the PCL and non-PCL macros used.