Point Cloud Library (PCL)  1.3.1
integral_image_2d.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  */
00035 
00036 /* \author Stefan Holzer */
00037 
00038 #include <cstddef>
00039 
00040 template <class DataType, class IIDataType>
00041 pcl::IntegralImage2D<DataType, IIDataType>::IntegralImage2D (
00042     DataType * data, const int width, const int height,
00043     const int dimensions, const bool compute_second_order_integral_images,
00044     const int element_stride,const int row_stride) : width_(width), height_(height),
00045   dimensions_(dimensions),
00046   element_stride_(element_stride),
00047   row_stride_(row_stride),
00048   are_second_order_ii_available_(compute_second_order_integral_images)
00049 {
00050   // allocate memory
00051   for (int dimension_index = 0; dimension_index < dimensions; ++dimension_index)
00052   {
00053     IIDataType * data_array = new IIDataType[width*height]; // reinterpret_cast<IIDataType*>(new unsigned char(width*height*sizeof(IIDataType)));
00054 
00055     first_order_integral_images_.push_back(data_array);
00056   }
00057 
00058   if (compute_second_order_integral_images)
00059   {
00060     for (int dimension_index1 = 0; dimension_index1 < dimensions; ++dimension_index1)
00061     {
00062       second_order_integral_images_.push_back(::std::vector< IIDataType* >());
00063       for (int dimension_index2 = 0; dimension_index2 < dimensions; ++dimension_index2)
00064       {
00065         IIDataType * data_array = new IIDataType[width*height];
00066 
00067         second_order_integral_images_[dimension_index1].push_back(data_array);
00068       }
00069     }
00070   }
00071 
00072 
00073   // compute integral images
00074   if (dimensions_ == 1)
00075   {
00076     this->computeIntegralImagesOneDimensional (data);
00077   }
00078   else
00079   {
00080     this->computeIntegralImages (data);
00081   }
00082 }
00083 
00084 
00085 // ---------------------------------------------------------------------------- 
00086 template <class DataType, class IIDataType>
00087 pcl::IntegralImage2D<DataType, IIDataType>::~IntegralImage2D()
00088 {
00089   for (unsigned int index = 0; index < first_order_integral_images_.size(); ++index)
00090   {
00091     delete[] (first_order_integral_images_[index]);
00092   }
00093   for (unsigned int index1 = 0; index1 < second_order_integral_images_.size(); ++index1)
00094   {
00095     for (unsigned int index2 = 0; index2 < second_order_integral_images_[index1].size(); ++index2)
00096     {
00097       delete[] (second_order_integral_images_[index1][index2]);
00098     }
00099   }
00100 }
00101 
00102 
00103 // ---------------------------------------------------------------------------- 
00104 template <class DataType, class IIDataType> IIDataType 
00105 pcl::IntegralImage2D<DataType, IIDataType>::getSum (
00106   const int start_x, const int start_y,
00107   const int width, const int height,
00108   const int dimension_index )
00109 {
00110   const int l_ulX = start_x;
00111   const int l_ulY = start_y;
00112   const int l_lrX = start_x + width;
00113   const int l_lrY = start_y + height;
00114   
00115   return 
00116      (first_order_integral_images_[dimension_index][l_ulY*width_ + l_ulX]
00117     + first_order_integral_images_[dimension_index][l_lrY*width_ + l_lrX]
00118     - first_order_integral_images_[dimension_index][l_lrY*width_ + l_ulX]
00119     - first_order_integral_images_[dimension_index][l_ulY*width_ + l_lrX]);
00120 }
00121 
00122 
00123 // ---------------------------------------------------------------------------- 
00124 template <class DataType, class IIDataType> IIDataType
00125 pcl::IntegralImage2D<DataType, IIDataType>::getSum (
00126   const int start_x, const int start_y,
00127   const int width, const int height,
00128   const int dimension_index1, const int dimension_index2)
00129 {
00130   const int l_ulX = start_x;
00131   const int l_ulY = start_y;
00132   const int l_lrX = start_x + width;
00133   const int l_lrY = start_y + height;
00134   
00135   return second_order_integral_images_[dimension_index1][dimension_index2][l_ulY*width_ + l_ulX]
00136     + second_order_integral_images_[dimension_index1][dimension_index2][l_lrY*width_ + l_lrX]
00137     - second_order_integral_images_[dimension_index1][dimension_index2][l_lrY*width_ + l_ulX]
00138     - second_order_integral_images_[dimension_index1][dimension_index2][l_ulY*width_ + l_lrX];
00139 }
00140 
00141 
00142 // ---------------------------------------------------------------------------- 
00143 template <class DataType, class IIDataType> void
00144 pcl::IntegralImage2D<DataType, IIDataType>::computeIntegralImages (DataType *data)
00145 {
00146   // first element
00147   {
00148     const int row_index = 0;
00149     const int col_index = 0;
00150     for (int dimension_index = 0; dimension_index < dimensions_; ++dimension_index)
00151     {
00152       const IIDataType data_value = static_cast<IIDataType>(data[row_index*row_stride_ + col_index*element_stride_ + dimension_index]);
00153 
00154       if (!pcl_isfinite (data_value))
00155       {
00156         first_order_integral_images_[dimension_index][row_index*width_ + col_index] = 0.0f;
00157 
00158         if (are_second_order_ii_available_)
00159         {
00160           for (int dimension_index2 = 0; dimension_index2 < dimensions_; ++dimension_index2)
00161           {
00162             second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + col_index] = 0.0f;
00163           }
00164         }
00165       }
00166       else
00167       {
00168         first_order_integral_images_[dimension_index][row_index*width_ + col_index] = data_value;
00169 
00170         if (are_second_order_ii_available_)
00171         {
00172           for (int dimension_index2 = 0; dimension_index2 < dimensions_; ++dimension_index2)
00173           {
00174             const IIDataType data_value2 = static_cast<IIDataType>(data[row_index*row_stride_ + col_index*element_stride_ + dimension_index2]);
00175 
00176             if (!pcl_isfinite (data_value2))
00177             {
00178               second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + col_index] = 0.0f;
00179             }
00180             else
00181             {
00182               second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + col_index] = data_value*data_value2;
00183             }
00184           }
00185         }
00186       }
00187     }
00188   }
00189 
00190   // first row
00191   {
00192     const int row_index = 0; 
00193     for (int col_index = 1; col_index < width_; ++col_index)
00194     {
00195       for (int dimension_index = 0; dimension_index < dimensions_; ++dimension_index)
00196       {
00197         const IIDataType data_value = static_cast<IIDataType>(data[row_index*row_stride_ + col_index*element_stride_ + dimension_index]);
00198 
00199         if (!pcl_isfinite (data_value))
00200         {
00201           first_order_integral_images_[dimension_index][row_index*width_ + col_index] = 
00202             first_order_integral_images_[dimension_index][row_index*width_ + (col_index-1)];
00203 
00204           if (are_second_order_ii_available_)
00205           {
00206             for (int dimension_index2 = 0; dimension_index2 < dimensions_; ++dimension_index2)
00207             {
00208               second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + col_index] = 
00209                 second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + (col_index-1)];
00210             }
00211           }
00212         }
00213         else
00214         {
00215           first_order_integral_images_[dimension_index][row_index*width_ + col_index] = data_value
00216             + first_order_integral_images_[dimension_index][row_index*width_ + (col_index-1)];
00217 
00218           if (are_second_order_ii_available_)
00219           {
00220             for (int dimension_index2 = 0; dimension_index2 < dimensions_; ++dimension_index2)
00221             {
00222               const IIDataType data_value2 = static_cast<IIDataType>(data[row_index*row_stride_ + col_index*element_stride_ + dimension_index2]);
00223 
00224               if (!pcl_isfinite (data_value2))
00225               {
00226                 second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + col_index] = 
00227                   second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + (col_index-1)];
00228               }
00229               else
00230               {
00231                 second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + col_index] = data_value*data_value2
00232                   + second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + (col_index-1)];
00233               }
00234             }
00235           }
00236         }
00237       }
00238     }
00239   }
00240 
00241   // first column
00242   {
00243     const int col_index = 0;
00244     for (int row_index = 1; row_index < height_; ++row_index)
00245     {
00246       for (int dimension_index = 0; dimension_index < dimensions_; ++dimension_index)
00247       {
00248         const IIDataType data_value = static_cast<IIDataType>(data[row_index*row_stride_ + col_index*element_stride_ + dimension_index]);
00249 
00250         if (!pcl_isfinite (data_value))
00251         {
00252           first_order_integral_images_[dimension_index][row_index*width_ + col_index] = 
00253             first_order_integral_images_[dimension_index][(row_index-1)*width_ + col_index];
00254 
00255           if (are_second_order_ii_available_)
00256           {
00257             for (int dimension_index2 = 0; dimension_index2 < dimensions_; ++dimension_index2)
00258             {
00259               second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + col_index] = 
00260                 second_order_integral_images_[dimension_index][dimension_index2][(row_index-1)*width_ + col_index];
00261             }
00262           }
00263         }
00264         else
00265         {
00266           first_order_integral_images_[dimension_index][row_index*width_ + col_index] = data_value
00267             + first_order_integral_images_[dimension_index][(row_index-1)*width_ + col_index];
00268 
00269           if (are_second_order_ii_available_)
00270           {
00271             for (int dimension_index2 = 0; dimension_index2 < dimensions_; ++dimension_index2)
00272             {
00273               const IIDataType data_value2 = static_cast<IIDataType>(data[row_index*row_stride_ + col_index*element_stride_ + dimension_index2]);
00274 
00275               if (!pcl_isfinite (data_value))
00276               {
00277                 second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + col_index] = second_order_integral_images_[dimension_index][dimension_index2][(row_index-1)*width_ + col_index];
00278               }
00279               else
00280               {
00281                 second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + col_index] = data_value*data_value2
00282                   + second_order_integral_images_[dimension_index][dimension_index2][(row_index-1)*width_ + col_index];
00283               }
00284             }
00285           }
00286         }
00287       }
00288     }
00289   }
00290 
00291   // the rest
00292   for (int row_index = 1; row_index < height_; ++row_index)
00293   {
00294     for (int col_index = 1; col_index < width_; ++col_index)
00295     {
00296       for (int dimension_index = 0; dimension_index < dimensions_; ++dimension_index)
00297       {
00298         const IIDataType data_value = static_cast<IIDataType>(data[row_index*row_stride_ + col_index*element_stride_ + dimension_index]);
00299 
00300         if (!pcl_isfinite (data_value))
00301         {
00302           first_order_integral_images_[dimension_index][row_index*width_ + col_index] = 
00303             first_order_integral_images_[dimension_index][row_index*width_ + (col_index-1)]
00304             + first_order_integral_images_[dimension_index][(row_index-1)*width_ + col_index]
00305             - first_order_integral_images_[dimension_index][(row_index-1)*width_ + (col_index-1)];
00306 
00307           if (are_second_order_ii_available_)
00308           {
00309             for (int dimension_index2 = 0; dimension_index2 < dimensions_; ++dimension_index2)
00310             {
00311               second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + col_index] = 
00312                 second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + (col_index-1)]
00313                 + second_order_integral_images_[dimension_index][dimension_index2][(row_index-1)*width_ + col_index]
00314                 - second_order_integral_images_[dimension_index][dimension_index2][(row_index-1)*width_ + (col_index-1)];
00315             }
00316           }
00317         }
00318         else
00319         {
00320           first_order_integral_images_[dimension_index][row_index*width_ + col_index] = data_value
00321             + first_order_integral_images_[dimension_index][row_index*width_ + (col_index-1)]
00322             + first_order_integral_images_[dimension_index][(row_index-1)*width_ + col_index]
00323             - first_order_integral_images_[dimension_index][(row_index-1)*width_ + (col_index-1)];
00324 
00325           if (are_second_order_ii_available_)
00326           {
00327             for (int dimension_index2 = 0; dimension_index2 < dimensions_; ++dimension_index2)
00328             {
00329               const IIDataType data_value2 = static_cast<IIDataType>(data[row_index*row_stride_ + col_index*element_stride_ + dimension_index2]);
00330 
00331               if (!pcl_isfinite (data_value2))
00332               {
00333                 second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + col_index] = 
00334                   second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + (col_index-1)]
00335                   + second_order_integral_images_[dimension_index][dimension_index2][(row_index-1)*width_ + col_index]
00336                   - second_order_integral_images_[dimension_index][dimension_index2][(row_index-1)*width_ + (col_index-1)];
00337               }
00338               else
00339               {
00340                 second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + col_index] = data_value*data_value2
00341                   + second_order_integral_images_[dimension_index][dimension_index2][row_index*width_ + (col_index-1)]
00342                   + second_order_integral_images_[dimension_index][dimension_index2][(row_index-1)*width_ + col_index]
00343                   - second_order_integral_images_[dimension_index][dimension_index2][(row_index-1)*width_ + (col_index-1)];
00344               }
00345             }
00346           }
00347         }
00348       }
00349     }
00350   }
00351 }
00352 
00353 
00354 // ---------------------------------------------------------------------------- 
00355 template <class DataType, class IIDataType> void
00356 pcl::IntegralImage2D<DataType, IIDataType>::computeIntegralImagesOneDimensional (DataType *data)
00357 {
00358   IIDataType * first_order_integral_image = first_order_integral_images_[0];
00359   IIDataType * second_order_integral_image = NULL;
00360   if (are_second_order_ii_available_)
00361   {
00362     second_order_integral_image = second_order_integral_images_[0][0];
00363   }
00364 
00365   // first element
00366   {
00367     const int row_index = 0;
00368     const int col_index = 0;
00369 
00370     const IIDataType data_value = static_cast<IIDataType>(data[row_index*row_stride_ + col_index*element_stride_]);
00371 
00372     if (!pcl_isfinite (data_value))
00373     {
00374       first_order_integral_image[row_index*width_ + col_index] = 0.0;
00375 
00376       if (are_second_order_ii_available_)
00377       {
00378         second_order_integral_image[row_index*width_ + col_index] = 0.0;
00379       }
00380     }
00381     else
00382     {
00383       first_order_integral_image[row_index*width_ + col_index] = data_value;
00384     }
00385   }
00386 
00387   // first row
00388   {
00389     const int row_index = 0; 
00390     for (int col_index = 1; col_index < width_; ++col_index)
00391     {
00392       const IIDataType data_value = static_cast<IIDataType>(data[row_index*row_stride_ + col_index*element_stride_]);
00393 
00394       if (!pcl_isfinite (data_value))
00395       {
00396         first_order_integral_image[row_index*width_ + col_index] = first_order_integral_image[row_index*width_ + (col_index-1)];
00397 
00398         if (are_second_order_ii_available_)
00399         {
00400           second_order_integral_image[row_index*width_ + col_index] = second_order_integral_image[row_index*width_ + (col_index-1)];
00401         }
00402       }
00403       else
00404       {
00405         first_order_integral_image[row_index*width_ + col_index] = data_value
00406           + first_order_integral_image[row_index*width_ + (col_index-1)];
00407 
00408         if (are_second_order_ii_available_)
00409         {
00410           second_order_integral_image[row_index*width_ + col_index] = data_value*data_value
00411             + second_order_integral_image[row_index*width_ + (col_index-1)];
00412         }
00413       }
00414     }
00415   }
00416 
00417   // first column
00418   {
00419     const int col_index = 0;
00420     for (int row_index = 1; row_index < height_; ++row_index)
00421     {
00422       const IIDataType data_value = static_cast<IIDataType>(data[row_index*row_stride_ + col_index*element_stride_]);
00423 
00424       if (!pcl_isfinite (data_value))
00425       {
00426         first_order_integral_image[row_index*width_ + col_index] = first_order_integral_image[(row_index-1)*width_ + col_index];
00427 
00428         if (are_second_order_ii_available_)
00429         {
00430           second_order_integral_image[row_index*width_ + col_index] = second_order_integral_image[(row_index-1)*width_ + col_index];
00431         }
00432       }
00433       else
00434       {
00435         first_order_integral_image[row_index*width_ + col_index] = data_value
00436           + first_order_integral_image[(row_index-1)*width_ + col_index];
00437 
00438         if (are_second_order_ii_available_)
00439         {
00440           second_order_integral_image[row_index*width_ + col_index] = data_value*data_value
00441             + second_order_integral_image[(row_index-1)*width_ + col_index];
00442         }
00443       }
00444     }
00445   }
00446 
00447   // the rest
00448   for (int row_index = 1; row_index < height_; ++row_index)
00449   {
00450     for (int col_index = 1; col_index < width_; ++col_index)
00451     {
00452       const IIDataType data_value = static_cast<IIDataType>(data[row_index*row_stride_ + col_index*element_stride_]);
00453 
00454       if (!pcl_isfinite (data_value))
00455       {
00456         first_order_integral_image[row_index*width_ + col_index] = first_order_integral_image[(row_index-1)*width_ + (col_index-1)]
00457           + first_order_integral_image[(row_index-1)*width_ + col_index]
00458           + first_order_integral_image[row_index*width_ + (col_index-1)];
00459 
00460         if (are_second_order_ii_available_)
00461         {
00462           second_order_integral_image[row_index*width_ + col_index] = second_order_integral_image[row_index*width_ + (col_index-1)]
00463             + second_order_integral_image[(row_index-1)*width_ + col_index]
00464             - second_order_integral_image[(row_index-1)*width_ + (col_index-1)];
00465         }
00466       }
00467       else
00468       {
00469         first_order_integral_image[row_index*width_ + col_index] = data_value
00470           - first_order_integral_image[(row_index-1)*width_ + (col_index-1)]
00471           + first_order_integral_image[(row_index-1)*width_ + col_index]
00472           + first_order_integral_image[row_index*width_ + (col_index-1)];
00473 
00474         if (are_second_order_ii_available_)
00475         {
00476           second_order_integral_image[row_index*width_ + col_index] = data_value*data_value
00477             + second_order_integral_image[row_index*width_ + (col_index-1)]
00478             + second_order_integral_image[(row_index-1)*width_ + col_index]
00479             - second_order_integral_image[(row_index-1)*width_ + (col_index-1)];
00480         }
00481       }
00482     }
00483   }
00484 }
00485 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines