Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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 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
72template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
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
86template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
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
100template <typename DataType, unsigned Dimension> unsigned
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
114template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
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
128template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
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
142template <typename DataType, unsigned Dimension> unsigned
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
156template <typename DataType, unsigned Dimension> void
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))
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
227template <typename DataType> void
228IntegralImage2D<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
244template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
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
258template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
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
272template <typename DataType> unsigned
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
286template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
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
300template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
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
314template <typename DataType> unsigned
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
328template <typename DataType> void
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
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.