songsenand
/
cgal
mirror of https://github.com/CGAL/cgal
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
cgal/Surface_reconstruction_poin.../include/CGAL/APSS_reconstruction_function.h

661 lines
20 KiB
C++
Raw Blame History

// Copyright (c) 2007-09 ETH Zurich (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) : Gael Guennebaud, Laurent Saboret
#ifndef CGAL_APSS_RECONSTRUCTION_FUNCTION_H
#define CGAL_APSS_RECONSTRUCTION_FUNCTION_H
#include <vector>
#include <algorithm>
#define MLS_USE_CUSTOM_KDTREE
#include <CGAL/Point_with_normal_3.h>
#include <CGAL/make_surface_mesh.h>
#ifdef MLS_USE_CUSTOM_KDTREE
#include <CGAL/Neighborhood/knn_point_neighbor_search.h>
#else
#include <CGAL/Orthogonal_k_neighbor_search.h>
#endif
#include <CGAL/Search_traits_3.h>
#include <CGAL/Surface_mesher/Implicit_surface_oracle_3.h>
#include <CGAL/Min_sphere_d.h>
#include <CGAL/Optimisation_d_traits_3.h>
#include <CGAL/surface_reconstruction_points_assertions.h>
CGAL_BEGIN_NAMESPACE
/// APSS_reconstruction_function computes an implicit function
/// that defines a Point Set Surface (PSS) based on
/// moving least squares (MLS) fitting of algebraic spheres.
/// See "Algebraic Point Set Surfaces" by Guennebaud and Gross [Guennebaud07].
///
/// @heading Is Model for the Concepts:
/// Model of the 'ImplicitFunction' concept.
///
/// @heading Parameters:
/// @param Gt Geometric traits class.
template <class Gt>
class APSS_reconstruction_function
{
// Public types
public:
typedef Gt Geom_traits; ///< Kernel's geometric traits
typedef typename Geom_traits::FT FT;
typedef typename Geom_traits::Point_3 Point;
typedef typename Geom_traits::Vector_3 Vector;
typedef typename Geom_traits::Iso_cuboid_3 Iso_cuboid;
typedef typename Geom_traits::Sphere_3 Sphere;
typedef Point_with_normal_3<Gt> Point_with_normal; ///< == Point_with_normal_3<Gt>
typedef typename Point_with_normal::Normal Normal; ///< == Vector_3
// Private types
private:
// Item in the Kd-tree: position (Point_3) + normal + index
class KdTreeElement : public Point_with_normal
{
public:
unsigned int index;
KdTreeElement(const Origin& o = ORIGIN, unsigned int id=0)
: Point_with_normal(o), index(id)
{}
template <class K, class N>
KdTreeElement(const Point_with_normal_3<K,N>& pwn, unsigned int id=0)
: Point_with_normal(pwn), index(id)
{}
KdTreeElement(const Point& p, unsigned int id=0)
: Point_with_normal(p), index(id)
{}
KdTreeElement(const KdTreeElement& other)
: Point_with_normal(other), index(other.index)
{}
};
// Helper class for the Kd-tree
class KdTreeGT : public Geom_traits
{
public:
typedef KdTreeElement Point_3;
};
class TreeTraits : public Search_traits_3<KdTreeGT>
{
public:
typedef Point PointType;
};
#ifdef MLS_USE_CUSTOM_KDTREE
typedef knn_point_neighbor_search<TreeTraits> Neighbor_search;
#else
typedef Orthogonal_k_neighbor_search<TreeTraits> Neighbor_search;
#endif
typedef typename Neighbor_search::Tree Tree;
// typedef Orthogonal_k_neighbor_search<TreeTraits> CNeighbor_search;
// typedef typename CNeighbor_search::Tree CTree;
typedef typename Neighbor_search::Point_with_transformed_distance
Point_with_transformed_distance;
// Public methods
public:
/// Create an APSS implicit function from a point set.
///
/// @commentheading Precondition:
/// InputIterator value_type must be convertible to Point_with_normal.
///
/// @param first Iterator over first point to add.
/// @param beyond Past-the-end iterator to add.
/// @param k #neighbors for APSS sphere fitting.
template < class InputIterator >
APSS_reconstruction_function(InputIterator first, InputIterator beyond,
unsigned int k)
{
// Allocate smart pointer to data
m = new Private;
int nb_points = std::distance(first, beyond);
// Number of nearest neighbors
m->nofNeighbors = k;
// Create kd-tree
m->treeElements.reserve(nb_points);
unsigned int i=0;
for (InputIterator it=first ; it != beyond ; ++it,++i)
{
m->treeElements.push_back(KdTreeElement(*it,i));
}
#ifdef MLS_USE_CUSTOM_KDTREE
m->tree = new Tree(ConstDataWrapper<Point>(static_cast<const Point*>(&(m->treeElements[0])), m->treeElements.size(), sizeof(KdTreeElement)), &(m->treeElements[0]));
#else
m->tree = new Tree(m->treeElements.begin(), m->treeElements.end());
#endif
// CTree* ctree = new CTree(m->treeElements.begin(), m->treeElements.end());
// Compute the radius of each point = (distance max to k nearest neighbors)/2.
// The union of these balls defines the surface definition domain.
m->radii.resize(nb_points);
// for (int y=0; y<40; ++y)
{
int i=0;
for (InputIterator it=first ; it != beyond ; ++it, ++i)
{
Neighbor_search search(*(m->tree), *it, 16); // why 16?
typename Neighbor_search::iterator last=search.end(); --last;
FT maxdist2 = last->second; // squared distance to furthest neighbor
m->radii[i] = sqrt(maxdist2)/2.;
// for (typename Neighbor_search::iterator it = search.begin(); it != search.end(); ++it)
// std::cout << it->first.index << " ";
// std::cout << "\n";
// CNeighbor_search csearch(*ctree, *it, 8);
// for (typename CNeighbor_search::iterator it = csearch.begin(); it != csearch.end(); ++it)
// std::cout << it->first.index << " ";
// std::cout << "\n\n";
}
}
// exit(0);
// Compute barycenter, bounding box, bounding sphere and standard deviation.
update_bounds(first, beyond);
// Find a point inside the surface.
find_inner_point();
// Dichotomy error when projecting point (squared)
m->sqError = 1e-7 * Gt().compute_squared_radius_3_object()(m->bounding_sphere);
}
/// Copy constructor
APSS_reconstruction_function(const APSS_reconstruction_function& other) {
m = other.m;
m->count++;
}
/// operator =()
APSS_reconstruction_function& operator = (const APSS_reconstruction_function& other) {
m = other.m;
m->count++;
}
/// Destructor
~APSS_reconstruction_function() {
if (--(m->count)==0)
delete m;
}
void set_numbers_of_neighbors(unsigned int k) {m->nofNeighbors = k;}
/// Get the bounding box.
Iso_cuboid bounding_box() const
{
return m->bounding_box;
}
/// Returns a sphere bounding the inferred surface.
const Sphere& bounding_sphere() const
{
return m->bounding_sphere;
}
/// Get the region of interest, ignoring the outliers.
/// This method is used to define the OpenGL arcball sphere.
Sphere region_of_interest() const
{
// A good candidate is a sphere containing the dense region of the point cloud:
// - center point is barycenter
// - Radius is 2 * standard deviation
FT radius = (FT)2 * (FT)m->diameter_standard_deviation;
return Sphere(m->barycenter, radius*radius);
}
private:
/** Fit an algebraic sphere on a set of neigbors in a Moving Least Square sense.
The weight function is scaled such that the weight of the furthest neighbor is 0.
*/
void fit(const Neighbor_search& search) const
{
typename Neighbor_search::iterator last=search.end(); --last;
FT r2 = last->second; // squared distance to furthest neighbor
Vector sumP(0,0,0);
Vector sumN(0,0,0);
FT sumDotPP = 0.;
FT sumDotPN = 0.;
FT sumW = 0.;
r2 *= 1.001;
FT invr2 = 1./r2;
for (typename Neighbor_search::iterator it = search.begin(); it != search.end(); ++it)
{
Vector p = it->first - CGAL::ORIGIN;
const Vector& n = it->first.normal();
FT w = 1. - it->second*invr2;
w = w*w; w = w*w;
sumP = add(sumP,mul(w,p));
sumN = add(sumN,mul(w,n));
sumDotPP += w * dot(p,p);
sumDotPN += w * dot(p,n);
sumW += w;
}
FT invSumW = 1./sumW;
m->as.u4 = 0.5 * (sumDotPN - invSumW*dot(sumP,sumN))/(sumDotPP - invSumW*dot(sumP,sumP));
m->as.u13 = mul(invSumW,add(sumN,mul(-2.*m->as.u4,sumP)));
m->as.u0 = -invSumW*(dot(m->as.u13,sumP)+m->as.u4*sumDotPP);
m->as.finalize();
}
/** Check whether the point 'p' is close to the input points or not.
We assume that it's not if it is far from its nearest neighbor.
*/
inline bool isValid(const Point_with_transformed_distance& nearest_neighbor, const Point& /* p */) const
{
FT r = 2. * m->radii[nearest_neighbor.first.index];
return (r*r > nearest_neighbor.second);
}
public:
/// Evaluate implicit function for any 3D point.
/// ('ImplicitFunction' interface)
//
// Implementation note: this function is called a large number of times,
// thus us heavily optimized. The bottleneck is Neighbor_search's constructor,
// which we try to avoid calling.
FT operator()(const Point& p) const
{
// Is 'p' close to the surface?
// Optimization: test first if 'p' is close to one of the neighbors
// computed during the previous call.
typename Geom_traits::Compute_squared_distance_3 sqd;
m->cached_nearest_neighbor.second = sqd(p, m->cached_nearest_neighbor.first);
if (!isValid(m->cached_nearest_neighbor, p))
{
// Compute the nearest neighbor and cache it
KdTreeElement query(p);
Neighbor_search search_1nn(*(m->tree), query, 1);
m->cached_nearest_neighbor = *(search_1nn.begin());
// Is 'p' close to the surface?
if (!isValid(m->cached_nearest_neighbor, p))
{
// If 'p' is far from the surface, project its nearest neighbor onto the surface...
Vector n;
Point pp = m->cached_nearest_neighbor.first;
project(pp,n,1);
// ...and return the (signed) distance to the surface
Vector h = sub(p,pp);
return length(h) * ( dot(n,h)>0. ? 1. : -1.);
}
}
// Compute k nearest neighbors and cache the first one
KdTreeElement query(p);
Neighbor_search search_knn(*(m->tree), query, m->nofNeighbors);
m->cached_nearest_neighbor = *(search_knn.begin());
// If 'p' is close to the surface, fit an algebraic sphere
// on a set of neigbors in a Moving Least Square sense.
fit(search_knn);
// return the distance to the sphere
return m->as.euclideanDistance(p);
}
/// Returns a point located inside the inferred surface.
Point get_inner_point() const
{
return m->inner_point;
}
// Private methods:
private:
/** Projects the point p onto the MLS surface.
*/
void project(Point& p, unsigned int maxNofIterations = 20) const
{
Vector n;
project(p,n,maxNofIterations);
}
/** Projects the point p onto the MLS surface, and returns an approximate normal.
*/
void project(Point& p, Vector& n, unsigned int maxNofIterations = 20) const
{
Point source = p;
FT delta2 = 0.;
unsigned int countIter = 0;
do {
Neighbor_search search(*(m->tree), p, m->nofNeighbors);
// if p is far away the input point cloud, start with the closest point.
if (!isValid(*(search.begin()),p))
{
p = search.begin()->first;
n = search.begin()->first.normal();
delta2 = search.begin()->second;
}
else
{
fit(search);
Point oldP = p;
if (m->as.state==AlgebraicSphere::SPHERE)
{
// projection onto a sphere.
Vector dir = normalize(sub(source,m->as.center));
p = add(m->as.center,mul(m->as.radius,dir));
FT flipN = m->as.u4<0. ? -1. : 1.;
if (!isValid(*(search.begin()),p))
{
// if the closest intersection is far away the input points,
// then we take the other one.
p = add(m->as.center,mul(-m->as.radius,dir));
flipN = -flipN;
}
n = mul(flipN,dir);
if (!isValid(*(search.begin()),p))
{
std::cout << "Invalid projection\n";
}
}
else if (m->as.state==AlgebraicSphere::PLANE)
{
// projection onto a plane.
p = sub(source, mul(dot(m->as.normal,source-CGAL::ORIGIN)+m->as.d,m->as.normal));
}
else
{
// iterative projection onto the algebraic sphere.
p = m->as.iProject(source);
}
Vector diff = sub(oldP,p);
delta2 = dot(diff,diff);
}
} while ( ((++countIter)<maxNofIterations) && (delta2<m->sqError) );
}
inline static FT dot(const Vector& a, const Vector& b) {
return a.x()*b.x() + a.y()*b.y() + a.z()*b.z();
}
inline static FT length(const Vector& a) {
return sqrt(dot(a,a));
}
inline static Vector mul(FT s, const Vector& p) {
return Vector(p.x()*s, p.y()*s, p.z()*s);
}
inline static Point add(FT s, const Point& p) {
return Point(p.x()+s, p.y()+s, p.z()+s);
}
inline static Point add(const Point& a, const Vector& b) {
return Point(a.x()+b.x(), a.y()+b.y(), a.z()+b.z());
}
inline static Vector add(const Vector& a, const Vector& b) {
return Vector(a.x()+b.x(), a.y()+b.y(), a.z()+b.z());
}
inline static Point sub(const Point& a, const Vector& b) {
return Point(a.x()-b.x(), a.y()-b.y(), a.z()-b.z());
}
inline static Vector sub(const Point& a, const Point& b) {
return Vector(a.x()-b.x(), a.y()-b.y(), a.z()-b.z());
}
inline static Vector normalize(const Vector& p) {
FT s = 1. / length(p);
return mul(s,p);
}
inline static Vector cross(const Vector& a, const Vector& b) {
return Vector(a.y()*b.z() - a.z()*b.y(),
a.z()*b.x() - a.x()*b.z(),
a.x()*b.y() - a.y()*b.x());
}
/// Compute barycenter, bounding box, bounding sphere and standard deviation.
///
/// @param first Iterator over first point of point set.
/// @param beyond Past-the-end iterator of point set.
template < class InputIterator >
void update_bounds(InputIterator first, InputIterator beyond)
{
if (first == beyond)
return;
// Update bounding box and barycenter.
// TODO: we should use the functions in PCA component instead.
FT xmin,xmax,ymin,ymax,zmin,zmax;
xmin = ymin = zmin = 1e38;
xmax = ymax = zmax = -1e38;
Vector v = CGAL::NULL_VECTOR;
FT norm = 0;
for (InputIterator it = first; it != beyond; it++)
{
Point p = *it;
// update bbox
xmin = (std::min)(p.x(),xmin);
ymin = (std::min)(p.y(),ymin);
zmin = (std::min)(p.z(),zmin);
xmax = (std::max)(p.x(),xmax);
ymax = (std::max)(p.y(),ymax);
zmax = (std::max)(p.z(),zmax);
// update barycenter
v = v + (p - CGAL::ORIGIN);
norm += 1;
}
//
Point p(xmin,ymin,zmin);
Point q(xmax,ymax,zmax);
m->bounding_box = Iso_cuboid(p,q);
//
m->barycenter = CGAL::ORIGIN + v / norm;
// sphere of smallest volume enclosing the input points
Min_sphere_d< CGAL::Optimisation_d_traits_3<Gt> > ms3(first, beyond);
m->bounding_sphere = Sphere(ms3.center(), ms3.squared_radius());
// Compute standard deviation of the distance to barycenter
typename Geom_traits::Compute_squared_distance_3 sqd;
FT sq_radius = 0;
for (InputIterator it = first; it != beyond; it++)
{
sq_radius += sqd(*it, m->barycenter);
}
sq_radius /= std::distance(first, beyond);
m->diameter_standard_deviation = CGAL::sqrt(sq_radius);
}
private:
struct AlgebraicSphere {
FT u0, u4;
Vector u13;
enum State {UNDETERMINED=0,PLANE=1,SPHERE=2};
State state;
Point center;
FT radius;
Vector normal;
FT d;
AlgebraicSphere() : state(UNDETERMINED) {}
/** Converts the algebraic sphere to an explicit sphere or plane.
*/
void finalize(void) {
if (fabs(u4)>1e-9)
{
state = SPHERE;
FT b = 1./u4;
center = CGAL::ORIGIN + mul(-0.5*b,u13);
radius = sqrt(dot(center - CGAL::ORIGIN,center - CGAL::ORIGIN) - b*u0);
}
else if (u4==0.)
{
state = PLANE;
FT s = 1./length(u13);
normal = mul(s,u13);
d = u0*s;
}
else
{
state = UNDETERMINED;
}
}
/** Compute the Euclidean distance between the algebraic surface and a point.
*/
FT euclideanDistance(const Point& p) {
if (state==SPHERE)
{
FT aux = length(sub(center,p)) - radius;
if (u4<0.)
aux = -aux;
return aux;
}
if (state==PLANE)
return dot(p - CGAL::ORIGIN,normal) + d;
// else, tedious case, fall back to an iterative method:
return iEuclideanDistance(p);
}
/** Euclidean distance via an iterative projection procedure.
This is an optimized version of distance(x,iProject(x)).
*/
inline FT iEuclideanDistance(const Point& x) const
{
FT d = 0.;
Vector grad;
Vector dir = add(mul(2.*u4,x-CGAL::ORIGIN),u13);
FT ilg = 1./length(dir);
dir = mul(ilg,dir);
FT ad = u0 + dot(u13,x-CGAL::ORIGIN) + u4 * dot(x-CGAL::ORIGIN,x-CGAL::ORIGIN);
FT delta = -ad*(ilg<1.?ilg:1.);
Point p = add(x, mul(delta,dir));
d += delta;
for (int i=0 ; i<5 ; ++i)
{
grad = add(mul(2.*u4,p-CGAL::ORIGIN),u13);
ilg = 1./length(grad);
delta = -(u0 + dot(u13,p-CGAL::ORIGIN) + u4 * dot(p-CGAL::ORIGIN,p-CGAL::ORIGIN))*(ilg<1.?ilg:1.);
p = add(p,mul(delta,dir));
d += delta;
}
return -d;
}
/** Iterative projection.
*/
inline Point iProject(const Point& x) const
{
Vector grad;
Vector dir = add(mul(2.*u4,x-CGAL::ORIGIN),u13);
FT ilg = 1./length(dir);
dir = mul(ilg,dir);
FT ad = u0 + dot(u13,x-CGAL::ORIGIN) + u4 * dot(x-CGAL::ORIGIN,x-CGAL::ORIGIN);
FT delta = -ad*(ilg<1.?ilg:1.);
Point p = add(x, mul(delta,dir));
for (int i=0 ; i<5 ; ++i)
{
grad = add(mul(2.*u4,p-CGAL::ORIGIN),u13);
ilg = 1./length(grad);
delta = -(u0 + dot(u13,p-CGAL::ORIGIN) + u4 * dot(p-CGAL::ORIGIN,p-CGAL::ORIGIN))*(ilg<1.?ilg:1.);
p = add(p,mul(delta,dir));
}
return p;
}
};
/// Find a random point inside the surface.
void find_inner_point()
{
m->inner_point = CGAL::ORIGIN;
FT min_f = 1e38;
// Try random points until we find a point / value < 0
Point center = m->bounding_sphere.center();
FT radius = sqrt(m->bounding_sphere.squared_radius());
CGAL::Random_points_in_sphere_3<Point> rnd(radius);
while (min_f > 0)
{
// Create random point in bounding sphere
Point p = center + (*rnd++ - CGAL::ORIGIN);
FT value = (*this)(p);
if(value < min_f)
{
m->inner_point = p;
min_f = value;
}
}
}
// Data members
private:
struct Private
{
Private()
: tree(NULL), count(1)
{}
~Private()
{
delete tree; tree = NULL;
}
Tree* tree;
std::vector<KdTreeElement> treeElements;
std::vector<FT> radii;
Iso_cuboid bounding_box; // Points' bounding box
Sphere bounding_sphere; // Points' bounding sphere
Point barycenter; // Points' barycenter
FT diameter_standard_deviation; // Standard deviation of the distance to barycenter
FT sqError; // Dichotomy error when projecting point (squared)
unsigned int nofNeighbors; // Number of nearest neighbors
Point inner_point; // Point inside the surface
mutable AlgebraicSphere as;
mutable Point_with_transformed_distance cached_nearest_neighbor;
int count; // reference counter
};
Private* m; // smart pointer to data
}; // end of APSS_reconstruction_function
CGAL_END_NAMESPACE
#endif // CGAL_APSS_RECONSTRUCTION_FUNCTION_H
Reference in New Issue View Git Blame Copy Permalink