...
 
Commits (4)
/*
Copyright (C) 2018 Nicolas Mellado <nmellado0@gmail.com>
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_MEAN_PLANE_FIT_
#define _GRENAILLE_MEAN_PLANE_FIT_
#include "enums.h"
namespace Grenaille
{
/*!
\brief Plane fitting procedure computing the mean position and orientation
from oriented points
\inherit Concept::FittingProcedureConcept
\see CompactPlane
\todo Add derivatives
*/
template < class DataPoint, class _WFunctor, typename T >
class MeanPlaneFit : public CompactPlane<DataPoint, _WFunctor>
{
private:
typedef CompactPlane<DataPoint, _WFunctor> Base;
protected:
enum
{
Check = Base::PROVIDES_PLANE
};
public:
/*! \brief Scalar type inherited from DataPoint*/
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;
protected:
// computation data
Scalar m_sumW; /*!< \brief Sum of queries weight.*/
VectorType m_sumN, /*!< \brief Sum of the normal vectors */
m_sumP, /*!< \brief Sum of the relative positions */
m_evalPos; /*!< \brief Center of the evaluation basis */
WFunctor m_w; /*!< \brief Weight function (must inherits BaseWeightFunc) */
public:
/*! \brief Default constructor */
MULTIARCH inline MeanPlaneFit() : Base() {}
/**************************************************************************/
/* Initialization */
/**************************************************************************/
/*! \copydoc Concept::FittingProcedureConcept::setWeightFunc() */
MULTIARCH inline void setWeightFunc (const WFunctor& _w) { m_w = _w; }
/*! \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();
}; //class MeanPlaneFit
#include "meanPlaneFit.hpp"
} //namespace Grenaille
#endif
/*
Copyright (C) 2018 Nicolas Mellado <nmellado0@gmail.com>
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/.
*/
template < class DataPoint, class _WFunctor, typename T>
void
MeanPlaneFit<DataPoint, _WFunctor, T>::init(const VectorType& _evalPos)
{
// Setup primitive
Base::resetPrimitive();
m_evalPos = _evalPos;
// Setup fitting internal values
m_sumP = VectorType::Zero();
m_sumN = VectorType::Zero();
m_sumW = Scalar(0.0);
}
template < class DataPoint, class _WFunctor, typename T>
bool
MeanPlaneFit<DataPoint, _WFunctor, T>::addNeighbor(const DataPoint& _nei)
{
VectorType q = _nei.pos() - m_evalPos;
// compute weight
Scalar w = m_w.w(q, _nei);
if (w > Scalar(0.))
{
m_sumP += q * w;
m_sumN += _nei.normal() * w;
m_sumW += w;
++(Base::m_nbNeighbors);
return true;
}
return false;
}
template < class DataPoint, class _WFunctor, typename T>
FIT_RESULT
MeanPlaneFit<DataPoint, _WFunctor, T>::finalize ()
{
// handle specific configurations
// With less than 3 neighbors the fitting is undefined
if(m_sumW == Scalar(0.) || Base::m_nbNeighbors == 0)
{
Base::resetPrimitive();
Base::m_eCurrentState = UNDEFINED;
return Base::m_eCurrentState;
}
Base::setPlane(m_sumN / m_sumW, m_sumP / m_sumW + m_evalPos);
return Base::m_eCurrentState = STABLE;
}
......@@ -11,6 +11,7 @@
#define _GRENAILLE_PLANE_
#include "primitive.h" // PrimitiveBase
#include <Eigen/Geometry>
namespace Grenaille
{
......@@ -24,28 +25,27 @@ namespace Grenaille
\f$ s_\mathbf{u}(\mathbf{x}) =
\left[ \mathbf{x}^T \; 1 \;\right]^T \cdot \mathbf{p} \f$.
This class uses a compact storage of n+1 scalars in n-dimensionnal space. It
can be sensitive to the data scale, leading to potential instabilities
due to round errors at large scales.
\todo Add standard plane storing 2n scalars (direction and center).
This class inherits Eigen::Hyperplane.
This primitive requires the definition of n-dimensionnal vectors
(VectorType) and homogeneous n-dimensionnal vectors (HVectorType) in
Concept::PointConcept.
(VectorType) in Concept::PointConcept.
This primitive provides:
\verbatim PROVIDES_PLANE \endverbatim
\note The first n-components of the plane must define a normalized vector
*/
template < class DataPoint, class _WFunctor, typename T = void >
class CompactPlane : public PrimitiveBase<DataPoint, _WFunctor>
class CompactPlane : public PrimitiveBase<DataPoint, _WFunctor>,
public Eigen::Hyperplane<typename DataPoint::Scalar, DataPoint::Dim >
{
private:
typedef PrimitiveBase<DataPoint, _WFunctor> Base;
using Base = PrimitiveBase<DataPoint, _WFunctor>;
public:
/// \brief Specialization of Eigen::Hyperplane inherited by Grenaille::CompactPlane
using EigenBase = Eigen::Hyperplane<typename DataPoint::Scalar, DataPoint::Dim >;
protected:
......@@ -62,21 +62,14 @@ public:
typedef typename DataPoint::VectorType VectorType;
/*! \brief Matrix type inherited from DataPoint */
typedef typename DataPoint::MatrixType MatrixType;
/*! \brief Homogeneous vector type inherited from DataPoint */
typedef typename DataPoint::HVectorType HVectorType;
/*! \brief Weight Function */
typedef _WFunctor WFunctor;
// results
public:
HVectorType m_p; /*!< \brief Homogeneous plane representation */
public:
/*! \brief Default constructor */
MULTIARCH inline CompactPlane()
: Base()
: Base(), EigenBase()
{
resetPrimitive();
}
......@@ -86,11 +79,12 @@ public:
MULTIARCH inline void resetPrimitive()
{
Base::resetPrimitive();
m_p = HVectorType::Zero();
EigenBase* cc = static_cast<EigenBase*>(this);
*cc = EigenBase();
}
MULTIARCH inline bool operator==(const CompactPlane<DataPoint, WFunctor, T>& other) const{
return m_p == other.m_p;
return EigenBase::isApprox(other);
}
/*! \brief Comparison operator, convenience function */
......@@ -99,36 +93,35 @@ public:
}
/* \brief Init the plane from a direction and a position
\param _dir Orientation of the plane
\param _dir Orientation of the plane, does not need to be normalized
\param _pos Position of the plane
*/
MULTIARCH inline void setPlane (const VectorType& _dir,
const VectorType& _pos)
{
m_p.template head<DataPoint::Dim>() = _dir.normalized();
m_p.template tail<1>()<< -_pos.dot(m_p.template head<DataPoint::Dim>());
EigenBase* cc = static_cast<EigenBase*>(this);
*cc = EigenBase(_dir.normalized(), _pos);
}
//! \brief Value of the scalar field at the location \f$ \mathbf{q} \f$
MULTIARCH inline Scalar potential (const VectorType& _q) const
{
// Project on the normal vector and add the offset value
return m_p.template head<DataPoint::Dim>().dot(_q) +
*( m_p.template tail<1>().data() );
// The potential is the distance from the point to the plane
return EigenBase::signedDistance(_q);
}
//! \brief Project a point on the plane
MULTIARCH inline VectorType project (const VectorType& _q) const
{
// The potential is the distance from the point to the plane
return _q - potential(_q) * m_p.template head<DataPoint::Dim>();
// Project on the normal vector and add the offset value
return EigenBase::projection(_q);
}
//! \brief Scalar field gradient direction at \f$ \mathbf{q}\f$
MULTIARCH inline VectorType primitiveGradient (const VectorType&) const
{
// Uniform gradient defined only by the orientation of the plane
return m_p.template head<DataPoint::Dim>();
return EigenBase::normal();
}
......
......@@ -31,7 +31,13 @@ SphereFit<DataPoint, _WFunctor, T>::addNeighbor(const DataPoint& _nei)
if (w > Scalar(0.))
{
VectorA a;
#ifdef __CUDACC__
a(0) = 1;
a.template segment<DataPoint::Dim>(1) = q;
a(DataPoint::Dim+1) = q.squaredNorm();
#else
a << 1, q, q.squaredNorm();
#endif
m_matA += w * a * a.transpose();
m_sumW += w;
......
......@@ -22,6 +22,7 @@
#include "Grenaille/Core/weightFunc.h"
#include "Grenaille/Core/plane.h"
#include "Grenaille/Core/meanPlaneFit.h"
#include "Grenaille/Core/covariancePlaneFit.h"
#include "Grenaille/Core/sphereFit.h"
......
......@@ -79,9 +79,9 @@ if(CMAKE_COMPILER_IS_GNUCXX)
if(CMAKE_SYSTEM_NAME MATCHES Linux)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${COVERAGE_FLAGS} -g2")
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} ${COVERAGE_FLAGS} -O2 -g2")
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} ${COVERAGE_FLAGS} -O2 -g2 -DEBUG")
set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${COVERAGE_FLAGS} -fno-inline-functions")
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${COVERAGE_FLAGS} -O0 -g3")
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${COVERAGE_FLAGS} -O0 -g3 -DEBUG")
endif(CMAKE_SYSTEM_NAME MATCHES Linux)
elseif(MSVC)
......
......@@ -31,7 +31,6 @@ public:
enum {Dim = 3};
typedef double Scalar;
typedef Eigen::Matrix<Scalar, Dim, 1> VectorType;
typedef Eigen::Matrix<Scalar, Dim+1, 1> HVectorType;
typedef Eigen::Matrix<Scalar, Dim, Dim> MatrixType;
MULTIARCH inline MyPoint( const VectorType& _pos = VectorType::Zero(),
......
......@@ -22,7 +22,21 @@
using namespace std;
using namespace Grenaille;
template<typename DataPoint, typename Fit, typename WeightFunc> //, typename Fit, typename WeightFunction>
template <bool check>
struct CheckSurfaceVariation {
template <typename Fit, typename Scalar>
static inline void run(const Fit& fit, Scalar epsilon){
VERIFY(fit.surfaceVariation() < epsilon);
}
};
template <>
template <typename Fit, typename Scalar>
void
CheckSurfaceVariation<false>::run(const Fit& /*fit*/, Scalar /*epsilon*/){ }
template<typename DataPoint, typename Fit, typename WeightFunc, bool _cSurfVar> //, typename Fit, typename WeightFunction>
void testFunction(bool _bUnoriented = false, bool _bAddPositionNoise = false, bool _bAddNormalNoise = false)
{
// Define related structure
......@@ -69,13 +83,15 @@ void testFunction(bool _bUnoriented = false, bool _bAddPositionNoise = false, bo
_bUnoriented);
}
epsilon = testEpsilon<Scalar>();
if ( _bAddPositionNoise) // relax a bit the testing threshold
epsilon = Scalar(0.001*MAX_NOISE);
// Test for each point if the fitted plane correspond to the theoretical plane
#ifdef DEBUG
#pragma omp parallel for
#endif
for(int i = 0; i < int(vectorPoints.size()); ++i)
{
epsilon = testEpsilon<Scalar>();
if ( _bAddPositionNoise) // relax a bit the testing threshold
epsilon = Scalar(0.001*MAX_NOISE);
Fit fit;
fit.setWeightFunc(WeightFunc(analysisScale));
......@@ -88,7 +104,7 @@ void testFunction(bool _bUnoriented = false, bool _bAddPositionNoise = false, bo
VERIFY(Scalar(1.) - std::abs(fit.primitiveGradient(vectorPoints[i].pos()).dot(direction)) <= epsilon);
// Check if the surface variation is small
VERIFY(fit.surfaceVariation() < epsilon);
CheckSurfaceVariation<_cSurfVar>::run(fit, _bAddPositionNoise ? epsilon*Scalar(10.): epsilon);
// Check if the query point is on the plane
if(!_bAddPositionNoise)
......@@ -140,20 +156,25 @@ void callSubTests()
typedef Basket<Point, WeightSmoothFunc, CompactPlane, CovariancePlaneFit> CovFitSmooth;
typedef Basket<Point, WeightConstantFunc, CompactPlane, CovariancePlaneFit> CovFitConstant;
typedef Basket<Point, WeightSmoothFunc, CompactPlane, MeanPlaneFit> MeanFitSmooth;
typedef Basket<Point, WeightConstantFunc, CompactPlane, MeanPlaneFit> MeanFitConstant;
cout << "Testing with perfect plane..." << endl;
for(int i = 0; i < g_repeat; ++i)
{
//Test with perfect plane
CALL_SUBTEST(( testFunction<Point, CovFitSmooth, WeightSmoothFunc>() ));
CALL_SUBTEST(( testFunction<Point, CovFitConstant, WeightConstantFunc>() ));
CALL_SUBTEST(( testFunction<Point, CovFitSmooth, WeightSmoothFunc, true>() ));
CALL_SUBTEST(( testFunction<Point, CovFitConstant, WeightConstantFunc, true>() ));
CALL_SUBTEST(( testFunction<Point, MeanFitSmooth, WeightSmoothFunc, false>() ));
CALL_SUBTEST(( testFunction<Point, MeanFitConstant, WeightConstantFunc, false>() ));
}
cout << "Ok!" << endl;
cout << "Testing with noise on position" << endl;
for(int i = 0; i < g_repeat; ++i)
{
CALL_SUBTEST(( testFunction<Point, CovFitSmooth, WeightSmoothFunc>(false, true, true) ));
CALL_SUBTEST(( testFunction<Point, CovFitConstant, WeightConstantFunc>(false, true, true) ));
CALL_SUBTEST(( testFunction<Point, CovFitSmooth, WeightSmoothFunc, true>(false, true, true) ));
CALL_SUBTEST(( testFunction<Point, CovFitConstant, WeightConstantFunc, true>(false, true, true) ));
}
cout << "Ok!" << endl;
}
......