Point Cloud Library (PCL)  1.12.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 
43 #include <pcl/pcl_macros.h>
44 
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 
68 namespace pcl
69 {
70 
71 namespace io
72 {
73 
74 template <typename T>
75 Buffer<T>::Buffer (std::size_t size)
76 : size_ (size)
77 {
78 }
79 
80 template <typename T>
82 {
83 }
84 
85 template <typename T>
87 : Buffer<T> (size)
88 , data_ (size, buffer_traits<T>::invalid ())
89 {
90 }
91 
92 template <typename T>
94 {
95 }
96 
97 template <typename T> T
98 SingleBuffer<T>::operator[] (std::size_t idx) const
99 {
100  assert (idx < size_);
101  return (data_[idx]);
102 }
103 
104 template <typename T> void
105 SingleBuffer<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 
113 template <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 
139 template <typename T>
141 {
142 }
143 
144 template <typename T> T
145 MedianBuffer<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 
152 template <typename T> void
153 MedianBuffer<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 
215 template <typename T> int
216 MedianBuffer<T>::compare (T a, T b)
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 
231 template <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 
249 template <typename T>
251 {
252 }
253 
254 template <typename T> T
255 AverageBuffer<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 
263 template <typename T> void
264 AverageBuffer<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 
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl::io::SingleBuffer::~SingleBuffer
~SingleBuffer()
Definition: buffers.hpp:93
pcl
Definition: convolution.h:46
pcl::io::AverageBuffer::operator[]
T operator[](std::size_t idx) const override
Access an element at a given index.
Definition: buffers.hpp:255
buffer_traits::invalid
static T invalid()
Definition: buffers.hpp:49
pcl::io::MedianBuffer::push
void push(std::vector< T > &data) override
Insert a new chunk of data into the buffer.
Definition: buffers.hpp:153
pcl::io::Buffer
An abstract base class for fixed-size data buffers.
Definition: buffers.h:62
buffer_traits< float >::invalid
static float invalid()
Definition: buffers.hpp:56
pcl::io::MedianBuffer
A buffer that computes running window median of the data inserted.
Definition: buffers.h:143
pcl::io::MedianBuffer::operator[]
T operator[](std::size_t idx) const override
Access an element at a given index.
Definition: buffers.hpp:145
pcl::io::SingleBuffer::SingleBuffer
SingleBuffer(std::size_t size)
Construct a buffer of given size.
Definition: buffers.hpp:86
pcl::io::AverageBuffer::AverageBuffer
AverageBuffer(std::size_t size, unsigned char window_size)
Construct a buffer of given size with given running window size.
Definition: buffers.hpp:232
pcl::io::AverageBuffer::push
void push(std::vector< T > &data) override
Insert a new chunk of data into the buffer.
Definition: buffers.hpp:264
buffer_traits< float >::is_invalid
static bool is_invalid(float value)
Definition: buffers.hpp:57
buffer_traits
Definition: buffers.hpp:47
buffer_traits< double >::invalid
static double invalid()
Definition: buffers.hpp:63
buffer_traits::is_invalid
static bool is_invalid(T value)
Definition: buffers.hpp:50
buffer_traits< double >::is_invalid
static bool is_invalid(double value)
Definition: buffers.hpp:64
pcl::io::Buffer::size_
const std::size_t size_
Definition: buffers.h:95
pcl::io::MedianBuffer::MedianBuffer
MedianBuffer(std::size_t size, unsigned char window_size)
Construct a buffer of given size with given running window size.
Definition: buffers.hpp:114
pcl::io::Buffer::~Buffer
virtual ~Buffer()
Definition: buffers.hpp:81
pcl::io::SingleBuffer::operator[]
T operator[](std::size_t idx) const override
Access an element at a given index.
Definition: buffers.hpp:98
pcl::io::MedianBuffer::~MedianBuffer
~MedianBuffer()
Definition: buffers.hpp:140
pcl::io::Buffer::Buffer
Buffer(std::size_t size)
Definition: buffers.hpp:75
pcl::io::AverageBuffer::~AverageBuffer
~AverageBuffer()
Definition: buffers.hpp:250
pcl::io::SingleBuffer::push
void push(std::vector< T > &data) override
Insert a new chunk of data into the buffer.
Definition: buffers.hpp:105