Point Cloud Library (PCL) 1.12.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
45namespace pcl
46{
47
48template <typename DataType, unsigned Dimension> void
49IntegralImage2D<DataType, Dimension>::setSecondOrderComputation (bool compute_second_order_integral_images)
50{
51 compute_second_order_integral_images_ = compute_second_order_integral_images;
52}
53
54
55template <typename DataType, unsigned Dimension> void
56IntegralImage2D<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
71template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
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
85template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
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
99template <typename DataType, unsigned Dimension> unsigned
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
113template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
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
127template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
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] );
139
140
141template <typename DataType, unsigned Dimension> unsigned
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
155template <typename DataType, unsigned Dimension> void
157 const DataType *data, unsigned row_stride, unsigned element_stride)
159 ElementType* previous_row = &first_order_integral_image_[0];
160 ElementType* current_row = previous_row + (width_ + 1);
161 *previous_row = ElementType::Zero(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)
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 }
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 *so_previous_row = SecondOrderType::Zero(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
224template <typename DataType> void
225IntegralImage2D<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
240template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
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
254template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
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
268template <typename DataType> unsigned
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
282template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
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
296template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
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
310template <typename DataType> unsigned
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
324template <typename DataType> void
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
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.