Commit 77dfa975 authored by Nicolas Mellado's avatar Nicolas Mellado

Add MongePatch fit + test

parent 3d7b3a0d
/*
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_MONGE_PATCH_
#define _GRENAILLE_MONGE_PATCH_
#include <iostream>
namespace Grenaille
{
/*!
* \brief Extension to compute the best fit quadric on 3d points expressed as f(u,v)=h
*
* \note This procedure requires at least two passes, the first one for plane fitting,
* the second one for quadric fitting.
* \warning This class is valid only in 3D.
*/
template < class DataPoint, class _WFunctor, typename T>
class MongePatch : public T
{
private:
using Base = T;
protected:
enum
{
Check = Base::PROVIDES_PLANE && Base::PROVIDES_TANGENT_PLANE_BASIS
};
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*/
typedef _WFunctor WFunctor; /*!< \brief Weight Function */
typedef Eigen::Matrix<Scalar,2,1> Vector2;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1> VectorX;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> MatrixX;
protected:
MatrixX m_A; /*!< \brief Quadric input samples */
MatrixX m_x; /*!< \brief Quadric parameters */
VectorX m_b; /*!< \brief Obervations */
int m_neiIdx; /*!< \brief Counter of observations, used in addNeighhor() */
bool m_planeIsReady;
public:
/*! \brief Explicit conversion to MongePatch, to access methods potentially hidden by inheritage */
MULTIARCH inline
MongePatch<DataPoint, WFunctor, T>& mongePatch()
{ return * static_cast<MongePatch<DataPoint, WFunctor, T>*>(this); }
/**************************************************************************/
/* 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 Returns an estimate of the mean curvature
MULTIARCH inline Scalar kMean() const;
//! \brief Returns an estimate of the Gaussian curvature
MULTIARCH inline Scalar GaussianCurvature() const;
MULTIARCH inline Scalar evalUV(Scalar u, Scalar v) const {
return h_uu()*u*u + h_vv()*v*v + h_uv()*u*v + h_u()*u + h_v()*v + h_c();
}
/*! \brief Value of the scalar field at the evaluation point */
MULTIARCH inline Scalar potential(const VectorType& _q) const {
VectorType x = Base::worldToTangentPlane(_q);
return evalUV(*(x.data()+1),*(x.data()+2)) - *(x.data());
}
//! \brief Orthogonal projecting on the patch, such that h = f(u,v)
MULTIARCH inline VectorType project (const VectorType& _q) const
{
VectorType x = Base::worldToTangentPlane(_q);
*(x.data()) = evalUV(*(x.data()+1),*(x.data()+2));
return Base::tangentPlaneToWorld(x);
}
MULTIARCH inline const Scalar & h_uu () const { return *(m_x.data()); }
MULTIARCH inline const Scalar & h_vv () const { return *(m_x.data()+1); }
MULTIARCH inline const Scalar & h_uv () const { return *(m_x.data()+2); }
MULTIARCH inline const Scalar & h_u () const { return *(m_x.data()+3); }
MULTIARCH inline const Scalar & h_v () const { return *(m_x.data()+4); }
MULTIARCH inline const Scalar & h_c () const { return *(m_x.data()+5); }
};
#include "mongePatch.hpp"
} //namespace Grenaille
#endif

#include <Eigen/SVD>
#include <Eigen/Geometry>
template < class DataPoint, class _WFunctor, typename T>
void
MongePatch<DataPoint, _WFunctor, T>::init(const VectorType& _evalPos)
{
Base::init(_evalPos);
m_x.setZero();
m_planeIsReady = false;
}
template < class DataPoint, class _WFunctor, typename T>
bool
MongePatch<DataPoint, _WFunctor, T>::addNeighbor(const DataPoint& _nei)
{
if(! m_planeIsReady)
{
return Base::addNeighbor(_nei);
}
else // base plane is ready, we can now fit the patch
{
VectorType q = _nei.pos() - Base::basisCenter();
Scalar w = Base::m_w.w(q, _nei);
if (w > Scalar(0.))
{
// express neighbor in local coordinate frame
VectorType local = Base::worldToTangentPlane(_nei.pos());
const Scalar& h = *(local.data());
const Scalar& u = *(local.data()+1);
const Scalar& v = *(local.data()+2);
Eigen::Matrix<Scalar, 6, 1 > p;
p << u*u, v*v, u*v, u, v, 1;
m_A += w*p*p.transpose();
m_b += w*h*p;
return true;
}
}
return false;
}
template < class DataPoint, class _WFunctor, typename T>
FIT_RESULT
MongePatch<DataPoint, _WFunctor, T>::finalize ()
{
// end of the fitting process, check plane is ready
if (! m_planeIsReady) {
FIT_RESULT res = Base::finalize();
if(res == STABLE) { // plane is ready
m_planeIsReady = true;
m_A = MatrixX(6,6/*5*/);
m_A.setZero();
m_b = VectorX(6);
m_b.setZero();
m_neiIdx = 0;
return Base::m_eCurrentState = NEED_OTHER_PASS;
}
return res;
}
// end of the monge patch fitting process
else {
// we use BDCSVD as the matrix size is 36
// http://eigen.tuxfamily.org/dox/classEigen_1_1BDCSVD.html
m_x = m_A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(m_b);
return Base::m_eCurrentState = STABLE;
}
}
template < class DataPoint, class _WFunctor, typename T>
typename MongePatch<DataPoint, _WFunctor, T>::Scalar
MongePatch<DataPoint, _WFunctor, T>::kMean() const {
MULTIARCH_STD_MATH(pow);
static const Scalar one (1);
static const Scalar two (2);
static const Scalar threeOverTwo (Scalar(3)/Scalar(2));
return ((one + pow(h_v(),two) ) * h_uu() * two*h_u()*h_v()*h_uv() + (one+pow(h_u(),two))*h_vv()) /
(two * pow(one +pow(h_u(),two) + pow(h_v(),two),threeOverTwo));
}
template < class DataPoint, class _WFunctor, typename T>
typename MongePatch<DataPoint, _WFunctor, T>::Scalar
MongePatch<DataPoint, _WFunctor, T>::GaussianCurvature() const {
MULTIARCH_STD_MATH(pow);
static const Scalar one (1);
static const Scalar two (2);
return (h_uu()*h_vv() - pow(h_uv(),two)) /
pow((one + pow(h_u(),two) + pow(h_v(),two) ), two);
}
......@@ -24,6 +24,7 @@
#include "Grenaille/Core/plane.h"
#include "Grenaille/Core/meanPlaneFit.h"
#include "Grenaille/Core/covariancePlaneFit.h"
#include "Grenaille/Core/mongePatch.h"
#include "Grenaille/Core/sphereFit.h"
#include "Grenaille/Core/orientedSphereFit.h"
......
......@@ -14,4 +14,5 @@ add_multi_test(fit_unoriented.cpp)
add_multi_test(gls_compare.cpp)
add_multi_test(plane_primitive.cpp)
add_multi_test(fit_plane.cpp)
add_multi_test(fit_monge_patch.cpp)
add_multi_test(basket.cpp)
/*
Copyright (C) 2014 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/.
*/
/*!
\file test/Grenaille/fit_monge_patch.cpp
\brief Test validity of monge patch fitting procedure(s)
*/
#include "../common/testing.h"
#include "../common/testUtils.h"
#include <vector>
using namespace std;
using namespace Grenaille;
template<typename DataPoint, typename Fit, typename WeightFunc> //, typename Fit, typename WeightFunction>
void testFunction(bool _bUnoriented = false, bool _bAddPositionNoise = false, bool _bAddNormalNoise = false)
{
// Define related structure
typedef typename DataPoint::Scalar Scalar;
typedef typename DataPoint::VectorType VectorType;
//generate sampled plane
int nbPoints = Eigen::internal::random<int>(100, 1000);
Scalar width = Eigen::internal::random<Scalar>(1., 10.);
Scalar height = width;
Scalar analysisScale = Scalar(15.) * std::sqrt( width * height / nbPoints);
Scalar centerScale = Eigen::internal::random<Scalar>(1,10000);
VectorType center = VectorType::Random() * centerScale;
VectorType direction = VectorType::Random().normalized();
Scalar epsilon = testEpsilon<Scalar>();
vector<DataPoint> vectorPoints(nbPoints);
for(unsigned int i = 0; i < vectorPoints.size(); ++i)
{
vectorPoints[i] = getPointOnPlane<DataPoint>(center,
direction,
width,
_bAddPositionNoise,
_bAddNormalNoise,
_bUnoriented);
}
epsilon = testEpsilon<Scalar>();
if ( _bAddPositionNoise) // relax a bit the testing threshold
epsilon = Scalar(0.01*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)
{
const auto& queryPos = vectorPoints[i].pos();
Fit fit;
fit.setWeightFunc(WeightFunc(analysisScale));
fit.init(queryPos);
fit.compute(vectorPoints.cbegin(), vectorPoints.cend());
if( fit.isStable() ){
// Check if the plane orientation is equal to the generation direction
VERIFY(Scalar(1.) - std::abs(fit.primitiveGradient(queryPos).dot(direction)) <= epsilon);
// Projecting to tangent plane and going back to world should not change the position
VERIFY((fit.tangentPlaneToWorld(fit.worldToTangentPlane(queryPos)) - queryPos).norm() <= epsilon);
// Check if the query point is on the plane
if(!_bAddPositionNoise)
VERIFY(fit.potential(queryPos) <= epsilon);
// check if we well have a plane
VERIFY(fit.kMean() <= epsilon);
VERIFY(fit.GaussianCurvature() <= epsilon);
}
}
}
template<typename Scalar, int Dim>
void callSubTests()
{
typedef PointPositionNormal<Scalar, Dim> Point;
typedef DistWeightFunc<Point, SmoothWeightKernel<Scalar> > WeightSmoothFunc;
typedef DistWeightFunc<Point, ConstantWeightKernel<Scalar> > WeightConstantFunc;
typedef Basket<Point, WeightSmoothFunc, CompactPlane, CovariancePlaneFit, MongePatch> CovFitSmooth;
typedef Basket<Point, WeightConstantFunc, CompactPlane, CovariancePlaneFit, MongePatch> 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>() ));
}
cout << "Ok!" << endl;
cout << "Testing with plane 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) ));
}
cout << "Ok!" << endl;
}
int main(int argc, char** argv)
{
if(!init_testing(argc, argv))
{
return EXIT_FAILURE;
}
cout << "Test Monge Patch fitting for different baskets..." << endl;
callSubTests<float, 3>();
callSubTests<double, 3>();
callSubTests<long double, 3>();
}
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