integral_image2D.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of the copyright holder(s) nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id: feature.h 2784 2011-10-15 22:05:38Z aichim $
00038  */
00039 
00040 #ifndef PCL_INTEGRAL_IMAGE2D_IMPL_H_
00041 #define PCL_INTEGRAL_IMAGE2D_IMPL_H_
00042 
00043 #include <cstddef>
00044 
00046 template <typename DataType, unsigned Dimension> void
00047 pcl::IntegralImage2D<DataType, Dimension>::setSecondOrderComputation (bool compute_second_order_integral_images)
00048 {
00049   compute_second_order_integral_images_ = compute_second_order_integral_images;
00050 }
00051 
00053 template <typename DataType, unsigned Dimension> void
00054 pcl::IntegralImage2D<DataType, Dimension>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride)
00055 {
00056   if ((width + 1) * (height + 1) > first_order_integral_image_.size () )
00057   {
00058     width_  = width;
00059     height_ = height;
00060     first_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
00061     finite_values_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
00062     if (compute_second_order_integral_images_)
00063       second_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
00064   }
00065   computeIntegralImages (data, row_stride, element_stride);
00066 }
00067 
00069 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
00070 pcl::IntegralImage2D<DataType, Dimension>::getFirstOrderSum (
00071     unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
00072 {
00073   const unsigned upper_left_idx      = start_y * (width_ + 1) + start_x;
00074   const unsigned upper_right_idx     = upper_left_idx + width;
00075   const unsigned lower_left_idx      = (start_y + height) * (width_ + 1) + start_x;
00076   const unsigned lower_right_idx     = lower_left_idx + width;
00077 
00078   return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx]  -
00079           first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx]  );
00080 }
00081 
00083 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
00084 pcl::IntegralImage2D<DataType, Dimension>::getSecondOrderSum (
00085     unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
00086 {
00087   const unsigned upper_left_idx      = start_y * (width_ + 1) + start_x;
00088   const unsigned upper_right_idx     = upper_left_idx + width;
00089   const unsigned lower_left_idx      = (start_y + height) * (width_ + 1) + start_x;
00090   const unsigned lower_right_idx     = lower_left_idx + width;
00091 
00092   return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx]  -
00093           second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx]  );
00094 }
00095 
00097 template <typename DataType, unsigned Dimension> unsigned
00098 pcl::IntegralImage2D<DataType, Dimension>::getFiniteElementsCount (
00099     unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
00100 {
00101   const unsigned upper_left_idx      = start_y * (width_ + 1) + start_x;
00102   const unsigned upper_right_idx     = upper_left_idx + width;
00103   const unsigned lower_left_idx      = (start_y + height) * (width_ + 1) + start_x;
00104   const unsigned lower_right_idx     = lower_left_idx + width;
00105 
00106   return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx]  -
00107           finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx]  );
00108 }
00109 
00111 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType
00112 pcl::IntegralImage2D<DataType, Dimension>::getFirstOrderSumSE (
00113     unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
00114 {
00115   const unsigned upper_left_idx      = start_y * (width_ + 1) + start_x;
00116   const unsigned upper_right_idx     = start_y * (width_ + 1) + end_x;
00117   const unsigned lower_left_idx      = end_y * (width_ + 1) + start_x;
00118   const unsigned lower_right_idx     = end_y * (width_ + 1) + end_x;
00119 
00120   return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx]  -
00121           first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx]  );
00122 }
00123 
00125 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType
00126 pcl::IntegralImage2D<DataType, Dimension>::getSecondOrderSumSE (
00127     unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
00128 {
00129   const unsigned upper_left_idx      = start_y * (width_ + 1) + start_x;
00130   const unsigned upper_right_idx     = start_y * (width_ + 1) + end_x;
00131   const unsigned lower_left_idx      = end_y * (width_ + 1) + start_x;
00132   const unsigned lower_right_idx     = end_y * (width_ + 1) + end_x;
00133 
00134   return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx]  -
00135           second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx]  );
00136 }
00137 
00139 template <typename DataType, unsigned Dimension> unsigned
00140 pcl::IntegralImage2D<DataType, Dimension>::getFiniteElementsCountSE (
00141     unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
00142 {
00143   const unsigned upper_left_idx      = start_y * (width_ + 1) + start_x;
00144   const unsigned upper_right_idx     = start_y * (width_ + 1) + end_x;
00145   const unsigned lower_left_idx      = end_y * (width_ + 1) + start_x;
00146   const unsigned lower_right_idx     = end_y * (width_ + 1) + end_x;
00147 
00148   return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx]  -
00149           finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx]  );
00150 }
00151 
00153 template <typename DataType, unsigned Dimension> void
00154 pcl::IntegralImage2D<DataType, Dimension>::computeIntegralImages (
00155     const DataType *data, unsigned row_stride, unsigned element_stride)
00156 {
00157   ElementType* previous_row = &first_order_integral_image_[0];
00158   ElementType* current_row  = previous_row + (width_ + 1);
00159   memset (previous_row, 0, sizeof (ElementType) * (width_ + 1));
00160 
00161   unsigned* count_previous_row = &finite_values_integral_image_[0];
00162   unsigned* count_current_row  = count_previous_row + (width_ + 1);
00163   memset (count_previous_row, 0, sizeof (unsigned) * (width_ + 1));
00164 
00165   if (!compute_second_order_integral_images_)
00166   {
00167     for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
00168                                                 previous_row = current_row, current_row += (width_ + 1),
00169                                                 count_previous_row = count_current_row, count_current_row += (width_ + 1))
00170     {
00171       current_row [0].setZero ();
00172       count_current_row [0] = 0;
00173       for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
00174       {
00175         current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
00176         count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
00177         const InputType* element = reinterpret_cast <const InputType*> (&data [valIdx]);
00178         if (pcl_isfinite (element->sum ()))
00179         {
00180           current_row [colIdx + 1] += element->template cast<typename IntegralImageTypeTraits<DataType>::IntegralType>();
00181           ++(count_current_row [colIdx + 1]);
00182         }
00183       }
00184     }
00185   }
00186   else
00187   {
00188     SecondOrderType* so_previous_row = &second_order_integral_image_[0];
00189     SecondOrderType* so_current_row  = so_previous_row + (width_ + 1);
00190     memset (so_previous_row, 0, sizeof (SecondOrderType) * (width_ + 1));
00191 
00192     SecondOrderType so_element;
00193     for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
00194                                                 previous_row = current_row, current_row += (width_ + 1),
00195                                                 count_previous_row = count_current_row, count_current_row += (width_ + 1),
00196                                                 so_previous_row = so_current_row, so_current_row += (width_ + 1))
00197     {
00198       current_row [0].setZero ();
00199       so_current_row [0].setZero ();
00200       count_current_row [0] = 0;
00201       for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
00202       {
00203         current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
00204         so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx];
00205         count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
00206 
00207         const InputType* element = reinterpret_cast <const InputType*> (&data [valIdx]);
00208         if (pcl_isfinite (element->sum ()))
00209         {
00210           current_row [colIdx + 1] += element->template cast<typename IntegralImageTypeTraits<DataType>::IntegralType>();
00211           ++(count_current_row [colIdx + 1]);
00212           for (unsigned myIdx = 0, elIdx = 0; myIdx < Dimension; ++myIdx)
00213             for (unsigned mxIdx = myIdx; mxIdx < Dimension; ++mxIdx, ++elIdx)
00214               so_current_row [colIdx + 1][elIdx] += (*element)[myIdx] * (*element)[mxIdx];
00215         }
00216       }
00217     }
00218   }
00219 }
00220 
00222 
00223 template <typename DataType> void
00224 pcl::IntegralImage2D<DataType, 1>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride)
00225 {
00226   if ((width + 1) * (height + 1) > first_order_integral_image_.size () )
00227   {
00228     width_  = width;
00229     height_ = height;
00230     first_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
00231     finite_values_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
00232     if (compute_second_order_integral_images_)
00233       second_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) );
00234   }
00235   computeIntegralImages (data, row_stride, element_stride);
00236 }
00237 
00239 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
00240 pcl::IntegralImage2D<DataType, 1>::getFirstOrderSum (
00241     unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
00242 {
00243   const unsigned upper_left_idx      = start_y * (width_ + 1) + start_x;
00244   const unsigned upper_right_idx     = upper_left_idx + width;
00245   const unsigned lower_left_idx      = (start_y + height) * (width_ + 1) + start_x;
00246   const unsigned lower_right_idx     = lower_left_idx + width;
00247 
00248   return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx]  -
00249           first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx]  );
00250 }
00251 
00253 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
00254 pcl::IntegralImage2D<DataType, 1>::getSecondOrderSum (
00255     unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
00256 {
00257   const unsigned upper_left_idx      = start_y * (width_ + 1) + start_x;
00258   const unsigned upper_right_idx     = upper_left_idx + width;
00259   const unsigned lower_left_idx      = (start_y + height) * (width_ + 1) + start_x;
00260   const unsigned lower_right_idx     = lower_left_idx + width;
00261 
00262   return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx]  -
00263           second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx]  );
00264 }
00265 
00267 template <typename DataType> unsigned
00268 pcl::IntegralImage2D<DataType, 1>::getFiniteElementsCount (
00269     unsigned start_x, unsigned start_y, unsigned width, unsigned height) const
00270 {
00271   const unsigned upper_left_idx      = start_y * (width_ + 1) + start_x;
00272   const unsigned upper_right_idx     = upper_left_idx + width;
00273   const unsigned lower_left_idx      = (start_y + height) * (width_ + 1) + start_x;
00274   const unsigned lower_right_idx     = lower_left_idx + width;
00275 
00276   return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx]  -
00277           finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx]  );
00278 }
00279 
00281 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType
00282 pcl::IntegralImage2D<DataType, 1>::getFirstOrderSumSE (
00283     unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
00284 {
00285   const unsigned upper_left_idx      = start_y * (width_ + 1) + start_x;
00286   const unsigned upper_right_idx     = start_y * (width_ + 1) + end_x;
00287   const unsigned lower_left_idx      = end_y * (width_ + 1) + start_x;
00288   const unsigned lower_right_idx     = end_y * (width_ + 1) + end_x;
00289 
00290   return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx]  -
00291           first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx]  );
00292 }
00293 
00295 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType
00296 pcl::IntegralImage2D<DataType, 1>::getSecondOrderSumSE (
00297     unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
00298 {
00299   const unsigned upper_left_idx      = start_y * (width_ + 1) + start_x;
00300   const unsigned upper_right_idx     = start_y * (width_ + 1) + end_x;
00301   const unsigned lower_left_idx      = end_y * (width_ + 1) + start_x;
00302   const unsigned lower_right_idx     = end_y * (width_ + 1) + end_x;
00303 
00304   return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx]  -
00305           second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx]  );
00306 }
00307 
00309 template <typename DataType> unsigned
00310 pcl::IntegralImage2D<DataType, 1>::getFiniteElementsCountSE (
00311     unsigned start_x, unsigned start_y, unsigned end_x, unsigned end_y) const
00312 {
00313   const unsigned upper_left_idx      = start_y * (width_ + 1) + start_x;
00314   const unsigned upper_right_idx     = start_y * (width_ + 1) + end_x;
00315   const unsigned lower_left_idx      = end_y * (width_ + 1) + start_x;
00316   const unsigned lower_right_idx     = end_y * (width_ + 1) + end_x;
00317 
00318   return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx]  -
00319           finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx]  );
00320 }
00321 
00323 template <typename DataType> void
00324 pcl::IntegralImage2D<DataType, 1>::computeIntegralImages (
00325     const DataType *data, unsigned row_stride, unsigned element_stride)
00326 {
00327   ElementType* previous_row = &first_order_integral_image_[0];
00328   ElementType* current_row  = previous_row + (width_ + 1);
00329   memset (previous_row, 0, sizeof (ElementType) * (width_ + 1));
00330 
00331   unsigned* count_previous_row = &finite_values_integral_image_[0];
00332   unsigned* count_current_row  = count_previous_row + (width_ + 1);
00333   memset (count_previous_row, 0, sizeof (unsigned) * (width_ + 1));
00334 
00335   if (!compute_second_order_integral_images_)
00336   {
00337     for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
00338                                                 previous_row = current_row, current_row += (width_ + 1),
00339                                                 count_previous_row = count_current_row, count_current_row += (width_ + 1))
00340     {
00341       current_row [0] = 0.0;
00342       count_current_row [0] = 0;
00343       for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
00344       {
00345         current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
00346         count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
00347         if (pcl_isfinite (data [valIdx]))
00348         {
00349           current_row [colIdx + 1] += data [valIdx];
00350           ++(count_current_row [colIdx + 1]);
00351         }
00352       }
00353     }
00354   }
00355   else
00356   {
00357     SecondOrderType* so_previous_row = &second_order_integral_image_[0];
00358     SecondOrderType* so_current_row  = so_previous_row + (width_ + 1);
00359     memset (so_previous_row, 0, sizeof (SecondOrderType) * (width_ + 1));
00360 
00361     for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride,
00362                                                 previous_row = current_row, current_row += (width_ + 1),
00363                                                 count_previous_row = count_current_row, count_current_row += (width_ + 1),
00364                                                 so_previous_row = so_current_row, so_current_row += (width_ + 1))
00365     {
00366       current_row [0] = 0.0;
00367       so_current_row [0] = 0.0;
00368       count_current_row [0] = 0;
00369       for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride)
00370       {
00371         current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx];
00372         so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx];
00373         count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx];
00374         if (pcl_isfinite (data[valIdx]))
00375         {
00376           current_row [colIdx + 1] += data[valIdx];
00377           so_current_row [colIdx + 1] += data[valIdx] * data[valIdx];
00378           ++(count_current_row [colIdx + 1]);
00379         }
00380       }
00381     }
00382   }
00383 }
00384 #endif    // PCL_INTEGRAL_IMAGE2D_IMPL_H_
00385 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:25:02