|
Point Cloud Library (PCL)
1.4.0
|
|
Point Cloud Library (PCL)
1.4.0
|
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 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id: integral_image2D.hpp 3771 2012-01-01 06:58:14Z rusu $ 00037 */ 00038 00039 #ifndef PCL_INTEGRAL_IMAGE2D_IMPL_H_ 00040 #define PCL_INTEGRAL_IMAGE2D_IMPL_H_ 00041 00042 #include <cstddef> 00043 00045 template <typename DataType, unsigned Dimension> void 00046 pcl::IntegralImage2D<DataType, Dimension>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride) 00047 { 00048 if ((width + 1) * (height + 1) > first_order_integral_image_.size () ) 00049 { 00050 width_ = width; 00051 height_ = height; 00052 first_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) ); 00053 finite_values_integral_image_.resize ( (width_ + 1) * (height_ + 1) ); 00054 if (compute_second_order_integral_images_) 00055 second_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) ); 00056 } 00057 computeIntegralImages (data, row_stride, element_stride); 00058 } 00059 00061 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::ElementType 00062 pcl::IntegralImage2D<DataType, Dimension>::getFirstOrderSum ( 00063 unsigned start_x, unsigned start_y, unsigned width, unsigned height) const 00064 { 00065 const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; 00066 const unsigned upper_right_idx = upper_left_idx + width; 00067 const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x; 00068 const unsigned lower_right_idx = lower_left_idx + width; 00069 00070 return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] - 00071 first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] ); 00072 } 00073 00075 template <typename DataType, unsigned Dimension> typename pcl::IntegralImage2D<DataType, Dimension>::SecondOrderType 00076 pcl::IntegralImage2D<DataType, Dimension>::getSecondOrderSum ( 00077 unsigned start_x, unsigned start_y, unsigned width, unsigned height) const 00078 { 00079 const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; 00080 const unsigned upper_right_idx = upper_left_idx + width; 00081 const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x; 00082 const unsigned lower_right_idx = lower_left_idx + width; 00083 00084 return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] - 00085 second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] ); 00086 } 00087 00089 template <typename DataType, unsigned Dimension> unsigned 00090 pcl::IntegralImage2D<DataType, Dimension>::getFiniteElementsCount ( 00091 unsigned start_x, unsigned start_y, unsigned width, unsigned height) const 00092 { 00093 const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; 00094 const unsigned upper_right_idx = upper_left_idx + width; 00095 const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x; 00096 const unsigned lower_right_idx = lower_left_idx + width; 00097 00098 return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] - 00099 finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] ); 00100 } 00101 00103 template <typename DataType, unsigned Dimension> void 00104 pcl::IntegralImage2D<DataType, Dimension>::computeIntegralImages ( 00105 const DataType *data, unsigned row_stride, unsigned element_stride) 00106 { 00107 ElementType* previous_row = &first_order_integral_image_[0]; 00108 ElementType* current_row = previous_row + (width_ + 1); 00109 memset (previous_row, 0, sizeof (ElementType) * (width_ + 1)); 00110 00111 unsigned* count_previous_row = &finite_values_integral_image_[0]; 00112 unsigned* count_current_row = count_previous_row + (width_ + 1); 00113 memset (count_previous_row, 0, sizeof (unsigned) * (width_ + 1)); 00114 00115 if (!compute_second_order_integral_images_) 00116 { 00117 for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride, 00118 previous_row = current_row, current_row += (width_ + 1), 00119 count_previous_row = count_current_row, count_current_row += (width_ + 1)) 00120 { 00121 current_row [0].setZero (); 00122 count_current_row [0] = 0; 00123 for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride) 00124 { 00125 current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx]; 00126 count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx]; 00127 const InputType* element = reinterpret_cast <const InputType*> (&data [valIdx]); 00128 if (pcl_isfinite (element->sum ())) 00129 { 00130 current_row [colIdx + 1] += element->template cast<typename IntegralImageTypeTraits<DataType>::IntegralType>(); 00131 ++(count_current_row [colIdx + 1]); 00132 } 00133 } 00134 } 00135 } 00136 else 00137 { 00138 SecondOrderType* so_previous_row = &second_order_integral_image_[0]; 00139 SecondOrderType* so_current_row = so_previous_row + (width_ + 1); 00140 memset (so_previous_row, 0, sizeof (SecondOrderType) * (width_ + 1)); 00141 00142 SecondOrderType so_element; 00143 for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride, 00144 previous_row = current_row, current_row += (width_ + 1), 00145 count_previous_row = count_current_row, count_current_row += (width_ + 1), 00146 so_previous_row = so_current_row, so_current_row += (width_ + 1)) 00147 { 00148 current_row [0].setZero (); 00149 so_current_row [0].setZero (); 00150 count_current_row [0] = 0; 00151 for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride) 00152 { 00153 current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx]; 00154 so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx]; 00155 count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx]; 00156 00157 const InputType* element = reinterpret_cast <const InputType*> (&data [valIdx]); 00158 if (pcl_isfinite (element->sum ())) 00159 { 00160 current_row [colIdx + 1] += element->template cast<typename IntegralImageTypeTraits<DataType>::IntegralType>(); 00161 ++(count_current_row [colIdx + 1]); 00162 for (unsigned myIdx = 0, elIdx = 0; myIdx < Dimension; ++myIdx) 00163 for (unsigned mxIdx = myIdx; mxIdx < Dimension; ++mxIdx, ++elIdx) 00164 so_current_row [colIdx + 1][elIdx] += (*element)[myIdx] * (*element)[mxIdx]; 00165 } 00166 } 00167 } 00168 } 00169 } 00170 00172 00173 template <typename DataType> void 00174 pcl::IntegralImage2D<DataType, 1>::setInput (const DataType * data, unsigned width,unsigned height, unsigned element_stride, unsigned row_stride) 00175 { 00176 if ((width + 1) * (height + 1) > first_order_integral_image_.size () ) 00177 { 00178 width_ = width; 00179 height_ = height; 00180 first_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) ); 00181 finite_values_integral_image_.resize ( (width_ + 1) * (height_ + 1) ); 00182 if (compute_second_order_integral_images_) 00183 second_order_integral_image_.resize ( (width_ + 1) * (height_ + 1) ); 00184 } 00185 computeIntegralImages (data, row_stride, element_stride); 00186 } 00187 00189 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::ElementType 00190 pcl::IntegralImage2D<DataType, 1>::getFirstOrderSum ( 00191 unsigned start_x, unsigned start_y, unsigned width, unsigned height) const 00192 { 00193 const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; 00194 const unsigned upper_right_idx = upper_left_idx + width; 00195 const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x; 00196 const unsigned lower_right_idx = lower_left_idx + width; 00197 00198 return (first_order_integral_image_[lower_right_idx] + first_order_integral_image_[upper_left_idx] - 00199 first_order_integral_image_[upper_right_idx] - first_order_integral_image_[lower_left_idx] ); 00200 } 00201 00203 template <typename DataType> typename pcl::IntegralImage2D<DataType, 1>::SecondOrderType 00204 pcl::IntegralImage2D<DataType, 1>::getSecondOrderSum ( 00205 unsigned start_x, unsigned start_y, unsigned width, unsigned height) const 00206 { 00207 const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; 00208 const unsigned upper_right_idx = upper_left_idx + width; 00209 const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x; 00210 const unsigned lower_right_idx = lower_left_idx + width; 00211 00212 return (second_order_integral_image_[lower_right_idx] + second_order_integral_image_[upper_left_idx] - 00213 second_order_integral_image_[upper_right_idx] - second_order_integral_image_[lower_left_idx] ); 00214 } 00215 00217 template <typename DataType> unsigned 00218 pcl::IntegralImage2D<DataType, 1>::getFiniteElementsCount ( 00219 unsigned start_x, unsigned start_y, unsigned width, unsigned height) const 00220 { 00221 const unsigned upper_left_idx = start_y * (width_ + 1) + start_x; 00222 const unsigned upper_right_idx = upper_left_idx + width; 00223 const unsigned lower_left_idx = (start_y + height) * (width_ + 1) + start_x; 00224 const unsigned lower_right_idx = lower_left_idx + width; 00225 00226 return (finite_values_integral_image_[lower_right_idx] + finite_values_integral_image_[upper_left_idx] - 00227 finite_values_integral_image_[upper_right_idx] - finite_values_integral_image_[lower_left_idx] ); 00228 } 00229 00231 template <typename DataType> void 00232 pcl::IntegralImage2D<DataType, 1>::computeIntegralImages ( 00233 const DataType *data, unsigned row_stride, unsigned element_stride) 00234 { 00235 ElementType* previous_row = &first_order_integral_image_[0]; 00236 ElementType* current_row = previous_row + (width_ + 1); 00237 memset (previous_row, 0, sizeof (ElementType) * (width_ + 1)); 00238 00239 unsigned* count_previous_row = &finite_values_integral_image_[0]; 00240 unsigned* count_current_row = count_previous_row + (width_ + 1); 00241 memset (count_previous_row, 0, sizeof (unsigned) * (width_ + 1)); 00242 00243 if (!compute_second_order_integral_images_) 00244 { 00245 for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride, 00246 previous_row = current_row, current_row += (width_ + 1), 00247 count_previous_row = count_current_row, count_current_row += (width_ + 1)) 00248 { 00249 current_row [0] = 0.0; 00250 count_current_row [0] = 0; 00251 for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride) 00252 { 00253 current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx]; 00254 count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx]; 00255 if (pcl_isfinite (data [valIdx])) 00256 { 00257 current_row [colIdx + 1] += data [valIdx]; 00258 ++(count_current_row [colIdx + 1]); 00259 } 00260 } 00261 } 00262 } 00263 else 00264 { 00265 SecondOrderType* so_previous_row = &second_order_integral_image_[0]; 00266 SecondOrderType* so_current_row = so_previous_row + (width_ + 1); 00267 memset (so_previous_row, 0, sizeof (SecondOrderType) * (width_ + 1)); 00268 00269 for (unsigned rowIdx = 0; rowIdx < height_; ++rowIdx, data += row_stride, 00270 previous_row = current_row, current_row += (width_ + 1), 00271 count_previous_row = count_current_row, count_current_row += (width_ + 1), 00272 so_previous_row = so_current_row, so_current_row += (width_ + 1)) 00273 { 00274 current_row [0] = 0.0; 00275 so_current_row [0] = 0.0; 00276 count_current_row [0] = 0; 00277 for (unsigned colIdx = 0, valIdx = 0; colIdx < width_; ++colIdx, valIdx += element_stride) 00278 { 00279 current_row [colIdx + 1] = previous_row [colIdx + 1] + current_row [colIdx] - previous_row [colIdx]; 00280 so_current_row [colIdx + 1] = so_previous_row [colIdx + 1] + so_current_row [colIdx] - so_previous_row [colIdx]; 00281 count_current_row [colIdx + 1] = count_previous_row [colIdx + 1] + count_current_row [colIdx] - count_previous_row [colIdx]; 00282 if (pcl_isfinite (data[valIdx])) 00283 { 00284 current_row [colIdx + 1] += data[valIdx]; 00285 so_current_row [colIdx + 1] += data[valIdx] * data[valIdx]; 00286 ++(count_current_row [colIdx + 1]); 00287 } 00288 } 00289 } 00290 } 00291 } 00292 #endif // PCL_INTEGRAL_IMAGE2D_IMPL_H_ 00293
1.7.6.1
