Point Cloud Library (PCL)  1.11.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
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 #include <cstring>
43 
44 #include <pcl/pcl_macros.h>
45 
46 
47 template <typename T>
49 {
50  static T invalid () { return 0; }
51  static bool is_invalid (T value) { return value == invalid (); };
52 };
53 
54 template <>
55 struct buffer_traits <float>
56 {
57  static float invalid () { return std::numeric_limits<float>::quiet_NaN (); };
58  static bool is_invalid (float value) { return std::isnan (value); };
59 };
60 
61 template <>
62 struct buffer_traits <double>
63 {
64  static double invalid () { return std::numeric_limits<double>::quiet_NaN (); };
65  static bool is_invalid (double value) { return std::isnan (value); };
66 };
67 
68 
69 namespace pcl
70 {
71 
72 namespace io
73 {
74 
75 template <typename T>
76 Buffer<T>::Buffer (std::size_t size)
77 : size_ (size)
78 {
79 }
80 
81 template <typename T>
83 {
84 }
85 
86 template <typename T>
88 : Buffer<T> (size)
89 , data_ (size, buffer_traits<T>::invalid ())
90 {
91 }
92 
93 template <typename T>
95 {
96 }
97 
98 template <typename T> T
99 SingleBuffer<T>::operator[] (std::size_t idx) const
100 {
101  assert (idx < size_);
102  return (data_[idx]);
103 }
104 
105 template <typename T> void
106 SingleBuffer<T>::push (std::vector<T>& data)
107 {
108  assert (data.size () == size_);
109  std::lock_guard<std::mutex> lock (data_mutex_);
110  data_.swap (data);
111  data.clear ();
112 }
113 
114 template <typename T>
116  unsigned char window_size)
117 : Buffer<T> (size)
118 , window_size_ (window_size)
119 , midpoint_ (window_size_ / 2)
120 , data_current_idx_ (window_size_ - 1)
121 {
122  assert (size_ > 0);
123  assert (window_size_ > 0);
124 
125  data_.resize (window_size_);
126  for (std::size_t i = 0; i < window_size_; ++i)
127  data_[i].resize (size_, buffer_traits<T>::invalid ());
128 
129  data_argsort_indices_.resize (size_);
130  for (std::size_t i = 0; i < size_; ++i)
131  {
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;
135  }
136 
137  data_invalid_count_.resize (size_, window_size_);
138 }
139 
140 template <typename T>
142 {
143 }
144 
145 template <typename T> T
146 MedianBuffer<T>::operator[] (std::size_t idx) const
147 {
148  assert (idx < size_);
149  int midpoint = (window_size_ - data_invalid_count_[idx]) / 2;
150  return (data_[data_argsort_indices_[idx][midpoint]][idx]);
151 }
152 
153 template <typename T> void
154 MedianBuffer<T>::push (std::vector<T>& data)
155 {
156  assert (data.size () == size_);
157  std::lock_guard<std::mutex> lock (data_mutex_);
158 
159  if (++data_current_idx_ >= window_size_)
160  data_current_idx_ = 0;
161 
162  // New data will replace the column with index data_current_idx_. Before
163  // overwriting it, we go through all the new-old value pairs and update
164  // data_argsort_indices_ to maintain sorted order.
165  for (std::size_t i = 0; i < size_; ++i)
166  {
167  const T& new_value = data[i];
168  const T& old_value = data_[data_current_idx_][i];
169  bool new_is_invalid = buffer_traits<T>::is_invalid (new_value);
170  bool old_is_invalid = buffer_traits<T>::is_invalid (old_value);
171  if (compare (new_value, old_value) == 0)
172  continue;
173  std::vector<unsigned char>& argsort_indices = data_argsort_indices_[i];
174  // Rewrite the argsort indices before or after the position where we insert
175  // depending on the relation between the old and new values
176  if (compare (new_value, old_value) == 1)
177  {
178  for (int j = 0; j < window_size_; ++j)
179  if (argsort_indices[j] == data_current_idx_)
180  {
181  int k = j + 1;
182  while (k < window_size_ && compare (new_value, data_[argsort_indices[k]][i]) == 1)
183  {
184  std::swap (argsort_indices[k - 1], argsort_indices[k]);
185  ++k;
186  }
187  break;
188  }
189  }
190  else
191  {
192  for (int j = window_size_ - 1; j >= 0; --j)
193  if (argsort_indices[j] == data_current_idx_)
194  {
195  int k = j - 1;
196  while (k >= 0 && compare (new_value, data_[argsort_indices[k]][i]) == -1)
197  {
198  std::swap (argsort_indices[k], argsort_indices[k + 1]);
199  --k;
200  }
201  break;
202  }
203  }
204 
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];
209  }
210 
211  // Finally overwrite the data
212  data_[data_current_idx_].swap (data);
213  data.clear ();
214 }
215 
216 template <typename T> int
217 MedianBuffer<T>::compare (T a, T b)
218 {
219  bool a_is_invalid = buffer_traits<T>::is_invalid (a);
220  bool b_is_invalid = buffer_traits<T>::is_invalid (b);
221  if (a_is_invalid && b_is_invalid)
222  return 0;
223  if (a_is_invalid)
224  return 1;
225  if (b_is_invalid)
226  return -1;
227  if (a == b)
228  return 0;
229  return a > b ? 1 : -1;
230 }
231 
232 template <typename T>
234  unsigned char window_size)
235 : Buffer<T> (size)
236 , window_size_ (window_size)
237 , data_current_idx_ (window_size_ - 1)
238 {
239  assert (size_ > 0);
240  assert (window_size_ > 0);
241 
242  data_.resize (window_size_);
243  for (std::size_t i = 0; i < window_size_; ++i)
244  data_[i].resize (size_, buffer_traits<T>::invalid ());
245 
246  data_sum_.resize (size_, 0);
247  data_invalid_count_.resize (size_, window_size_);
248 }
249 
250 template <typename T>
252 {
253 }
254 
255 template <typename T> T
256 AverageBuffer<T>::operator[] (std::size_t idx) const
257 {
258  assert (idx < size_);
259  if (data_invalid_count_[idx] == window_size_)
260  return (buffer_traits<T>::invalid ());
261  return (data_sum_[idx] / static_cast<T> (window_size_ - data_invalid_count_[idx]));
262 }
263 
264 template <typename T> void
265 AverageBuffer<T>::push (std::vector<T>& data)
266 {
267  assert (data.size () == size_);
268  std::lock_guard<std::mutex> lock (data_mutex_);
269 
270  if (++data_current_idx_ >= window_size_)
271  data_current_idx_ = 0;
272 
273  // New data will replace the column with index data_current_idx_. Before
274  // overwriting it, we go through the old values and subtract them from the
275  // data_sum_
276  for (std::size_t i = 0; i < size_; ++i)
277  {
278  const float& new_value = data[i];
279  const float& old_value = data_[data_current_idx_][i];
280  bool new_is_invalid = buffer_traits<T>::is_invalid (new_value);
281  bool old_is_invalid = buffer_traits<T>::is_invalid (old_value);
282 
283  if (!old_is_invalid)
284  data_sum_[i] -= old_value;
285  if (!new_is_invalid)
286  data_sum_[i] += new_value;
287 
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];
292  }
293 
294  // Finally overwrite the data
295  data_[data_current_idx_].swap (data);
296  data.clear ();
297 }
298 
299 } // namespace io
300 } // namespace pcl
301 
302 #endif /* PCL_IO_IMPL_BUFFERS_HPP */
303 
AverageBuffer(std::size_t size, unsigned char window_size)
Construct a buffer of given size with given running window size.
Definition: buffers.hpp:233
void push(std::vector< T > &data) override
Insert a new chunk of data into the buffer.
Definition: buffers.hpp:154
const std::size_t size_
Definition: buffers.h:99
static float invalid()
Definition: buffers.hpp:57
static T invalid()
Definition: buffers.hpp:50
void push(std::vector< T > &data) override
Insert a new chunk of data into the buffer.
Definition: buffers.hpp:265
static double invalid()
Definition: buffers.hpp:64
static bool is_invalid(T value)
Definition: buffers.hpp:51
T operator[](std::size_t idx) const override
Access an element at a given index.
Definition: buffers.hpp:146
A buffer that computes running window median of the data inserted.
Definition: buffers.h:147
SingleBuffer(std::size_t size)
Construct a buffer of given size.
Definition: buffers.hpp:87
static bool is_invalid(float value)
Definition: buffers.hpp:58
T operator[](std::size_t idx) const override
Access an element at a given index.
Definition: buffers.hpp:99
void push(std::vector< T > &data) override
Insert a new chunk of data into the buffer.
Definition: buffers.hpp:106
static bool is_invalid(double value)
Definition: buffers.hpp:65
T operator[](std::size_t idx) const override
Access an element at a given index.
Definition: buffers.hpp:256
An abstract base class for fixed-size data buffers.
Definition: buffers.h:66
virtual ~Buffer()
Definition: buffers.hpp:82
Buffer(std::size_t size)
Definition: buffers.hpp:76
MedianBuffer(std::size_t size, unsigned char window_size)
Construct a buffer of given size with given running window size.
Definition: buffers.hpp:115