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); };
97 template <
typename T> T
100 assert (idx < size_);
104 template <
typename T>
void
107 assert (data.size () == size_);
108 std::lock_guard<std::mutex> lock (data_mutex_);
113 template <
typename T>
115 unsigned char window_size)
117 , window_size_ (window_size)
118 , midpoint_ (window_size_ / 2)
119 , data_current_idx_ (window_size_ - 1)
122 assert (window_size_ > 0);
124 data_.resize (window_size_);
125 for (std::size_t i = 0; i < window_size_; ++i)
128 data_argsort_indices_.resize (
size_);
129 for (std::size_t i = 0; i <
size_; ++i)
131 data_argsort_indices_[i].resize (window_size_);
132 for (std::size_t j = 0; j < window_size_; ++j)
133 data_argsort_indices_[i][j] = j;
136 data_invalid_count_.resize (
size_, window_size_);
139 template <
typename T>
144 template <
typename T> T
147 assert (idx < size_);
148 int midpoint = (window_size_ - data_invalid_count_[idx]) / 2;
149 return (data_[data_argsort_indices_[idx][midpoint]][idx]);
152 template <
typename T>
void
155 assert (data.size () == size_);
156 std::lock_guard<std::mutex> lock (data_mutex_);
158 if (++data_current_idx_ >= window_size_)
159 data_current_idx_ = 0;
164 for (std::size_t i = 0; i < size_; ++i)
166 const T& new_value = data[i];
167 const T& old_value = data_[data_current_idx_][i];
170 if (compare (new_value, old_value) == 0)
172 std::vector<unsigned char>& argsort_indices = data_argsort_indices_[i];
175 if (compare (new_value, old_value) == 1)
177 for (
int j = 0; j < window_size_; ++j)
178 if (argsort_indices[j] == data_current_idx_)
181 while (k < window_size_ && compare (new_value, data_[argsort_indices[k]][i]) == 1)
183 std::swap (argsort_indices[k - 1], argsort_indices[k]);
191 for (
int j = window_size_ - 1; j >= 0; --j)
192 if (argsort_indices[j] == data_current_idx_)
195 while (k >= 0 && compare (new_value, data_[argsort_indices[k]][i]) == -1)
197 std::swap (argsort_indices[k], argsort_indices[k + 1]);
204 if (new_is_invalid && !old_is_invalid)
205 ++data_invalid_count_[i];
206 else if (!new_is_invalid && old_is_invalid)
207 --data_invalid_count_[i];
211 data_[data_current_idx_].swap (data);
215 template <
typename T>
int
220 if (a_is_invalid && b_is_invalid)
228 return a > b ? 1 : -1;
231 template <
typename T>
233 unsigned char window_size)
235 , window_size_ (window_size)
236 , data_current_idx_ (window_size_ - 1)
239 assert (window_size_ > 0);
241 data_.resize (window_size_);
242 for (std::size_t i = 0; i < window_size_; ++i)
245 data_sum_.resize (
size_, 0);
246 data_invalid_count_.resize (
size_, window_size_);
249 template <
typename T>
254 template <
typename T> T
257 assert (idx < size_);
258 if (data_invalid_count_[idx] == window_size_)
260 return (data_sum_[idx] /
static_cast<T
> (window_size_ - data_invalid_count_[idx]));
263 template <
typename T>
void
266 assert (data.size () == size_);
267 std::lock_guard<std::mutex> lock (data_mutex_);
269 if (++data_current_idx_ >= window_size_)
270 data_current_idx_ = 0;
275 for (std::size_t i = 0; i < size_; ++i)
277 const float& new_value = data[i];
278 const float& old_value = data_[data_current_idx_][i];
283 data_sum_[i] -= old_value;
285 data_sum_[i] += new_value;
287 if (new_is_invalid && !old_is_invalid)
288 ++data_invalid_count_[i];
289 else if (!new_is_invalid && old_is_invalid)
290 --data_invalid_count_[i];
294 data_[data_current_idx_].swap (data);