normal_refinement.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder(s) nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 *
 */

#ifndef PCL_FILTERS_NORMAL_REFINEMENT_H_
#define PCL_FILTERS_NORMAL_REFINEMENT_H_

#include <pcl/filters/filter.h>

namespace pcl
{
  template <typename NormalT> inline std::vector<float>
  assignNormalWeights (const PointCloud<NormalT>&,
                       int,
                       const std::vector<int>& k_indices,
                       const std::vector<float>& k_sqr_distances)
  {
    // Check inputs
    if (k_indices.size () != k_sqr_distances.size ())
      PCL_ERROR("[pcl::assignNormalWeights] inequal size of neighbor indices and distances!\n");
    
    // TODO: For now we use uniform weights
    return (std::vector<float> (k_indices.size (), 1.0f));
  }
  
  template <typename NormalT> inline bool
  refineNormal (const PointCloud<NormalT>& cloud,
                int index,
                const std::vector<int>& k_indices,
                const std::vector<float>& k_sqr_distances,
                NormalT& point)
  {
    // Start by zeroing result
    point.normal_x = 0.0f;
    point.normal_y = 0.0f;
    point.normal_z = 0.0f;
    
    // Check inputs
    if (k_indices.size () != k_sqr_distances.size ())
    {
      PCL_ERROR("[pcl::refineNormal] inequal size of neighbor indices and distances!\n");
      return (false);
    }
    
    // Get weights
    const std::vector<float> weights = assignNormalWeights (cloud, index, k_indices, k_sqr_distances);
    
    // Loop over all neighbors and accumulate sum of normal components
    float nx = 0.0f;
    float ny = 0.0f;
    float nz = 0.0f;
    for (unsigned int i = 0; i < k_indices.size (); ++i) {
      // Neighbor
      const NormalT& pointi = cloud[k_indices[i]];
      
      // Accumulate if not NaN
      if (pcl_isfinite (pointi.normal_x) && pcl_isfinite (pointi.normal_y) && pcl_isfinite (pointi.normal_z))
      {
        const float& weighti = weights[i];
        nx += weighti * pointi.normal_x;
        ny += weighti * pointi.normal_y;
        nz += weighti * pointi.normal_z;
      }
    }
    
    // Normalize if norm valid and non-zero
    const float norm = sqrtf (nx * nx + ny * ny + nz * nz);
    if (pcl_isfinite (norm) && norm > std::numeric_limits<float>::epsilon ())
    {
      point.normal_x = nx / norm;
      point.normal_y = ny / norm;
      point.normal_z = nz / norm;

      return (true);
    }

    return (false);
  }
  
  template<typename NormalT>
  class NormalRefinement : public Filter<NormalT>
  {
    using Filter<NormalT>::input_;
    using Filter<NormalT>::filter_name_;
    using Filter<NormalT>::getClassName;

    typedef typename Filter<NormalT>::PointCloud PointCloud;
    typedef typename PointCloud::Ptr PointCloudPtr;
    typedef typename PointCloud::ConstPtr PointCloudConstPtr;

    public:
      NormalRefinement () :
        Filter<NormalT>::Filter ()
      {
        filter_name_ = "NormalRefinement";
        setMaxIterations (15);
        setConvergenceThreshold (0.00001f);
      }
      
      NormalRefinement (const std::vector< std::vector<int> >& k_indices, const std::vector< std::vector<float> >& k_sqr_distances) :
        Filter<NormalT>::Filter ()
      {
        filter_name_ = "NormalRefinement";
        setCorrespondences (k_indices, k_sqr_distances);
        setMaxIterations (15);
        setConvergenceThreshold (0.00001f);
      }
      
      inline void
      setCorrespondences (const std::vector< std::vector<int> >& k_indices, const std::vector< std::vector<float> >& k_sqr_distances)
      {
        k_indices_ = k_indices;
        k_sqr_distances_ = k_sqr_distances;
      }
      
      inline void
      getCorrespondences (std::vector< std::vector<int> >& k_indices, std::vector< std::vector<float> >& k_sqr_distances)
      {
        k_indices.assign (k_indices_.begin (), k_indices_.end ());
        k_sqr_distances.assign (k_sqr_distances_.begin (), k_sqr_distances_.end ());
      }
      
      inline void
      setMaxIterations (unsigned int max_iterations)
      {
        max_iterations_ = max_iterations;
      }
      
      inline unsigned int
      getMaxIterations ()
      {
        return max_iterations_;
      }
      
      inline void
      setConvergenceThreshold (float convergence_threshold)
      {
        convergence_threshold_ = convergence_threshold;
      }

      inline float
      getConvergenceThreshold ()
      {
        return convergence_threshold_;
      }

    protected:
      void
      applyFilter (PointCloud &output);
      
    private:
      std::vector< std::vector<int> > k_indices_;
      
      std::vector< std::vector<float> > k_sqr_distances_;
      
      unsigned int max_iterations_;
      
      float convergence_threshold_;
  };
}

#ifdef PCL_NO_PRECOMPILE
#include <pcl/filters/impl/normal_refinement.hpp>
#else
#define PCL_INSTANTIATE_NormalRefinement(T) template class PCL_EXPORTS pcl::NormalRefinement<T>;
#endif

#endif