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 DGTAL_WITH_PONCA
#error You need to have activated Ponca (DGTAL_WITH_PONCA) to include this file.
#endif
 
//Ponca includes
#include <Ponca/Fitting>
#include <Eigen/Eigen>
#include <vector>

 
namespace DGtal
{
  namespace functors
  {
    // template class SphereFittingEstimator
    template <typename TSurfel,
              typename TEmbedder,
              typename TNormalVectorEstimatorCache>
    class SphereFittingEstimator
    {
    public:
 
 
      class PoncaPoint
      {
      public:
        enum {Dim = 3};
        typedef double Scalar;
        typedef Eigen::Matrix<Scalar, Dim, 1>   VectorType;
        typedef Eigen::Matrix<Scalar, Dim, Dim> MatrixType;
 
        PONCA_MULTIARCH inline PoncaPoint(const VectorType& _pos    = VectorType::Zero(),
                                     const VectorType& _normal = VectorType::Zero())
          : m_pos(_pos), m_normal(_normal) {}
 
        PONCA_MULTIARCH inline const VectorType& pos()    const { return m_pos; }
        PONCA_MULTIARCH inline const VectorType& normal() const { return m_normal; }
 
        PONCA_MULTIARCH inline VectorType& pos()    { return m_pos; }
        PONCA_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 PoncaPoint::Scalar Scalar;
      typedef typename PoncaPoint::VectorType VectorType;
 
      typedef Ponca::DistWeightFunc<PoncaPoint, Ponca::SmoothWeightKernel<Scalar>> WeightFunc;
      typedef Ponca::Basket<PoncaPoint, WeightFunc, Ponca::OrientedSphereFit, Ponca::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),  myRadius(radius), myNormalEsitmatorCache(&anEstimator)
      {
        myFit = new Fit();
      }
 

      ~SphereFittingEstimator( )
      {
        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];
        PoncaPoint point(pp,  normal);
        if (myFirstPoint)
          {
            myFirstPoint = false;
            myFit->setWeightFunc({pp, myRadius});
            myFit->init();
          }
        else
          myFit->addNeighbor(point);
 
#ifdef DGTAL_DEV_VERBOSE
        trace.info() <<"#";
#endif
      }

      Quantity eval()
      {
        myFit->finalize();
 
#ifdef DGTAL_DEV_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;
     }
 
 
    private:

      const SCellEmbedder * myEmbedder;

      Fit *myFit;

      double myH;

      double myRadius;

      bool myFirstPoint;

      const NormalVectorEstimatorCache *myNormalEsitmatorCache;
 
    }; // end of class SphereFittingEstimator
  }
} // namespace DGtal
 
 
//                                                                           //
 
#endif // !defined SphereFittingEstimator_h
 
#undef SphereFittingEstimator_RECURSES
#endif // else defined(SphereFittingEstimator_RECURSES)