Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
integral_image2D.hpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, 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 Willow Garage, Inc. 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  * $Id: integral_image2D.hpp 6144 2012-07-04 22:06:28Z rusu $
37  */
38 
39 #ifndef PCL_INTEGRAL_IMAGE2D_IMPL_H_
40 #define PCL_INTEGRAL_IMAGE2D_IMPL_H_
41 
42 #include <cstddef>
43 
45 template <typename DataType, unsigned Dimension> void
46 pcl::IntegralImage2D<DataType, Dimension>::setSecondOrderComputation (bool compute_second_order_integral_images)
47 {
48  compute_second_order_integral_images_ = compute_second_order_integral_images;
49 }
50 
52 template <typename DataType, unsigned Dimension> void
53 pcl::IntegralImage2D<DataType, Dimension>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride)
54 {
55  if ((width + 1) * (height + 1) > first_order_integral_image_.size () )
56  {
57  width_ = width;
58  height_ = height;
59  first_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
60  finite_values_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
61  if (compute_second_order_integral_images_)
62  second_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
63  }
64  computeIntegralImages (data, row_stride, element_stride);
65 }
66 
68 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
69 pcl::IntegralImage2D<DataType, Dimension>::getFirstOrderSum (
70  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
71 {
72  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
73  const unsigned upper_right_idx = upper_left_idx + width;
74  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
75  const unsigned lower_right_idx = lower_left_idx + width;
76 
77  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
78  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
79 }
80 
82 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
83 pcl::IntegralImage2D<DataType, Dimension>::getSecondOrderSum (
84  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
85 {
86  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
87  const unsigned upper_right_idx = upper_left_idx + width;
88  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
89  const unsigned lower_right_idx = lower_left_idx + width;
90 
91  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
92  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
93 }
94 
96 template <typename DataType, unsigned Dimension> unsigned
97 pcl::IntegralImage2D<DataType, Dimension>::getFiniteElementsCount (
98  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
99 {
100  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
101  const unsigned upper_right_idx = upper_left_idx + width;
102  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
103  const unsigned lower_right_idx = lower_left_idx + width;
104 
105  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
106  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
107 }
108 
110 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
111 pcl::IntegralImage2D<DataType, Dimension>::getFirstOrderSumSE (
112  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
113 {
114  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
115  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
116  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
117  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
118 
119  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
120  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
121 }
122 
124 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
125 pcl::IntegralImage2D<DataType, Dimension>::getSecondOrderSumSE (
126  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
127 {
128  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
129  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
130  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
131  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
132 
133  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
134  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
135 }
136 
138 template <typename DataType, unsigned Dimension> unsigned
139 pcl::IntegralImage2D<DataType, Dimension>::getFiniteElementsCountSE (
140  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
141 {
142  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
143  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
144  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
145  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
146 
147  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
148  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
149 }
150 
152 template <typename DataType, unsigned Dimension> void
153 pcl::IntegralImage2D<DataType, Dimension>::computeIntegralImages (
154  const DataType *data, unsigned row_stride, unsigned element_stride)
155 {
156  ElementType* previous_row = &first_order_integral_image_[0];
157  ElementType* current_row = previous_row + (width_ + 1);
158  memset (previous_row, 0, sizeof (ElementType) * (width_ + 1));
159 
160  unsigned* count_previous_row = &finite_values_integral_image_[0];
161  unsigned* count_current_row = count_previous_row + (width_ + 1);
162  memset (count_previous_row, 0, sizeof (unsigned) * (width_ + 1));
163 
164  if (!compute_second_order_integral_images_)
165  {
166  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
167  previous_row = current_row, current_row += (width_ + 1),
168  count_previous_row = count_current_row, count_current_row += (width_ + 1))
169  {
170  current_row [0].setZero ();
171  count_current_row [0] = 0;
172  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
173  {
174  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
175  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
176  const InputType* element = reinterpret_cast <const InputType*> (&data [valIdx]);
177  if (pcl_isfinite (element->sum ()))
178  {
179  current_row [colIdx + 1] += element->template cast<typename IntegralImageTypeTraits<DataType>::IntegralType>();
180  ++(count_current_row [colIdx + 1]);
181  }
182  }
183  }
184  }
185  else
186  {
187  SecondOrderType* so_previous_row = &second_order_integral_image_[0];
188  SecondOrderType* so_current_row = so_previous_row + (width_ + 1);
189  memset (so_previous_row, 0, sizeof (SecondOrderType) * (width_ + 1));
190 
191  SecondOrderType so_element;
192  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
193  previous_row = current_row, current_row += (width_ + 1),
194  count_previous_row = count_current_row, count_current_row += (width_ + 1),
195  so_previous_row = so_current_row, so_current_row += (width_ + 1))
196  {
197  current_row [0].setZero ();
198  so_current_row [0].setZero ();
199  count_current_row [0] = 0;
200  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
201  {
202  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
203  so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx];
204  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
205 
206  const InputType* element = reinterpret_cast <const InputType*> (&data [valIdx]);
207  if (pcl_isfinite (element->sum ()))
208  {
209  current_row [colIdx + 1] += element->template cast<typename IntegralImageTypeTraits<DataType>::IntegralType>();
210  ++(count_current_row [colIdx + 1]);
211  for (unsigned myIdx = 0, elIdx = 0; myIdx < Dimension; ++myIdx)
212  for (unsigned mxIdx = myIdx; mxIdx < Dimension; ++mxIdx, ++elIdx)
213  so_current_row [colIdx + 1][elIdx] += (*element)[myIdx] * (*element)[mxIdx];
214  }
215  }
216  }
217  }
218 }
219 
221 
222 template <typename DataType> void
223 pcl::IntegralImage2D<DataType, 1>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride)
224 {
225  if ((width + 1) * (height + 1) > first_order_integral_image_.size () )
226  {
227  width_ = width;
228  height_ = height;
229  first_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
230  finite_values_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
231  if (compute_second_order_integral_images_)
232  second_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
233  }
234  computeIntegralImages (data, row_stride, element_stride);
235 }
236 
238 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
239 pcl::IntegralImage2D<DataType, 1>::getFirstOrderSum (
240  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
241 {
242  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
243  const unsigned upper_right_idx = upper_left_idx + width;
244  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
245  const unsigned lower_right_idx = lower_left_idx + width;
246 
247  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
248  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
249 }
250 
252 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
253 pcl::IntegralImage2D<DataType, 1>::getSecondOrderSum (
254  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
255 {
256  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
257  const unsigned upper_right_idx = upper_left_idx + width;
258  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
259  const unsigned lower_right_idx = lower_left_idx + width;
260 
261  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
262  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
263 }
264 
266 template <typename DataType> unsigned
267 pcl::IntegralImage2D<DataType, 1>::getFiniteElementsCount (
268  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
269 {
270  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
271  const unsigned upper_right_idx = upper_left_idx + width;
272  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
273  const unsigned lower_right_idx = lower_left_idx + width;
274 
275  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
276  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
277 }
278 
280 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
281 pcl::IntegralImage2D<DataType, 1>::getFirstOrderSumSE (
282  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
283 {
284  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
285  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
286  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
287  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
288 
289  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
290  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
291 }
292 
294 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
295 pcl::IntegralImage2D<DataType, 1>::getSecondOrderSumSE (
296  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
297 {
298  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
299  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
300  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
301  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
302 
303  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
304  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
305 }
306 
308 template <typename DataType> unsigned
309 pcl::IntegralImage2D<DataType, 1>::getFiniteElementsCountSE (
310  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
311 {
312  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
313  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
314  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
315  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
316 
317  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
318  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
319 }
320 
322 template <typename DataType> void
323 pcl::IntegralImage2D<DataType, 1>::computeIntegralImages (
324  const DataType *data, unsigned row_stride, unsigned element_stride)
325 {
326  ElementType* previous_row = &first_order_integral_image_[0];
327  ElementType* current_row = previous_row + (width_ + 1);
328  memset (previous_row, 0, sizeof (ElementType) * (width_ + 1));
329 
330  unsigned* count_previous_row = &finite_values_integral_image_[0];
331  unsigned* count_current_row = count_previous_row + (width_ + 1);
332  memset (count_previous_row, 0, sizeof (unsigned) * (width_ + 1));
333 
334  if (!compute_second_order_integral_images_)
335  {
336  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
337  previous_row = current_row, current_row += (width_ + 1),
338  count_previous_row = count_current_row, count_current_row += (width_ + 1))
339  {
340  current_row [0] = 0.0;
341  count_current_row [0] = 0;
342  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
343  {
344  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
345  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
346  if (pcl_isfinite (data [valIdx]))
347  {
348  current_row [colIdx + 1] += data [valIdx];
349  ++(count_current_row [colIdx + 1]);
350  }
351  }
352  }
353  }
354  else
355  {
356  SecondOrderType* so_previous_row = &second_order_integral_image_[0];
357  SecondOrderType* so_current_row = so_previous_row + (width_ + 1);
358  memset (so_previous_row, 0, sizeof (SecondOrderType) * (width_ + 1));
359 
360  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
361  previous_row = current_row, current_row += (width_ + 1),
362  count_previous_row = count_current_row, count_current_row += (width_ + 1),
363  so_previous_row = so_current_row, so_current_row += (width_ + 1))
364  {
365  current_row [0] = 0.0;
366  so_current_row [0] = 0.0;
367  count_current_row [0] = 0;
368  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
369  {
370  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
371  so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx];
372  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
373  if (pcl_isfinite (data[valIdx]))
374  {
375  current_row [colIdx + 1] += data[valIdx];
376  so_current_row [colIdx + 1] += data[valIdx] * data[valIdx];
377  ++(count_current_row [colIdx + 1]);
378  }
379  }
380  }
381  }
382 }
383 #endif // PCL_INTEGRAL_IMAGE2D_IMPL_H_
384 
Generated on Sat Aug 24 2013 17:28:29 for Point Cloud Library (PCL) by   doxygen 1.8.4