Point Cloud Library (PCL)  1.9.1-dev
integral_image2D.hpp
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  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id: feature.h 2784 2011-10-15 22:05:38Z aichim $
38  */
39 
40 #ifndef PCL_INTEGRAL_IMAGE2D_IMPL_H_
41 #define PCL_INTEGRAL_IMAGE2D_IMPL_H_
42 
43 #include <cstddef>
44 
45 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
46 template <typename DataType, unsigned Dimension> void
48 {
49  compute_second_order_integral_images_ = compute_second_order_integral_images;
50 }
51 
52 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
53 template <typename DataType, unsigned Dimension> void
54 pcl::IntegralImage2D<DataType, Dimension>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride)
55 {
56  if ((width + 1) * (height + 1) > first_order_integral_image_.size () )
57  {
58  width_ = width;
59  height_ = height;
60  first_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
61  finite_values_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
62  if (compute_second_order_integral_images_)
63  second_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
64  }
65  computeIntegralImages (data, row_stride, element_stride);
66 }
67 
68 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
69 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
71  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
72 {
73  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
74  const unsigned upper_right_idx = upper_left_idx + width;
75  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
76  const unsigned lower_right_idx = lower_left_idx + width;
77 
78  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
79  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
80 }
81 
82 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
83 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
85  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
86 {
87  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
88  const unsigned upper_right_idx = upper_left_idx + width;
89  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
90  const unsigned lower_right_idx = lower_left_idx + width;
91 
92  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
93  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
94 }
95 
96 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
97 template <typename DataType, unsigned Dimension> unsigned
99  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
100 {
101  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
102  const unsigned upper_right_idx = upper_left_idx + width;
103  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
104  const unsigned lower_right_idx = lower_left_idx + width;
105 
106  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
107  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
108 }
109 
110 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
111 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
113  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
114 {
115  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
116  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
117  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
118  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
119 
120  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
121  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
122 }
123 
124 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
125 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
127  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
128 {
129  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
130  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
131  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
132  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
133 
134  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
135  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
136 }
137 
138 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
139 template <typename DataType, unsigned Dimension> unsigned
141  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
142 {
143  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
144  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
145  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
146  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
147 
148  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
149  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
150 }
151 
152 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
153 template <typename DataType, unsigned Dimension> void
155  const DataType *data, unsigned row_stride, unsigned element_stride)
156 {
157  ElementType* previous_row = &first_order_integral_image_[0];
158  ElementType* current_row = previous_row + (width_ + 1);
159  memset (previous_row, 0, sizeof (ElementType) * (width_ + 1));
160 
161  unsigned* count_previous_row = &finite_values_integral_image_[0];
162  unsigned* count_current_row = count_previous_row + (width_ + 1);
163  memset (count_previous_row, 0, sizeof (unsigned) * (width_ + 1));
164 
165  if (!compute_second_order_integral_images_)
166  {
167  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
168  previous_row = current_row, current_row += (width_ + 1),
169  count_previous_row = count_current_row, count_current_row += (width_ + 1))
170  {
171  current_row [0].setZero ();
172  count_current_row [0] = 0;
173  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
174  {
175  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
176  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
177  const InputType* element = reinterpret_cast <const InputType*> (&data [valIdx]);
178  if (std::isfinite (element->sum ()))
179  {
180  current_row [colIdx + 1] += element->template cast<typename IntegralImageTypeTraits<DataType>::IntegralType>();
181  ++(count_current_row [colIdx + 1]);
182  }
183  }
184  }
185  }
186  else
187  {
188  SecondOrderType* so_previous_row = &second_order_integral_image_[0];
189  SecondOrderType* so_current_row = so_previous_row + (width_ + 1);
190  memset (so_previous_row, 0, sizeof (SecondOrderType) * (width_ + 1));
191 
192  SecondOrderType so_element;
193  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
194  previous_row = current_row, current_row += (width_ + 1),
195  count_previous_row = count_current_row, count_current_row += (width_ + 1),
196  so_previous_row = so_current_row, so_current_row += (width_ + 1))
197  {
198  current_row [0].setZero ();
199  so_current_row [0].setZero ();
200  count_current_row [0] = 0;
201  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
202  {
203  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
204  so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx];
205  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
206 
207  const InputType* element = reinterpret_cast <const InputType*> (&data [valIdx]);
208  if (std::isfinite (element->sum ()))
209  {
210  current_row [colIdx + 1] += element->template cast<typename IntegralImageTypeTraits<DataType>::IntegralType>();
211  ++(count_current_row [colIdx + 1]);
212  for (unsigned myIdx = 0, elIdx = 0; myIdx < Dimension; ++myIdx)
213  for (unsigned mxIdx = myIdx; mxIdx < Dimension; ++mxIdx, ++elIdx)
214  so_current_row [colIdx + 1][elIdx] += (*element)[myIdx] * (*element)[mxIdx];
215  }
216  }
217  }
218  }
219 }
220 
221 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
222 
223 template <typename DataType> void
224 pcl::IntegralImage2D<DataType, 1>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride)
225 {
226  if ((width + 1) * (height + 1) > first_order_integral_image_.size () )
227  {
228  width_ = width;
229  height_ = height;
230  first_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
231  finite_values_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
232  if (compute_second_order_integral_images_)
233  second_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
234  }
235  computeIntegralImages (data, row_stride, element_stride);
236 }
237 
238 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
239 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
241  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
242 {
243  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
244  const unsigned upper_right_idx = upper_left_idx + width;
245  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
246  const unsigned lower_right_idx = lower_left_idx + width;
247 
248  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
249  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
250 }
251 
252 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
253 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
255  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
256 {
257  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
258  const unsigned upper_right_idx = upper_left_idx + width;
259  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
260  const unsigned lower_right_idx = lower_left_idx + width;
261 
262  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
263  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
264 }
265 
266 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
267 template <typename DataType> unsigned
269  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
270 {
271  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
272  const unsigned upper_right_idx = upper_left_idx + width;
273  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
274  const unsigned lower_right_idx = lower_left_idx + width;
275 
276  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
277  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
278 }
279 
280 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
281 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
283  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
284 {
285  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
286  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
287  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
288  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
289 
290  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
291  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
292 }
293 
294 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
295 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
297  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
298 {
299  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
300  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
301  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
302  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
303 
304  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
305  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
306 }
307 
308 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
309 template <typename DataType> unsigned
311  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
312 {
313  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
314  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
315  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
316  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
317 
318  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
319  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
320 }
321 
322 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
323 template <typename DataType> void
325  const DataType *data, unsigned row_stride, unsigned element_stride)
326 {
327  ElementType* previous_row = &first_order_integral_image_[0];
328  ElementType* current_row = previous_row + (width_ + 1);
329  memset (previous_row, 0, sizeof (ElementType) * (width_ + 1));
330 
331  unsigned* count_previous_row = &finite_values_integral_image_[0];
332  unsigned* count_current_row = count_previous_row + (width_ + 1);
333  memset (count_previous_row, 0, sizeof (unsigned) * (width_ + 1));
334 
335  if (!compute_second_order_integral_images_)
336  {
337  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
338  previous_row = current_row, current_row += (width_ + 1),
339  count_previous_row = count_current_row, count_current_row += (width_ + 1))
340  {
341  current_row [0] = 0.0;
342  count_current_row [0] = 0;
343  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
344  {
345  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
346  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
347  if (std::isfinite (data [valIdx]))
348  {
349  current_row [colIdx + 1] += data [valIdx];
350  ++(count_current_row [colIdx + 1]);
351  }
352  }
353  }
354  }
355  else
356  {
357  SecondOrderType* so_previous_row = &second_order_integral_image_[0];
358  SecondOrderType* so_current_row = so_previous_row + (width_ + 1);
359  memset (so_previous_row, 0, sizeof (SecondOrderType) * (width_ + 1));
360 
361  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
362  previous_row = current_row, current_row += (width_ + 1),
363  count_previous_row = count_current_row, count_current_row += (width_ + 1),
364  so_previous_row = so_current_row, so_current_row += (width_ + 1))
365  {
366  current_row [0] = 0.0;
367  so_current_row [0] = 0.0;
368  count_current_row [0] = 0;
369  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
370  {
371  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
372  so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx];
373  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
374  if (std::isfinite (data[valIdx]))
375  {
376  current_row [colIdx + 1] += data[valIdx];
377  so_current_row [colIdx + 1] += data[valIdx] * data[valIdx];
378  ++(count_current_row [colIdx + 1]);
379  }
380  }
381  }
382  }
383 }
384 #endif // PCL_INTEGRAL_IMAGE2D_IMPL_H_
385 
typename IntegralImageTypeTraits< DataType >::IntegralType ElementType
void setSecondOrderComputation(bool compute_second_order_integral_images)
sets the computation for second order integral images on or off.
SecondOrderType getSecondOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the second order sum within a given rectangle.
void setInput(const DataType *data, unsigned width, unsigned height, unsigned element_stride, unsigned row_stride)
Set the input data to compute the integral image for.
unsigned getFiniteElementsCount(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the number of finite elements within a given rectangle.
ElementType getFirstOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the first order sum within a given rectangle.
ElementType getFirstOrderSumSE(unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
Compute the first order sum within a given rectangle.
unsigned getFiniteElementsCountSE(unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
Compute the number of finite elements within a given rectangle.
typename IntegralImageTypeTraits< DataType >::IntegralType SecondOrderType
Determines an integral image representation for a given organized data array.
SecondOrderType getSecondOrderSumSE(unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
Compute the second order sum within a given rectangle.
Eigen::Matrix< typename IntegralImageTypeTraits< DataType >::IntegralType, second_order_size, 1 > SecondOrderType
Eigen::Matrix< typename IntegralImageTypeTraits< DataType >::IntegralType, Dimension, 1 > ElementType