38 #ifndef PCL_IO_IMPL_BUFFERS_HPP
39 #define PCL_IO_IMPL_BUFFERS_HPP
56 static float invalid () {
return std::numeric_limits<float>::quiet_NaN (); };
57 static bool is_invalid (
float value) {
return std::isnan (value); };
63 static double invalid () {
return std::numeric_limits<double>::quiet_NaN (); };
64 static bool is_invalid (
double value) {
return std::isnan (value); };
90 template <
typename T> T
97 template <
typename T>
void
100 assert (data.size () == size_);
101 std::lock_guard<std::mutex> 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 (std::size_t i = 0; i < window_size_; ++i)
121 data_argsort_indices_.resize (
size_);
122 for (std::size_t i = 0; i <
size_; ++i)
124 data_argsort_indices_[i].resize (window_size_);
125 for (std::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 std::lock_guard<std::mutex> lock (data_mutex_);
151 if (++data_current_idx_ >= window_size_)
152 data_current_idx_ = 0;
157 for (std::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 (std::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_)
253 return (data_sum_[idx] /
static_cast<T
> (window_size_ - data_invalid_count_[idx]));
256 template <
typename T>
void
259 assert (data.size () == size_);
260 std::lock_guard<std::mutex> lock (data_mutex_);
262 if (++data_current_idx_ >= window_size_)
263 data_current_idx_ = 0;
268 for (std::size_t i = 0; i < size_; ++i)
270 const float& new_value = data[i];
271 const float& old_value = data_[data_current_idx_][i];
276 data_sum_[i] -= old_value;
278 data_sum_[i] += new_value;
280 if (new_is_invalid && !old_is_invalid)
281 ++data_invalid_count_[i];
282 else if (!new_is_invalid && old_is_invalid)
283 --data_invalid_count_[i];
287 data_[data_current_idx_].swap (data);