Point Cloud Library (PCL)  1.15.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  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  const std::size_t ii_size = static_cast<std::size_t>(width_ + 1) * static_cast<std::size_t>(height_ + 1);
234  first_order_integral_image_.resize (ii_size);
235  finite_values_integral_image_.resize (ii_size);
236  if (compute_second_order_integral_images_)
237  second_order_integral_image_.resize (ii_size);
238  }
239  computeIntegralImages (data, row_stride, element_stride);
240 }
241 
242 
243 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
244 IntegralImage2D<DataType, 1>::getFirstOrderSum (
245  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
246 {
247  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
248  const unsigned upper_right_idx = upper_left_idx + width;
249  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
250  const unsigned lower_right_idx = lower_left_idx + width;
251 
252  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
253  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
254 }
255 
256 
257 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
258 IntegralImage2D<DataType, 1>::getSecondOrderSum (
259  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
260 {
261  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
262  const unsigned upper_right_idx = upper_left_idx + width;
263  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
264  const unsigned lower_right_idx = lower_left_idx + width;
265 
266  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
267  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
268 }
269 
270 
271 template <typename DataType> unsigned
272 IntegralImage2D<DataType, 1>::getFiniteElementsCount (
273  unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
274 {
275  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
276  const unsigned upper_right_idx = upper_left_idx + width;
277  const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
278  const unsigned lower_right_idx = lower_left_idx + width;
279 
280  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
281  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
282 }
283 
284 
285 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
286 IntegralImage2D<DataType, 1>::getFirstOrderSumSE (
287  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
288 {
289  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
290  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
291  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
292  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
293 
294  return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
295  first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
296 }
297 
298 
299 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
300 IntegralImage2D<DataType, 1>::getSecondOrderSumSE (
301  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
302 {
303  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
304  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
305  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
306  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
307 
308  return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
309  second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
310 }
311 
312 
313 template <typename DataType> unsigned
314 IntegralImage2D<DataType, 1>::getFiniteElementsCountSE (
315  unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
316 {
317  const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
318  const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
319  const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
320  const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
321 
322  return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
323  finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
324 }
325 
326 
327 template <typename DataType> void
328 IntegralImage2D<DataType, 1>::computeIntegralImages (
329  const DataType *data, unsigned row_stride, unsigned element_stride)
330 {
331  ElementType* previous_row = first_order_integral_image_.data();
332  ElementType* current_row = previous_row + (width_ + 1);
333  std::fill_n(previous_row, width_ + 1, 0);
334 
335  unsigned* count_previous_row = finite_values_integral_image_.data();
336  unsigned* count_current_row = count_previous_row + (width_ + 1);
337  std::fill_n(count_previous_row, width_ + 1, 0);
338 
339  if (!compute_second_order_integral_images_)
340  {
341  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
342  previous_row = current_row, current_row += (width_ + 1),
343  count_previous_row = count_current_row, count_current_row += (width_ + 1))
344  {
345  current_row [0] = 0.0;
346  count_current_row [0] = 0;
347  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
348  {
349  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
350  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
351  if (std::isfinite (data [valIdx]))
352  {
353  current_row [colIdx + 1] += data [valIdx];
354  ++(count_current_row [colIdx + 1]);
355  }
356  }
357  }
358  }
359  else
360  {
361  SecondOrderType* so_previous_row = second_order_integral_image_.data();
362  SecondOrderType* so_current_row = so_previous_row + (width_ + 1);
363  std::fill_n(so_previous_row, width_ + 1, 0);
364 
365  for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
366  previous_row = current_row, current_row += (width_ + 1),
367  count_previous_row = count_current_row, count_current_row += (width_ + 1),
368  so_previous_row = so_current_row, so_current_row += (width_ + 1))
369  {
370  current_row [0] = 0.0;
371  so_current_row [0] = 0.0;
372  count_current_row [0] = 0;
373  for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
374  {
375  current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
376  so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx];
377  count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
378  if (std::isfinite (data[valIdx]))
379  {
380  current_row [colIdx + 1] += data[valIdx];
381  so_current_row [colIdx + 1] += data[valIdx] * data[valIdx];
382  ++(count_current_row [colIdx + 1]);
383  }
384  }
385  }
386  }
387 }
388 
389 } // namespace pcl
390 
391 #endif // PCL_INTEGRAL_IMAGE2D_IMPL_H_
392 
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