390 lines
18 KiB
C++
390 lines
18 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2010-2011, Willow Garage, Inc.
|
|
* Copyright (c) 2012-, Open Perception, Inc.
|
|
*
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above
|
|
* copyright notice, this list of conditions and the following
|
|
* disclaimer in the documentation and/or other materials provided
|
|
* with the distribution.
|
|
* * Neither the name of the copyright holder(s) nor the names of its
|
|
* contributors may be used to endorse or promote products derived
|
|
* from this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
* $Id: feature.h 2784 2011-10-15 22:05:38Z aichim $
|
|
*/
|
|
|
|
|
|
#ifndef PCL_INTEGRAL_IMAGE2D_IMPL_H_
|
|
#define PCL_INTEGRAL_IMAGE2D_IMPL_H_
|
|
|
|
|
|
namespace pcl
|
|
{
|
|
|
|
template <typename DataType, unsigned Dimension> void
|
|
IntegralImage2D<DataType, Dimension>::setSecondOrderComputation (bool compute_second_order_integral_images)
|
|
{
|
|
compute_second_order_integral_images_ = compute_second_order_integral_images;
|
|
}
|
|
|
|
|
|
template <typename DataType, unsigned Dimension> void
|
|
IntegralImage2D<DataType, Dimension>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride)
|
|
{
|
|
if ((width + 1) * (height + 1) > first_order_integral_image_.size () )
|
|
{
|
|
width_ = width;
|
|
height_ = height;
|
|
first_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
|
|
finite_values_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
|
|
if (compute_second_order_integral_images_)
|
|
second_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
|
|
}
|
|
computeIntegralImages (data, row_stride, element_stride);
|
|
}
|
|
|
|
|
|
template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
|
|
IntegralImage2D<DataType, Dimension>::getFirstOrderSum (
|
|
unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
|
|
{
|
|
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
|
|
const unsigned upper_right_idx = upper_left_idx + width;
|
|
const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
|
|
const unsigned lower_right_idx = lower_left_idx + width;
|
|
|
|
return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
|
|
first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
|
|
}
|
|
|
|
|
|
template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
|
|
IntegralImage2D<DataType, Dimension>::getSecondOrderSum (
|
|
unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
|
|
{
|
|
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
|
|
const unsigned upper_right_idx = upper_left_idx + width;
|
|
const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
|
|
const unsigned lower_right_idx = lower_left_idx + width;
|
|
|
|
return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
|
|
second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
|
|
}
|
|
|
|
|
|
template <typename DataType, unsigned Dimension> unsigned
|
|
IntegralImage2D<DataType, Dimension>::getFiniteElementsCount (
|
|
unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
|
|
{
|
|
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
|
|
const unsigned upper_right_idx = upper_left_idx + width;
|
|
const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
|
|
const unsigned lower_right_idx = lower_left_idx + width;
|
|
|
|
return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
|
|
finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
|
|
}
|
|
|
|
|
|
template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
|
|
IntegralImage2D<DataType, Dimension>::getFirstOrderSumSE (
|
|
unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
|
|
{
|
|
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
|
|
const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
|
|
const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
|
|
const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
|
|
|
|
return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
|
|
first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
|
|
}
|
|
|
|
|
|
template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
|
|
IntegralImage2D<DataType, Dimension>::getSecondOrderSumSE (
|
|
unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
|
|
{
|
|
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
|
|
const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
|
|
const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
|
|
const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
|
|
|
|
return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
|
|
second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
|
|
}
|
|
|
|
|
|
template <typename DataType, unsigned Dimension> unsigned
|
|
IntegralImage2D<DataType, Dimension>::getFiniteElementsCountSE (
|
|
unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
|
|
{
|
|
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
|
|
const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
|
|
const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
|
|
const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
|
|
|
|
return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
|
|
finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
|
|
}
|
|
|
|
|
|
template <typename DataType, unsigned Dimension> void
|
|
IntegralImage2D<DataType, Dimension>::computeIntegralImages (
|
|
const DataType *data, unsigned row_stride, unsigned element_stride)
|
|
{
|
|
ElementType* previous_row = &first_order_integral_image_[0];
|
|
ElementType* current_row = previous_row + (width_ + 1);
|
|
*previous_row = ElementType::Zero(width_ + 1);
|
|
|
|
unsigned* count_previous_row = &finite_values_integral_image_[0];
|
|
unsigned* count_current_row = count_previous_row + (width_ + 1);
|
|
memset (count_previous_row, 0, sizeof (unsigned) * (width_ + 1));
|
|
|
|
if (!compute_second_order_integral_images_)
|
|
{
|
|
for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
|
|
previous_row = current_row, current_row += (width_ + 1),
|
|
count_previous_row = count_current_row, count_current_row += (width_ + 1))
|
|
{
|
|
current_row [0].setZero ();
|
|
count_current_row [0] = 0;
|
|
for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
|
|
{
|
|
current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
|
|
count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
|
|
const InputType* element = reinterpret_cast <const InputType*> (&data [valIdx]);
|
|
if (std::isfinite (element->sum ()))
|
|
{
|
|
current_row [colIdx + 1] += element->template cast<typename IntegralImageTypeTraits<DataType>::IntegralType>();
|
|
++(count_current_row [colIdx + 1]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
SecondOrderType* so_previous_row = &second_order_integral_image_[0];
|
|
SecondOrderType* so_current_row = so_previous_row + (width_ + 1);
|
|
*so_previous_row = SecondOrderType::Zero(width_ + 1);
|
|
|
|
SecondOrderType so_element;
|
|
for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
|
|
previous_row = current_row, current_row += (width_ + 1),
|
|
count_previous_row = count_current_row, count_current_row += (width_ + 1),
|
|
so_previous_row = so_current_row, so_current_row += (width_ + 1))
|
|
{
|
|
current_row [0].setZero ();
|
|
so_current_row [0].setZero ();
|
|
count_current_row [0] = 0;
|
|
for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
|
|
{
|
|
current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
|
|
so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx];
|
|
count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
|
|
|
|
const InputType* element = reinterpret_cast <const InputType*> (&data [valIdx]);
|
|
if (std::isfinite (element->sum ()))
|
|
{
|
|
current_row [colIdx + 1] += element->template cast<typename IntegralImageTypeTraits<DataType>::IntegralType>();
|
|
++(count_current_row [colIdx + 1]);
|
|
for (unsigned myIdx = 0, elIdx = 0; myIdx < Dimension; ++myIdx)
|
|
for (unsigned mxIdx = myIdx; mxIdx < Dimension; ++mxIdx, ++elIdx)
|
|
so_current_row [colIdx + 1][elIdx] += (*element)[myIdx] * (*element)[mxIdx];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
template <typename DataType> void
|
|
IntegralImage2D<DataType, 1>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride)
|
|
{
|
|
if ((width + 1) * (height + 1) > first_order_integral_image_.size () )
|
|
{
|
|
width_ = width;
|
|
height_ = height;
|
|
first_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
|
|
finite_values_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
|
|
if (compute_second_order_integral_images_)
|
|
second_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
|
|
}
|
|
computeIntegralImages (data, row_stride, element_stride);
|
|
}
|
|
|
|
|
|
template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
|
|
IntegralImage2D<DataType, 1>::getFirstOrderSum (
|
|
unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
|
|
{
|
|
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
|
|
const unsigned upper_right_idx = upper_left_idx + width;
|
|
const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
|
|
const unsigned lower_right_idx = lower_left_idx + width;
|
|
|
|
return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
|
|
first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
|
|
}
|
|
|
|
|
|
template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
|
|
IntegralImage2D<DataType, 1>::getSecondOrderSum (
|
|
unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
|
|
{
|
|
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
|
|
const unsigned upper_right_idx = upper_left_idx + width;
|
|
const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
|
|
const unsigned lower_right_idx = lower_left_idx + width;
|
|
|
|
return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
|
|
second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
|
|
}
|
|
|
|
|
|
template <typename DataType> unsigned
|
|
IntegralImage2D<DataType, 1>::getFiniteElementsCount (
|
|
unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
|
|
{
|
|
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
|
|
const unsigned upper_right_idx = upper_left_idx + width;
|
|
const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x;
|
|
const unsigned lower_right_idx = lower_left_idx + width;
|
|
|
|
return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
|
|
finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
|
|
}
|
|
|
|
|
|
template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
|
|
IntegralImage2D<DataType, 1>::getFirstOrderSumSE (
|
|
unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
|
|
{
|
|
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
|
|
const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
|
|
const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
|
|
const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
|
|
|
|
return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] -
|
|
first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] );
|
|
}
|
|
|
|
|
|
template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
|
|
IntegralImage2D<DataType, 1>::getSecondOrderSumSE (
|
|
unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
|
|
{
|
|
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
|
|
const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
|
|
const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
|
|
const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
|
|
|
|
return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] -
|
|
second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] );
|
|
}
|
|
|
|
|
|
template <typename DataType> unsigned
|
|
IntegralImage2D<DataType, 1>::getFiniteElementsCountSE (
|
|
unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
|
|
{
|
|
const unsigned upper_left_idx = start_y * (width_ + 1) + start_x;
|
|
const unsigned upper_right_idx = start_y * (width_ + 1) + end_x;
|
|
const unsigned lower_left_idx = end_y * (width_ + 1) + start_x;
|
|
const unsigned lower_right_idx = end_y * (width_ + 1) + end_x;
|
|
|
|
return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] -
|
|
finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] );
|
|
}
|
|
|
|
|
|
template <typename DataType> void
|
|
IntegralImage2D<DataType, 1>::computeIntegralImages (
|
|
const DataType *data, unsigned row_stride, unsigned element_stride)
|
|
{
|
|
ElementType* previous_row = &first_order_integral_image_[0];
|
|
ElementType* current_row = previous_row + (width_ + 1);
|
|
memset (previous_row, 0, sizeof (ElementType) * (width_ + 1));
|
|
|
|
unsigned* count_previous_row = &finite_values_integral_image_[0];
|
|
unsigned* count_current_row = count_previous_row + (width_ + 1);
|
|
memset (count_previous_row, 0, sizeof (unsigned) * (width_ + 1));
|
|
|
|
if (!compute_second_order_integral_images_)
|
|
{
|
|
for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
|
|
previous_row = current_row, current_row += (width_ + 1),
|
|
count_previous_row = count_current_row, count_current_row += (width_ + 1))
|
|
{
|
|
current_row [0] = 0.0;
|
|
count_current_row [0] = 0;
|
|
for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
|
|
{
|
|
current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
|
|
count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
|
|
if (std::isfinite (data [valIdx]))
|
|
{
|
|
current_row [colIdx + 1] += data [valIdx];
|
|
++(count_current_row [colIdx + 1]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
SecondOrderType* so_previous_row = &second_order_integral_image_[0];
|
|
SecondOrderType* so_current_row = so_previous_row + (width_ + 1);
|
|
memset (so_previous_row, 0, sizeof (SecondOrderType) * (width_ + 1));
|
|
|
|
for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
|
|
previous_row = current_row, current_row += (width_ + 1),
|
|
count_previous_row = count_current_row, count_current_row += (width_ + 1),
|
|
so_previous_row = so_current_row, so_current_row += (width_ + 1))
|
|
{
|
|
current_row [0] = 0.0;
|
|
so_current_row [0] = 0.0;
|
|
count_current_row [0] = 0;
|
|
for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
|
|
{
|
|
current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
|
|
so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx];
|
|
count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
|
|
if (std::isfinite (data[valIdx]))
|
|
{
|
|
current_row [colIdx + 1] += data[valIdx];
|
|
so_current_row [colIdx + 1] += data[valIdx] * data[valIdx];
|
|
++(count_current_row [colIdx + 1]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace pcl
|
|
|
|
#endif // PCL_INTEGRAL_IMAGE2D_IMPL_H_
|
|
|