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$ 00038 * 00039 */ 00040 #ifndef PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_ 00041 #define PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_ 00042 00043 #include <pcl/common/io.h> 00044 00045 ////////////////////////////////////////////////////////////////////////////////////////////// 00046 template <typename PointT> void 00047 pcl::FastBilateralFilter<PointT>::applyFilter (PointCloud &output) 00048 { 00049 if (!input_->isOrganized ()) 00050 { 00051 PCL_ERROR ("[pcl::FastBilateralFilter] Input cloud needs to be organized.\n"); 00052 return; 00053 } 00054 00055 copyPointCloud (*input_, output); 00056 float base_max = -std::numeric_limits<float>::max (), 00057 base_min = std::numeric_limits<float>::max (); 00058 bool found_finite = false; 00059 for (size_t x = 0; x < output.width; ++x) 00060 { 00061 for (size_t y = 0; y < output.height; ++y) 00062 { 00063 if (pcl_isfinite (output (x, y).z)) 00064 { 00065 if (base_max < output (x, y).z) 00066 base_max = output (x, y).z; 00067 if (base_min > output (x, y).z) 00068 base_min = output (x, y).z; 00069 found_finite = true; 00070 } 00071 } 00072 } 00073 if (!found_finite) 00074 { 00075 PCL_WARN ("[pcl::FastBilateralFilter] Given an empty cloud. Doing nothing.\n"); 00076 return; 00077 } 00078 00079 for (size_t x = 0; x < output.width; ++x) 00080 for (size_t y = 0; y < output.height; ++y) 00081 if (!pcl_isfinite (output (x, y).z)) 00082 output (x, y).z = base_max; 00083 00084 const float base_delta = base_max - base_min; 00085 00086 const size_t padding_xy = 2; 00087 const size_t padding_z = 2; 00088 00089 const size_t small_width = static_cast<size_t> (static_cast<float> (input_->width - 1) / sigma_s_) + 1 + 2 * padding_xy; 00090 const size_t small_height = static_cast<size_t> (static_cast<float> (input_->height - 1) / sigma_s_) + 1 + 2 * padding_xy; 00091 const size_t small_depth = static_cast<size_t> (base_delta / sigma_r_) + 1 + 2 * padding_z; 00092 00093 00094 Array3D data (small_width, small_height, small_depth); 00095 for (size_t x = 0; x < input_->width; ++x) 00096 { 00097 const size_t small_x = static_cast<size_t> (static_cast<float> (x) / sigma_s_ + 0.5f) + padding_xy; 00098 for (size_t y = 0; y < input_->height; ++y) 00099 { 00100 const float z = output (x,y).z - base_min; 00101 00102 const size_t small_y = static_cast<size_t> (static_cast<float> (y) / sigma_s_ + 0.5f) + padding_xy; 00103 const size_t small_z = static_cast<size_t> (static_cast<float> (z) / sigma_r_ + 0.5f) + padding_z; 00104 00105 Eigen::Vector2f& d = data (small_x, small_y, small_z); 00106 d[0] += output (x,y).z; 00107 d[1] += 1.0f; 00108 } 00109 } 00110 00111 00112 std::vector<long int> offset (3); 00113 offset[0] = &(data (1,0,0)) - &(data (0,0,0)); 00114 offset[1] = &(data (0,1,0)) - &(data (0,0,0)); 00115 offset[2] = &(data (0,0,1)) - &(data (0,0,0)); 00116 00117 Array3D buffer (small_width, small_height, small_depth); 00118 00119 for (size_t dim = 0; dim < 3; ++dim) 00120 { 00121 const long int off = offset[dim]; 00122 for (size_t n_iter = 0; n_iter < 2; ++n_iter) 00123 { 00124 std::swap (buffer, data); 00125 for(size_t x = 1; x < small_width - 1; ++x) 00126 for(size_t y = 1; y < small_height - 1; ++y) 00127 { 00128 Eigen::Vector2f* d_ptr = &(data (x,y,1)); 00129 Eigen::Vector2f* b_ptr = &(buffer (x,y,1)); 00130 00131 for(size_t z = 1; z < small_depth - 1; ++z, ++d_ptr, ++b_ptr) 00132 *d_ptr = (*(b_ptr - off) + *(b_ptr + off) + 2.0 * (*b_ptr)) / 4.0; 00133 } 00134 } 00135 } 00136 00137 if (early_division_) 00138 { 00139 for (std::vector<Eigen::Vector2f >::iterator d = data.begin (); d != data.end (); ++d) 00140 *d /= ((*d)[0] != 0) ? (*d)[1] : 1; 00141 00142 for (size_t x = 0; x < input_->width; x++) 00143 for (size_t y = 0; y < input_->height; y++) 00144 { 00145 const float z = output (x,y).z - base_min; 00146 const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy, 00147 static_cast<float> (y) / sigma_s_ + padding_xy, 00148 z / sigma_r_ + padding_z); 00149 output(x,y).z = D[0]; 00150 } 00151 } 00152 else 00153 { 00154 for (size_t x = 0; x < input_->width; ++x) 00155 for (size_t y = 0; y < input_->height; ++y) 00156 { 00157 const float z = output (x,y).z - base_min; 00158 const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy, 00159 static_cast<float> (y) / sigma_s_ + padding_xy, 00160 z / sigma_r_ + padding_z); 00161 output (x,y).z = D[0] / D[1]; 00162 } 00163 } 00164 } 00165 00166 00167 00168 ////////////////////////////////////////////////////////////////////////////////////////////// 00169 template <typename PointT> size_t 00170 pcl::FastBilateralFilter<PointT>::Array3D::clamp (const size_t min_value, 00171 const size_t max_value, 00172 const size_t x) 00173 { 00174 if (x >= min_value && x <= max_value) 00175 { 00176 return x; 00177 } 00178 else if (x < min_value) 00179 { 00180 return (min_value); 00181 } 00182 else 00183 { 00184 return (max_value); 00185 } 00186 } 00187 00188 ////////////////////////////////////////////////////////////////////////////////////////////// 00189 template <typename PointT> Eigen::Vector2f 00190 pcl::FastBilateralFilter<PointT>::Array3D::trilinear_interpolation (const float x, 00191 const float y, 00192 const float z) 00193 { 00194 const size_t x_index = clamp (0, x_dim_ - 1, static_cast<size_t> (x)); 00195 const size_t xx_index = clamp (0, x_dim_ - 1, x_index + 1); 00196 00197 const size_t y_index = clamp (0, y_dim_ - 1, static_cast<size_t> (y)); 00198 const size_t yy_index = clamp (0, y_dim_ - 1, y_index + 1); 00199 00200 const size_t z_index = clamp (0, z_dim_ - 1, static_cast<size_t> (z)); 00201 const size_t zz_index = clamp (0, z_dim_ - 1, z_index + 1); 00202 00203 const float x_alpha = x - static_cast<float> (x_index); 00204 const float y_alpha = y - static_cast<float> (y_index); 00205 const float z_alpha = z - static_cast<float> (z_index); 00206 00207 return 00208 (1.0f-x_alpha) * (1.0f-y_alpha) * (1.0f-z_alpha) * (*this)(x_index, y_index, z_index) + 00209 x_alpha * (1.0f-y_alpha) * (1.0f-z_alpha) * (*this)(xx_index, y_index, z_index) + 00210 (1.0f-x_alpha) * y_alpha * (1.0f-z_alpha) * (*this)(x_index, yy_index, z_index) + 00211 x_alpha * y_alpha * (1.0f-z_alpha) * (*this)(xx_index, yy_index, z_index) + 00212 (1.0f-x_alpha) * (1.0f-y_alpha) * z_alpha * (*this)(x_index, y_index, zz_index) + 00213 x_alpha * (1.0f-y_alpha) * z_alpha * (*this)(xx_index, y_index, zz_index) + 00214 (1.0f-x_alpha) * y_alpha * z_alpha * (*this)(x_index, yy_index, zz_index) + 00215 x_alpha * y_alpha * z_alpha * (*this)(xx_index, yy_index, zz_index); 00216 } 00217 00218 #endif /* PCL_FILTERS_IMPL_FAST_BILATERAL_HPP_ */