Point Cloud Library (PCL)
1.3.1
|
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