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)