SphereFittingEstimator.h

 
#pragma once
 
#if defined(SphereFittingEstimator_RECURSES)
#error Recursive header files inclusion detected in SphereFittingEstimator.h
#else // defined(SphereFittingEstimator_RECURSES)
#define SphereFittingEstimator_RECURSES
 
#if !defined SphereFittingEstimator_h
#define SphereFittingEstimator_h
 
// Inclusions
#include <iostream>
#include <DGtal/base/Common.h>
#include <DGtal/topology/SCellsFunctors.h>
 
#ifndef WITH_PATATE
#error You need to have activated Patate (WITH_PATATE) to include this file.
#endif
 
//Patate includes
#include <Patate/grenaille.h>
#include <Eigen/Eigen>
#include <vector>
 
 
namespace DGtal
{
  namespace functors
  {
    // template class SphereFittingEstimator
    template <typename TSurfel,
              typename TEmbedder,
              typename TNormalVectorEstimatorCache>
    class SphereFittingEstimator
    {
    public:
 
      
      class PatatePoint
      {
      public:
        enum {Dim = 3};
        typedef double Scalar;
        typedef Eigen::Matrix<Scalar, Dim, 1>   VectorType;
        typedef Eigen::Matrix<Scalar, Dim, Dim> MatrixType;
        
        MULTIARCH inline PatatePoint(const VectorType& _pos    = VectorType::Zero(), 
                                     const VectorType& _normal = VectorType::Zero())
          : m_pos(_pos), m_normal(_normal) {}
        
        MULTIARCH inline const VectorType& pos()    const { return m_pos; }  
        MULTIARCH inline const VectorType& normal() const { return m_normal; }
 
        MULTIARCH inline VectorType& pos()    { return m_pos; }  
        MULTIARCH inline VectorType& normal() { return m_normal; }
 
      private:
        VectorType m_pos, m_normal;
      };
 
     
      typedef TSurfel Surfel;
      typedef TEmbedder SCellEmbedder;
      typedef typename SCellEmbedder::RealPoint RealPoint;
 
      typedef TNormalVectorEstimatorCache NormalVectorEstimatorCache;
 
      typedef typename PatatePoint::Scalar Scalar;
      typedef typename PatatePoint::VectorType VectorType;
      
      typedef Grenaille::DistWeightFunc<PatatePoint,Grenaille::SmoothWeightKernel<Scalar> > WeightFunc; 
      typedef Grenaille::Basket<PatatePoint,WeightFunc,Grenaille::OrientedSphereFit, Grenaille::GLSParam> Fit;
 
      struct Quantity
      {
        RealPoint center;
        double radius;
        double tau;
        double kappa;
        RealPoint eta;
        
        Quantity(){}
        Quantity(RealPoint p, double rad, double _tau,
                 double _kappa, RealPoint _eta): center(p), radius(rad),
                                                 tau(_tau), kappa(_kappa),
                                                 eta(_eta) {}
        ~Quantity(){}
        bool operator==(Quantity aq) {return (center==aq.center) && (radius==aq.radius);}
        bool operator<(Quantity aq) {return (center<aq.center) && (radius<aq.radius);}        
        bool operator!=(Quantity aq) {return !(*this == aq);}
      };
 
 
      SphereFittingEstimator(ConstAlias<SCellEmbedder> anEmbedder,
                             const double h,
                             const double radius,
                             ConstAlias<NormalVectorEstimatorCache> anEstimator):
        myEmbedder(&anEmbedder), myH(h), myNormalEsitmatorCache(&anEstimator)
      {
        //From Mellado's example
        myFit = new Fit();
        myWeightFunction = new WeightFunc(radius);
        myFit->setWeightFunc(*myWeightFunction);
      }
 
 
      ~SphereFittingEstimator( )
      {
        delete myWeightFunction;
        delete myFit ;
      }
 
      
      void pushSurfel(const Surfel & aSurf,
                      const double aDistance)
      {
        BOOST_VERIFY(aDistance==aDistance);
 
        RealPoint p = myEmbedder->operator()(aSurf);
        RealPoint norm = myNormalEsitmatorCache->eval(aSurf);          
        VectorType pp;
        pp(0) = p[0]*myH;
        pp(1) = p[1]*myH;
        pp(2) = p[2]*myH;
        VectorType normal;
        normal(0) = norm[0];
        normal(1) = norm[1];
        normal(2) = norm[2];
        PatatePoint point(pp,  normal);
        if (myFirstPoint)
          {
            myFirstPoint = false;
            myFit->init(pp);
          }
        else
          myFit->addNeighbor(point);
        
#ifdef DEBUG_VERBOSE
        trace.info() <<"#";
#endif
      }
 
      Quantity eval()
      {
        myFit->finalize();
 
#ifdef DEBUG_VERBOSE
        trace.info() <<std::endl;
        
        //Test if the fitting ended without errors
        if(myFit->isStable())
        {
                std::cout << "Center: [" << myFit->center().transpose() << "] ;  radius: " << myFit->radius() << std::endl;
 
                std::cout << "Pratt normalization" 
                        << (myFit->applyPrattNorm() ? " is now done." : " has already been applied.") << std::endl;
 
        
                std::cout << "Fitted Sphere: " << std::endl
                        << "\t Tau  : "      << myFit->tau()             << std::endl
                        << "\t Eta  : "      << myFit->eta().transpose() << std::endl
                        << "\t Kappa: "      << myFit->kappa()           << std::endl;
 
        }
        else
          {
            std::cout << "Ooops... not stable result"<<std::endl;
          }
#endif
        Quantity res;
        res.center = RealPoint((myFit->center())(0),
                               (myFit->center())(1),
                               (myFit->center())(2));
        res.radius =  myFit->radius();
        res.tau = myFit->tau();
        res.kappa = myFit->kappa();
        res.eta = RealPoint((myFit->eta())(0),
                            (myFit->eta())(1),
                            (myFit->eta())(2));
        return res;
      }
                             
 
      void reset()
      {
        delete myFit;
        myFit = new Fit();
        myFirstPoint = true;
        myFit->setWeightFunc(*myWeightFunction);
     }
 
 
    private:
 
      const SCellEmbedder * myEmbedder;
 
      Fit *myFit;
      
      double myH;
 
      bool myFirstPoint;
      
      const NormalVectorEstimatorCache *myNormalEsitmatorCache;
 
      const  WeightFunc *myWeightFunction;
      
    }; // end of class SphereFittingEstimator
  }
} // namespace DGtal
 
 
//                                                                           //
 
#endif // !defined SphereFittingEstimator_h
 
#undef SphereFittingEstimator_RECURSES
#endif // else defined(SphereFittingEstimator_RECURSES)