SphericalHoughNormalVectorEstimator.h

 
#pragma once
 
#if defined(SphericalHoughNormalVectorEstimator_RECURSES)
#error Recursive header files inclusion detected in SphericalHoughNormalVectorEstimator.h
#else // defined(SphericalHoughNormalVectorEstimator_RECURSES)
#define SphericalHoughNormalVectorEstimator_RECURSES
 
#if !defined SphericalHoughNormalVectorEstimator_h
#define SphericalHoughNormalVectorEstimator_h
 
// Inclusions
#include <cmath>
#include <DGtal/base/Common.h>
#include <DGtal/topology/SCellsFunctors.h>
#include <vector>
#include "DGtal/geometry/tools/SphericalAccumulator.h"
#include <random>
#include "DGtal/math/linalg/SimpleMatrix.h"
 
namespace DGtal
{
  namespace functors
  {
    // template class SphericalHoughNormalVectorEstimator
    template <typename TSurfel, typename TEmbedder>
    class SphericalHoughNormalVectorEstimator
    {
    public:
      
      typedef TSurfel Surfel;
      typedef TEmbedder SCellEmbedder;
      typedef typename SCellEmbedder::RealPoint RealPoint;
      typedef RealPoint Quantity;
      typedef SimpleMatrix<double,3,3> Matrix;
      
      SphericalHoughNormalVectorEstimator(ConstAlias<SCellEmbedder> anEmbedder,
                                          const double h,
                                          const double minimalAspectRatio = 0.001,
                                          const unsigned int nbTrials = 100,
                                          const unsigned int accumulatorSize = 10,
                                          const unsigned int nbAccumulators = 5) :
      myEmbedder(&anEmbedder),myH(h), myAspectRatio(minimalAspectRatio),
      myNbTrials( nbTrials), mySize(accumulatorSize) , myNbAccumulators(nbAccumulators)
      {
        SphericalAccumulator<RealPoint> accum(mySize);
        
        //We precompute the random rotations and accumulators
        for(auto i = 0u; i < myNbAccumulators; ++i)
        {
          Matrix m = randomRotation();
          myAccumulators.push_back( accum );
          myRotations.push_back( m );
          myInverseRotations.push_back( m.inverse() );
        }
      }
 
      SphericalHoughNormalVectorEstimator() = delete;
 
      
      void pushSurfel(const Surfel & aSurf,
                      const double aDistance)
      {
        BOOST_VERIFY(aDistance == aDistance);
        RealPoint p = myH * ( myEmbedder->operator()(aSurf) );
        myPoints.push_back(p);
      }
      
      Quantity eval( )
      {
        std::default_random_engine generator;
        std::uniform_int_distribution<int> distribution(0,
                                                        static_cast<int>(myPoints.size()) - 1 );
        double aspect;
        
        for(auto t = 0u; t < myNbTrials ; ++t)
        {
          unsigned int i,j,k;
          
          //We pick 3 distinct point indices.
          i = distribution(generator);
          j = distribution(generator);
          while ( (j = distribution(generator)) == i);
          while (( (k = distribution(generator)) == i) || (k == j) );
          
          RealPoint vector = getNormal(i,j,k,aspect);
          if ((vector.norm() > 0.00001) && (aspect > myAspectRatio))
          {
            //we have an admissible triangle, we push both normal vectors
            for(auto acc = 0u; acc < myNbAccumulators; ++acc)
            {
              RealPoint shifted = myRotations[acc]*vector;
              myAccumulators[acc].addDirection( shifted );
              myAccumulators[acc].addDirection( -shifted );
            }
          }
        }
        //We return the max bin orientation summing up all accumulators vote
        typename SphericalAccumulator<RealPoint>::Size posPhi,posTheta;
        RealPoint vote;
        for(auto acc = 0u; acc < myNbAccumulators; ++acc)
        {
          myAccumulators[acc].maxCountBin(posPhi, posTheta);
          RealPoint dir = myInverseRotations[acc]*myAccumulators[acc].representativeDirection(posPhi, posTheta).getNormalized() ;
          
          //We only consider z-oriented normals (since we pushed vector and -vector)
          if ( dir.dot(RealPoint(0,0,1)) > 0.0 )
          vote += dir;
          else
          vote += -dir;
        }
        return vote.getNormalized();
      }
      
      void reset()
      {
        myPoints.clear();
        //accumulators cleanup
        for(auto i = 0u; i < myNbAccumulators; ++i)
          myAccumulators[i].clear();
      }
      
    private:
      
      Matrix randomRotation() const
      {
        const double theta = (rand()+0.f)/RAND_MAX * 2* M_PI;
        const double phi = (rand()+0.f)/RAND_MAX * 2* M_PI;
        const  double psi = (rand()+0.f)/RAND_MAX * 2* M_PI;
        Matrix Rt;
        Rt.setComponent(0,0,1);
        Rt.setComponent(1,0,0);
        Rt.setComponent(2,0,0);
        Rt.setComponent(0,1,0);
        Rt.setComponent(1,1,cos(theta));
        Rt.setComponent(2,1,-sin(theta));
        Rt.setComponent(0,2,0);
        Rt.setComponent(1,2,sin(theta));
        Rt.setComponent(2,2,cos(theta));
        
        Matrix Rph;
        Rph.setComponent(0,0,cos(phi));
        Rph.setComponent(1,0,0);
        Rph.setComponent(2,0,sin(phi));
        Rph.setComponent(0,1,0);
        Rph.setComponent(1,1,1);
        Rph.setComponent(2,1,0);
        Rph.setComponent(0,2,-sin(phi));
        Rph.setComponent(1,2,0);
        Rph.setComponent(2,2,cos(phi));
        
        Matrix Rps;
        Rps.setComponent(0,0,cos(psi));
        Rps.setComponent(1,0,-sin(psi));
        Rps.setComponent(2,0,0);
        Rps.setComponent(0,1,sin(psi));
        Rps.setComponent(1,1,cos(psi));
        Rps.setComponent(2,1,0);
        Rps.setComponent(0,2,0);
        Rps.setComponent(1,2,0);
        Rps.setComponent(2,2,1);
        
        return Rt*Rph*Rps;
      }
      
      Quantity getNormal(const unsigned int i,
                         const unsigned int j,
                         const unsigned int k,
                         double &aspect) const
      {
        ASSERT( i < myPoints.size());
        ASSERT( j < myPoints.size());
        ASSERT( k < myPoints.size());
        
        const RealPoint v = myPoints[i] - myPoints[j];
        const RealPoint u = myPoints[i] - myPoints[k];
        const RealPoint w = myPoints[j] - myPoints[k];
        
        //aspect ratio
        const double a = u.norm() , b = v.norm();
        const double c = w.norm();
        const double s = (a+b+c)/2.0;
        double denom = (8.0*(s-a)*(s-b)*(s-c));
        if ( std::abs( denom ) <= std::abs( denom ) * 1e-6 )
          aspect = 0.;
        else
          aspect = a*b*c / denom;
 
        return v.crossProduct(u);
      }
      
      const SCellEmbedder * myEmbedder;
      
      const double myH;
      
      const double myAspectRatio;
      
      const unsigned int myNbTrials;
      
      const unsigned int mySize;
      
      const unsigned int myNbAccumulators;
      
      std::vector<RealPoint> myPoints;
      
      std::vector< SphericalAccumulator<RealPoint> > myAccumulators;
      
      std::vector< Matrix > myRotations;
      
      std::vector< Matrix > myInverseRotations;
      
      
    }; // end of class SphericalHoughNormalVectorEstimator
  } // namespace functors
} // namespace DGtal
 
 
//                                                                           //
 
#endif // !defined SphericalHoughNormalVectorEstimator_h
 
#undef SphericalHoughNormalVectorEstimator_RECURSES
#endif // else defined(SphericalHoughNormalVectorEstimator_RECURSES)