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