Commit 9073ac85 authored by Nicolas Mellado's avatar Nicolas Mellado

Add new primitive MeanPlaneFit

parent 15cd0042
/*
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;
}
...@@ -22,6 +22,7 @@ ...@@ -22,6 +22,7 @@
#include "Grenaille/Core/weightFunc.h" #include "Grenaille/Core/weightFunc.h"
#include "Grenaille/Core/plane.h" #include "Grenaille/Core/plane.h"
#include "Grenaille/Core/meanPlaneFit.h"
#include "Grenaille/Core/covariancePlaneFit.h" #include "Grenaille/Core/covariancePlaneFit.h"
#include "Grenaille/Core/sphereFit.h" #include "Grenaille/Core/sphereFit.h"
......
...@@ -22,7 +22,21 @@ ...@@ -22,7 +22,21 @@
using namespace std; using namespace std;
using namespace Grenaille; 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) void testFunction(bool _bUnoriented = false, bool _bAddPositionNoise = false, bool _bAddNormalNoise = false)
{ {
// Define related structure // Define related structure
...@@ -69,13 +83,13 @@ void testFunction(bool _bUnoriented = false, bool _bAddPositionNoise = false, bo ...@@ -69,13 +83,13 @@ void testFunction(bool _bUnoriented = false, bool _bAddPositionNoise = false, bo
_bUnoriented); _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 // Test for each point if the fitted plane correspond to the theoretical plane
#pragma omp parallel for #pragma omp parallel for
for(int i = 0; i < int(vectorPoints.size()); ++i) 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 fit;
fit.setWeightFunc(WeightFunc(analysisScale)); fit.setWeightFunc(WeightFunc(analysisScale));
...@@ -88,7 +102,7 @@ void testFunction(bool _bUnoriented = false, bool _bAddPositionNoise = false, bo ...@@ -88,7 +102,7 @@ void testFunction(bool _bUnoriented = false, bool _bAddPositionNoise = false, bo
VERIFY(Scalar(1.) - std::abs(fit.primitiveGradient(vectorPoints[i].pos()).dot(direction)) <= epsilon); VERIFY(Scalar(1.) - std::abs(fit.primitiveGradient(vectorPoints[i].pos()).dot(direction)) <= epsilon);
// Check if the surface variation is small // 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 // Check if the query point is on the plane
if(!_bAddPositionNoise) if(!_bAddPositionNoise)
...@@ -140,20 +154,25 @@ void callSubTests() ...@@ -140,20 +154,25 @@ void callSubTests()
typedef Basket<Point, WeightSmoothFunc, CompactPlane, CovariancePlaneFit> CovFitSmooth; typedef Basket<Point, WeightSmoothFunc, CompactPlane, CovariancePlaneFit> CovFitSmooth;
typedef Basket<Point, WeightConstantFunc, CompactPlane, CovariancePlaneFit> CovFitConstant; 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; cout << "Testing with perfect plane..." << endl;
for(int i = 0; i < g_repeat; ++i) for(int i = 0; i < g_repeat; ++i)
{ {
//Test with perfect plane //Test with perfect plane
CALL_SUBTEST(( testFunction<Point, CovFitSmooth, WeightSmoothFunc>() )); CALL_SUBTEST(( testFunction<Point, CovFitSmooth, WeightSmoothFunc, true>() ));
CALL_SUBTEST(( testFunction<Point, CovFitConstant, WeightConstantFunc>() )); 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 << "Ok!" << endl;
cout << "Testing with noise on position" << endl; cout << "Testing with noise on position" << endl;
for(int i = 0; i < g_repeat; ++i) for(int i = 0; i < g_repeat; ++i)
{ {
CALL_SUBTEST(( testFunction<Point, CovFitSmooth, WeightSmoothFunc>(false, true, true) )); CALL_SUBTEST(( testFunction<Point, CovFitSmooth, WeightSmoothFunc, true>(false, true, true) ));
CALL_SUBTEST(( testFunction<Point, CovFitConstant, WeightConstantFunc>(false, true, true) )); CALL_SUBTEST(( testFunction<Point, CovFitConstant, WeightConstantFunc, true>(false, true, true) ));
} }
cout << "Ok!" << endl; cout << "Ok!" << endl;
} }
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment