...
 
Commits (50)
/*
Copyright (C) 2014 Nicolas Mellado <nmellado0@gmail.com>
Copyright (C) 2015 Gael Guennebaud <gael.guennebaud@inria.fr>
This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.
file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#ifndef _GRENAILLE_COVARIANCE_PLANE_FIT_
#define _GRENAILLE_COVARIANCE_PLANE_FIT_
#include <Eigen/Eigenvalues>
#include <Eigen/Eigenvalues>
#include "enums.h"
namespace Grenaille
......@@ -19,17 +19,13 @@ namespace Grenaille
/*!
\brief Plane fitting procedure using only points position
\note This procedure requires two passes to fit a plane
This class can also computes the surface variation measure introduced in
\cite Pauly:2002:PSSimplification. The solver used to analyse the covariance
\cite Pauly:2002:PSSimplification. The solver used to analyse the covariance
matrix is stored for further use.
\inherit Concept::FittingProcedureConcept
\warning This class is currently untested and should not be used !
\see CompactPlane
*/
template < class DataPoint, class _WFunctor, typename T >
......@@ -47,13 +43,13 @@ protected:
public:
/*! \brief Scalar type inherited from DataPoint*/
typedef typename Base::Scalar Scalar;
typedef typename Base::Scalar Scalar;
/*! \brief Vector type inherited from DataPoint*/
typedef typename Base::VectorType VectorType;
/*! \brief Vector type inherited from DataPoint*/
typedef typename Base::MatrixType MatrixType;
/*! \brief Weight Function*/
typedef _WFunctor WFunctor;
typedef _WFunctor WFunctor;
/*! \brief Solver used to analyse the covariance matrix*/
typedef Eigen::SelfAdjointEigenSolver<MatrixType> Solver;
......@@ -95,23 +91,23 @@ public:
/**************************************************************************/
/* Results */
/**************************************************************************/
using Base::potential;
/*! \brief Value of the scalar field at the evaluation point */
MULTIARCH inline Scalar potential() const { return Base::potential(m_evalPos); }
/*! \brief Value of the normal of the primitive at the evaluation point */
MULTIARCH inline VectorType normal() const { return Base::m_p.template head<DataPoint::Dim>(); }
/*! \brief Reading access to the Solver used to analyse the covariance
/*! \brief Reading access to the Solver used to analyse the covariance
matrix */
MULTIARCH inline const Solver& solver() const { return m_solver; }
/*! \brief Implements \cite Pauly:2002:PSSimplification surface variation.
It computes the ratio \f$ d \frac{\lambda_0}{\sum_i \lambda_i} \f$ with \c d the dimension of the ambient space.
\return 0 for invalid fits
*/
MULTIARCH inline Scalar surfaceVariation() const;
......@@ -122,20 +118,20 @@ namespace internal {
using ::Grenaille::internal::FitSpaceDer;
using ::Grenaille::internal::FitScaleDer;
/*!
/*!
\brief Internal generic class computing the derivatives of covariance plane fits
\inherit Concept::FittingExtensionConcept
The differentiation can be done automatically in scale and/or space, by
combining the enum values FitScaleDer and FitSpaceDer in the template
combining the enum values FitScaleDer and FitSpaceDer in the template
parameter Type.
The differenciated values are stored in static arrays. The size of the
arrays is computed with respect to the derivation type (scale and/or space)
and the number of the dimension of the ambiant space.
By convention, the scale derivatives are stored at index 0 when Type
and the number of the dimension of the ambiant space.
By convention, the scale derivatives are stored at index 0 when Type
contains at least FitScaleDer. The size of these arrays can be known using
derDimension(), and the differentiation type by isScaleDer() and
derDimension(), and the differentiation type by isScaleDer() and
isSpaceDer().
*/
template < class DataPoint, class _WFunctor, typename T, int Type>
......@@ -143,7 +139,7 @@ class CovariancePlaneDer : public T
{
private:
typedef T Base; /*!< \brief Generic base type */
protected:
enum
......@@ -152,7 +148,7 @@ protected:
PROVIDES_COVARIANCE_PLANE_DERIVATIVE, /*!< \brief Provides derivatives for hyper-planes */
PROVIDES_NORMAL_DERIVATIVE
};
static const int NbDerivatives = ((Type & FitScaleDer) ? 1 : 0 ) + ((Type & FitSpaceDer) ? DataPoint::Dim : 0);
static const int DerStorageOrder = (Type & FitSpaceDer) ? Eigen::RowMajor : Eigen::ColMajor;
......@@ -171,7 +167,7 @@ private:
// computation data
ScalarArray m_dSumW; /*!< \brief Sum of weight derivatives */
MatrixType m_dCov[NbDerivatives];
VectorArray m_dCog; /*!< \brief Derivatives of the centroid */
VectorArray m_dNormal; /*!< \brief Derivatives of the hyper-plane normal */
ScalarArray m_dDist; /*!< \brief Derivatives of the MLS scalar field */
......@@ -196,10 +192,10 @@ public:
/**************************************************************************/
/* Use results */
/**************************************************************************/
/*! \brief Returns the derivatives of the scalar field at the evaluation point */
MULTIARCH inline ScalarArray dPotential() const { return m_dDist; }
/*! \brief Returns the derivatives of the primitive normal */
MULTIARCH inline VectorArray dNormal() const { return m_dNormal; }
......@@ -212,15 +208,15 @@ public:
}; //class CovariancePlaneDer
}// namespace internal
}// namespace internal
/*!
\brief Differentiation in scale of the CovariancePlaneFit
\inherit Concept::FittingExtensionConcept
Requierement:
Requierement:
\verbatim PROVIDES_COVARIANCE_PLANE \endverbatim
Provide:
Provide:
\verbatim PROVIDES_COVARIANCE_PLANE_SCALE_DERIVATIVE \endverbatim
*/
template < class DataPoint, class _WFunctor, typename T>
......@@ -237,9 +233,9 @@ protected:
\brief Spatial differentiation of the CovariancePlaneFit
\inherit Concept::FittingExtensionConcept
Requierement:
Requierement:
\verbatim PROVIDES_COVARIANCE_PLANE \endverbatim
Provide:
Provide:
\verbatim PROVIDES_COVARIANCE_PLANE_SPACE_DERIVATIVE \endverbatim
*/
template < class DataPoint, class _WFunctor, typename T>
......@@ -256,10 +252,10 @@ protected:
\brief Differentiation both in scale and space of the CovariancePlaneFit
\inherit Concept::FittingExtensionConcept
Requierement:
Requierement:
\verbatim PROVIDES_COVARIANCE_PLANE \endverbatim
Provide:
\verbatim PROVIDES_COVARIANCE_PLANE_SCALE_DERIVATIVE
Provide:
\verbatim PROVIDES_COVARIANCE_PLANE_SCALE_DERIVATIVE
PROVIDES_COVARIANCE_PLANE_SPACE_DERIVATIVE
\endverbatim
*/
......
/*
Copyright (C) 2014 Nicolas Mellado <nmellado0@gmail.com>
Copyright (C) 2015 Gael Guennebaud <gael.guennebaud@inria.fr>
This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.
file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
template < class DataPoint, class _WFunctor, typename T>
void
void
CovariancePlaneFit<DataPoint, _WFunctor, T>::init(const VectorType& _evalPos)
{
// Setup primitive
......@@ -24,7 +24,7 @@ CovariancePlaneFit<DataPoint, _WFunctor, T>::init(const VectorType& _evalPos)
}
template < class DataPoint, class _WFunctor, typename T>
bool
bool
CovariancePlaneFit<DataPoint, _WFunctor, T>::addNeighbor(const DataPoint& _nei)
{
VectorType q = _nei.pos() - m_evalPos;
......@@ -36,7 +36,7 @@ CovariancePlaneFit<DataPoint, _WFunctor, T>::addNeighbor(const DataPoint& _nei)
m_cog += w * q;
m_sumW += w;
m_cov += w * q * q.transpose();
++(Base::m_nbNeighbors);
return true;
}
......@@ -61,15 +61,20 @@ CovariancePlaneFit<DataPoint, _WFunctor, T>::finalize ()
// Center the covariance on the centroid:
m_cov -= m_cog * m_cog.transpose() / m_sumW;
// \note The covariance matrix should be here normalized by m_sumW.
// As it does not affect the eigen decomposition, we skip this normalization
// to save computation.
// Finalize the centroid
m_cog = m_cog/m_sumW + m_evalPos;
#ifdef __CUDACC__
m_solver.computeDirect(m_cov);
#else
m_solver.compute(m_cov);
#endif
Base::setPlane(m_solver.eigenvectors().col(0), m_cog);
// \todo Use the output of the solver to check stability
......@@ -93,7 +98,7 @@ namespace internal
{
template < class DataPoint, class _WFunctor, typename T, int Type>
void
void
CovariancePlaneDer<DataPoint, _WFunctor, T, Type>::init(const VectorType& _evalPos)
{
Base::init(_evalPos);
......@@ -106,7 +111,7 @@ CovariancePlaneDer<DataPoint, _WFunctor, T, Type>::init(const VectorType& _evalP
template < class DataPoint, class _WFunctor, typename T, int Type>
bool
bool
CovariancePlaneDer<DataPoint, _WFunctor, T, Type>::addNeighbor(const DataPoint &_nei)
{
bool bResult = Base::addNeighbor(_nei);
......@@ -140,7 +145,7 @@ CovariancePlaneDer<DataPoint, _WFunctor, T, Type>::addNeighbor(const DataPoint
template < class DataPoint, class _WFunctor, typename T, int Type>
FIT_RESULT
FIT_RESULT
CovariancePlaneDer<DataPoint, _WFunctor, T, Type>::finalize()
{
MULTIARCH_STD_MATH(sqrt);
......@@ -155,7 +160,7 @@ CovariancePlaneDer<DataPoint, _WFunctor, T, Type>::finalize()
Eigen::Matrix<Scalar,2,1> shifted_eivals = Base::m_solver.eigenvalues().template tail<2>().array() - Base::m_solver.eigenvalues()(0);
if(shifted_eivals(0) < consider_as_zero || shifted_eivals(0) < epsilon * shifted_eivals(1)) shifted_eivals(0) = 0;
if(shifted_eivals(1) < consider_as_zero) shifted_eivals(1) = 0;
for(int k=0; k<NbDerivatives; ++k)
{
// Finalize the computation of dCov.
......@@ -167,10 +172,10 @@ CovariancePlaneDer<DataPoint, _WFunctor, T, Type>::finalize()
- shifted_cog * m_dCog.col(k).transpose()
- m_dCog.col(k) * shifted_cog.transpose()
+ m_dSumW[k] * shifted_cog * shifted_cog.transpose();
// cancel centered basis of dCog:
m_dCog.col(k) += m_dSumW[k] * Base::m_evalPos;
// apply normalization by sumW:
m_dCog.col(k) = (m_dCog.col(k) - m_dSumW(k) * Base::m_cog) / Base::m_sumW;
......@@ -187,7 +192,7 @@ CovariancePlaneDer<DataPoint, _WFunctor, T, Type>::finalize()
if(shifted_eivals(0)>0) z(0) /= shifted_eivals(0);
if(shifted_eivals(1)>0) z(1) /= shifted_eivals(1);
m_dNormal.col(k) = Base::m_solver.eigenvectors().template rightCols<2>() * z;
VectorType dDiff = -m_dCog.col(k);
if(k>0 || !isScaleDer())
dDiff(isScaleDer() ? k-1 : k) += 1;
......
......@@ -5,6 +5,7 @@
*/
#include <Eigen/Eigenvalues>
#include <Eigen/Core>
#ifndef _GRENAILLE_CURVATURE_
#define _GRENAILLE_CURVATURE_
......@@ -45,6 +46,10 @@ public:
typedef typename Base::VectorType VectorType; /*!< \brief Inherited vector type*/
typedef typename DataPoint::MatrixType MatrixType; /*!< \brief Matrix type inherited from DataPoint*/
private:
typedef Eigen::Matrix<Scalar,3,2> Mat32; /*!< \brief Matrix type for tangent plane basis */
typedef Eigen::Matrix<Scalar,2,2> Mat22; /*!< \brief Matrix type for shape operator */
private:
Scalar m_k1, m_k2;
VectorType m_v1, m_v2;
......@@ -87,6 +92,20 @@ public:
//! \brief Returns an estimate of the Gaussian curvature
MULTIARCH inline Scalar GaussianCurvature() const { return m_k1 * m_k2;}
//! \brief Compute principal curvature directions
//!
//! The tangent plane can be calculated from the normal vector or from its
//! derivatives, depending of the useNormal parameter
//!
//! The finalize() method calls this function with useNormal=false by
//! default.
//!
MULTIARCH inline FIT_RESULT computeCurvature(bool useNormal = false);
protected:
//! \brief Compute a tangent plane basis
MULTIARCH inline Mat32 tangentPlane(bool useNormal = false) const;
};
#include "curvature.hpp"
......
......@@ -6,31 +6,111 @@
file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
template < class DataPoint, class _WFunctor, typename T>
FIT_RESULT
CurvatureEstimator<DataPoint, _WFunctor, T>::finalize()
{
FIT_RESULT bResult = Base::finalize();
if(bResult != UNDEFINED)
{
return computeCurvature(false);
}
return bResult;
}
template < class DataPoint, class _WFunctor, typename T>
FIT_RESULT CurvatureEstimator<DataPoint, _WFunctor, T>::computeCurvature(bool useNormal)
{
// Get the object space Weingarten map dN
MatrixType dN = Base::dNormal().template middleCols<DataPoint::Dim>(Base::isScaleDer() ? 1: 0);
// Compute tangent-space basis
Mat32 B = tangentPlane(useNormal);
// Compute the 2x2 matrix representing the shape operator by transforming dN to the basis B.
// Recall that dN is a bilinear form, it thus transforms as follows:
Mat22 S = B.transpose() * dN * B;
// Recall that at this stage, the shape operator represented by S describes the normal curvature K_n(u) in the direction u \in R^2 as follows:
// K_n(u) = u^T S u
// The principal curvatures are fully defined by the values and the directions of the extrema of K_n.
//
// If the normal field N(x) comes from the gradient of a scalar field, then N(x) is curl-free, and dN and S are symmetric matrices.
// In this case, the extrema of the previous quadratic form are directly obtained by the the eigenvalue decomposition of S.
// However, if N(x) is only an approximation of the normal field of a surface, then N(x) is not necessarily curl-free, and in this case S is not symmetric.
// In this case, we first have to find an equivalent symmetric matrix S' such that:
// K_n(u) = u^T S' u,
// for any u \in R^2.
// It is easy to see that such a S' is simply obtained as:
// S' = (S + S^T)/2
S(0,1) = S(1,0) = (S(0,1) + S(1,0))/Scalar(2);
Eigen::SelfAdjointEigenSolver<Mat22> eig2;
eig2.computeDirect(S);
if (eig2.info() != Eigen::Success){
return UNDEFINED;
}
m_k1 = eig2.eigenvalues()(0);
m_k2 = eig2.eigenvalues()(1);
m_v1 = B * eig2.eigenvectors().col(0);
m_v2 = B * eig2.eigenvectors().col(1);
if(abs(m_k1)<abs(m_k2))
{
#ifdef __CUDACC__
Scalar tmpk = m_k1;
m_k1 = m_k2;
m_k2 = tmpk;
VectorType tmpv = m_v1;
m_v1 = m_v2;
m_v2 = tmpv;
#else
std::swap(m_k1, m_k2);
std::swap(m_v1, m_v2);
#endif
}
return this->getCurrentState();
}
template < class DataPoint, class _WFunctor, typename T>
typename CurvatureEstimator<DataPoint, _WFunctor, T>::Mat32
CurvatureEstimator<DataPoint, _WFunctor, T>::tangentPlane(bool useNormal) const
{
typedef typename VectorType::Index Index;
typedef Eigen::Matrix<Scalar,3,2> Mat32;
typedef Eigen::Matrix<Scalar,2,2> Mat22;
MULTIARCH_STD_MATH(sqrt);
MULTIARCH_STD_MATH(abs);
FIT_RESULT bResult = Base::finalize();
Mat32 B;
Index i0=Index(-1), i1=Index(-1), i2=Index(-1);
if(bResult != UNDEFINED)
if(useNormal)
{
VectorType n = Base::normal();
n.minCoeff(&i0);
i1 = (i0+1)%3;
i2 = (i0+2)%3;
B.col(0)[i0] = 0;
B.col(0)[i1] = n[i2];
B.col(0)[i2] = -n[i1];
B.col(0).normalize();
B.col(1) = B.col(0).cross(n);
}
else
{
// Get the object space Weingarten map dN
MatrixType dN = Base::dNormal().template middleCols<DataPoint::Dim>(Base::isScaleDer() ? 1: 0);
// Compute tangent-space basis from dN
// 1 - pick the column with maximal norm as the first tangent vector,
Index i0, i1, i2;
Scalar sqNorm = dN.colwise().squaredNorm().maxCoeff(&i0);
Mat32 B;
B.col(0) = dN.col(i0) / sqrt(sqNorm);
// 2 - orthogonalize the other column vectors, and pick the most reliable one
i1 = (i0+1)%3;
......@@ -41,53 +121,8 @@ CurvatureEstimator<DataPoint, _WFunctor, T>::finalize()
Scalar v2norm2 = v2.squaredNorm();
if(v1norm2 > v2norm2) B.col(1) = v1 / sqrt(v1norm2);
else B.col(1) = v2 / sqrt(v2norm2);
// Compute the 2x2 matrix representing the shape operator by transforming dN to the basis B.
// Recall that dN is a bilinear form, it thus transforms as follows:
Mat22 S = B.transpose() * dN * B;
// Recall that at this stage, the shape operator represented by S describes the normal curvature K_n(u) in the direction u \in R^2 as follows:
// K_n(u) = u^T S u
// The principal curvatures are fully defined by the values and the directions of the extrema of K_n.
//
// If the normal field N(x) comes from the gradient of a scalar field, then N(x) is curl-free, and dN and S are symmetric matrices.
// In this case, the extrema of the previous quadratic form are directly obtained by the the eigenvalue decomposition of S.
// However, if N(x) is only an approximation of the normal field of a surface, then N(x) is not necessarily curl-free, and in this case S is not symmetric.
// In this case, we first have to find an equivalent symmetric matrix S' such that:
// K_n(u) = u^T S' u,
// for any u \in R^2.
// It is easy to see that such a S' is simply obtained as:
// S' = (S + S^T)/2
S(0,1) = S(1,0) = (S(0,1) + S(1,0))/Scalar(2);
Eigen::SelfAdjointEigenSolver<Mat22> eig2;
eig2.computeDirect(S);
if (eig2.info() != Eigen::Success){
return UNDEFINED;
}
m_k1 = eig2.eigenvalues()(0);
m_k2 = eig2.eigenvalues()(1);
m_v1 = B * eig2.eigenvectors().col(0);
m_v2 = B * eig2.eigenvectors().col(1);
if(abs(m_k1)<abs(m_k2))
{
#ifdef __CUDACC__
Scalar tmpk = m_k1;
m_k1 = m_k2;
m_k2 = tmpk;
VectorType tmpv = m_v1;
m_v1 = m_v2;
m_v2 = tmpv;
#else
std::swap(m_k1, m_k2);
std::swap(m_v1, m_v2);
#endif
}
}
return bResult;
return B;
}

#ifndef _GRENAILLE_CURVATUREESTIMATION_
#define _GRENAILLE_CURVATUREESTIMATION_
namespace Grenaille
{
template < class DataPoint, class _WFunctor, typename T>
class BaseCurvatureEstimator : public T
{
private:
typedef T Base;
protected:
enum
{
PROVIDES_PRINCIPALE_CURVATURES
};
public:
typedef typename Base::Scalar Scalar; /*!< \brief Inherited scalar type*/
typedef typename Base::VectorType VectorType; /*!< \brief Inherited vector type*/
typedef typename DataPoint::MatrixType MatrixType; /*!< \brief Matrix type inherited from DataPoint*/
protected:
Scalar m_k1, m_k2;
VectorType m_v1, m_v2;
public:
/*! \brief Default constructor */
MULTIARCH inline BaseCurvatureEstimator() : m_k1(0), m_k2(0) {}
public:
/**************************************************************************/
/* Initialization */
/**************************************************************************/
/*! \copydoc Concept::FittingProcedureConcept::init() */
MULTIARCH inline void init (const VectorType& _evalPos);
/**************************************************************************/
/* Use results */
/**************************************************************************/
//! \brief Returns an estimate of the first principal curvature value
//!
//! It is the greatest curvature in <b>absolute value</b>.
MULTIARCH inline Scalar k1() const { return m_k1; }
//! \brief Returns an estimate of the second principal curvature value
//!
//! It is the smallest curvature in <b>absolute value</b>.
MULTIARCH inline Scalar k2() const { return m_k2; }
//! \brief Returns an estimate of the first principal curvature direction
//!
//! It is the greatest curvature in <b>absolute value</b>.
MULTIARCH inline VectorType k1Direction() const { return m_v1; }
//! \brief Returns an estimate of the second principal curvature direction
//!
//! It is the smallest curvature in <b>absolute value</b>.
MULTIARCH inline VectorType k2Direction() const { return m_v2; }
//! \brief Returns an estimate of the mean curvature
MULTIARCH inline Scalar kMean() const { return (m_k1 + m_k2)/2.;}
//! \brief Returns an estimate of the Gaussian curvature
MULTIARCH inline Scalar GaussianCurvature() const { return m_k1 * m_k2;}
};
/*!
* \brief Extension to compute curvature values based on a covariance analysis
* of normal vectors of neighbors.
*
* A 3D covariance matrix is computed from the normals of the neighbors and the
* two principal curvature values and directions are given by the two extreme
* eigenvalues and associated eigenvectors of the covariance matrix
* \cite Liang:1990:RRSS.
*
* \warning This class is valid only in 3D.
*/
template < class DataPoint, class _WFunctor, typename T>
class NormalCovarianceCurvature : public BaseCurvatureEstimator<DataPoint,_WFunctor,T>
{
private:
typedef BaseCurvatureEstimator<DataPoint,_WFunctor,T> Base;
//TODO(thib) check the curvature values that might be wrong
//TODO(thib) use weighting function
//TODO(thib) which eigenvectors should be selected ? extreme of maximal ?
public:
typedef typename Base::Scalar Scalar; /*!< \brief Inherited scalar type*/
typedef typename Base::VectorType VectorType; /*!< \brief Inherited vector type*/
typedef typename DataPoint::MatrixType MatrixType; /*!< \brief Matrix type inherited from DataPoint*/
/*! \brief Solver used to analyse the covariance matrix*/
typedef Eigen::SelfAdjointEigenSolver<MatrixType> Solver;
protected:
MatrixType m_cov; /*!< \brief Covariance matrix */
VectorType m_cog; /*!< \brief Gravity center */
Solver m_solver; /*!< \brief Solver used to analyse the covariance matrix */
public:
/*! \brief Default constructor */
MULTIARCH inline NormalCovarianceCurvature() : Base() {}
public:
/**************************************************************************/
/* Initialization */
/**************************************************************************/
/*! \copydoc Concept::FittingProcedureConcept::init() */
MULTIARCH inline void init (const VectorType& _evalPos);
/**************************************************************************/
/* Processing */
/**************************************************************************/
/*! \copydoc Concept::FittingProcedureConcept::addNeighbor() */
MULTIARCH inline bool addNeighbor(const DataPoint &_nei);
/*! \copydoc Concept::FittingProcedureConcept::finalize() */
MULTIARCH inline FIT_RESULT finalize();
};
/*!
* \brief Extension to compute curvature values based on a covariance analysis
* of normal vectors of neighbors projected onto the tangent plane.
*
* A 2D covariance matrix is computed from the projections of normals of the
* neighbors and the two principal curvature values and directions are given by
* the eigenvalues and associated eigenvectors of the covariance matrix
* \cite Berkmann:1994:CSG.
*
* \note This procedure requires two passes, the first one for plane fitting,
* the second one for covariance analysis.
* \warning This class is valid only in 3D.
*/
template < class DataPoint, class _WFunctor, typename T>
class ProjectedNormalCovarianceCurvature : public BaseCurvatureEstimator<DataPoint,_WFunctor,T>
{
private:
typedef BaseCurvatureEstimator<DataPoint,_WFunctor,T> Base;
//TODO(thib) check the curvature values that might be wrong
//TODO(thib) use weighting function
protected:
enum
{
Check = Base::PROVIDES_PLANE
};
enum PASS : int
{
FIRST_PASS = 0,
SECOND_PASS,
PASS_COUNT
};
public:
typedef typename Base::Scalar Scalar; /*!< \brief Inherited scalar type*/
typedef typename Base::VectorType VectorType; /*!< \brief Inherited vector type*/
typedef typename DataPoint::MatrixType MatrixType; /*!< \brief Matrix type inherited from DataPoint*/
//TODO(thib) use of Eigen::RowAtCompileTime-1 ?
typedef Eigen::Matrix<Scalar,2,2> Mat22;
typedef Eigen::Matrix<Scalar,3,2> Mat32;
typedef Eigen::Matrix<Scalar,2,1> Vector2;
typedef typename VectorType::Index Index;
/*! \brief Solver used to analyse the covariance matrix*/
typedef Eigen::SelfAdjointEigenSolver<Mat22> Solver;
protected:
Vector2 m_cog; /*!< \brief Gravity center */
Mat22 m_cov; /*!< \brief Covariance matrix */
Solver m_solver; /*!< \brief Solver used to analyse the covariance matrix */
PASS m_pass; /*!< \brief Current pass */
Mat32 m_tframe; /*!< \brief Tangent frame */
public:
/**************************************************************************/
/* Initialization */
/**************************************************************************/
/*! \copydoc Concept::FittingProcedureConcept::init() */
MULTIARCH inline void init (const VectorType& _evalPos);
/**************************************************************************/
/* Processing */
/**************************************************************************/
/*! \copydoc Concept::FittingProcedureConcept::addNeighbor() */
MULTIARCH inline bool addNeighbor(const DataPoint &_nei);
/*! \copydoc Concept::FittingProcedureConcept::finalize() */
MULTIARCH inline FIT_RESULT finalize();
};
#include "curvatureEstimation.hpp"
} //namespace Grenaille
#endif

template < class DataPoint, class _WFunctor, typename T>
void
BaseCurvatureEstimator<DataPoint, _WFunctor, T>::init(const VectorType& _evalPos)
{
Base::init(_evalPos);
m_k1 = 0;
m_k2 = 0;
m_v1 = VectorType::Zero();
m_v2 = VectorType::Zero();
}
template < class DataPoint, class _WFunctor, typename T>
void
NormalCovarianceCurvature<DataPoint, _WFunctor, T>::init(const VectorType& _evalPos)
{
Base::init(_evalPos);
m_cov = MatrixType::Zero();
m_cog = VectorType::Zero();
}
template < class DataPoint, class _WFunctor, typename T>
bool
NormalCovarianceCurvature<DataPoint, _WFunctor, T>::addNeighbor(const DataPoint& _nei)
{
bool bResult = Base::addNeighbor(_nei);
if(bResult)
{
m_cov += _nei.normal() * _nei.normal().transpose();
m_cog += _nei.normal();
}
return bResult;
}
template < class DataPoint, class _WFunctor, typename T>
FIT_RESULT
NormalCovarianceCurvature<DataPoint, _WFunctor, T>::finalize ()
{
typedef typename VectorType::Index Index;
MULTIARCH_STD_MATH(abs);
FIT_RESULT res = Base::finalize();
if(this->isReady())
{
// center of gravity (mean)
m_cog /= Base::m_nbNeighbors;
// covariance matrix
m_cov /= Base::m_nbNeighbors;
m_cov -= m_cog*m_cog.transpose();
m_solver.computeDirect(m_cov);
Base::m_k1 = m_solver.eigenvalues()(1);
Base::m_k2 = m_solver.eigenvalues()(2);
Base::m_v1 = m_solver.eigenvectors().col(1);
Base::m_v2 = m_solver.eigenvectors().col(2);
//TODO(thib) which epsilon value should be chosen ?
// Scalar epsilon = Eigen::NumTraits<Scalar>::dummy_precision();
Scalar epsilon = Scalar(1e-3);
if(Base::m_k1<epsilon && Base::m_k2<epsilon)
{
Base::m_k1 = Scalar(0);
Base::m_k2 = Scalar(0);
// set principal directions from normals center of gravity
VectorType n = m_cog.normalized();
Index i0 = -1, i1 = -1, i2 = -1;
n.rowwise().squaredNorm().minCoeff(&i0);
i1 = (i0+1)%3;
i2 = (i0+2)%3;
Base::m_v1[i0] = 0;
Base::m_v1[i1] = n[i2];
Base::m_v1[i2] = -n[i1];
Base::m_v2[i0] = n[i1]*n[i1] + n[i2]*n[i2];
Base::m_v2[i1] = -n[i1]*n[i0];
Base::m_v2[i2] = -n[i2]*n[i0];
}
}
return res;
}
template < class DataPoint, class _WFunctor, typename T>
void
ProjectedNormalCovarianceCurvature<DataPoint, _WFunctor, T>::init(const VectorType& _evalPos)
{
Base::init(_evalPos);
m_cog = Vector2::Zero();
m_cov = Mat22::Zero();
m_pass = FIRST_PASS;
m_tframe = Mat32::Zero();
}
template < class DataPoint, class _WFunctor, typename T>
bool
ProjectedNormalCovarianceCurvature<DataPoint, _WFunctor, T>::addNeighbor(const DataPoint& _nei)
{
if(m_pass == FIRST_PASS)
{
return Base::addNeighbor(_nei);
}
else if(m_pass == SECOND_PASS)
{
VectorType q = _nei.pos() - Base::m_evalPos;
if(Base::m_w.w(q, _nei)>0)
{
// project normal on plane
VectorType n = _nei.normal();
Vector2 proj = m_tframe.transpose() * n;
m_cov += proj * proj.transpose();
m_cog += proj;
return true;
}
}
return false;
}
template < class DataPoint, class _WFunctor, typename T>
FIT_RESULT
ProjectedNormalCovarianceCurvature<DataPoint, _WFunctor, T>::finalize ()
{
if(m_pass == FIRST_PASS)
{
FIT_RESULT res = Base::finalize();
if(res != UNDEFINED)
{
// get normal of fitted plane
VectorType n = this->primitiveGradient(VectorType());
// compute orthonormal frame of the tangent plane
Index i0 = -1, i1 = -1, i2 = -1;
n.rowwise().squaredNorm().minCoeff(&i0);
i1 = (i0+1)%3;
i2 = (i0+2)%3;
m_tframe.col(0)[i0] = 0;
m_tframe.col(0)[i1] = n[i2];
m_tframe.col(0)[i2] = -n[i1];
m_tframe.col(1)[i0] = n[i1]*n[i1] + n[i2]*n[i2];
m_tframe.col(1)[i1] = -n[i1]*n[i0];
m_tframe.col(1)[i2] = -n[i2]*n[i0];
// go to second pass
m_pass = SECOND_PASS;
return NEED_OTHER_PASS;
}
else
{
return UNDEFINED;
}
}
else if(m_pass == SECOND_PASS)
{
// center of gravity (mean)
m_cog /= Base::m_nbNeighbors;
// covariance matrix
m_cov /= Base::m_nbNeighbors;
m_cov -= m_cog*m_cog.transpose();
m_solver.computeDirect(m_cov);
Base::m_k1 = m_solver.eigenvalues()(0);
Base::m_k2 = m_solver.eigenvalues()(1);
// transform from local plane coordinates to world coordinates
Base::m_v1 = m_tframe * m_solver.eigenvectors().col(0);
Base::m_v2 = m_tframe * m_solver.eigenvectors().col(1);
//TODO(thib) which epsilon value should be chosen ?
// Scalar epsilon = Eigen::NumTraits<Scalar>::dummy_precision();
Scalar epsilon = Scalar(1e-3);
if(Base::m_k1<epsilon && Base::m_k2<epsilon)
{
Base::m_k1 = Scalar(0);
Base::m_k2 = Scalar(0);
// set principal directions from fitted plane
Base::m_v2 = m_tframe.col(0);
Base::m_v1 = m_tframe.col(1);
}
return STABLE;
}
return UNDEFINED;
}
/*
This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
#ifndef _GRENAILLE_MLS_SPHERE_FIT_DER_
#define _GRENAILLE_MLS_SPHERE_FIT_DER_
namespace Grenaille
{
/*!
* \brief Extension performing derivation of the mls surface
* \inherit Concept::FittingExtensionConcept
*
* The differentiation is determined by a previous basket elements that must
* provides first order derivatives of the algebraic sphere parameters.
*
*/
template < class DataPoint, class _WFunctor, typename T>
class MlsSphereFitDer : public T
{
private:
typedef T Base;
protected:
enum
{
Check = Base::PROVIDES_ALGEBRAIC_SPHERE_DERIVATIVE,
PROVIDES_NORMAL_SPACE_DERIVATIVE
};
enum
{
Dim = DataPoint::Dim, //!< Dimension of the ambient space
DerDim = Base::derDimension() //!< Number of dimensions used for the differentiation
};
public:
typedef typename Base::Scalar Scalar; /*!< \brief Inherited scalar type*/
typedef typename Base::VectorType VectorType; /*!< \brief Inherited vector type*/
typedef typename Base::VectorArray VectorArray; /*!< \brief Inherited vector array type */
typedef typename Base::ScalarArray ScalarArray; /*!< \brief Inherited scalar array type */
/*!
\brief Static squared matrix of scalars with a size adapted to the
differentiation type.
The size is adapted to the differentiation type (scale and/or space)
only. Such Matrix is typically used to represent Hessian matrix of
multivariate single-valued functions such as \f$ u_c \f$ and \f$ u_q \f$
of an AlgebraicSphere obtained from a fitting procedure.
*/
typedef Eigen::Matrix< Scalar, DerDim, DerDim > Matrix;
/*!
\brief Static matrix of scalars with a size adapted to the
differentiation type and the dimension of the ambient space.
The size is adapted to the differentiation type (scale and/or space) and
the dimension of the ambient space. Such Matrix is typically used to
represent Hessian matrices of multivariate multi-valued functions such
as \f$ u_l \f$ of an AlgebraicSphere obtained from a fitting procedure.
A MatrixArray is an array of Matrix (that mimics a 3D matrix) where the
number of Matrix is equal to the dimension of the ambient space.
The i-th Matrix can be accesed by the following block operation:
\code
MatrixArray a;
int i;
//...
Matrix m_i = a.template block<DerDim, DerDim>(0, i*DerDim);
\endcode
*/
typedef Eigen::Matrix< Scalar, DerDim, Dim*DerDim > MatrixArray;
protected:
// computation data
Matrix m_d2SumDotPN, /*!< \brief Sum of the dot product betwen relative positions and normals with twice differenciated weights */
m_d2SumDotPP, /*!< \brief Sum of the squared relative positions with twice differenciated weights */
m_d2SumW; /*!< \brief Sum of queries weight with twice differenciated weights */
MatrixArray m_d2SumP, /*!< \brief Sum of relative positions with twice differenciated weights */
m_d2SumN; /*!< \brief Sum of normal vectors with twice differenciated weights */
public:
// results
Matrix m_d2Uc, /*!< \brief Second derivative of the hyper-sphere constant term */
m_d2Uq; /*!< \brief Second derivative of the hyper-sphere quadratic term */
MatrixArray m_d2Ul; /*!< \brief Second derivative of the hyper-sphere linear term */
public:
/************************************************************************/
/* Initialization */
/************************************************************************/
/*! \see Concept::FittingProcedureConcept::init() */
MULTIARCH void init(const VectorType &evalPos);
/************************************************************************/
/* Processing */
/************************************************************************/
/*! \see Concept::FittingProcedureConcept::addNeighbor() */
MULTIARCH bool addNeighbor(const DataPoint &nei);
/*! \see Concept::FittingProcedureConcept::finalize() */
MULTIARCH FIT_RESULT finalize();
/**************************************************************************/
/* Use results */
/**************************************************************************/
/*! \brief Returns the derivatives of the scalar field at the evaluation point */
MULTIARCH inline ScalarArray dPotential() const;
/*! \brief Value of the normal of the primitive at the evaluation point */
MULTIARCH inline VectorType normal() const;
/*! \brief Returns the second derivatives of the scalar field at the evaluation point */
MULTIARCH inline VectorArray dNormal() const;
}; //class MlsSphereFitDer
#include "mlsSphereFitDer.hpp"
} //namespace Grenaille
#endif
This diff is collapsed.
/*
This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.
file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
......@@ -27,16 +27,16 @@ class OrientedSphereFit : public AlgebraicSphere<DataPoint, _WFunctor>
{
private:
typedef AlgebraicSphere<DataPoint, _WFunctor> Base;
typedef AlgebraicSphere<DataPoint, _WFunctor> Base;
public:
/*! \brief Scalar type inherited from DataPoint*/
typedef typename Base::Scalar Scalar;
typedef typename Base::Scalar Scalar;
/*! \brief Vector type inherited from DataPoint*/
typedef typename Base::VectorType VectorType;
/*! \brief Weight Function*/
typedef _WFunctor WFunctor;
typedef _WFunctor WFunctor;
protected:
......@@ -45,7 +45,9 @@ public:
m_sumP; /*!< \brief Sum of the relative positions */
Scalar m_sumDotPN, /*!< \brief Sum of the dot product betwen relative positions and normals */
m_sumDotPP, /*!< \brief Sum of the squared relative positions */
m_sumW; /*!< \brief Sum of queries weight */
m_sumW, /*!< \brief Sum of queries weight */
m_nume, /*!< \brief Numerator of the quadratic parameter (excluding the 0.5 coefficient) */
m_deno; /*!< \brief Denominator of the quadratic parameter (excluding the 0.5 coefficient) */
WFunctor m_w; /*!< \brief Weight function (must inherits BaseWeightFunc) */
......@@ -73,43 +75,43 @@ public:
/*! \copydoc Concept::FittingProcedureConcept::finalize() */
MULTIARCH inline FIT_RESULT finalize();
/**************************************************************************/
/* Results */
/**************************************************************************/
using Base::potential;
/*! \brief Value of the scalar field at the evaluation point */
MULTIARCH inline Scalar potential() const { return Base::m_uc; }
/*! \brief Value of the normal of the primitive at the evaluation point */
MULTIARCH inline VectorType normal() const { return Base::m_ul.normalized(); }
}; //class OrientedSphereFit
namespace internal
{
/*!
\brief Internal generic class performing the Fit derivation
/*!
\brief Internal generic class performing the Fit derivation
\inherit Concept::FittingExtensionConcept
The differentiation can be done automatically in scale and/or space, by
combining the enum values FitScaleDer and FitSpaceDer in the template
combining the enum values FitScaleDer and FitSpaceDer in the template
parameter Type.
The differenciated values are stored in static arrays. The size of the
arrays is computed with respect to the derivation type (scale and/or space)
and the number of the dimension of the ambiant space.
By convention, the scale derivatives are stored at index 0 when Type
and the number of the dimension of the ambiant space.
By convention, the scale derivatives are stored at index 0 when Type
contains at least FitScaleDer. The size of these arrays can be known using
derDimension(), and the differentiation type by isScaleDer() and
derDimension(), and the differentiation type by isScaleDer() and
isSpaceDer().
*/
template < class DataPoint, class _WFunctor, typename T, int Type>
template < class DataPoint, class _WFunctor, typename T, int _Type>
class OrientedSphereDer : public T
{
private:
......@@ -132,23 +134,26 @@ public:
#define GLS_DER_STORAGE_ORDER(TYPE) ((TYPE & FitSpaceDer) ? Eigen::RowMajor : Eigen::ColMajor )
/*! \brief Static array of scalars with a size adapted to the differentiation type */
typedef Eigen::Matrix < Scalar,
DataPoint::Dim,
GLS_DER_NB_DERIVATIVES(Type,DataPoint::Dim),
GLS_DER_STORAGE_ORDER(Type) > VectorArray;
typedef Eigen::Matrix < Scalar,
DataPoint::Dim,
GLS_DER_NB_DERIVATIVES(_Type,DataPoint::Dim),
GLS_DER_STORAGE_ORDER(_Type) > VectorArray;
/*! \brief Static array of scalars with a size adapted to the differentiation type */
typedef Eigen::Matrix < Scalar,
1,
GLS_DER_NB_DERIVATIVES(Type,DataPoint::Dim)/*,
typedef Eigen::Matrix < Scalar,
1,
GLS_DER_NB_DERIVATIVES(_Type,DataPoint::Dim)/*,
GLS_DER_STORAGE_ORDER(Type)*/ > ScalarArray;
private:
protected:
// computation data
VectorArray m_dSumN, /*!< \brief Sum of the normal vectors with differenciated weights */
m_dSumP; /*!< \brief Sum of the relative positions with differenciated weights*/
ScalarArray m_dSumDotPN, /*!< \brief Sum of the dot product betwen relative positions and normals with differenciated weights */
m_dSumDotPP, /*!< \brief Sum of the squared relative positions with differenciated weights */
m_dSumW; /*!< \brief Sum of queries weight with differenciated weights */
m_dSumW, /*!< \brief Sum of queries weight with differenciated weights */
m_dNume, /*!< \brief Differenciation of the numerator of the quadratic parameter */
m_dDeno; /*!< \brief Differenciation of the denominator of the quadratic parameter */
public:
// results
......@@ -174,17 +179,17 @@ public:
/**************************************************************************/
/* Use results */
/**************************************************************************/
/*! \brief Returns the derivatives of the scalar field at the evaluation point */
MULTIARCH inline ScalarArray dPotential() const;
/*! \brief Returns the derivatives of the primitive normal */
MULTIARCH inline VectorArray dNormal() const;
MULTIARCH inline VectorArray dNormal() const;
/*! \brief compute the square of the Pratt norm derivative */
MULTIARCH inline ScalarArray dprattNorm2() const
{
return Scalar(2.) * Base::m_ul.transpose() * m_dUl
return Scalar(2.) * Base::m_ul.transpose() * m_dUl
- Scalar(4.) * Base::m_uq * m_dUc
- Scalar(4.) * Base::m_uc * m_dUq;
}
......@@ -192,13 +197,13 @@ public:
/*! \brief compute the square of the Pratt norm derivative for dimension _d */
MULTIARCH inline Scalar dprattNorm2(unsigned int _d) const
{
return Scalar(2.) * m_dUl.col(_d).dot(Base::m_ul)
return Scalar(2.) * m_dUl.col(_d).dot(Base::m_ul)
- Scalar(4.) * m_dUc.col(_d)[0]*Base::m_uq
- Scalar(4.) * m_dUq.col(_d)[0]*Base::m_uc;
}
/*! \brief compute the Pratt norm derivative for the dimension _d */
MULTIARCH inline Scalar dprattNorm(unsigned int _d) const
MULTIARCH inline Scalar dprattNorm(unsigned int _d) const
{
MULTIARCH_STD_MATH(sqrt);
return sqrt(dprattNorm2(_d));
......@@ -212,12 +217,11 @@ public:
}
/*! \brief State specified at compilation time to differenciate the fit in scale */
MULTIARCH inline bool isScaleDer() const {return bool(Type & FitScaleDer);}
static constexpr MULTIARCH inline bool isScaleDer() {return bool(_Type & FitScaleDer);}
/*! \brief State specified at compilation time to differenciate the fit in space */
MULTIARCH inline bool isSpaceDer() const {return bool(Type & FitSpaceDer);}
static constexpr MULTIARCH inline bool isSpaceDer() {return bool(_Type & FitSpaceDer);}
/*! \brief Number of dimensions used for the differentiation */
MULTIARCH inline unsigned int derDimension() const { return GLS_DER_NB_DERIVATIVES(Type,DataPoint::Dim);}
static constexpr MULTIARCH inline unsigned int derDimension() { return GLS_DER_NB_DERIVATIVES(_Type,DataPoint::Dim);}
//! Normalize the scalar field by the Pratt norm
/*!
......@@ -228,15 +232,15 @@ public:
}; //class OrientedSphereFitDer
}// namespace internal
}// namespace internal
/*!
\brief Differentiation in scale of the OrientedSphereFit
\inherit Concept::FittingExtensionConcept
Requierement:
Requierement:
\verbatim PROVIDES_ALGEBRAIC_SPHERE \endverbatim
Provide:
Provide:
\verbatim PROVIDES_ALGEBRAIC_SPHERE_SCALE_DERIVATIVE \endverbatim
*/
template < class DataPoint, class _WFunctor, typename T>
......@@ -257,9 +261,9 @@ protected:
\brief Spatial differentiation of the OrientedSphereFit
\inherit Concept::FittingExtensionConcept
Requierement:
Requierement:
\verbatim PROVIDES_ALGEBRAIC_SPHERE \endverbatim
Provide:
Provide:
\verbatim PROVIDES_ALGEBRAIC_SPHERE_SPACE_DERIVATIVE \endverbatim
*/
template < class DataPoint, class _WFunctor, typename T>
......@@ -280,10 +284,10 @@ protected:
\brief Differentiation both in scale and space of the OrientedSphereFit
\inherit Concept::FittingExtensionConcept
Requierement:
Requierement:
\verbatim PROVIDES_ALGEBRAIC_SPHERE \endverbatim
Provide:
\verbatim PROVIDES_ALGEBRAIC_SPHERE_SCALE_DERIVATIVE
Provide:
\verbatim PROVIDES_ALGEBRAIC_SPHERE_SCALE_DERIVATIVE
PROVIDES_ALGEBRAIC_SPHERE_SPACE_DERIVATIVE
\endverbatim
*/
......
/*
This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.
file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
template < class DataPoint, class _WFunctor, typename T>
void
void
OrientedSphereFit<DataPoint, _WFunctor, T>::init(const VectorType& _evalPos)
{
// Setup primitive
......@@ -19,20 +19,22 @@ OrientedSphereFit<DataPoint, _WFunctor, T>::init(const VectorType& _evalPos)
m_sumDotPN = Scalar(0.0);
m_sumDotPP = Scalar(0.0);
m_sumW = Scalar(0.0);
m_nume = Scalar(0.0);
m_deno = Scalar(0.0);
}
template < class DataPoint, class _WFunctor, typename T>
bool
bool
OrientedSphereFit<DataPoint, _WFunctor, T>::addNeighbor(const DataPoint& _nei)
{
// centered basis
VectorType q = _nei.pos() - Base::basisCenter();
// compute weight
Scalar w = m_w.w(q, _nei);
Scalar w = m_w.w(q, _nei);
if (w > Scalar(0.))
{
{
// increment matrix
m_sumP += q * w;
m_sumN += _nei.normal() * w;
......@@ -74,12 +76,12 @@ OrientedSphereFit<DataPoint, _WFunctor, T>::finalize ()
Scalar invSumW = Scalar(1.)/m_sumW;
Scalar num = (m_sumDotPN - invSumW * m_sumP.dot(m_sumN));
m_nume = (m_sumDotPN - invSumW * m_sumP.dot(m_sumN));
Scalar den1 = invSumW * m_sumP.dot(m_sumP);
Scalar den = m_sumDotPP - den1;
m_deno = m_sumDotPP - den1;
// Deal with degenerate cases
if(abs(den) < epsilon * max(m_sumDotPP, den1))
if(abs(m_deno) < epsilon * max(m_sumDotPP, den1))
{
//plane
Scalar s = Scalar(1.) / Base::m_ul.norm();
......@@ -90,7 +92,7 @@ OrientedSphereFit<DataPoint, _WFunctor, T>::finalize ()
else
{
//Generic case
Base::m_uq = Scalar(.5) * num / den;
Base::m_uq = Scalar(.5) * m_nume / m_deno;
Base::m_ul = (m_sumN - m_sumP * (Scalar(2.) * Base::m_uq)) * invSumW;
Base::m_uc = -invSumW * (Base::m_ul.dot(m_sumP) + m_sumDotPP * Base::m_uq);
}
......@@ -114,7 +116,7 @@ namespace internal
{
template < class DataPoint, class _WFunctor, typename T, int Type>
void
void
OrientedSphereDer<DataPoint, _WFunctor, T, Type>::init(const VectorType& _evalPos)
{
Base::init(_evalPos);
......@@ -125,6 +127,8 @@ OrientedSphereDer<DataPoint, _WFunctor, T, Type>::init(const VectorType& _evalPo
m_dSumDotPN = ScalarArray::Zero();
m_dSumDotPP = ScalarArray::Zero();
m_dSumW = ScalarArray::Zero();
m_dNume = ScalarArray::Zero();
m_dDeno = ScalarArray::Zero();
m_dUc = ScalarArray::Zero();
m_dUq = ScalarArray::Zero();
......@@ -133,7 +137,7 @@ OrientedSphereDer<DataPoint, _WFunctor, T, Type>::init(const VectorType& _evalPo
template < class DataPoint, class _WFunctor, typename T, int Type>
bool
bool
OrientedSphereDer<DataPoint, _WFunctor, T, Type>::addNeighbor(const DataPoint &_nei)
{
bool bResult = Base::addNeighbor(_nei);
......@@ -144,13 +148,13 @@ OrientedSphereDer<DataPoint, _WFunctor, T, Type>::addNeighbor(const DataPoint &
// centered basis
VectorType q = _nei.pos() - Base::basisCenter();
// compute weight derivatives
if (Type & FitScaleDer)
dw[0] = Base::m_w.scaledw(q, _nei);
if (Type & FitSpaceDer)
dw.template tail<int(DataPoint::Dim)>() = -Base::m_w.spacedw(q, _nei).transpose();
dw.template tail<int(DataPoint::Dim)>() = Base::m_w.spacedw(q, _nei).transpose();
// increment
m_dSumW += dw;
......@@ -159,6 +163,15 @@ OrientedSphereDer<DataPoint, _WFunctor, T, Type>::addNeighbor(const DataPoint &
m_dSumDotPN += dw * _nei.normal().dot(q);
m_dSumDotPP += dw * q.squaredNorm();
if (isSpaceDer())
{
Scalar w = Base::m_w.w(q, _nei);
m_dSumDotPN.template tail<DataPoint::Dim>() -= w * _nei.normal();
m_dSumDotPP.template tail<DataPoint::Dim>() -= Scalar(2.) * w * q;
m_dSumP.template rightCols<DataPoint::Dim>().diagonal().array() -= w;
}
return true;
}
......@@ -167,7 +180,7 @@ OrientedSphereDer<DataPoint, _WFunctor, T, Type>::addNeighbor(const DataPoint &
template < class DataPoint, class _WFunctor, typename T, int Type>
FIT_RESULT
FIT_RESULT
OrientedSphereDer<DataPoint, _WFunctor, T, Type>::finalize()
{
MULTIARCH_STD_MATH(sqrt);
......@@ -180,26 +193,26 @@ OrientedSphereDer<DataPoint, _WFunctor, T, Type>::finalize()
Scalar nume = Base::m_sumDotPN - invSumW*Base::m_sumP.dot(Base::m_sumN);
Scalar deno = Base::m_sumDotPP - invSumW*Base::m_sumP.dot(Base::m_sumP);
// FIXME, the following product "Base::m_sumN.transpose() * m_dSumP" is prone to numerical cancellation
// issues for spacial derivatives because, (d sum w_i P_i)/(d x) is supposed to be tangent to the surface whereas
// "sum w_i N_i" is normal to the surface...
ScalarArray dNume = m_dSumDotPN
m_dNume = m_dSumDotPN
- invSumW*invSumW * ( Base::m_sumW * ( Base::m_sumN.transpose() * m_dSumP + Base::m_sumP.transpose() * m_dSumN )
- m_dSumW*Base::m_sumP.dot(Base::m_sumN) );
ScalarArray dDeno = m_dSumDotPP
m_dDeno = m_dSumDotPP
- invSumW*invSumW*( Scalar(2.) * Base::m_sumW * Base::m_sumP.transpose() * m_dSumP
- m_dSumW*Base::m_sumP.dot(Base::m_sumP) );
m_dUq = Scalar(.5) * (deno * dNume - dDeno * nume)/(deno*deno);
m_dUq = Scalar(.5) * (deno * m_dNume - m_dDeno * nume)/(deno*deno);