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, Yani Ioannou <yani.ioannou@gmail.com> 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 */ 00037 #ifndef PCL_FILTERS_DON_IMPL_H_ 00038 #define PCL_FILTERS_DON_IMPL_H_ 00039 00040 #include <pcl/features/don.h> 00041 00042 ////////////////////////////////////////////////////////////////////////////////////////////// 00043 template <typename PointInT, typename PointNT, typename PointOutT> bool 00044 pcl::DifferenceOfNormalsEstimation<PointInT, PointNT, PointOutT>::initCompute () 00045 { 00046 // Check if input normals are set 00047 if (!input_normals_small_) 00048 { 00049 PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing small support radius normals was given!\n", getClassName().c_str ()); 00050 Feature<PointInT, PointOutT>::deinitCompute(); 00051 return (false); 00052 } 00053 00054 if (!input_normals_large_) 00055 { 00056 PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing large support radius normals was given!\n", getClassName().c_str ()); 00057 Feature<PointInT, PointOutT>::deinitCompute(); 00058 return (false); 00059 } 00060 00061 // Check if the size of normals is the same as the size of the surface 00062 if (input_normals_small_->points.size () != input_->points.size ()) 00063 { 00064 PCL_ERROR ("[pcl::%s::initCompute] ", getClassName().c_str ()); 00065 PCL_ERROR ("The number of points in the input dataset differs from "); 00066 PCL_ERROR ("the number of points in the dataset containing the small support radius normals!\n"); 00067 Feature<PointInT, PointOutT>::deinitCompute (); 00068 return (false); 00069 } 00070 00071 if (input_normals_large_->points.size () != input_->points.size ()) 00072 { 00073 PCL_ERROR ("[pcl::%s::initCompute] ", getClassName().c_str ()); 00074 PCL_ERROR ("The number of points in the input dataset differs from "); 00075 PCL_ERROR ("the number of points in the dataset containing the large support radius normals!\n"); 00076 Feature<PointInT, PointOutT>::deinitCompute (); 00077 return (false); 00078 } 00079 00080 return (true); 00081 } 00082 00083 ////////////////////////////////////////////////////////////////////////////////////////////// 00084 template <typename PointInT, typename PointNT, typename PointOutT> void 00085 pcl::DifferenceOfNormalsEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00086 { 00087 //perform DoN subtraction and return results 00088 for (size_t point_id = 0; point_id < input_->points.size (); ++point_id) 00089 { 00090 output.points[point_id].getNormalVector3fMap () = (input_normals_small_->points[point_id].getNormalVector3fMap () 00091 - input_normals_large_->points[point_id].getNormalVector3fMap ()) / 2.0; 00092 if(!pcl_isfinite (output.points[point_id].normal_x) || 00093 !pcl_isfinite (output.points[point_id].normal_y) || 00094 !pcl_isfinite (output.points[point_id].normal_z)){ 00095 output.points[point_id].getNormalVector3fMap () = Eigen::Vector3f(0,0,0); 00096 } 00097 output.points[point_id].curvature = output.points[point_id].getNormalVector3fMap ().norm(); 00098 } 00099 } 00100 00101 00102 #define PCL_INSTANTIATE_DifferenceOfNormalsEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::DifferenceOfNormalsEstimation<T,NT,OutT>; 00103 00104 #endif // PCL_FILTERS_DON_H_