Point Cloud Library (PCL)  1.11.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 
41 #ifndef PCL_INTEGRAL_IMAGE2D_IMPL_H_
42 #define PCL_INTEGRAL_IMAGE2D_IMPL_H_
43 
44 
45 namespace pcl
46 {
47 
48 template <typename DataType, unsigned Dimension> void
49 IntegralImage2D<DataType, Dimension>::setSecondOrderComputation (bool compute_second_order_integral_images)
50 {
51  compute_second_order_integral_images_ = compute_second_order_integral_images;
52 }
53 
54 
55 template <typename DataType, unsigned Dimension> void
56 IntegralImage2D<DataType, Dimension>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride)
57 {
58  if ((width + 1) * (height + 1) > first_order_integral_image_.size () )
59  {
60  width_ = width;
61  height_ = height;
62  first_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
63  finite_values_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
64  if (compute_second_order_integral_images_)
65  second_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
66  }
67  computeIntegralImages (data, row_stride, element_stride);
68 }
69 
70 
71 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
72 IntegralImage2D<DataType, Dimension>::getFirstOrderSum (
73  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
74 {
75  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
76  const unsigned upper_right_idx = upper_left_idx + width;
77  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
78  const unsigned lower_right_idx = lower_left_idx + width;
79 
80  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
81  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
82 }
83 
84 
85 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
86 IntegralImage2D<DataType, Dimension>::getSecondOrderSum (
87  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
88 {
89  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
90  const unsigned upper_right_idx = upper_left_idx + width;
91  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
92  const unsigned lower_right_idx = lower_left_idx + width;
93 
94  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
95  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
96 }
97 
98 
99 template <typename DataType, unsigned Dimension> unsigned
100 IntegralImage2D<DataType, Dimension>::getFiniteElementsCount (
101  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
102 {
103  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
104  const unsigned upper_right_idx = upper_left_idx + width;
105  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
106  const unsigned lower_right_idx = lower_left_idx + width;
107 
108  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
109  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
110 }
111 
112 
113 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
114 IntegralImage2D<DataType, Dimension>::getFirstOrderSumSE (
115  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
116 {
117  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
118  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
119  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
120  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
121 
122  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
123  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
124 }
125 
126 
127 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
128 IntegralImage2D<DataType, Dimension>::getSecondOrderSumSE (
129  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
130 {
131  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
132  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
133  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
134  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
135 
136  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
137  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
138 }
139 
140 
141 template <typename DataType, unsigned Dimension> unsigned
142 IntegralImage2D<DataType, Dimension>::getFiniteElementsCountSE (
143  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
144 {
145  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
146  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
147  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
148  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
149 
150  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
151  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
152 }
153 
154 
155 template <typename DataType, unsigned Dimension> void
156 IntegralImage2D<DataType, Dimension>::computeIntegralImages (
157  const DataType *data, unsigned row_stride, unsigned element_stride)
158 {
159  ElementType* previous_row = &first_order_integral_image_[0];
160  ElementType* current_row = previous_row + (width_ + 1);
161  memset (previous_row, 0, sizeof (ElementType) * (width_ + 1));
162 
163  unsigned* count_previous_row = &finite_values_integral_image_[0];
164  unsigned* count_current_row = count_previous_row + (width_ + 1);
165  memset (count_previous_row, 0, sizeof (unsigned) * (width_ + 1));
166 
167  if (!compute_second_order_integral_images_)
168  {
169  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
170  previous_row = current_row, current_row += (width_ + 1),
171  count_previous_row = count_current_row, count_current_row += (width_ + 1))
172  {
173  current_row [0].setZero ();
174  count_current_row [0] = 0;
175  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
176  {
177  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
178  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
179  const InputType* element = reinterpret_cast <const InputType*> (&data [valIdx]);
180  if (std::isfinite (element->sum ()))
181  {
182  current_row [colIdx + 1] += element->template cast<typename IntegralImageTypeTraits<DataType>::IntegralType>();
183  ++(count_current_row [colIdx + 1]);
184  }
185  }
186  }
187  }
188  else
189  {
190  SecondOrderType* so_previous_row = &second_order_integral_image_[0];
191  SecondOrderType* so_current_row = so_previous_row + (width_ + 1);
192  memset (so_previous_row, 0, sizeof (SecondOrderType) * (width_ + 1));
193 
194  SecondOrderType so_element;
195  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
196  previous_row = current_row, current_row += (width_ + 1),
197  count_previous_row = count_current_row, count_current_row += (width_ + 1),
198  so_previous_row = so_current_row, so_current_row += (width_ + 1))
199  {
200  current_row [0].setZero ();
201  so_current_row [0].setZero ();
202  count_current_row [0] = 0;
203  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
204  {
205  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
206  so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx];
207  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
208 
209  const InputType* element = reinterpret_cast <const InputType*> (&data [valIdx]);
210  if (std::isfinite (element->sum ()))
211  {
212  current_row [colIdx + 1] += element->template cast<typename IntegralImageTypeTraits<DataType>::IntegralType>();
213  ++(count_current_row [colIdx + 1]);
214  for (unsigned myIdx = 0, elIdx = 0; myIdx < Dimension; ++myIdx)
215  for (unsigned mxIdx = myIdx; mxIdx < Dimension; ++mxIdx, ++elIdx)
216  so_current_row [colIdx + 1][elIdx] += (*element)[myIdx] * (*element)[mxIdx];
217  }
218  }
219  }
220  }
221 }
222 
223 
224 template <typename DataType> void
225 IntegralImage2D<DataType, 1>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride)
226 {
227  if ((width + 1) * (height + 1) > first_order_integral_image_.size () )
228  {
229  width_ = width;
230  height_ = height;
231  first_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
232  finite_values_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
233  if (compute_second_order_integral_images_)
234  second_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
235  }
236  computeIntegralImages (data, row_stride, element_stride);
237 }
238 
239 
240 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
241 IntegralImage2D<DataType, 1>::getFirstOrderSum (
242  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
243 {
244  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
245  const unsigned upper_right_idx = upper_left_idx + width;
246  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
247  const unsigned lower_right_idx = lower_left_idx + width;
248 
249  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
250  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
251 }
252 
253 
254 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
255 IntegralImage2D<DataType, 1>::getSecondOrderSum (
256  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
257 {
258  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
259  const unsigned upper_right_idx = upper_left_idx + width;
260  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
261  const unsigned lower_right_idx = lower_left_idx + width;
262 
263  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
264  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
265 }
266 
267 
268 template <typename DataType> unsigned
269 IntegralImage2D<DataType, 1>::getFiniteElementsCount (
270  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
271 {
272  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
273  const unsigned upper_right_idx = upper_left_idx + width;
274  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
275  const unsigned lower_right_idx = lower_left_idx + width;
276 
277  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
278  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
279 }
280 
281 
282 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
283 IntegralImage2D<DataType, 1>::getFirstOrderSumSE (
284  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
285 {
286  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
287  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
288  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
289  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
290 
291  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
292  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
293 }
294 
295 
296 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
297 IntegralImage2D<DataType, 1>::getSecondOrderSumSE (
298  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
299 {
300  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
301  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
302  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
303  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
304 
305  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
306  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
307 }
308 
309 
310 template <typename DataType> unsigned
311 IntegralImage2D<DataType, 1>::getFiniteElementsCountSE (
312  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
313 {
314  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
315  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
316  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
317  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
318 
319  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
320  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
321 }
322 
323 
324 template <typename DataType> void
325 IntegralImage2D<DataType, 1>::computeIntegralImages (
326  const DataType *data, unsigned row_stride, unsigned element_stride)
327 {
328  ElementType* previous_row = &first_order_integral_image_[0];
329  ElementType* current_row = previous_row + (width_ + 1);
330  memset (previous_row, 0, sizeof (ElementType) * (width_ + 1));
331 
332  unsigned* count_previous_row = &finite_values_integral_image_[0];
333  unsigned* count_current_row = count_previous_row + (width_ + 1);
334  memset (count_previous_row, 0, sizeof (unsigned) * (width_ + 1));
335 
336  if (!compute_second_order_integral_images_)
337  {
338  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
339  previous_row = current_row, current_row += (width_ + 1),
340  count_previous_row = count_current_row, count_current_row += (width_ + 1))
341  {
342  current_row [0] = 0.0;
343  count_current_row [0] = 0;
344  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
345  {
346  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
347  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
348  if (std::isfinite (data [valIdx]))
349  {
350  current_row [colIdx + 1] += data [valIdx];
351  ++(count_current_row [colIdx + 1]);
352  }
353  }
354  }
355  }
356  else
357  {
358  SecondOrderType* so_previous_row = &second_order_integral_image_[0];
359  SecondOrderType* so_current_row = so_previous_row + (width_ + 1);
360  memset (so_previous_row, 0, sizeof (SecondOrderType) * (width_ + 1));
361 
362  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
363  previous_row = current_row, current_row += (width_ + 1),
364  count_previous_row = count_current_row, count_current_row += (width_ + 1),
365  so_previous_row = so_current_row, so_current_row += (width_ + 1))
366  {
367  current_row [0] = 0.0;
368  so_current_row [0] = 0.0;
369  count_current_row [0] = 0;
370  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
371  {
372  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
373  so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx];
374  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
375  if (std::isfinite (data[valIdx]))
376  {
377  current_row [colIdx + 1] += data[valIdx];
378  so_current_row [colIdx + 1] += data[valIdx] * data[valIdx];
379  ++(count_current_row [colIdx + 1]);
380  }
381  }
382  }
383  }
384 }
385 
386 } // namespace pcl
387 
388 #endif // PCL_INTEGRAL_IMAGE2D_IMPL_H_
389 
pcl::IntegralImage2D
Determines an integral image representation for a given organized data array.
Definition: integral_image2D.h:109
pcl::IntegralImage2D::getFiniteElementsCountSE
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.
Definition: integral_image2D.hpp:142
pcl
Definition: convolution.h:46
pcl::IntegralImage2D::setSecondOrderComputation
void setSecondOrderComputation(bool compute_second_order_integral_images)
sets the computation for second order integral images on or off.
Definition: integral_image2D.hpp:49
pcl::IntegralImage2D::getFirstOrderSumSE
ElementType getFirstOrderSumSE(unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
Compute the first order sum within a given rectangle.
Definition: integral_image2D.hpp:114
pcl::IntegralImage2D::getFirstOrderSum
ElementType getFirstOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the first order sum within a given rectangle.
Definition: integral_image2D.hpp:72
pcl::IntegralImage2D::getFiniteElementsCount
unsigned getFiniteElementsCount(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the number of finite elements within a given rectangle.
Definition: integral_image2D.hpp:100
pcl::IntegralImage2D::ElementType
Eigen::Matrix< typename IntegralImageTypeTraits< DataType >::IntegralType, Dimension, 1 > ElementType
Definition: integral_image2D.h:115
pcl::IntegralImage2D::SecondOrderType
Eigen::Matrix< typename IntegralImageTypeTraits< DataType >::IntegralType, second_order_size, 1 > SecondOrderType
Definition: integral_image2D.h:116
pcl::IntegralImage2D::getSecondOrderSumSE
SecondOrderType getSecondOrderSumSE(unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
Compute the second order sum within a given rectangle.
Definition: integral_image2D.hpp:128
pcl::IntegralImage2D::getSecondOrderSum
SecondOrderType getSecondOrderSum(unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
Compute the second order sum within a given rectangle.
Definition: integral_image2D.hpp:86
pcl::IntegralImage2D::setInput
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.
Definition: integral_image2D.hpp:56