don.hpp
Go to the documentation of this file.
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 
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 
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_


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:23:24