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) 2009-2012, 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 */ 00037 00038 00039 #ifndef PCL_SURFACE_IMPL_BILATERAL_UPSAMPLING_H_ 00040 #define PCL_SURFACE_IMPL_BILATERAL_UPSAMPLING_H_ 00041 00042 #include <pcl/surface/bilateral_upsampling.h> 00043 #include <algorithm> 00044 #include <pcl/console/print.h> 00045 00046 ////////////////////////////////////////////////////////////////////////////////////////////// 00047 template <typename PointInT, typename PointOutT> void 00048 pcl::BilateralUpsampling<PointInT, PointOutT>::process (pcl::PointCloud<PointOutT> &output) 00049 { 00050 // Copy the header 00051 output.header = input_->header; 00052 00053 if (!initCompute ()) 00054 { 00055 output.width = output.height = 0; 00056 output.points.clear (); 00057 return; 00058 } 00059 00060 if (input_->isOrganized () == false) 00061 { 00062 PCL_ERROR ("Input cloud is not organized.\n"); 00063 return; 00064 } 00065 00066 // Invert projection matrix 00067 unprojection_matrix_ = projection_matrix_.inverse (); 00068 00069 for (int i = 0; i < 3; ++i) 00070 { 00071 for (int j = 0; j < 3; ++j) 00072 printf ("%f ", unprojection_matrix_(i, j)); 00073 00074 printf ("\n"); 00075 } 00076 00077 00078 // Perform the actual surface reconstruction 00079 performProcessing (output); 00080 00081 deinitCompute (); 00082 } 00083 00084 ////////////////////////////////////////////////////////////////////////////////////////////// 00085 template <typename PointInT, typename PointOutT> void 00086 pcl::BilateralUpsampling<PointInT, PointOutT>::performProcessing (PointCloudOut &output) 00087 { 00088 output.resize (input_->size ()); 00089 float nan = std::numeric_limits<float>::quiet_NaN (); 00090 00091 00092 for (int x = 0; x < static_cast<int> (input_->width); ++x) 00093 for (int y = 0; y < static_cast<int> (input_->height); ++y) 00094 { 00095 int start_window_x = std::max (x - window_size_, 0), 00096 start_window_y = std::max (y - window_size_, 0), 00097 end_window_x = std::min (x + window_size_, static_cast<int> (input_->width)), 00098 end_window_y = std::min (y + window_size_, static_cast<int> (input_->height)); 00099 00100 float sum = 0.0f, 00101 norm_sum = 0.0f; 00102 00103 for (int x_w = start_window_x; x_w < end_window_x; ++ x_w) 00104 for (int y_w = start_window_y; y_w < end_window_y; ++ y_w) 00105 { 00106 float dx = float (x - x_w), 00107 dy = float (y - y_w); 00108 00109 float val_exp_depth = expf (- (dx*dx + dy*dy) / (2.0f * static_cast<float> (sigma_depth_ * sigma_depth_))); 00110 00111 float d_color = static_cast<float> ( 00112 abs (input_->points[y_w * input_->width + x_w].r - input_->points[y * input_->width + x].r) + 00113 abs (input_->points[y_w * input_->width + x_w].g - input_->points[y * input_->width + x].g) + 00114 abs (input_->points[y_w * input_->width + x_w].b - input_->points[y * input_->width + x].b)); 00115 float val_exp_rgb = expf (- d_color * d_color / (2.0f * sigma_color_ * sigma_color_)); 00116 00117 if (pcl_isfinite (input_->points[y_w*input_->width + x_w].z)) 00118 { 00119 sum += val_exp_depth * val_exp_rgb * input_->points[y_w*input_->width + x_w].z; 00120 norm_sum += val_exp_depth * val_exp_rgb; 00121 } 00122 } 00123 00124 output.points[y*input_->width + x].r = input_->points[y*input_->width + x].r; 00125 output.points[y*input_->width + x].g = input_->points[y*input_->width + x].g; 00126 output.points[y*input_->width + x].b = input_->points[y*input_->width + x].b; 00127 00128 if (norm_sum != 0.0f) 00129 { 00130 float depth = sum / norm_sum; 00131 Eigen::Vector3f pc (static_cast<float> (x) * depth, static_cast<float> (y) * depth, depth); 00132 Eigen::Vector3f pw (unprojection_matrix_ * pc); 00133 output.points[y*input_->width + x].x = pw[0]; 00134 output.points[y*input_->width + x].y = pw[1]; 00135 output.points[y*input_->width + x].z = pw[2]; 00136 } 00137 else 00138 { 00139 output.points[y*input_->width + x].x = nan; 00140 output.points[y*input_->width + x].y = nan; 00141 output.points[y*input_->width + x].z = nan; 00142 } 00143 } 00144 00145 output.header = input_->header; 00146 output.width = input_->width; 00147 output.height = input_->height; 00148 } 00149 00150 00151 00152 #define PCL_INSTANTIATE_BilateralUpsampling(T,OutT) template class PCL_EXPORTS pcl::BilateralUpsampling<T,OutT>; 00153 00154 00155 #endif /* PCL_SURFACE_IMPL_BILATERAL_UPSAMPLING_H_ */