LinearLeastSquareFittingNormalVectorEstimator.h

#pragma once

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

#define LinearLeastSquareFittingNormalVectorEstimator_RECURSES

#if !defined LinearLeastSquareFittingNormalVectorEstimator_h

#define LinearLeastSquareFittingNormalVectorEstimator_h

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

#ifndef WITH_CGAL
#error You need to have activated CGAL (WITH_CGAL) to include this file.
#endif

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


namespace DGtal
{
  namespace functors
  {
  // template class LinearLeastSquareFittingNormalVectorEstimator
  template <typename TSurfel, typename TEmbedder>
  class LinearLeastSquareFittingNormalVectorEstimator
  {
  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::Plane_3  CGALPlane;
    typedef CGALKernel::Vector_3  CGALVector;

    LinearLeastSquareFittingNormalVectorEstimator(ConstAlias<SCellEmbedder> anEmbedder, const double h):
      myEmbedder(&anEmbedder), myH(h)
    {
    }

    void pushSurfel(const Surfel & aSurf,
                    const double aDistance)
    {
      BOOST_VERIFY(aDistance == aDistance);
      RealPoint p = myEmbedder->operator()(aSurf);
      CGALPoint pp(p[0],p[1],p[2]);
      myPoints.push_back(pp);
    }

    Quantity eval( )
    {
      CGALPlane plane;
      CGAL::linear_least_squares_fitting_3(myPoints.begin(),myPoints.end(), plane, CGAL::Dimension_tag<0>());
      CGALVector v = plane.orthogonal_vector();
      RealPoint vv(v.x(),v.y(),v.z());
      return vv.getNormalized();
    }

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


  private:

    const SCellEmbedder * myEmbedder;

    std::vector<CGALPoint> myPoints;

    double myH;



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


//                                                                           //

#endif // !defined LinearLeastSquareFittingNormalVectorEstimator_h

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