Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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$ 00037 * 00038 */ 00039 00040 #ifndef PCL_FILTERS_IMPL_PYRAMID_HPP 00041 #define PCL_FILTERS_IMPL_PYRAMID_HPP 00042 00043 template <typename PointT> bool 00044 pcl::filters::Pyramid<PointT>::initCompute () 00045 { 00046 if (!input_->isOrganized ()) 00047 { 00048 PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!", getClassName ().c_str ()); 00049 return (false); 00050 } 00051 00052 if (levels_ < 2) 00053 { 00054 PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should be at least 2!", getClassName ().c_str ()); 00055 return (false); 00056 } 00057 00058 // size_t ratio (std::pow (2, levels_)); 00059 // size_t last_width = input_->width / ratio; 00060 // size_t last_height = input_->height / ratio; 00061 00062 if (levels_ > 4) 00063 { 00064 PCL_ERROR ("[pcl::fileters::%s::initCompute] Number of levels should not exceed 4!", getClassName ().c_str ()); 00065 return (false); 00066 } 00067 00068 if (large_) 00069 { 00070 Eigen::VectorXf k (5); 00071 k << 1.f/16.f, 1.f/4.f, 3.f/8.f, 1.f/4.f, 1.f/16.f; 00072 kernel_ = k * k.transpose (); 00073 if (threshold_ != std::numeric_limits<float>::infinity ()) 00074 threshold_ *= 2 * threshold_; 00075 00076 } 00077 else 00078 { 00079 Eigen::VectorXf k (3); 00080 k << 1.f/4.f, 1.f/2.f, 1.f/4.f; 00081 kernel_ = k * k.transpose (); 00082 if (threshold_ != std::numeric_limits<float>::infinity ()) 00083 threshold_ *= threshold_; 00084 } 00085 00086 return (true); 00087 } 00088 00089 template <typename PointT> void 00090 pcl::filters::Pyramid<PointT>::compute (std::vector<PointCloudPtr>& output) 00091 { 00092 std::cout << "compute" << std::endl; 00093 if (!initCompute ()) 00094 { 00095 PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ()); 00096 return; 00097 } 00098 00099 int kernel_rows = static_cast<int> (kernel_.rows ()); 00100 int kernel_cols = static_cast<int> (kernel_.cols ()); 00101 int kernel_center_x = kernel_cols / 2; 00102 int kernel_center_y = kernel_rows / 2; 00103 00104 output.resize (levels_ + 1); 00105 output[0].reset (new pcl::PointCloud<PointT>); 00106 *(output[0]) = *input_; 00107 00108 if (input_->is_dense) 00109 { 00110 for (int l = 1; l <= levels_; ++l) 00111 { 00112 output[l].reset (new pcl::PointCloud<PointT> (output[l-1]->width/2, output[l-1]->height/2)); 00113 const PointCloud<PointT> &previous = *output[l-1]; 00114 PointCloud<PointT> &next = *output[l]; 00115 #ifdef _OPENMP 00116 #pragma omp parallel for shared (next) num_threads(threads_) 00117 #endif 00118 for(int i=0; i < next.height; ++i) 00119 { 00120 for(int j=0; j < next.width; ++j) 00121 { 00122 for(int m=0; m < kernel_rows; ++m) 00123 { 00124 int mm = kernel_rows - 1 - m; 00125 for(int n=0; n < kernel_cols; ++n) 00126 { 00127 int nn = kernel_cols - 1 - n; 00128 00129 int ii = 2*i + (m - kernel_center_y); 00130 int jj = 2*j + (n - kernel_center_x); 00131 00132 if (ii < 0) ii = 0; 00133 if (ii >= previous.height) ii = previous.height - 1; 00134 if (jj < 0) jj = 0; 00135 if (jj >= previous.width) jj = previous.width - 1; 00136 next.at (j,i) += previous.at (jj,ii) * kernel_ (mm,nn); 00137 } 00138 } 00139 } 00140 } 00141 } 00142 } 00143 else 00144 { 00145 for (int l = 1; l <= levels_; ++l) 00146 { 00147 output[l].reset (new pcl::PointCloud<PointT> (output[l-1]->width/2, output[l-1]->height/2)); 00148 const PointCloud<PointT> &previous = *output[l-1]; 00149 PointCloud<PointT> &next = *output[l]; 00150 #ifdef _OPENMP 00151 #pragma omp parallel for shared (next) num_threads(threads_) 00152 #endif 00153 for(int i=0; i < next.height; ++i) 00154 { 00155 for(int j=0; j < next.width; ++j) 00156 { 00157 float weight = 0; 00158 for(int m=0; m < kernel_rows; ++m) 00159 { 00160 int mm = kernel_rows - 1 - m; 00161 for(int n=0; n < kernel_cols; ++n) 00162 { 00163 int nn = kernel_cols - 1 - n; 00164 int ii = 2*i + (m - kernel_center_y); 00165 int jj = 2*j + (n - kernel_center_x); 00166 if (ii < 0) ii = 0; 00167 if (ii >= previous.height) ii = previous.height - 1; 00168 if (jj < 0) jj = 0; 00169 if (jj >= previous.width) jj = previous.width - 1; 00170 if (!isFinite (previous.at (jj,ii))) 00171 continue; 00172 if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_) 00173 { 00174 next.at (j,i) += previous.at (jj,ii).x * kernel_ (mm,nn); 00175 weight+= kernel_ (mm,nn); 00176 } 00177 } 00178 } 00179 if (weight == 0) 00180 nullify (next.at (j,i)); 00181 else 00182 { 00183 weight = 1.f/weight; 00184 next.at (j,i)*= weight; 00185 } 00186 } 00187 } 00188 } 00189 } 00190 } 00191 00192 namespace pcl 00193 { 00194 namespace filters 00195 { 00196 template <> void 00197 Pyramid<pcl::PointXYZRGB>::compute (std::vector<Pyramid<pcl::PointXYZRGB>::PointCloudPtr> &output) 00198 { 00199 std::cout << "PointXYZRGB" << std::endl; 00200 if (!initCompute ()) 00201 { 00202 PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ()); 00203 return; 00204 } 00205 00206 int kernel_rows = static_cast<int> (kernel_.rows ()); 00207 int kernel_cols = static_cast<int> (kernel_.cols ()); 00208 int kernel_center_x = kernel_cols / 2; 00209 int kernel_center_y = kernel_rows / 2; 00210 00211 output.resize (levels_ + 1); 00212 output[0].reset (new pcl::PointCloud<pcl::PointXYZRGB>); 00213 *(output[0]) = *input_; 00214 00215 if (input_->is_dense) 00216 { 00217 for (int l = 1; l <= levels_; ++l) 00218 { 00219 output[l].reset (new pcl::PointCloud<pcl::PointXYZRGB> (output[l-1]->width/2, output[l-1]->height/2)); 00220 const PointCloud<pcl::PointXYZRGB> &previous = *output[l-1]; 00221 PointCloud<pcl::PointXYZRGB> &next = *output[l]; 00222 #ifdef _OPENMP 00223 #pragma omp parallel for shared (next) num_threads(threads_) 00224 #endif 00225 for(int i=0; i < next.height; ++i) // rows 00226 { 00227 for(int j=0; j < next.width; ++j) // columns 00228 { 00229 float r = 0, g = 0, b = 0; 00230 for(int m=0; m < kernel_rows; ++m) // kernel rows 00231 { 00232 int mm = kernel_rows - 1 - m; // row index of flipped kernel 00233 for(int n=0; n < kernel_cols; ++n) // kernel columns 00234 { 00235 int nn = kernel_cols - 1 - n; // column index of flipped kernel 00236 // index of input signal, used for checking boundary 00237 int ii = 2*i + (m - kernel_center_y); 00238 int jj = 2*j + (n - kernel_center_x); 00239 00240 // ignore input samples which are out of bound 00241 if (ii < 0) ii = 0; 00242 if (ii >= previous.height) ii = previous.height - 1; 00243 if (jj < 0) jj = 0; 00244 if (jj >= previous.width) jj = previous.width - 1; 00245 next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn); 00246 next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn); 00247 next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn); 00248 b += previous.at (jj,ii).b * kernel_ (mm,nn); 00249 g += previous.at (jj,ii).g * kernel_ (mm,nn); 00250 r += previous.at (jj,ii).r * kernel_ (mm,nn); 00251 } 00252 } 00253 next.at (j,i).b = static_cast<pcl::uint8_t> (b); 00254 next.at (j,i).g = static_cast<pcl::uint8_t> (g); 00255 next.at (j,i).r = static_cast<pcl::uint8_t> (r); 00256 } 00257 } 00258 } 00259 } 00260 else 00261 { 00262 for (int l = 1; l <= levels_; ++l) 00263 { 00264 output[l].reset (new pcl::PointCloud<pcl::PointXYZRGB> (output[l-1]->width/2, output[l-1]->height/2)); 00265 const PointCloud<pcl::PointXYZRGB> &previous = *output[l-1]; 00266 PointCloud<pcl::PointXYZRGB> &next = *output[l]; 00267 #ifdef _OPENMP 00268 #pragma omp parallel for shared (next) num_threads(threads_) 00269 #endif 00270 for(int i=0; i < next.height; ++i) 00271 { 00272 for(int j=0; j < next.width; ++j) 00273 { 00274 float weight = 0; 00275 float r = 0, g = 0, b = 0; 00276 for(int m=0; m < kernel_rows; ++m) 00277 { 00278 int mm = kernel_rows - 1 - m; 00279 for(int n=0; n < kernel_cols; ++n) 00280 { 00281 int nn = kernel_cols - 1 - n; 00282 int ii = 2*i + (m - kernel_center_y); 00283 int jj = 2*j + (n - kernel_center_x); 00284 if (ii < 0) ii = 0; 00285 if (ii >= previous.height) ii = previous.height - 1; 00286 if (jj < 0) jj = 0; 00287 if (jj >= previous.width) jj = previous.width - 1; 00288 if (!isFinite (previous.at (jj,ii))) 00289 continue; 00290 if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_) 00291 { 00292 next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn); 00293 next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn); 00294 next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn); 00295 b += previous.at (jj,ii).b * kernel_ (mm,nn); 00296 g += previous.at (jj,ii).g * kernel_ (mm,nn); 00297 r += previous.at (jj,ii).r * kernel_ (mm,nn); 00298 weight+= kernel_ (mm,nn); 00299 } 00300 } 00301 } 00302 if (weight == 0) 00303 nullify (next.at (j,i)); 00304 else 00305 { 00306 weight = 1.f/weight; 00307 r*= weight; g*= weight; b*= weight; 00308 next.at (j,i).x*= weight; next.at (j,i).y*= weight; next.at (j,i).z*= weight; 00309 next.at (j,i).b = static_cast<pcl::uint8_t> (b); 00310 next.at (j,i).g = static_cast<pcl::uint8_t> (g); 00311 next.at (j,i).r = static_cast<pcl::uint8_t> (r); 00312 } 00313 } 00314 } 00315 } 00316 } 00317 } 00318 00319 template <> void 00320 Pyramid<pcl::PointXYZRGBA>::compute (std::vector<Pyramid<pcl::PointXYZRGBA>::PointCloudPtr> &output) 00321 { 00322 std::cout << "PointXYZRGBA" << std::endl; 00323 if (!initCompute ()) 00324 { 00325 PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ()); 00326 return; 00327 } 00328 00329 int kernel_rows = static_cast<int> (kernel_.rows ()); 00330 int kernel_cols = static_cast<int> (kernel_.cols ()); 00331 int kernel_center_x = kernel_cols / 2; 00332 int kernel_center_y = kernel_rows / 2; 00333 00334 output.resize (levels_ + 1); 00335 output[0].reset (new pcl::PointCloud<pcl::PointXYZRGBA>); 00336 *(output[0]) = *input_; 00337 00338 if (input_->is_dense) 00339 { 00340 for (int l = 1; l <= levels_; ++l) 00341 { 00342 output[l].reset (new pcl::PointCloud<pcl::PointXYZRGBA> (output[l-1]->width/2, output[l-1]->height/2)); 00343 const PointCloud<pcl::PointXYZRGBA> &previous = *output[l-1]; 00344 PointCloud<pcl::PointXYZRGBA> &next = *output[l]; 00345 #ifdef _OPENMP 00346 #pragma omp parallel for shared (next) num_threads(threads_) 00347 #endif 00348 for(int i=0; i < next.height; ++i) // rows 00349 { 00350 for(int j=0; j < next.width; ++j) // columns 00351 { 00352 float r = 0, g = 0, b = 0, a = 0; 00353 for(int m=0; m < kernel_rows; ++m) // kernel rows 00354 { 00355 int mm = kernel_rows - 1 - m; // row index of flipped kernel 00356 for(int n=0; n < kernel_cols; ++n) // kernel columns 00357 { 00358 int nn = kernel_cols - 1 - n; // column index of flipped kernel 00359 // index of input signal, used for checking boundary 00360 int ii = 2*i + (m - kernel_center_y); 00361 int jj = 2*j + (n - kernel_center_x); 00362 00363 // ignore input samples which are out of bound 00364 if (ii < 0) ii = 0; 00365 if (ii >= previous.height) ii = previous.height - 1; 00366 if (jj < 0) jj = 0; 00367 if (jj >= previous.width) jj = previous.width - 1; 00368 next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn); 00369 next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn); 00370 next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn); 00371 b += previous.at (jj,ii).b * kernel_ (mm,nn); 00372 g += previous.at (jj,ii).g * kernel_ (mm,nn); 00373 r += previous.at (jj,ii).r * kernel_ (mm,nn); 00374 a += previous.at (jj,ii).a * kernel_ (mm,nn); 00375 } 00376 } 00377 next.at (j,i).b = static_cast<pcl::uint8_t> (b); 00378 next.at (j,i).g = static_cast<pcl::uint8_t> (g); 00379 next.at (j,i).r = static_cast<pcl::uint8_t> (r); 00380 next.at (j,i).a = static_cast<pcl::uint8_t> (a); 00381 } 00382 } 00383 } 00384 } 00385 else 00386 { 00387 for (int l = 1; l <= levels_; ++l) 00388 { 00389 output[l].reset (new pcl::PointCloud<pcl::PointXYZRGBA> (output[l-1]->width/2, output[l-1]->height/2)); 00390 const PointCloud<pcl::PointXYZRGBA> &previous = *output[l-1]; 00391 PointCloud<pcl::PointXYZRGBA> &next = *output[l]; 00392 #ifdef _OPENMP 00393 #pragma omp parallel for shared (next) num_threads(threads_) 00394 #endif 00395 for(int i=0; i < next.height; ++i) 00396 { 00397 for(int j=0; j < next.width; ++j) 00398 { 00399 float weight = 0; 00400 float r = 0, g = 0, b = 0, a = 0; 00401 for(int m=0; m < kernel_rows; ++m) 00402 { 00403 int mm = kernel_rows - 1 - m; 00404 for(int n=0; n < kernel_cols; ++n) 00405 { 00406 int nn = kernel_cols - 1 - n; 00407 int ii = 2*i + (m - kernel_center_y); 00408 int jj = 2*j + (n - kernel_center_x); 00409 if (ii < 0) ii = 0; 00410 if (ii >= previous.height) ii = previous.height - 1; 00411 if (jj < 0) jj = 0; 00412 if (jj >= previous.width) jj = previous.width - 1; 00413 if (!isFinite (previous.at (jj,ii))) 00414 continue; 00415 if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_) 00416 { 00417 next.at (j,i).x += previous.at (jj,ii).x * kernel_ (mm,nn); 00418 next.at (j,i).y += previous.at (jj,ii).y * kernel_ (mm,nn); 00419 next.at (j,i).z += previous.at (jj,ii).z * kernel_ (mm,nn); 00420 b += previous.at (jj,ii).b * kernel_ (mm,nn); 00421 g += previous.at (jj,ii).g * kernel_ (mm,nn); 00422 r += previous.at (jj,ii).r * kernel_ (mm,nn); 00423 a += previous.at (jj,ii).a * kernel_ (mm,nn); 00424 weight+= kernel_ (mm,nn); 00425 } 00426 } 00427 } 00428 if (weight == 0) 00429 nullify (next.at (j,i)); 00430 else 00431 { 00432 weight = 1.f/weight; 00433 r*= weight; g*= weight; b*= weight; a*= weight; 00434 next.at (j,i).x*= weight; next.at (j,i).y*= weight; next.at (j,i).z*= weight; 00435 next.at (j,i).b = static_cast<pcl::uint8_t> (b); 00436 next.at (j,i).g = static_cast<pcl::uint8_t> (g); 00437 next.at (j,i).r = static_cast<pcl::uint8_t> (r); 00438 next.at (j,i).a = static_cast<pcl::uint8_t> (a); 00439 } 00440 } 00441 } 00442 } 00443 } 00444 } 00445 00446 template<> void 00447 Pyramid<pcl::RGB>::nullify (pcl::RGB& p) 00448 { 00449 p.r = 0; p.g = 0; p.b = 0; 00450 } 00451 00452 template <> void 00453 Pyramid<pcl::RGB>::compute (std::vector<Pyramid<pcl::RGB>::PointCloudPtr> &output) 00454 { 00455 std::cout << "RGB" << std::endl; 00456 if (!initCompute ()) 00457 { 00458 PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ()); 00459 return; 00460 } 00461 00462 int kernel_rows = static_cast<int> (kernel_.rows ()); 00463 int kernel_cols = static_cast<int> (kernel_.cols ()); 00464 int kernel_center_x = kernel_cols / 2; 00465 int kernel_center_y = kernel_rows / 2; 00466 00467 output.resize (levels_ + 1); 00468 output[0].reset (new pcl::PointCloud<pcl::RGB>); 00469 *(output[0]) = *input_; 00470 00471 if (input_->is_dense) 00472 { 00473 for (int l = 1; l <= levels_; ++l) 00474 { 00475 output[l].reset (new pcl::PointCloud<pcl::RGB> (output[l-1]->width/2, output[l-1]->height/2)); 00476 const PointCloud<pcl::RGB> &previous = *output[l-1]; 00477 PointCloud<pcl::RGB> &next = *output[l]; 00478 #ifdef _OPENMP 00479 #pragma omp parallel for shared (next) num_threads(threads_) 00480 #endif 00481 for(int i=0; i < next.height; ++i) 00482 { 00483 for(int j=0; j < next.width; ++j) 00484 { 00485 float r = 0, g = 0, b = 0; 00486 for(int m=0; m < kernel_rows; ++m) 00487 { 00488 int mm = kernel_rows - 1 - m; 00489 for(int n=0; n < kernel_cols; ++n) 00490 { 00491 int nn = kernel_cols - 1 - n; 00492 int ii = 2*i + (m - kernel_center_y); 00493 int jj = 2*j + (n - kernel_center_x); 00494 if (ii < 0) ii = 0; 00495 if (ii >= previous.height) ii = previous.height - 1; 00496 if (jj < 0) jj = 0; 00497 if (jj >= previous.width) jj = previous.width - 1; 00498 b += previous.at (jj,ii).b * kernel_ (mm,nn); 00499 g += previous.at (jj,ii).g * kernel_ (mm,nn); 00500 r += previous.at (jj,ii).r * kernel_ (mm,nn); 00501 } 00502 } 00503 next.at (j,i).b = static_cast<pcl::uint8_t> (b); 00504 next.at (j,i).g = static_cast<pcl::uint8_t> (g); 00505 next.at (j,i).r = static_cast<pcl::uint8_t> (r); 00506 } 00507 } 00508 } 00509 } 00510 else 00511 { 00512 for (int l = 1; l <= levels_; ++l) 00513 { 00514 output[l].reset (new pcl::PointCloud<pcl::RGB> (output[l-1]->width/2, output[l-1]->height/2)); 00515 const PointCloud<pcl::RGB> &previous = *output[l-1]; 00516 PointCloud<pcl::RGB> &next = *output[l]; 00517 #ifdef _OPENMP 00518 #pragma omp parallel for shared (next) num_threads(threads_) 00519 #endif 00520 for(int i=0; i < next.height; ++i) 00521 { 00522 for(int j=0; j < next.width; ++j) 00523 { 00524 float weight = 0; 00525 float r = 0, g = 0, b = 0; 00526 for(int m=0; m < kernel_rows; ++m) 00527 { 00528 int mm = kernel_rows - 1 - m; 00529 for(int n=0; n < kernel_cols; ++n) 00530 { 00531 int nn = kernel_cols - 1 - n; 00532 int ii = 2*i + (m - kernel_center_y); 00533 int jj = 2*j + (n - kernel_center_x); 00534 if (ii < 0) ii = 0; 00535 if (ii >= previous.height) ii = previous.height - 1; 00536 if (jj < 0) jj = 0; 00537 if (jj >= previous.width) jj = previous.width - 1; 00538 if (!isFinite (previous.at (jj,ii))) 00539 continue; 00540 if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_) 00541 { 00542 b += previous.at (jj,ii).b * kernel_ (mm,nn); 00543 g += previous.at (jj,ii).g * kernel_ (mm,nn); 00544 r += previous.at (jj,ii).r * kernel_ (mm,nn); 00545 weight+= kernel_ (mm,nn); 00546 } 00547 } 00548 } 00549 if (weight == 0) 00550 nullify (next.at (j,i)); 00551 else 00552 { 00553 weight = 1.f/weight; 00554 r*= weight; g*= weight; b*= weight; 00555 next.at (j,i).b = static_cast<pcl::uint8_t> (b); 00556 next.at (j,i).g = static_cast<pcl::uint8_t> (g); 00557 next.at (j,i).r = static_cast<pcl::uint8_t> (r); 00558 } 00559 } 00560 } 00561 } 00562 } 00563 } 00564 00565 } 00566 } 00567 00568 #endif