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) 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 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_CONVOLUTION_IMPL_HPP 00041 #define PCL_FILTERS_CONVOLUTION_IMPL_HPP 00042 00043 #include <pcl/pcl_config.h> 00044 #include <pcl/common/distances.h> 00045 00046 template <typename PointIn, typename PointOut> 00047 pcl::filters::Convolution<PointIn, PointOut>::Convolution () 00048 : borders_policy_ (BORDERS_POLICY_IGNORE) 00049 , distance_threshold_ (std::numeric_limits<float>::infinity ()) 00050 , input_ () 00051 , kernel_ () 00052 , half_width_ () 00053 , kernel_width_ () 00054 , threads_ (1) 00055 {} 00056 00057 template <typename PointIn, typename PointOut> void 00058 pcl::filters::Convolution<PointIn, PointOut>::initCompute (PointCloud<PointOut>& output) 00059 { 00060 if (borders_policy_ != BORDERS_POLICY_IGNORE && 00061 borders_policy_ != BORDERS_POLICY_MIRROR && 00062 borders_policy_ != BORDERS_POLICY_DUPLICATE) 00063 PCL_THROW_EXCEPTION (InitFailedException, 00064 "[pcl::filters::Convolution::initCompute] unknown borders policy."); 00065 00066 if(kernel_.size () % 2 == 0) 00067 PCL_THROW_EXCEPTION (InitFailedException, 00068 "[pcl::filters::Convolution::initCompute] convolving element width must be odd."); 00069 00070 if (distance_threshold_ != std::numeric_limits<float>::infinity ()) 00071 distance_threshold_ *= static_cast<float> (kernel_.size () % 2) * distance_threshold_; 00072 00073 half_width_ = static_cast<int> (kernel_.size ()) / 2; 00074 kernel_width_ = static_cast<int> (kernel_.size () - 1); 00075 00076 if (&(*input_) != &output) 00077 { 00078 if (output.height != input_->height || output.width != input_->width) 00079 { 00080 output.resize (input_->width * input_->height); 00081 output.width = input_->width; 00082 output.height = input_->height; 00083 } 00084 } 00085 output.is_dense = input_->is_dense; 00086 } 00087 00088 template <typename PointIn, typename PointOut> inline void 00089 pcl::filters::Convolution<PointIn, PointOut>::convolveRows (PointCloudOut& output) 00090 { 00091 try 00092 { 00093 initCompute (output); 00094 switch (borders_policy_) 00095 { 00096 case BORDERS_POLICY_MIRROR : convolve_rows_mirror (output); 00097 case BORDERS_POLICY_DUPLICATE : convolve_rows_duplicate (output); 00098 case BORDERS_POLICY_IGNORE : convolve_rows (output); 00099 } 00100 } 00101 catch (InitFailedException& e) 00102 { 00103 PCL_THROW_EXCEPTION (InitFailedException, 00104 "[pcl::filters::Convolution::convolveRows] init failed " << e.what ()); 00105 } 00106 } 00107 00108 template <typename PointIn, typename PointOut> inline void 00109 pcl::filters::Convolution<PointIn, PointOut>::convolveCols (PointCloudOut& output) 00110 { 00111 try 00112 { 00113 initCompute (output); 00114 switch (borders_policy_) 00115 { 00116 case BORDERS_POLICY_MIRROR : convolve_cols_mirror (output); 00117 case BORDERS_POLICY_DUPLICATE : convolve_cols_duplicate (output); 00118 case BORDERS_POLICY_IGNORE : convolve_cols (output); 00119 } 00120 } 00121 catch (InitFailedException& e) 00122 { 00123 PCL_THROW_EXCEPTION (InitFailedException, 00124 "[pcl::filters::Convolution::convolveCols] init failed " << e.what ()); 00125 } 00126 } 00127 00128 template <typename PointIn, typename PointOut> inline void 00129 pcl::filters::Convolution<PointIn, PointOut>::convolve (const Eigen::ArrayXf& h_kernel, 00130 const Eigen::ArrayXf& v_kernel, 00131 PointCloud<PointOut>& output) 00132 { 00133 try 00134 { 00135 PointCloudInPtr tmp (new PointCloud<PointIn> ()); 00136 setKernel (h_kernel); 00137 convolveRows (*tmp); 00138 setInputCloud (tmp); 00139 setKernel (v_kernel); 00140 convolveCols (output); 00141 } 00142 catch (InitFailedException& e) 00143 { 00144 PCL_THROW_EXCEPTION (InitFailedException, 00145 "[pcl::filters::Convolution::convolve] init failed " << e.what ()); 00146 } 00147 } 00148 00149 template <typename PointIn, typename PointOut> inline void 00150 pcl::filters::Convolution<PointIn, PointOut>::convolve (PointCloud<PointOut>& output) 00151 { 00152 try 00153 { 00154 PointCloudInPtr tmp (new PointCloud<PointIn> ()); 00155 convolveRows (*tmp); 00156 setInputCloud (tmp); 00157 convolveCols (output); 00158 } 00159 catch (InitFailedException& e) 00160 { 00161 PCL_THROW_EXCEPTION (InitFailedException, 00162 "[pcl::filters::Convolution::convolve] init failed " << e.what ()); 00163 } 00164 } 00165 00166 template <typename PointIn, typename PointOut> inline PointOut 00167 pcl::filters::Convolution<PointIn, PointOut>::convolveOneRowDense (int i, int j) 00168 { 00169 using namespace pcl::common; 00170 PointOut result; 00171 for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) 00172 result+= (*input_) (l,j) * kernel_[k]; 00173 return (result); 00174 } 00175 00176 template <typename PointIn, typename PointOut> inline PointOut 00177 pcl::filters::Convolution<PointIn, PointOut>::convolveOneColDense (int i, int j) 00178 { 00179 using namespace pcl::common; 00180 PointOut result; 00181 for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) 00182 result+= (*input_) (i,l) * kernel_[k]; 00183 return (result); 00184 } 00185 00186 template <typename PointIn, typename PointOut> inline PointOut 00187 pcl::filters::Convolution<PointIn, PointOut>::convolveOneRowNonDense (int i, int j) 00188 { 00189 using namespace pcl::common; 00190 PointOut result; 00191 float weight = 0; 00192 for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) 00193 { 00194 if (!isFinite ((*input_) (l,j))) 00195 continue; 00196 if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (l,j)) < distance_threshold_) 00197 { 00198 result+= (*input_) (l,j) * kernel_[k]; 00199 weight += kernel_[k]; 00200 } 00201 } 00202 if (weight == 0) 00203 result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN (); 00204 else 00205 { 00206 weight = 1.f/weight; 00207 result*= weight; 00208 } 00209 return (result); 00210 } 00211 00212 template <typename PointIn, typename PointOut> inline PointOut 00213 pcl::filters::Convolution<PointIn, PointOut>::convolveOneColNonDense (int i, int j) 00214 { 00215 using namespace pcl::common; 00216 PointOut result; 00217 float weight = 0; 00218 for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) 00219 { 00220 if (!isFinite ((*input_) (i,l))) 00221 continue; 00222 if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (i,l)) < distance_threshold_) 00223 { 00224 result+= (*input_) (i,l) * kernel_[k]; 00225 weight += kernel_[k]; 00226 } 00227 } 00228 if (weight == 0) 00229 result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN (); 00230 else 00231 { 00232 weight = 1.f/weight; 00233 result*= weight; 00234 } 00235 return (result); 00236 } 00237 00238 namespace pcl 00239 { 00240 namespace filters 00241 { 00242 template<> pcl::PointXYZRGB 00243 Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowDense (int i, int j) 00244 { 00245 pcl::PointXYZRGB result; 00246 float r = 0, g = 0, b = 0; 00247 for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) 00248 { 00249 result.x += (*input_) (l,j).x * kernel_[k]; 00250 result.y += (*input_) (l,j).y * kernel_[k]; 00251 result.z += (*input_) (l,j).z * kernel_[k]; 00252 r += kernel_[k] * static_cast<float> ((*input_) (l,j).r); 00253 g += kernel_[k] * static_cast<float> ((*input_) (l,j).g); 00254 b += kernel_[k] * static_cast<float> ((*input_) (l,j).b); 00255 } 00256 result.r = static_cast<pcl::uint8_t> (r); 00257 result.g = static_cast<pcl::uint8_t> (g); 00258 result.b = static_cast<pcl::uint8_t> (b); 00259 return (result); 00260 } 00261 00262 template<> pcl::PointXYZRGB 00263 Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColDense (int i, int j) 00264 { 00265 pcl::PointXYZRGB result; 00266 float r = 0, g = 0, b = 0; 00267 for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) 00268 { 00269 result.x += (*input_) (i,l).x * kernel_[k]; 00270 result.y += (*input_) (i,l).y * kernel_[k]; 00271 result.z += (*input_) (i,l).z * kernel_[k]; 00272 r += kernel_[k] * static_cast<float> ((*input_) (i,l).r); 00273 g += kernel_[k] * static_cast<float> ((*input_) (i,l).g); 00274 b += kernel_[k] * static_cast<float> ((*input_) (i,l).b); 00275 } 00276 result.r = static_cast<pcl::uint8_t> (r); 00277 result.g = static_cast<pcl::uint8_t> (g); 00278 result.b = static_cast<pcl::uint8_t> (b); 00279 return (result); 00280 } 00281 00282 template<> pcl::PointXYZRGB 00283 Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneRowNonDense (int i, int j) 00284 { 00285 pcl::PointXYZRGB result; 00286 float weight = 0; 00287 float r = 0, g = 0, b = 0; 00288 for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) 00289 { 00290 if (!isFinite ((*input_) (l,j))) 00291 continue; 00292 if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (l,j)) < distance_threshold_) 00293 { 00294 result.x += (*input_) (l,j).x * kernel_[k]; result.y += (*input_) (l,j).y * kernel_[k]; result.z += (*input_) (l,j).z * kernel_[k]; 00295 r+= kernel_[k] * static_cast<float> ((*input_) (l,j).r); 00296 g+= kernel_[k] * static_cast<float> ((*input_) (l,j).g); 00297 b+= kernel_[k] * static_cast<float> ((*input_) (l,j).b); 00298 weight += kernel_[k]; 00299 } 00300 } 00301 00302 if (weight == 0) 00303 result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN (); 00304 else 00305 { 00306 weight = 1.f/weight; 00307 r*= weight; g*= weight; b*= weight; 00308 result.x*= weight; result.y*= weight; result.z*= weight; 00309 result.r = static_cast<pcl::uint8_t> (r); 00310 result.g = static_cast<pcl::uint8_t> (g); 00311 result.b = static_cast<pcl::uint8_t> (b); 00312 } 00313 return (result); 00314 } 00315 00316 template<> pcl::PointXYZRGB 00317 Convolution<pcl::PointXYZRGB, pcl::PointXYZRGB>::convolveOneColNonDense (int i, int j) 00318 { 00319 pcl::PointXYZRGB result; 00320 float weight = 0; 00321 float r = 0, g = 0, b = 0; 00322 for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) 00323 { 00324 if (!isFinite ((*input_) (i,l))) 00325 continue; 00326 if (pcl::squaredEuclideanDistance ((*input_) (i,j), (*input_) (i,l)) < distance_threshold_) 00327 { 00328 result.x += (*input_) (i,l).x * kernel_[k]; result.y += (*input_) (i,l).y * kernel_[k]; result.z += (*input_) (i,l).z * kernel_[k]; 00329 r+= kernel_[k] * static_cast<float> ((*input_) (i,l).r); 00330 g+= kernel_[k] * static_cast<float> ((*input_) (i,l).g); 00331 b+= kernel_[k] * static_cast<float> ((*input_) (i,l).b); 00332 weight+= kernel_[k]; 00333 } 00334 } 00335 if (weight == 0) 00336 result.x = result.y = result.z = std::numeric_limits<float>::quiet_NaN (); 00337 else 00338 { 00339 weight = 1.f/weight; 00340 r*= weight; g*= weight; b*= weight; 00341 result.x*= weight; result.y*= weight; result.z*= weight; 00342 result.r = static_cast<pcl::uint8_t> (r); 00343 result.g = static_cast<pcl::uint8_t> (g); 00344 result.b = static_cast<pcl::uint8_t> (b); 00345 } 00346 return (result); 00347 } 00348 00349 /////////////////////////////////////////////////////////////////////////////////////////////// 00350 template<> pcl::RGB 00351 Convolution<pcl::RGB, pcl::RGB>::convolveOneRowDense (int i, int j) 00352 { 00353 pcl::RGB result; 00354 float r = 0, g = 0, b = 0; 00355 for (int k = kernel_width_, l = i - half_width_; k > -1; --k, ++l) 00356 { 00357 r += kernel_[k] * static_cast<float> ((*input_) (l,j).r); 00358 g += kernel_[k] * static_cast<float> ((*input_) (l,j).g); 00359 b += kernel_[k] * static_cast<float> ((*input_) (l,j).b); 00360 } 00361 result.r = static_cast<pcl::uint8_t> (r); 00362 result.g = static_cast<pcl::uint8_t> (g); 00363 result.b = static_cast<pcl::uint8_t> (b); 00364 return (result); 00365 } 00366 00367 template<> pcl::RGB 00368 Convolution<pcl::RGB, pcl::RGB>::convolveOneColDense (int i, int j) 00369 { 00370 pcl::RGB result; 00371 float r = 0, g = 0, b = 0; 00372 for (int k = kernel_width_, l = j - half_width_; k > -1; --k, ++l) 00373 { 00374 r += kernel_[k] * static_cast<float> ((*input_) (i,l).r); 00375 g += kernel_[k] * static_cast<float> ((*input_) (i,l).g); 00376 b += kernel_[k] * static_cast<float> ((*input_) (i,l).b); 00377 } 00378 result.r = static_cast<pcl::uint8_t> (r); 00379 result.g = static_cast<pcl::uint8_t> (g); 00380 result.b = static_cast<pcl::uint8_t> (b); 00381 return (result); 00382 } 00383 00384 template<> pcl::RGB 00385 Convolution<pcl::RGB, pcl::RGB>::convolveOneRowNonDense (int i, int j) 00386 { 00387 return (convolveOneRowDense (i,j)); 00388 } 00389 00390 template<> pcl::RGB 00391 Convolution<pcl::RGB, pcl::RGB>::convolveOneColNonDense (int i, int j) 00392 { 00393 return (convolveOneColDense (i,j)); 00394 } 00395 00396 template<> void 00397 Convolution<pcl::RGB, pcl::RGB>::makeInfinite (pcl::RGB& p) 00398 { 00399 p.r = 0; p.g = 0; p.b = 0; 00400 } 00401 } 00402 } 00403 00404 template <typename PointIn, typename PointOut> void 00405 pcl::filters::Convolution<PointIn, PointOut>::convolve_rows (PointCloudOut& output) 00406 { 00407 using namespace pcl::common; 00408 00409 int width = input_->width; 00410 int height = input_->height; 00411 int last = input_->width - half_width_; 00412 if (input_->is_dense) 00413 { 00414 #ifdef _OPENMP 00415 #pragma omp parallel for shared (output, last) num_threads (threads_) 00416 #endif 00417 for(int j = 0; j < height; ++j) 00418 { 00419 for (int i = 0; i < half_width_; ++i) 00420 makeInfinite (output (i,j)); 00421 00422 for (int i = half_width_; i < last; ++i) 00423 output (i,j) = convolveOneRowDense (i,j); 00424 00425 for (int i = last; i < width; ++i) 00426 makeInfinite (output (i,j)); 00427 } 00428 } 00429 else 00430 { 00431 #ifdef _OPENMP 00432 #pragma omp parallel for shared (output, last) num_threads (threads_) 00433 #endif 00434 for(int j = 0; j < height; ++j) 00435 { 00436 for (int i = 0; i < half_width_; ++i) 00437 makeInfinite (output (i,j)); 00438 00439 for (int i = half_width_; i < last; ++i) 00440 output (i,j) = convolveOneRowNonDense (i,j); 00441 00442 for (int i = last; i < width; ++i) 00443 makeInfinite (output (i,j)); 00444 } 00445 } 00446 } 00447 00448 template <typename PointIn, typename PointOut> void 00449 pcl::filters::Convolution<PointIn, PointOut>::convolve_rows_duplicate (PointCloudOut& output) 00450 { 00451 using namespace pcl::common; 00452 00453 int width = input_->width; 00454 int height = input_->height; 00455 int last = input_->width - half_width_; 00456 int w = last - 1; 00457 if (input_->is_dense) 00458 { 00459 #ifdef _OPENMP 00460 #pragma omp parallel for shared (output, last) num_threads (threads_) 00461 #endif 00462 for(int j = 0; j < height; ++j) 00463 { 00464 for (int i = half_width_; i < last; ++i) 00465 output (i,j) = convolveOneRowDense (i,j); 00466 00467 for (int i = last; i < width; ++i) 00468 output (i,j) = output (w, j); 00469 00470 for (int i = 0; i < half_width_; ++i) 00471 output (i,j) = output (half_width_, j); 00472 } 00473 } 00474 else 00475 { 00476 #ifdef _OPENMP 00477 #pragma omp parallel for shared (output, last) num_threads (threads_) 00478 #endif 00479 for(int j = 0; j < height; ++j) 00480 { 00481 for (int i = half_width_; i < last; ++i) 00482 output (i,j) = convolveOneRowNonDense (i,j); 00483 00484 for (int i = last; i < width; ++i) 00485 output (i,j) = output (w, j); 00486 00487 for (int i = 0; i < half_width_; ++i) 00488 output (i,j) = output (half_width_, j); 00489 } 00490 } 00491 } 00492 00493 template <typename PointIn, typename PointOut> void 00494 pcl::filters::Convolution<PointIn, PointOut>::convolve_rows_mirror (PointCloudOut& output) 00495 { 00496 using namespace pcl::common; 00497 00498 int width = input_->width; 00499 int height = input_->height; 00500 int last = input_->width - half_width_; 00501 int w = last - 1; 00502 if (input_->is_dense) 00503 { 00504 #ifdef _OPENMP 00505 #pragma omp parallel for shared (output, last) num_threads (threads_) 00506 #endif 00507 for(int j = 0; j < height; ++j) 00508 { 00509 for (int i = half_width_; i < last; ++i) 00510 output (i,j) = convolveOneRowDense (i,j); 00511 00512 for (int i = last, l = 0; i < width; ++i, ++l) 00513 output (i,j) = output (w-l, j); 00514 00515 for (int i = 0; i < half_width_; ++i) 00516 output (i,j) = output (half_width_+1-i, j); 00517 } 00518 } 00519 else 00520 { 00521 #ifdef _OPENMP 00522 #pragma omp parallel for shared (output, last) num_threads (threads_) 00523 #endif 00524 for(int j = 0; j < height; ++j) 00525 { 00526 for (int i = half_width_; i < last; ++i) 00527 output (i,j) = convolveOneRowNonDense (i,j); 00528 00529 for (int i = last, l = 0; i < width; ++i, ++l) 00530 output (i,j) = output (w-l, j); 00531 00532 for (int i = 0; i < half_width_; ++i) 00533 output (i,j) = output (half_width_+1-i, j); 00534 } 00535 } 00536 } 00537 00538 template <typename PointIn, typename PointOut> void 00539 pcl::filters::Convolution<PointIn, PointOut>::convolve_cols (PointCloudOut& output) 00540 { 00541 using namespace pcl::common; 00542 00543 int width = input_->width; 00544 int height = input_->height; 00545 int last = input_->height - half_width_; 00546 if (input_->is_dense) 00547 { 00548 #ifdef _OPENMP 00549 #pragma omp parallel for shared (output, last) num_threads (threads_) 00550 #endif 00551 for(int i = 0; i < width; ++i) 00552 { 00553 for (int j = 0; j < half_width_; ++j) 00554 makeInfinite (output (i,j)); 00555 00556 for (int j = half_width_; j < last; ++j) 00557 output (i,j) = convolveOneColDense (i,j); 00558 00559 for (int j = last; j < height; ++j) 00560 makeInfinite (output (i,j)); 00561 } 00562 } 00563 else 00564 { 00565 #ifdef _OPENMP 00566 #pragma omp parallel for shared (output, last) num_threads (threads_) 00567 #endif 00568 for(int i = 0; i < width; ++i) 00569 { 00570 for (int j = 0; j < half_width_; ++j) 00571 makeInfinite (output (i,j)); 00572 00573 for (int j = half_width_; j < last; ++j) 00574 output (i,j) = convolveOneColNonDense (i,j); 00575 00576 for (int j = last; j < height; ++j) 00577 makeInfinite (output (i,j)); 00578 } 00579 } 00580 } 00581 00582 template <typename PointIn, typename PointOut> void 00583 pcl::filters::Convolution<PointIn, PointOut>::convolve_cols_duplicate (PointCloudOut& output) 00584 { 00585 using namespace pcl::common; 00586 00587 int width = input_->width; 00588 int height = input_->height; 00589 int last = input_->height - half_width_; 00590 int h = last -1; 00591 if (input_->is_dense) 00592 { 00593 #ifdef _OPENMP 00594 #pragma omp parallel for shared (output, last) num_threads (threads_) 00595 #endif 00596 for(int i = 0; i < width; ++i) 00597 { 00598 for (int j = half_width_; j < last; ++j) 00599 output (i,j) = convolveOneColDense (i,j); 00600 00601 for (int j = last; j < height; ++j) 00602 output (i,j) = output (i,h); 00603 00604 for (int j = 0; j < half_width_; ++j) 00605 output (i,j) = output (i, half_width_); 00606 } 00607 } 00608 else 00609 { 00610 #ifdef _OPENMP 00611 #pragma omp parallel for shared (output, last) num_threads (threads_) 00612 #endif 00613 for(int i = 0; i < width; ++i) 00614 { 00615 for (int j = half_width_; j < last; ++j) 00616 output (i,j) = convolveOneColNonDense (i,j); 00617 00618 for (int j = last; j < height; ++j) 00619 output (i,j) = output (i,h); 00620 00621 for (int j = 0; j < half_width_; ++j) 00622 output (i,j) = output (i,half_width_); 00623 } 00624 } 00625 } 00626 00627 template <typename PointIn, typename PointOut> void 00628 pcl::filters::Convolution<PointIn, PointOut>::convolve_cols_mirror (PointCloudOut& output) 00629 { 00630 using namespace pcl::common; 00631 00632 int width = input_->width; 00633 int height = input_->height; 00634 int last = input_->height - half_width_; 00635 int h = last -1; 00636 if (input_->is_dense) 00637 { 00638 #ifdef _OPENMP 00639 #pragma omp parallel for shared (output, last) num_threads (threads_) 00640 #endif 00641 for(int i = 0; i < width; ++i) 00642 { 00643 for (int j = half_width_; j < last; ++j) 00644 output (i,j) = convolveOneColDense (i,j); 00645 00646 for (int j = last, l = 0; j < height; ++j, ++l) 00647 output (i,j) = output (i,h-l); 00648 00649 for (int j = 0; j < half_width_; ++j) 00650 output (i,j) = output (i, half_width_+1-j); 00651 } 00652 } 00653 else 00654 { 00655 #ifdef _OPENMP 00656 #pragma omp parallel for shared (output, last) num_threads (threads_) 00657 #endif 00658 for(int i = 0; i < width; ++i) 00659 { 00660 for (int j = half_width_; j < last; ++j) 00661 output (i,j) = convolveOneColNonDense (i,j); 00662 00663 for (int j = last, l = 0; j < height; ++j, ++l) 00664 output (i,j) = output (i,h-l); 00665 00666 for (int j = 0; j < half_width_; ++j) 00667 output (i,j) = output (i,half_width_+1-j); 00668 } 00669 } 00670 } 00671 00672 #endif //PCL_FILTERS_CONVOLUTION_IMPL_HPP