Point Cloud Library (PCL)  1.14.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  const std::size_t ii_size = static_cast<std::size_t>(width_ + 1) * static_cast<std::size_t>(height_ + 1);
63  first_order_integral_image_.resize (ii_size);
64  finite_values_integral_image_.resize (ii_size);
65  if (compute_second_order_integral_images_)
66  second_order_integral_image_.resize (ii_size);
67  }
68  computeIntegralImages (data, row_stride, element_stride);
69 }
70 
71 
72 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
73 IntegralImage2D<DataType, Dimension>::getFirstOrderSum (
74  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
75 {
76  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
77  const unsigned upper_right_idx = upper_left_idx + width;
78  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
79  const unsigned lower_right_idx = lower_left_idx + width;
80 
81  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
82  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
83 }
84 
85 
86 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
87 IntegralImage2D<DataType, Dimension>::getSecondOrderSum (
88  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
89 {
90  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
91  const unsigned upper_right_idx = upper_left_idx + width;
92  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
93  const unsigned lower_right_idx = lower_left_idx + width;
94 
95  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
96  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
97 }
98 
99 
100 template <typename DataType, unsigned Dimension> unsigned
101 IntegralImage2D<DataType, Dimension>::getFiniteElementsCount (
102  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
103 {
104  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
105  const unsigned upper_right_idx = upper_left_idx + width;
106  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
107  const unsigned lower_right_idx = lower_left_idx + width;
108 
109  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
110  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
111 }
112 
113 
114 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
115 IntegralImage2D<DataType, Dimension>::getFirstOrderSumSE (
116  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
117 {
118  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
119  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
120  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
121  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
122 
123  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
124  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
125 }
126 
127 
128 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
129 IntegralImage2D<DataType, Dimension>::getSecondOrderSumSE (
130  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
131 {
132  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
133  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
134  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
135  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
136 
137  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
138  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
139 }
140 
141 
142 template <typename DataType, unsigned Dimension> unsigned
143 IntegralImage2D<DataType, Dimension>::getFiniteElementsCountSE (
144  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
145 {
146  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
147  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
148  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
149  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
150 
151  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
152  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
153 }
154 
155 
156 template <typename DataType, unsigned Dimension> void
157 IntegralImage2D<DataType, Dimension>::computeIntegralImages (
158  const DataType *data, unsigned row_stride, unsigned element_stride)
159 {
160  ElementType* previous_row = first_order_integral_image_.data();
161  ElementType* current_row = previous_row + (width_ + 1);
162  for (unsigned int i = 0; i < (width_ + 1); ++i)
163  previous_row[i].setZero();
164 
165  unsigned* count_previous_row = finite_values_integral_image_.data();
166  unsigned* count_current_row = count_previous_row + (width_ + 1);
167  std::fill_n(count_previous_row, width_ + 1, 0);
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 auto* 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_.data();
193  SecondOrderType* so_current_row = so_previous_row + (width_ + 1);
194  for (unsigned int i = 0; i < (width_ + 1); ++i)
195  so_previous_row[i].setZero();
196 
197  SecondOrderType so_element;
198  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
199  previous_row = current_row, current_row += (width_ + 1),
200  count_previous_row = count_current_row, count_current_row += (width_ + 1),
201  so_previous_row = so_current_row, so_current_row += (width_ + 1))
202  {
203  current_row [0].setZero ();
204  so_current_row [0].setZero ();
205  count_current_row [0] = 0;
206  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
207  {
208  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
209  so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx];
210  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
211 
212  const auto* element = reinterpret_cast <const InputType*> (&data [valIdx]);
213  if (std::isfinite (element->sum ()))
214  {
215  current_row [colIdx + 1] += element->template cast<typename IntegralImageTypeTraits<DataType>::IntegralType>();
216  ++(count_current_row [colIdx + 1]);
217  for (unsigned myIdx = 0, elIdx = 0; myIdx < Dimension; ++myIdx)
218  for (unsigned mxIdx = myIdx; mxIdx < Dimension; ++mxIdx, ++elIdx)
219  so_current_row [colIdx + 1][elIdx] += (*element)[myIdx] * (*element)[mxIdx];
220  }
221  }
222  }
223  }
224 }
225 
226 
227 template <typename DataType> void
228 IntegralImage2D<DataType, 1>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride)
229 {
230  if ((width + 1) * (height + 1) > first_order_integral_image_.size () )
231  {
232  width_ = width;
233  height_ = height;
234  const std::size_t ii_size = static_cast<std::size_t>(width_ + 1) * static_cast<std::size_t>(height_ + 1);
235  first_order_integral_image_.resize (ii_size);
236  finite_values_integral_image_.resize (ii_size);
237  if (compute_second_order_integral_images_)
238  second_order_integral_image_.resize (ii_size);
239  }
240  computeIntegralImages (data, row_stride, element_stride);
241 }
242 
243 
244 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
245 IntegralImage2D<DataType, 1>::getFirstOrderSum (
246  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
247 {
248  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
249  const unsigned upper_right_idx = upper_left_idx + width;
250  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
251  const unsigned lower_right_idx = lower_left_idx + width;
252 
253  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
254  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
255 }
256 
257 
258 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
259 IntegralImage2D<DataType, 1>::getSecondOrderSum (
260  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
261 {
262  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
263  const unsigned upper_right_idx = upper_left_idx + width;
264  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
265  const unsigned lower_right_idx = lower_left_idx + width;
266 
267  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
268  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
269 }
270 
271 
272 template <typename DataType> unsigned
273 IntegralImage2D<DataType, 1>::getFiniteElementsCount (
274  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
275 {
276  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
277  const unsigned upper_right_idx = upper_left_idx + width;
278  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
279  const unsigned lower_right_idx = lower_left_idx + width;
280 
281  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
282  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
283 }
284 
285 
286 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
287 IntegralImage2D<DataType, 1>::getFirstOrderSumSE (
288  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
289 {
290  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
291  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
292  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
293  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
294 
295  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
296  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
297 }
298 
299 
300 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
301 IntegralImage2D<DataType, 1>::getSecondOrderSumSE (
302  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
303 {
304  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
305  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
306  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
307  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
308 
309  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
310  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
311 }
312 
313 
314 template <typename DataType> unsigned
315 IntegralImage2D<DataType, 1>::getFiniteElementsCountSE (
316  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
317 {
318  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
319  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
320  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
321  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
322 
323  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
324  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
325 }
326 
327 
328 template <typename DataType> void
329 IntegralImage2D<DataType, 1>::computeIntegralImages (
330  const DataType *data, unsigned row_stride, unsigned element_stride)
331 {
332  ElementType* previous_row = first_order_integral_image_.data();
333  ElementType* current_row = previous_row + (width_ + 1);
334  std::fill_n(previous_row, width_ + 1, 0);
335 
336  unsigned* count_previous_row = finite_values_integral_image_.data();
337  unsigned* count_current_row = count_previous_row + (width_ + 1);
338  std::fill_n(count_previous_row, width_ + 1, 0);
339 
340  if (!compute_second_order_integral_images_)
341  {
342  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
343  previous_row = current_row, current_row += (width_ + 1),
344  count_previous_row = count_current_row, count_current_row += (width_ + 1))
345  {
346  current_row [0] = 0.0;
347  count_current_row [0] = 0;
348  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
349  {
350  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
351  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
352  if (std::isfinite (data [valIdx]))
353  {
354  current_row [colIdx + 1] += data [valIdx];
355  ++(count_current_row [colIdx + 1]);
356  }
357  }
358  }
359  }
360  else
361  {
362  SecondOrderType* so_previous_row = second_order_integral_image_.data();
363  SecondOrderType* so_current_row = so_previous_row + (width_ + 1);
364  std::fill_n(so_previous_row, width_ + 1, 0);
365 
366  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
367  previous_row = current_row, current_row += (width_ + 1),
368  count_previous_row = count_current_row, count_current_row += (width_ + 1),
369  so_previous_row = so_current_row, so_current_row += (width_ + 1))
370  {
371  current_row [0] = 0.0;
372  so_current_row [0] = 0.0;
373  count_current_row [0] = 0;
374  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
375  {
376  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
377  so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx];
378  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
379  if (std::isfinite (data[valIdx]))
380  {
381  current_row [colIdx + 1] += data[valIdx];
382  so_current_row [colIdx + 1] += data[valIdx] * data[valIdx];
383  ++(count_current_row [colIdx + 1]);
384  }
385  }
386  }
387  }
388 }
389 
390 } // namespace pcl
391 
392 #endif // PCL_INTEGRAL_IMAGE2D_IMPL_H_
393 
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:115
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:73
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:101
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:129
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:87
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:143