facet.hpp

#ifndef _FACET_HPP_
#define _FACET_HPP_

#include "camera/cameraPinhole.hpp"
#include "image/Image.hpp"
#include "jmath/jblas.hpp"
#include "gfm/InterestFeature.hpp"

#include "facetsmap/FacetsGroupBase.hpp"
#include "facetsmap/params.hpp"
#include "facetsmap/repere.hpp"
#include "facetsmap/types.hpp"
#include "homography/HomographyEstimator.hpp"

namespace jafar {
  namespace facetsmap {
    class AmersMap; // HACK looks amersmap code source for more reason
    class FacetsParams;
    struct CamParam {
      jblas::mat33 intrinsec, intrinsecInv, rot;
    };
    class Facet : private Repere {
      friend bool FacetsGroupBase::add(Facet*);
      friend vFacets_it FacetsGroupBase::remove(const vFacets_it&);
      friend class AmersMap; // HACK looks amersmap code source for more reason
      friend class FacetsMap;
      friend FacetsGroupBase::~FacetsGroupBase();
      public:
        Facet(const FacetsParams* params, unsigned int id, const ::jafar::gfm::InterestFeature& lpoint, const ::jafar::gfm::InterestFeature& rpoint, const jafar::image::Image& imgl, const homography::ImageGradients& imggradl, const jafar::image::Image& imgr, const homography::ImageGradients& imggradr, const camera::StereoBench& stereoBench, int time);
        ~Facet();
      private:
        inline Facet() : Repere(Repere::global()) {};
      public:
        void update( const Facet* f, int time);
        void updatePreviousObservation(const boost::numeric::ublas::bounded_matrix<double,4,4>& robotmovement);
      public:
//-------- Getter and setter --------//
      public:
        inline void changeRef(const jafar::facetsmap::Repere* r) { Repere::changeRef(r); }
        inline void setRef(const jafar::facetsmap::Repere* r) { Repere::setRef(r); }
        inline jblas::vec3 localOrigin() const { return origin(); };
        inline jblas::vec3 globalOrigin() const { return ref()->localToGlobal(origin(), 1); };
        inline jafar::facetsmap::Repere* repere() { return this; };
        inline double halfSize() const { return m_halfSize; };
        inline jblas::vec3 globalAxe1() const { return ref()->localToGlobal(j(), 0); };
        inline jblas::vec3 localAxe1() const { return j(); };
        inline jblas::vec3 globalAxe2() const { return ref()->localToGlobal(k(), 0); };
        inline jblas::vec3 localAxe2() const { return k(); };
        inline jblas::vec3 globalNormal() const { return ref()->localToGlobal(i(), 0); };
        inline jblas::vec3 localNormal() const { return i(); };
        std::vector<jblas::vec3> t_points;
        inline const jafar::image::Image* image() const { return m_imagette; };
        inline unsigned int id() const { return m_id; };
        inline int time() const { return m_time; };
      private:
        inline void setNormal(jblas::vec3 n) { setI(n); };
        inline void setAxe1(jblas::vec3 n) { setJ(n); };
        inline void setAxe2(jblas::vec3 n) { setK(n); };
      public:
        inline double distanceTo(jblas::vec3 pos) const { return ublas::norm_2( pos - globalOrigin()) ; };
        inline bool insideValidity(jblas::vec3 pos) const { return insideValidity( distanceTo(pos) ); }
        inline bool insideValidity(double d) const { return (m_inf_distance < d ) && ( d < m_sup_distance ); }
        bool insideFacet(jblas::vec3 pos);
        inline vUints_cit neighborhoodBegin() const { return m_neighborhood.begin(); };
        inline vUints_cit neighborhoodEnd() const { return m_neighborhood.end(); };
        inline void neighborhoodAdd(uint fid) { m_neighborhood.push_back(fid); };
        inline void neighborhoodAdd(Facet* f) { neighborhoodAdd(f->id()); };
        inline double orientation() const { return m_orientation; };
        inline double quality() const { return 1.-m_squareError/THRESHOLD_GOOD_PLAN; }
        double compare(Facet*f);
        inline FacetsGroupBase* group() const { return  m_facetsGroup; };
        inline geom::InterestPoint ipDetection() { return m_ipDetection; };
        
        inline jblas::mat33 positionVariance() const { return m_positionVariance; }
      public: // Age functions
        inline int countSeen() { return m_countSeen; }
        inline int age() { return m_age; }
        inline void incAge() { m_age++; }
      private:
        inline void seenAgain() { m_countSeen++; }
      public:
        inline const jblas::vec3& previousObservationOrigin() const
        {return m_previousObservationOrigin;}
        inline const jblas::vec3& previousObservationNormal() const
        {return m_previousObservationNormal;}
        inline const jblas::vec3& previousObservationAxe1() const
        {return m_previousObservationAxe1;}
        inline const jblas::vec3& previousObservationAxe2() const
        {return m_previousObservationAxe2;}
        
      public:
        void print();
        friend std::ostream& operator<< (std::ostream& , const Facet&);
      private:
        inline void setGroup(FacetsGroupBase* fg) { m_facetsGroup = fg; };
      private: // "Utility" functions for the facet creation or update
//         image::Image* extractImagette();
        static void computeAxes( jblas::vec3&, jblas::vec3&, jblas::vec3 normal, double orientation);
        void computeHomography(const FacetsParams* params, jblas::mat33& homography, const ::jafar::gfm::InterestFeature& lpoint, const ::jafar::gfm::InterestFeature& rpoint, const jafar::image::Image& imgl, const homography::ImageGradients& imggradl, const jafar::image::Image& imgr, const homography::ImageGradients& imggradr);
        static void computeOriginAndNormal(jblas::vec3& origin, jblas::mat33& positionJac, jblas::vec3& vnormal, const jblas::mat33& homography, const ::jafar::gfm::InterestFeature& lpoint, const camera::StereoBench& stereoBench);
      public:
        static void computeTexture(const FacetsParams* params, image::Image& texdst, const image::Image& imgl, const jblas::vec3& origin, const jblas::vec3& axe1, const jblas::vec3 axe2, const camera::StereoBench& stereoBench);
      private:
        int m_sizefacet; 
        int m_time;
        uint m_id; 
        //jblas::vec3 m_normal; /** This vector contains the parameters of the plan */
        //jblas::vec3 m_origin; /** The coordinate of the origin */
        double m_squareError; 
        double m_halfSize; 
        double m_inf_distance, m_sup_distance; 
        double m_best_distance; 
        double m_best_angle; 
        //jblas::vec3 m_axe1, m_axe2; /** Two axes of the plan, they are orthogonal are their length is equal to the dimension of the facet */
        image::Image* m_imagette;
        vUints m_neighborhood;
        double m_orientation; 
        FacetsGroupBase* m_facetsGroup; 
        homography::HomographyEstimator::Statistics m_stats;
        geom::InterestPoint m_ipDetection;
        int m_countSeen;
        int m_age;
        jblas::mat33 m_positionVariance;
        jblas::mat66 m_variance;
        jblas::vec m_mean;
        bool meannotset;
        jblas::vec3 m_previousObservationOrigin, m_previousObservationNormal, m_previousObservationAxe1, m_previousObservationAxe2;
    };
  };
};

#endif