From 9fe8881ed78de1dbcc09b21c3a84e84a3a3a9ac5 Mon Sep 17 00:00:00 2001 From: Nader Salman Date: Thu, 2 Apr 2009 16:27:01 +0000 Subject: [PATCH] New algorithm to smooth 3D point cloud inspired y Improved Laplacian smoothing of noisy surface meshes --- .../CGAL/improved_laplacian_smoothing_3.h | 430 ++++++++++++++++++ 1 file changed, 430 insertions(+) create mode 100644 Point_set_processing_3/include/CGAL/improved_laplacian_smoothing_3.h diff --git a/Point_set_processing_3/include/CGAL/improved_laplacian_smoothing_3.h b/Point_set_processing_3/include/CGAL/improved_laplacian_smoothing_3.h new file mode 100644 index 00000000000..10c21d459a2 --- /dev/null +++ b/Point_set_processing_3/include/CGAL/improved_laplacian_smoothing_3.h @@ -0,0 +1,430 @@ +// Copyright (c) 2007-09 INRIA Sophia-Antipolis (France). +// All rights reserved. +// +// This file is part of CGAL (www.cgal.org); you may redistribute it under +// the terms of the Q Public License version 1.0. +// See the file LICENSE.QPL distributed with CGAL. +// +// Licensees holding a valid commercial license may use this file in +// accordance with the commercial license agreement provided with the software. +// +// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE +// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. +// +// $URL$ +// $Id$ +// +// Author(s) : Nader Salman and Laurent Saboret + +#ifndef CGAL_IMPROVED_LAPLACIAN_SMOOTHING_3_H +#define CGAL_IMPROVED_LAPLACIAN_SMOOTHING_3_H + +#include +#include +#include +#include + +CGAL_BEGIN_NAMESPACE + + +// ---------------------------------------------------------------------------- +// Private section +// ---------------------------------------------------------------------------- +namespace CGALi { + + +// Item in the Kd-tree: position (Point_3) + index +template +class KdTreeElement : public Kernel::Point_3 +{ +public: + unsigned int index; + + // basic geometric types + typedef typename CGAL::Origin Origin; + typedef typename Kernel::Point_3 Point_3; + + KdTreeElement(const Origin& o = ORIGIN, unsigned int id=0) + : Point_3(o), index(id) + {} + KdTreeElement(const Point_3& p, unsigned int id=0) + : Point_3(p), index(id) + {} + KdTreeElement(const KdTreeElement& other) + : Point_3(other), index(other.index) + {} +}; + +// Helper class for the Kd-tree +template +class KdTreeGT : public Kernel +{ +public: + typedef KdTreeElement Point_3; +}; + +template +class KdTreeTraits : public CGAL::Search_traits_3 > +{ +public: + typedef typename Kernel::Point_3 PointType; +}; + +/* Usage: + typedef CGALi::KdTreeElement KdTreeElement; + typedef CGALi::KdTreeTraits Tree_traits; + typedef CGAL::Orthogonal_k_neighbor_search Neighbor_search; + typedef typename Neighbor_search::Tree Tree; + typedef typename Neighbor_search::iterator Search_iterator; +*/ + + +/// Smooth one point position using jet fitting on the k +/// nearest neighbors and reprojection onto the jet. +/// +/// @commentheading Precondition: k >= 2. +/// +/// @commentheading Template Parameters: +/// @param Kernel Geometric traits class. +/// @param Tree KD-tree. +/// +/// @return computed point +template +typename Kernel::Point_3 +laplacian_smoothing_3( + const typename Kernel::Point_3& pi, ///< 3D point to smooth + Tree& tree, ///< KD-tree + const unsigned int k) +{ + // basic geometric types + typedef typename Kernel::Point_3 Point_3; + typedef typename Kernel::Vector_3 Vector_3; + + // types for K nearest neighbors search + //typedef typename CGAL::Search_traits_3 Tree_traits; + typedef CGALi::KdTreeTraits Tree_traits; + typedef CGAL::Orthogonal_k_neighbor_search Neighbor_search; + typedef typename Neighbor_search::iterator Search_iterator; + + // Compute Laplacian (centroid) of k neighboring points. + // Note: we perform k+1 queries and skip the query point which is output first. + // TODO: we should use the functions in PCA component instead. + Vector_3 v = CGAL::NULL_VECTOR; + Neighbor_search search(tree,pi,k+1); + Search_iterator search_iterator; + for(search_iterator = search.begin(), search_iterator++; // skip pi point + search_iterator != search.end(); + search_iterator++ ) + { + const Point_3& p = search_iterator->first; + v = v + (p - CGAL::ORIGIN); + } + + Point_3 centroid = CGAL::ORIGIN + v / k; + + return centroid; +} + + +/// Smooth one point position using jet fitting on the k +/// nearest neighbors and reprojection onto the jet. +/// +/// @commentheading Precondition: k >= 2. +/// +/// @commentheading Template Parameters: +/// @param Kernel Geometric traits class. +/// @param Tree KD-tree. +/// +/// @return computed point +template +typename Kernel::Point_3 +improved_laplacian_smoothing_3( + const typename Kernel::Point_3& pi, ///< 3D point to smooth + const typename Kernel::Vector_3& bi, ///< bi movement + Tree& tree, ///< KD-tree + const std::vector& b, + const unsigned int k, + typename Kernel::FT beta) +{ + // basic geometric types + typedef typename Kernel::Point_3 Point_3; + typedef typename Kernel::Vector_3 Vector_3; + + // types for K nearest neighbors search + //typedef typename CGAL::Search_traits_3 Tree_traits; + typedef CGALi::KdTreeTraits Tree_traits; + typedef CGAL::Orthogonal_k_neighbor_search Neighbor_search; + typedef typename Neighbor_search::iterator Search_iterator; + + // Gather set of k neighboring points and compute the sum of their b[] values. + // Note: we perform k+1 queries and skip the query point which is output first. + Vector_3 bj_sum; + Neighbor_search search(tree,pi,k+1); + Search_iterator search_iterator; + for(search_iterator = search.begin(), search_iterator++; // skip pi point + search_iterator != search.end(); + search_iterator++ ) + { + bj_sum = bj_sum + b[search_iterator->first.index]; + } + + return pi - (beta * bi + ((1-beta)/k)*bj_sum); +} + + +} /* namespace CGALi */ + + +// ---------------------------------------------------------------------------- +// Public section +// ---------------------------------------------------------------------------- + + +/// Improved Laplacian smoothing (Vollmer et al) +/// on the k nearest neighbors. +/// This variant requires the kernel. +/// +/// @commentheading Precondition: k >= 2. +/// +/// @commentheading Template Parameters: +/// @param InputIterator value_type convertible to Point_3. +/// @param OutputIterator value_type convertible to Point_3. +/// @param Kernel Geometric traits class. +/// +/// @return past-the-end output iterator. +template +OutputIterator +improved_laplacian_smoothing_3( + InputIterator first, ///< iterator over the first input point + InputIterator beyond, ///< past-the-end iterator over input points + OutputIterator output, ///< iterator over the first output point + const unsigned int k, ///< number of neighbors + const unsigned int iter_number, + const Kernel& /*kernel*/, + typename Kernel::FT alpha, + typename Kernel::FT beta) +{ + // Point_3 types + typedef typename std::iterator_traits::value_type Input_point_3; + typedef typename Kernel::Point_3 Point_3; + typedef typename Kernel::Vector_3 Vector_3; + + // types for K nearest neighbors search structure + //typedef typename CGAL::Search_traits_3 Tree_traits; + typedef CGALi::KdTreeElement KdTreeElement; + typedef CGALi::KdTreeTraits Tree_traits; + typedef CGAL::Orthogonal_k_neighbor_search Neighbor_search; + typedef typename Neighbor_search::Tree Tree; + typedef typename Neighbor_search::iterator Search_iterator; + + // precondition: at least one element in the container. + // to fix: should have at least three distinct points + // but this is costly to check + CGAL_precondition(first != beyond); + + // precondition: at least 2 nearest neighbors + CGAL_precondition(k >= 2); + + unsigned int i; // point index + + // Create kd-tree + //Tree tree(first,beyond); + std::vector treeElements; + for (InputIterator it = first, i=0 ; it != beyond ; ++it,++i) + { + treeElements.push_back(KdTreeElement(*it,i)); + } + Tree tree(treeElements.begin(), treeElements.end()); + + std::vector p; // positions at step iter_n + std::vector b; // ... + + for(InputIterator it = first, i=0; it != beyond; it++, ++i) + p[i] = *it; + + for(int iter_n = 0; iter_n < iter_number ; ++iter_n) + { + // Iterate over input points, compute (original) Laplacian smooth and b[]. + for(InputIterator it = first, i=0; it != beyond; it++, ++i) + { + Point_3 np = CGALi::laplacian_smoothing_3(*it,tree,k); + b[i] = alpha*(np - *it) + (1-alpha)*(np - p[i]); + p[i] = np; + } + + // Iterate over input points, compute and output smooth points. + // Note: the cast to (Point_3&) ensures compatibility with classes derived from Point_3. + for(InputIterator it = first, i=0; it != beyond; it++, ++i) + { + p[i] = CGALi::improved_laplacian_smoothing_3(p[i],b[i],tree,b,k,beta); + } + } + + // Iterate over input points and output smooth points. + // Note: the cast to (Point_3&) ensures compatibility with classes derived from Point_3. + for(InputIterator it = first, i=0; it != beyond; it++, ++i) + { + Input_point_3 point = *it; + (Point_3&)(point) = p[i]; + *output++ = point; + } + + return output; +} + +/// Improved Laplacian smoothing (Vollmer et al) +/// on the k nearest neighbors. +/// This function is mutating the input point set. +/// This variant requires the kernel. +/// +/// Warning: +/// This method moves the points, thus +/// should not be called on containers sorted wrt points position. +/// +/// @commentheading Precondition: k >= 2. +/// +/// @commentheading Template Parameters: +/// @param ForwardIterator value_type convertible to Point_3. +/// @param Kernel Geometric traits class. +template +void +improved_laplacian_smoothing_3( + ForwardIterator first, ///< iterator over the first input/output point + ForwardIterator beyond, ///< past-the-end iterator + const unsigned int k, ///< number of neighbors + const unsigned int iter_number, + const Kernel& /*kernel*/, + typename Kernel::FT alpha, + typename Kernel::FT beta) +{ + // Point_3 types + typedef typename std::iterator_traits::value_type Input_point_3; + typedef typename Kernel::Point_3 Point_3; + typedef typename Kernel::Vector_3 Vector_3; + + // types for K nearest neighbors search structure + //typedef typename CGAL::Search_traits_3 Tree_traits; + typedef CGALi::KdTreeElement KdTreeElement; + typedef CGALi::KdTreeTraits Tree_traits; + typedef CGAL::Orthogonal_k_neighbor_search Neighbor_search; + typedef typename Neighbor_search::Tree Tree; + typedef typename Neighbor_search::iterator Search_iterator; + + // precondition: at least one element in the container. + // to fix: should have at least three distinct points + // but this is costly to check + CGAL_precondition(first != beyond); + + // precondition: at least 2 nearest neighbors + CGAL_precondition(k >= 2); + + unsigned int i; // point index + ForwardIterator it; // point iterator + + // Number of input points + int nb_points = std::distance(first, beyond); + + // Create kd-tree + //Tree tree(first,beyond); + std::vector treeElements; + for (it = first, i=0 ; it != beyond ; ++it,++i) + { + treeElements.push_back(KdTreeElement(*it,i)); + } + Tree tree(treeElements.begin(), treeElements.end()); + + std::vector p(nb_points); // positions at step iter_n + std::vector b(nb_points); // ... + + for(it = first, i=0; it != beyond; it++, ++i) + p[i] = *it; + + for(int iter_n = 0; iter_n < iter_number ; ++iter_n) + { + // Iterate over input points, compute (original) Laplacian smooth and b[]. + for(it = first, i=0; it != beyond; it++, ++i) + { + Point_3 np = CGALi::laplacian_smoothing_3(*it,tree,k); + b[i] = alpha*(np - *it) + (1-alpha)*(np - p[i]); + p[i] = np; + } + + // Iterate over input points, compute and output smooth points. + // Note: the cast to (Point_3&) ensures compatibility with classes derived from Point_3. + for(it = first, i=0; it != beyond; it++, ++i) + { + p[i] = CGALi::improved_laplacian_smoothing_3(p[i],b[i],tree,b,k,beta); + } + } + + // Iterate over input points and mutate them. + // Note: the cast to (Point_3&) ensures compatibility with classes derived from Point_3. + for(it = first, i=0; it != beyond; it++, ++i) + (Point_3&)(*it) = p[i]; +} + + +/// Improved Laplacian smoothing (Vollmer et al) +/// on the k nearest neighbors. +/// This variant deduces the kernel from iterator types. +/// +/// @commentheading Precondition: k >= 2. +/// +/// @return past-the-end output iterator. +template +OutputIterator +improved_laplacian_smoothing_3( + InputIterator first, ///< iterator over the first input point + InputIterator beyond, ///< past-the-end iterator over input points + OutputIterator output, ///< iterator over the first output point + unsigned int k, ///< number of neighbors + const unsigned int iter_number, + double alpha, + double beta) +{ + typedef typename std::iterator_traits::value_type Input_point_3; + typedef typename Kernel_traits::Kernel Kernel; + return improved_laplacian_smoothing_3(first,beyond,output,k,iter_number,Kernel(),alpha, beta); +} + +/// Improved Laplacian smoothing (Vollmer et al) +/// on the k nearest neighbors. +/// This function is mutating the input point set. +/// This variant deduces the kernel from iterator types. +/// +/// Warning: +/// As this method relocates the points, it +/// should not be called on containers sorted w.r.t. point locations. +/// +/// @commentheading Precondition: k >= 2. +/// +/// @commentheading Template Parameters: +/// @param ForwardIterator value_type convertible to Point_3. +template +void +improved_laplacian_smoothing_3( + ForwardIterator first, ///< iterator over the first input/output point + ForwardIterator beyond, ///< past-the-end iterator + unsigned int k, ///< number of neighbors + const unsigned int iter_number, + double alpha, + double beta) +{ + typedef typename std::iterator_traits::value_type Input_point_3; + typedef typename Kernel_traits::Kernel Kernel; + improved_laplacian_smoothing_3(first,beyond,k,iter_number,Kernel(),alpha, beta); +} + + +CGAL_END_NAMESPACE + +#endif // CGAL_IMPROVED_LAPLACIAN_SMOOTHING_3_H +