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 * Copyright (c) 2012-, Open Perception, Inc. 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 00041 #ifndef PCL_FILTERS_IMPL_NORMAL_REFINEMENT_H_ 00042 #define PCL_FILTERS_IMPL_NORMAL_REFINEMENT_H_ 00043 00044 #include <pcl/filters/normal_refinement.h> 00045 00046 /////////////////////////////////////////////////////////////////////////////////////////// 00047 template <typename NormalT> void 00048 pcl::NormalRefinement<NormalT>::applyFilter (PointCloud &output) 00049 { 00050 // Check input 00051 if (input_->empty ()) 00052 { 00053 PCL_ERROR ("[pcl::%s::applyFilter] No source was input!\n", 00054 getClassName ().c_str ()); 00055 } 00056 00057 // Copy to output 00058 output = *input_; 00059 00060 // Check that correspondences are non-empty 00061 if (k_indices_.empty () || k_sqr_distances_.empty ()) 00062 { 00063 PCL_ERROR ("[pcl::%s::applyFilter] No point correspondences given! Returning original input.\n", 00064 getClassName ().c_str ()); 00065 return; 00066 } 00067 00068 // Check that correspondences are OK 00069 const unsigned int size = k_indices_.size (); 00070 if (k_sqr_distances_.size () != size || input_->size () != size) 00071 { 00072 PCL_ERROR ("[pcl::%s::applyFilter] Inconsistency between size of correspondence indices/distances or input! Returning original input.\n", 00073 getClassName ().c_str ()); 00074 return; 00075 } 00076 00077 // Run refinement while monitoring convergence 00078 for (unsigned int i = 0; i < max_iterations_; ++i) 00079 { 00080 // Output of the current iteration 00081 PointCloud tmp = output; 00082 00083 // Mean change in direction, measured by dot products 00084 float ddot = 0.0f; 00085 00086 // Loop over all points in current output and write refined normal to tmp 00087 int num_valids = 0; 00088 for(unsigned int j = 0; j < size; ++j) 00089 { 00090 // Point to write to 00091 NormalT& tmpj = tmp[j]; 00092 00093 // Refine 00094 if (refineNormal (output, j, k_indices_[j], k_sqr_distances_[j], tmpj)) 00095 { 00096 // Accumulate using similarity in direction between previous iteration and current 00097 const NormalT& outputj = output[j]; 00098 ddot += tmpj.normal_x * outputj.normal_x + tmpj.normal_y * outputj.normal_y + tmpj.normal_z * outputj.normal_z; 00099 ++num_valids; 00100 } 00101 } 00102 00103 // Take mean of similarities 00104 ddot /= static_cast<float> (num_valids); 00105 00106 // Negate to since we want an error measure to approach zero 00107 ddot = 1.0f - ddot; 00108 00109 // Update output 00110 output = tmp; 00111 00112 // Break if converged 00113 if (ddot < convergence_threshold_) 00114 { 00115 PCL_DEBUG("[pcl::%s::applyFilter] Converged after %i iterations with mean error of %f.\n", 00116 getClassName ().c_str (), i+1, ddot); 00117 break; 00118 } 00119 } 00120 } 00121 00122 #endif