MongeJetFittingNormalVectorEstimator.h

#pragma once

#if defined(MongeJetFittingNormalVectorEstimator_RECURSES)
#error Recursive header files inclusion detected in MongeJetFittingNormalVectorEstimator.h
#else // defined(MongeJetFittingNormalVectorEstimator_RECURSES)

#define MongeJetFittingNormalVectorEstimator_RECURSES

#if !defined MongeJetFittingNormalVectorEstimator_h

#define MongeJetFittingNormalVectorEstimator_h

// Inclusions
#include <iostream>
#include <DGtal/base/Common.h>
#include <DGtal/topology/SCellsFunctors.h>

//CGAL
#include <CGAL/Cartesian.h>
#include <CGAL/Monge_via_jet_fitting.h>
#include <vector>


namespace DGtal
{
  namespace functors
  {
  // template class MongeJetFittingNormalVectorEstimator
  template <typename TSurfel, typename TEmbedder>
  class MongeJetFittingNormalVectorEstimator
  {
  public:

    typedef TSurfel Surfel;
    typedef TEmbedder SCellEmbedder;
    typedef typename SCellEmbedder::RealPoint RealPoint;
    typedef RealPoint Quantity;

    typedef CGAL::Cartesian<double> CGALKernel;
    typedef CGALKernel::Point_3  CGALPoint;
    typedef CGALKernel::Vector_3  CGALVector;
    typedef CGAL::Monge_via_jet_fitting<CGALKernel>  CGALMongeViaJet;
    typedef CGALMongeViaJet::Monge_form CGALMongeForm;

    MongeJetFittingNormalVectorEstimator(ConstAlias<SCellEmbedder> anEmbedder,
                                         const double h, unsigned int d = 4):
      myEmbedder(&anEmbedder), myH(h), myD(d)
    {
      FATAL_ERROR_MSG(d>=2, "Polynomial surface degree must be greater than 2");
    }

    void pushSurfel(const Surfel & aSurf,
                    const double aDistance)
    {
      BOOST_VERIFY(aDistance == aDistance);

      RealPoint p = myEmbedder->operator()(aSurf);
      CGALPoint pp(p[0]*myH,p[1]*myH,p[2]*myH);
      myPoints.push_back(pp);
    }

    Quantity eval( )
    {
      CGALMongeForm monge_form;
      CGALMongeViaJet monge_fit;

      monge_form = monge_fit(myPoints.begin() , myPoints.end(), myD, (2<myD)? myD : 2);

      CGALVector v= monge_form.normal_direction (  );

      return RealPoint(v.x(),v.y(),v.z());
    }

    void reset()
    {
      myPoints.clear();
    }


  private:

    const SCellEmbedder * myEmbedder;

    std::vector<CGALPoint> myPoints;

     //Grid step
    double myH;

    unsigned int myD;

  }; // end of class MongeJetFittingNormalVectorEstimator
  }
} // namespace DGtal


//                                                                           //

#endif // !defined MongeJetFittingNormalVectorEstimator_h

#undef MongeJetFittingNormalVectorEstimator_RECURSES
#endif // else defined(MongeJetFittingNormalVectorEstimator_RECURSES)