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