Point Cloud Library (PCL)  1.14.0-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_.data();
160  ElementType* current_row = previous_row + (width_ + 1);
161  for (unsigned int i = 0; i < (width_ + 1); ++i)
162  previous_row[i].setZero();
163 
164  unsigned* count_previous_row = finite_values_integral_image_.data();
165  unsigned* count_current_row = count_previous_row + (width_ + 1);
166  std::fill_n(count_previous_row, width_ + 1, 0);
167 
168  if (!compute_second_order_integral_images_)
169  {
170  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
171  previous_row = current_row, current_row += (width_ + 1),
172  count_previous_row = count_current_row, count_current_row += (width_ + 1))
173  {
174  current_row [0].setZero ();
175  count_current_row [0] = 0;
176  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
177  {
178  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
179  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
180  const auto* element = reinterpret_cast <const InputType*> (&data [valIdx]);
181  if (std::isfinite (element->sum ()))
182  {
183  current_row [colIdx + 1] += element->template cast<typename IntegralImageTypeTraits<DataType>::IntegralType>();
184  ++(count_current_row [colIdx + 1]);
185  }
186  }
187  }
188  }
189  else
190  {
191  SecondOrderType* so_previous_row = second_order_integral_image_.data();
192  SecondOrderType* so_current_row = so_previous_row + (width_ + 1);
193  for (unsigned int i = 0; i < (width_ + 1); ++i)
194  so_previous_row[i].setZero();
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 auto* 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
243 IntegralImage2D<DataType, 1>::getFirstOrderSum (
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
257 IntegralImage2D<DataType, 1>::getSecondOrderSum (
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
271 IntegralImage2D<DataType, 1>::getFiniteElementsCount (
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
285 IntegralImage2D<DataType, 1>::getFirstOrderSumSE (
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
299 IntegralImage2D<DataType, 1>::getSecondOrderSumSE (
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
313 IntegralImage2D<DataType, 1>::getFiniteElementsCountSE (
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
327 IntegralImage2D<DataType, 1>::computeIntegralImages (
328  const DataType *data, unsigned row_stride, unsigned element_stride)
329 {
330  ElementType* previous_row = first_order_integral_image_.data();
331  ElementType* current_row = previous_row + (width_ + 1);
332  std::fill_n(previous_row, width_ + 1, 0);
333 
334  unsigned* count_previous_row = finite_values_integral_image_.data();
335  unsigned* count_current_row = count_previous_row + (width_ + 1);
336  std::fill_n(count_previous_row, width_ + 1, 0);
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_.data();
361  SecondOrderType* so_current_row = so_previous_row + (width_ + 1);
362  std::fill_n(so_previous_row, width_ + 1, 0);
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 
pcl::IntegralImage2D
Determines an integral image representation for a given organized data array.
Definition: integral_image2D.h:110
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::ElementType
Eigen::Matrix< typename IntegralImageTypeTraits< DataType >::IntegralType, Dimension, 1 > ElementType
Definition: integral_image2D.h:115
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::SecondOrderType
Eigen::Matrix< typename IntegralImageTypeTraits< DataType >::IntegralType, second_order_size, 1 > SecondOrderType
Definition: integral_image2D.h:116
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
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::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