normal_refinement.h
/*
* 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
{
/** \brief Assign weights of nearby normals used for refinement
* \todo Currently, this function equalizes all weights to 1
* @param cloud the point cloud data
* @param index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
* @param k_indices indices of neighboring points
* @param k_sqr_distances squared distances to the neighboring points
* @return weights
* \ingroup filters
*/
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));
}
/** \brief Refine an indexed point based on its neighbors, this function only writes to the normal_* fields
*
* \note If the indexed point has only NaNs in its neighborhood, the resulting normal will be zero.
*
* @param cloud the point cloud data
* @param index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
* @param k_indices indices of neighboring points
* @param k_sqr_distances squared distances to the neighboring points
* @param point the output point, only normal_* fields are written
* @return false if an error occurred (norm of summed normals zero or all neighbors NaN)
* \ingroup filters
*/
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);
}
/** \brief %Normal vector refinement class
*
* This class refines a set of already estimated normals by iteratively updating each normal to the (weighted)
* mean of all normals in its neighborhood. The intention is that you reuse the same point correspondences
* as used when estimating the original normals in order to avoid repeating a nearest neighbor search.
*
* \note This class avoids points for which a NaN is encountered in the neighborhood. In the special case
* where a point has only NaNs in its neighborhood, the resultant refined normal will be set to zero,
* i.e. this class only produces finite normals.
*
* \details Usage example:
*
* \code
* // Input point cloud
* pcl::PointCloud<PointT> cloud;
*
* // Fill cloud...
*
* // Estimated and refined normals
* pcl::PointCloud<NormalT> normals;
* pcl::PointCloud<NormalT> normals_refined;
*
* // Search parameters
* const int k = 5;
* std::vector<std::vector<int> > k_indices;
* std::vector<std::vector<float> > k_sqr_distances;
*
* // Run search
* pcl::search::KdTree<pcl::PointXYZRGB> search;
* search.setInputCloud (cloud.makeShared ());
* search.nearestKSearch (cloud, std::vector<int> (), k, k_indices, k_sqr_distances);
*
* // Use search results for normal estimation
* pcl::NormalEstimation<PointT, NormalT> ne;
* for (unsigned int i = 0; i < cloud.size (); ++i)
* {
* NormalT normal;
* ne.computePointNormal (cloud, k_indices[i]
* normal.normal_x, normal.normal_y, normal.normal_z, normal.curvature);
* pcl::flipNormalTowardsViewpoint (cloud[i], cloud.sensor_origin_[0], cloud.sensor_origin_[1], cloud.sensor_origin_[2],
* normal.normal_x, normal.normal_y, normal.normal_z);
* normals.push_back (normal);
* }
*
* // Run refinement using search results
* pcl::NormalRefinement<NormalT> nr (k_indices, k_sqr_distances);
* nr.setInputCloud (normals.makeShared ());
* nr.filter (normals_refined);
* \endcode
*
* \author Anders Glent Buch
* \ingroup filters
*/
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:
/** \brief Empty constructor, sets default convergence parameters
*/
NormalRefinement () :
Filter<NormalT>::Filter ()
{
filter_name_ = "NormalRefinement";
setMaxIterations (15);
setConvergenceThreshold (0.00001f);
}
/** \brief Constructor for setting correspondences, sets default convergence parameters
* @param k_indices indices of neighboring points
* @param k_sqr_distances squared distances to the neighboring points
*/
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);
}
/** \brief Set correspondences calculated from nearest neighbor search
* @param k_indices indices of neighboring points
* @param k_sqr_distances squared distances to the neighboring points
*/
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;
}
/** \brief Get correspondences (copy)
* @param k_indices indices of neighboring points
* @param k_sqr_distances squared distances to the neighboring points
*/
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 ());
}
/** \brief Set maximum iterations
* @param max_iterations maximum iterations
*/
inline void
setMaxIterations (unsigned int max_iterations)
{
max_iterations_ = max_iterations;
}
/** \brief Get maximum iterations
* @return maximum iterations
*/
inline unsigned int
getMaxIterations ()
{
return max_iterations_;
}
/** \brief Set convergence threshold
* @param convergence_threshold convergence threshold
*/
inline void
setConvergenceThreshold (float convergence_threshold)
{
convergence_threshold_ = convergence_threshold;
}
/** \brief Get convergence threshold
* @return convergence threshold
*/
inline float
getConvergenceThreshold ()
{
return convergence_threshold_;
}
protected:
/** \brief Filter a Point Cloud.
* \param output the resultant point cloud message
*/
void
applyFilter (PointCloud &output);
private:
/** \brief indices of neighboring points */
std::vector< std::vector<int> > k_indices_;
/** \brief squared distances to the neighboring points */
std::vector< std::vector<float> > k_sqr_distances_;
/** \brief Maximum allowable iterations over the whole point cloud for refinement */
unsigned int max_iterations_;
/** \brief Convergence threshold in the interval [0,1] on the mean of 1 - dot products between previous iteration and the current */
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
pcl::Normal
A point structure representing normal coordinates and the surface curvature estimate.
Definition: point_types.hpp:765
pcl
Definition: cloud_iterator.h:46
pcl::NormalRefinement::applyFilter
void applyFilter(PointCloud &output)
Filter a Point Cloud.
Definition: normal_refinement.hpp:48
pcl::NormalRefinement::NormalRefinement
NormalRefinement()
Empty constructor, sets default convergence parameters.
Definition: normal_refinement.h:201
pcl::PointCloud::Ptr
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
pcl::assignNormalWeights
std::vector< float > assignNormalWeights(const PointCloud< NormalT > &, int, const std::vector< int > &k_indices, const std::vector< float > &k_sqr_distances)
Assign weights of nearby normals used for refinement.
Definition: normal_refinement.h:55
pcl::NormalRefinement::getConvergenceThreshold
float getConvergenceThreshold()
Get convergence threshold.
Definition: normal_refinement.h:275
pcl::Filter
Filter represents the base filter class.
Definition: filter.h:83
pcl::NormalRefinement
Normal vector refinement class
Definition: normal_refinement.h:188
pcl::NormalRefinement::NormalRefinement
NormalRefinement(const std::vector< std::vector< int > > &k_indices, const std::vector< std::vector< float > > &k_sqr_distances)
Constructor for setting correspondences, sets default convergence parameters.
Definition: normal_refinement.h:213
pcl::refineNormal
bool refineNormal(const PointCloud< NormalT > &cloud, int index, const std::vector< int > &k_indices, const std::vector< float > &k_sqr_distances, NormalT &point)
Refine an indexed point based on its neighbors, this function only writes to the normal_* fields...
Definition: normal_refinement.h:81
pcl::NormalRefinement::getCorrespondences
void getCorrespondences(std::vector< std::vector< int > > &k_indices, std::vector< std::vector< float > > &k_sqr_distances)
Get correspondences (copy)
Definition: normal_refinement.h:238
pcl::PointCloud::ConstPtr
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
pcl::PointCloud< NormalT >
pcl::NormalRefinement::setMaxIterations
void setMaxIterations(unsigned int max_iterations)
Set maximum iterations.
Definition: normal_refinement.h:248
pcl::NormalRefinement::setCorrespondences
void setCorrespondences(const std::vector< std::vector< int > > &k_indices, const std::vector< std::vector< float > > &k_sqr_distances)
Set correspondences calculated from nearest neighbor search.
Definition: normal_refinement.h:227
pcl::Filter< NormalT >::filter_name_
std::string filter_name_
The filter name.
Definition: filter.h:160
pcl::NormalRefinement::setConvergenceThreshold
void setConvergenceThreshold(float convergence_threshold)
Set convergence threshold.
Definition: normal_refinement.h:266
pcl::NormalRefinement::getMaxIterations
unsigned int getMaxIterations()
Get maximum iterations.
Definition: normal_refinement.h:257