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 * Copyright (c) 2004, Sylvain Paris and Francois Sillion 00007 00008 * All rights reserved. 00009 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id: fast_bilateral_omp.hpp 8381 2013-01-02 23:12:44Z sdmiller $ 00038 * 00039 */ 00040 #ifndef PCL_FILTERS_IMPL_FAST_BILATERAL_OMP_HPP_ 00041 #define PCL_FILTERS_IMPL_FAST_BILATERAL_OMP_HPP_ 00042 00043 #include <pcl/filters/fast_bilateral_omp.h> 00044 #include <pcl/common/io.h> 00045 #include <pcl/console/time.h> 00046 #include <assert.h> 00047 00048 ////////////////////////////////////////////////////////////////////////////////////////////// 00049 template <typename PointT> void 00050 pcl::FastBilateralFilterOMP<PointT>::applyFilter (PointCloud &output) 00051 { 00052 if (!input_->isOrganized ()) 00053 { 00054 PCL_ERROR ("[pcl::FastBilateralFilterOMP] Input cloud needs to be organized.\n"); 00055 return; 00056 } 00057 00058 copyPointCloud (*input_, output); 00059 float base_max = -std::numeric_limits<float>::max (), 00060 base_min = std::numeric_limits<float>::max (); 00061 bool found_finite = false; 00062 for (size_t x = 0; x < output.width; ++x) 00063 { 00064 for (size_t y = 0; y < output.height; ++y) 00065 { 00066 if (pcl_isfinite (output (x, y).z)) 00067 { 00068 if (base_max < output (x, y).z) 00069 base_max = output (x, y).z; 00070 if (base_min > output (x, y).z) 00071 base_min = output (x, y).z; 00072 found_finite = true; 00073 } 00074 } 00075 } 00076 if (!found_finite) 00077 { 00078 PCL_WARN ("[pcl::FastBilateralFilterOMP] Given an empty cloud. Doing nothing.\n"); 00079 return; 00080 } 00081 #ifdef _OPENMP 00082 #pragma omp parallel for num_threads (threads_) 00083 #endif 00084 for (long int i = 0; i < static_cast<long int> (output.size ()); ++i) 00085 if (!pcl_isfinite (output.at(i).z)) 00086 output.at(i).z = base_max; 00087 00088 const float base_delta = base_max - base_min; 00089 00090 const size_t padding_xy = 2; 00091 const size_t padding_z = 2; 00092 00093 const size_t small_width = static_cast<size_t> (static_cast<float> (input_->width - 1) / sigma_s_) + 1 + 2 * padding_xy; 00094 const size_t small_height = static_cast<size_t> (static_cast<float> (input_->height - 1) / sigma_s_) + 1 + 2 * padding_xy; 00095 const size_t small_depth = static_cast<size_t> (base_delta / sigma_r_) + 1 + 2 * padding_z; 00096 00097 Array3D data (small_width, small_height, small_depth); 00098 #ifdef _OPENMP 00099 #pragma omp parallel for num_threads (threads_) 00100 #endif 00101 for (long int i = 0; i < static_cast<long int> (small_width * small_height); ++i) 00102 { 00103 size_t small_x = static_cast<size_t> (i % small_width); 00104 size_t small_y = static_cast<size_t> (i / small_width); 00105 size_t start_x = static_cast<size_t>( 00106 std::max ((static_cast<float> (small_x) - static_cast<float> (padding_xy) - 0.5f) * sigma_s_ + 1, 0.f)); 00107 size_t end_x = static_cast<size_t>( 00108 std::max ((static_cast<float> (small_x) - static_cast<float> (padding_xy) + 0.5f) * sigma_s_ + 1, 0.f)); 00109 size_t start_y = static_cast<size_t>( 00110 std::max ((static_cast<float> (small_y) - static_cast<float> (padding_xy) - 0.5f) * sigma_s_ + 1, 0.f)); 00111 size_t end_y = static_cast<size_t>( 00112 std::max ((static_cast<float> (small_y) - static_cast<float> (padding_xy) + 0.5f) * sigma_s_ + 1, 0.f)); 00113 for (size_t x = start_x; x < end_x && x < input_->width; ++x) 00114 { 00115 for (size_t y = start_y; y < end_y && y < input_->height; ++y) 00116 { 00117 const float z = output (x,y).z - base_min; 00118 const size_t small_z = static_cast<size_t> (static_cast<float> (z) / sigma_r_ + 0.5f) + padding_z; 00119 Eigen::Vector2f& d = data (small_x, small_y, small_z); 00120 d[0] += output (x,y).z; 00121 d[1] += 1.0f; 00122 } 00123 } 00124 } 00125 00126 std::vector<long int> offset (3); 00127 offset[0] = &(data (1,0,0)) - &(data (0,0,0)); 00128 offset[1] = &(data (0,1,0)) - &(data (0,0,0)); 00129 offset[2] = &(data (0,0,1)) - &(data (0,0,0)); 00130 00131 Array3D buffer (small_width, small_height, small_depth); 00132 00133 for (size_t dim = 0; dim < 3; ++dim) 00134 { 00135 for (size_t n_iter = 0; n_iter < 2; ++n_iter) 00136 { 00137 Array3D* current_buffer = (n_iter % 2 == 1 ? &buffer : &data); 00138 Array3D* current_data =(n_iter % 2 == 1 ? &data : &buffer); 00139 #ifdef _OPENMP 00140 #pragma omp parallel for num_threads (threads_) 00141 #endif 00142 for(long int i = 0; i < static_cast<long int> ((small_width - 2)*(small_height - 2)); ++i) 00143 { 00144 size_t x = static_cast<size_t> (i % (small_width - 2) + 1); 00145 size_t y = static_cast<size_t> (i / (small_width - 2) + 1); 00146 const long int off = offset[dim]; 00147 Eigen::Vector2f* d_ptr = &(current_data->operator() (x,y,1)); 00148 Eigen::Vector2f* b_ptr = &(current_buffer->operator() (x,y,1)); 00149 00150 for(size_t z = 1; z < small_depth - 1; ++z, ++d_ptr, ++b_ptr) 00151 *d_ptr = (*(b_ptr - off) + *(b_ptr + off) + 2.0 * (*b_ptr)) / 4.0; 00152 } 00153 } 00154 } 00155 // Note: this works because there are an even number of iterations. 00156 // If there were an odd number, we would need to end with a: 00157 // std::swap (data, buffer); 00158 00159 if (early_division_) 00160 { 00161 for (std::vector<Eigen::Vector2f >::iterator d = data.begin (); d != data.end (); ++d) 00162 *d /= ((*d)[0] != 0) ? (*d)[1] : 1; 00163 00164 #ifdef _OPENMP 00165 #pragma omp parallel for num_threads (threads_) 00166 #endif 00167 for (long int i = 0; i < static_cast<long int> (input_->size ()); ++i) 00168 { 00169 size_t x = static_cast<size_t> (i % input_->width); 00170 size_t y = static_cast<size_t> (i / input_->width); 00171 const float z = output (x,y).z - base_min; 00172 const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy, 00173 static_cast<float> (y) / sigma_s_ + padding_xy, 00174 z / sigma_r_ + padding_z); 00175 output(x,y).z = D[0]; 00176 } 00177 } 00178 else 00179 { 00180 #ifdef _OPENMP 00181 #pragma omp parallel for num_threads (threads_) 00182 #endif 00183 for (long i = 0; i < static_cast<long int> (input_->size ()); ++i) 00184 { 00185 size_t x = static_cast<size_t> (i % input_->width); 00186 size_t y = static_cast<size_t> (i / input_->width); 00187 const float z = output (x,y).z - base_min; 00188 const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy, 00189 static_cast<float> (y) / sigma_s_ + padding_xy, 00190 z / sigma_r_ + padding_z); 00191 output (x,y).z = D[0] / D[1]; 00192 } 00193 } 00194 } 00195 00196 00197 00198 #endif /* PCL_FILTERS_IMPL_FAST_BILATERAL_OMP_HPP_ */ 00199