Point Cloud Library (PCL) 1.12.1
buffers.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2014-, Open Perception, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 */
37
38#ifndef PCL_IO_IMPL_BUFFERS_HPP
39#define PCL_IO_IMPL_BUFFERS_HPP
40
41#include <iostream>
42
43#include <pcl/pcl_macros.h>
44
45
46template <typename T>
48{
49 static T invalid () { return 0; }
50 static bool is_invalid (T value) { return value == invalid (); };
51};
52
53template <>
54struct buffer_traits <float>
55{
56 static float invalid () { return std::numeric_limits<float>::quiet_NaN (); };
57 static bool is_invalid (float value) { return std::isnan (value); };
58};
59
60template <>
61struct buffer_traits <double>
62{
63 static double invalid () { return std::numeric_limits<double>::quiet_NaN (); };
64 static bool is_invalid (double value) { return std::isnan (value); };
65};
66
67
68namespace pcl
69{
70
71namespace io
72{
73
74template <typename T>
75Buffer<T>::Buffer (std::size_t size)
76: size_ (size)
77{
78}
79
80template <typename T>
82{
83}
84
85template <typename T>
87: Buffer<T> (size)
88, data_ (size, buffer_traits<T>::invalid ())
89{
90}
91
92template <typename T>
94{
95}
96
97template <typename T> T
98SingleBuffer<T>::operator[] (std::size_t idx) const
99{
100 assert (idx < size_);
101 return (data_[idx]);
102}
103
104template <typename T> void
105SingleBuffer<T>::push (std::vector<T>& data)
106{
107 assert (data.size () == size_);
108 std::lock_guard<std::mutex> lock (data_mutex_);
109 data_.swap (data);
110 data.clear ();
111}
112
113template <typename T>
115 unsigned char window_size)
116: Buffer<T> (size)
117, window_size_ (window_size)
118, midpoint_ (window_size_ / 2)
119, data_current_idx_ (window_size_ - 1)
120{
121 assert (size_ > 0);
122 assert (window_size_ > 0);
123
124 data_.resize (window_size_);
125 for (std::size_t i = 0; i < window_size_; ++i)
126 data_[i].resize (size_, buffer_traits<T>::invalid ());
127
128 data_argsort_indices_.resize (size_);
129 for (std::size_t i = 0; i < size_; ++i)
130 {
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;
134 }
135
136 data_invalid_count_.resize (size_, window_size_);
137}
138
139template <typename T>
141{
142}
143
144template <typename T> T
145MedianBuffer<T>::operator[] (std::size_t idx) const
146{
147 assert (idx < size_);
148 int midpoint = (window_size_ - data_invalid_count_[idx]) / 2;
149 return (data_[data_argsort_indices_[idx][midpoint]][idx]);
150}
151
152template <typename T> void
153MedianBuffer<T>::push (std::vector<T>& data)
154{
155 assert (data.size () == size_);
156 std::lock_guard<std::mutex> lock (data_mutex_);
157
158 if (++data_current_idx_ >= window_size_)
159 data_current_idx_ = 0;
160
161 // New data will replace the column with index data_current_idx_. Before
162 // overwriting it, we go through all the new-old value pairs and update
163 // data_argsort_indices_ to maintain sorted order.
164 for (std::size_t i = 0; i < size_; ++i)
165 {
166 const T& new_value = data[i];
167 const T& old_value = data_[data_current_idx_][i];
168 bool new_is_invalid = buffer_traits<T>::is_invalid (new_value);
169 bool old_is_invalid = buffer_traits<T>::is_invalid (old_value);
170 if (compare (new_value, old_value) == 0)
171 continue;
172 std::vector<unsigned char>& argsort_indices = data_argsort_indices_[i];
173 // Rewrite the argsort indices before or after the position where we insert
174 // depending on the relation between the old and new values
175 if (compare (new_value, old_value) == 1)
176 {
177 for (int j = 0; j < window_size_; ++j)
178 if (argsort_indices[j] == data_current_idx_)
179 {
180 int k = j + 1;
181 while (k < window_size_ && compare (new_value, data_[argsort_indices[k]][i]) == 1)
182 {
183 std::swap (argsort_indices[k - 1], argsort_indices[k]);
184 ++k;
185 }
186 break;
187 }
188 }
189 else
190 {
191 for (int j = window_size_ - 1; j >= 0; --j)
192 if (argsort_indices[j] == data_current_idx_)
193 {
194 int k = j - 1;
195 while (k >= 0 && compare (new_value, data_[argsort_indices[k]][i]) == -1)
196 {
197 std::swap (argsort_indices[k], argsort_indices[k + 1]);
198 --k;
199 }
200 break;
201 }
202 }
203
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];
208 }
209
210 // Finally overwrite the data
211 data_[data_current_idx_].swap (data);
212 data.clear ();
213}
214
215template <typename T> int
217{
218 bool a_is_invalid = buffer_traits<T>::is_invalid (a);
219 bool b_is_invalid = buffer_traits<T>::is_invalid (b);
220 if (a_is_invalid && b_is_invalid)
221 return 0;
222 if (a_is_invalid)
223 return 1;
224 if (b_is_invalid)
225 return -1;
226 if (a == b)
227 return 0;
228 return a > b ? 1 : -1;
229}
230
231template <typename T>
233 unsigned char window_size)
234: Buffer<T> (size)
235, window_size_ (window_size)
236, data_current_idx_ (window_size_ - 1)
237{
238 assert (size_ > 0);
239 assert (window_size_ > 0);
240
241 data_.resize (window_size_);
242 for (std::size_t i = 0; i < window_size_; ++i)
243 data_[i].resize (size_, buffer_traits<T>::invalid ());
244
245 data_sum_.resize (size_, 0);
246 data_invalid_count_.resize (size_, window_size_);
247}
248
249template <typename T>
251{
252}
253
254template <typename T> T
255AverageBuffer<T>::operator[] (std::size_t idx) const
256{
257 assert (idx < size_);
258 if (data_invalid_count_[idx] == window_size_)
259 return (buffer_traits<T>::invalid ());
260 return (data_sum_[idx] / static_cast<T> (window_size_ - data_invalid_count_[idx]));
261}
262
263template <typename T> void
264AverageBuffer<T>::push (std::vector<T>& data)
265{
266 assert (data.size () == size_);
267 std::lock_guard<std::mutex> lock (data_mutex_);
268
269 if (++data_current_idx_ >= window_size_)
270 data_current_idx_ = 0;
271
272 // New data will replace the column with index data_current_idx_. Before
273 // overwriting it, we go through the old values and subtract them from the
274 // data_sum_
275 for (std::size_t i = 0; i < size_; ++i)
276 {
277 const float& new_value = data[i];
278 const float& old_value = data_[data_current_idx_][i];
279 bool new_is_invalid = buffer_traits<T>::is_invalid (new_value);
280 bool old_is_invalid = buffer_traits<T>::is_invalid (old_value);
281
282 if (!old_is_invalid)
283 data_sum_[i] -= old_value;
284 if (!new_is_invalid)
285 data_sum_[i] += new_value;
286
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];
291 }
292
293 // Finally overwrite the data
294 data_[data_current_idx_].swap (data);
295 data.clear ();
296}
297
298} // namespace io
299} // namespace pcl
300
301#endif /* PCL_IO_IMPL_BUFFERS_HPP */
302
void push(std::vector< T > &data) override
Insert a new chunk of data into the buffer.
Definition: buffers.hpp:264
AverageBuffer(std::size_t size, unsigned char window_size)
Construct a buffer of given size with given running window size.
Definition: buffers.hpp:232
T operator[](std::size_t idx) const override
Access an element at a given index.
Definition: buffers.hpp:255
An abstract base class for fixed-size data buffers.
Definition: buffers.h:63
const std::size_t size_
Definition: buffers.h:95
Buffer(std::size_t size)
Definition: buffers.hpp:75
virtual ~Buffer()
Definition: buffers.hpp:81
A buffer that computes running window median of the data inserted.
Definition: buffers.h:144
MedianBuffer(std::size_t size, unsigned char window_size)
Construct a buffer of given size with given running window size.
Definition: buffers.hpp:114
void push(std::vector< T > &data) override
Insert a new chunk of data into the buffer.
Definition: buffers.hpp:153
T operator[](std::size_t idx) const override
Access an element at a given index.
Definition: buffers.hpp:145
void push(std::vector< T > &data) override
Insert a new chunk of data into the buffer.
Definition: buffers.hpp:105
T operator[](std::size_t idx) const override
Access an element at a given index.
Definition: buffers.hpp:98
SingleBuffer(std::size_t size)
Construct a buffer of given size.
Definition: buffers.hpp:86
Defines all the PCL and non-PCL macros used.
static bool is_invalid(double value)
Definition: buffers.hpp:64
static double invalid()
Definition: buffers.hpp:63
static float invalid()
Definition: buffers.hpp:56
static bool is_invalid(float value)
Definition: buffers.hpp:57
static T invalid()
Definition: buffers.hpp:49
static bool is_invalid(T value)
Definition: buffers.hpp:50