observationAbstract.hpp

Go to the documentation of this file.

#ifndef __ObservationAbstract_H__
#define __ObservationAbstract_H__

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#include <jmath/jblas.hpp>
#include <boost/smart_ptr.hpp>

#include "rtslam/rtSlam.hpp"
// include parents
#include "rtslam/parents.hpp"
#include "rtslam/objectAbstract.hpp"
#include "rtslam/sensorAbstract.hpp"
#include "rtslam/landmarkAbstract.hpp"
#include "rtslam/appearanceAbstract.hpp"

#include "rtslam/gaussian.hpp"
#include "rtslam/innovation.hpp"
#include "rtslam/dataManagerAbstract.hpp"

namespace jafar {
  namespace rtslam {
    using namespace jblas;

    class ObservationModelAbstract
    {
      public:
        typedef boost::weak_ptr<SensorAbstract> sensor_wptr_t;
      protected:
        sensor_wptr_t sensorWPtr;
      public:
        void linkToSensor( sensor_ptr_t ptr )
        {
          sensorWPtr = ptr;
        }
        sensor_ptr_t sensorPtr( void )
        {
          sensor_ptr_t sptr = sensorWPtr.lock();
          if (!sptr) {
            std::cerr << __FILE__ << ":" << __LINE__ << " ObsSpec::sensor threw weak" << std::endl;
            throw "WEAK";
          }
          return sptr;
        }
        // Cast to specific sensor type in the specialized task.
        virtual void linkToSensorSpecific( sensor_ptr_t ptr ) = 0;
      public:
        virtual void
        project_func(const vec7 & sg, const vec & lmk, vec & meas, vec & nobs) = 0;

        virtual void
        project_func(const vec7 & sg, const vec & lmk, vec & exp, vec & nobs, mat & EXP_sg, mat & EXP_lmk) = 0;

        virtual void
        project_func(const vec7 & sg, const vec & lmk, vec & exp, vec & nobs, mat & EXP_sg, mat & EXP_lmk, const vec lmk_fej)
          { project_func(sg, lmk, exp, nobs, EXP_sg, EXP_lmk); }

        virtual void backProject_func(const vec7 & sg, const vec & meas, const vec & nobs, vec & lmk) = 0;

        virtual void backProject_func(const vec7 & sg, const vec & meas, const vec & nobs, vec & lmk, mat & LMK_sg,
                                      mat & LMK_meas, mat & LMK_nobs) = 0;

        virtual bool predictVisibility_func(jblas::vec x, jblas::vec nobs) = 0;
    };


    class ObservationAbstract: public ObjectAbstract,
        public ChildOf<LandmarkAbstract> ,
        public boost::enable_shared_from_this<ObservationAbstract>,
        public ChildOf<DataManagerAbstract> {

        friend std::ostream& operator <<(std::ostream & s, ObservationAbstract const & obs);
        friend image::oimstream& operator <<(image::oimstream & s, ObservationAbstract const & obs);

        // define the function linkToParentLandmark().
        ENABLE_LINK_TO_PARENT(LandmarkAbstract,Landmark,ObservationAbstract)
        ;
        // define the functions landmarkPtr() and landmark().
        ENABLE_ACCESS_TO_PARENT(LandmarkAbstract,landmark)
        ;
        // define the function linkToParentDataManager().
        ENABLE_LINK_TO_PARENT(DataManagerAbstract,DataManager,ObservationAbstract)
        ;
        // define the functions dataManagerPtr() and dataManager().
        ENABLE_ACCESS_TO_PARENT(DataManagerAbstract,dataManager)
        ;

        void linkToSensor( sensor_ptr_t ptr ) { model->linkToSensor(ptr); }
        sensor_ptr_t sensorPtr( void ) { return model->sensorPtr(); }
        const sensor_ptr_t sensorPtr( void ) const { return model->sensorPtr(); }
        void linkToSensorSpecific( sensor_ptr_t ptr ) { model->linkToSensorSpecific(ptr); }

      public:

        boost::shared_ptr<ObservationModelAbstract> model;
        
        enum type_enum {
          PNT_PH_EUC, 
               PNT_PH_AH, 
               PNT_PH_AHPL 
        };


        ObservationAbstract(const sensor_ptr_t & _senPtr, const landmark_ptr_t & _lmkPtr, const size_t _size,
                            const size_t size_nonobs = 0);

        ObservationAbstract(const sensor_ptr_t & _senPtr, const landmark_ptr_t & _lmkPtr, const size_t _size_meas,
                            const size_t _size_exp, const size_t _size_inn, const size_t _size_nonobs = 0);

        virtual ~ObservationAbstract();

        void setPrior(const Gaussian & _prior);

        //
        static const unsigned max_hypotheses;
        bool use_hypotheses;
        struct Hypothesis
        {
          Gaussian nobs;
          Expectation expectation;
          mat EXP_l;
          mat EXP_sg;
          Hypothesis(unsigned lmk_size, unsigned exp_size, unsigned nobs_size):
            nobs(nobs_size), expectation(exp_size, nobs_size), EXP_l(exp_size, lmk_size), EXP_sg(exp_size, 7) {}
        };
        jblas::mat hyp_EXP_rsl;
        jblas::sym_mat hyp_rslP;
        std::vector<Hypothesis> hypotheses;
        bool clear_hypotheses;
        bool findBounds(jblas::vec & lmk, unsigned iid, jblas::vec7 const & sg, jblas::vec & ids);
        bool checkExtremities(jblas::vec & lmk, unsigned iid, double id_sig, Expectation const & hypexp, jblas::vec & id_bounds, jblas::vec & ids, jblas::vec7 const & sg, jblas::vec & exp,  jblas::sym_mat & expP, jblas::vec & nobs, double k);
        void projectHypothesis(Hypothesis & hyp, jblas::vec & lmk, unsigned iid, jblas::vec7 const & sg, jblas::vec & exp, jblas::vec & nobs);
        void linearizeUncertainty();

        // Data
        Expectation expectation;
        Measurement measurement;
        Innovation innovation;
        Gaussian prior;
        appearance_ptr_t predictedAppearance;
        appearance_ptr_t observedAppearance;
        jblas::sym_mat noiseCovariance;

        // indirect arrays
        ind_array ia_rsl; 

      public:
        // Jacobians
        mat SG_rs;     
        mat EXP_sg;    
        mat EXP_l;     
        mat EXP_rsl;   
        mat INN_meas;  
        mat INN_exp;   
        mat INN_rsl;   
        mat LMK_sg;    
        mat LMK_meas;  
        mat LMK_prior; 
        mat LMK_rs;    

      public:
        struct Counters {
            int nSearch; 
            int nMatch;  
            int nInlier; 
            int nSearchSinceLastInlier; 
            int nFrameSinceLastVisible; 
        } counters;

        struct Events {
            bool predicted;    
            bool visible;      
            bool measured;     
            bool matched;      
            bool updated;      
            bool killed;
        } events;
        bool updatable; 

        void markUpdated() { events.updated = true; clear_hypotheses = true; }

        struct Tasks {
            bool predictedApp; 
        } tasks;

        int searchSize;

        type_enum type;

        void setId(){
          id(landmarkPtr()->id());
        }

        virtual std::string typeName() const {return "Abstract";}
        std::string categoryName() const {
          return "OBSERVATION";
        }

        void project(bool multi_hyp = true);

        void projectMean();
        
        virtual bool isVisible() {
          return events.visible;
        }
        
        void backProject();

        virtual void computeInnovation();
        virtual void computeInnovationMean(vec &inn, const vec &meas, const vec &exp) const;
        virtual double computeRelevance();

        virtual void predictInfoGain();

        virtual bool compatibilityTest(const double mahaDist);

        static bool compatibilityTest(Measurement const & meas, Expectation const & exp, double mahaDist);

        void clearFlags();
        void clearCounters();


        virtual bool predictVisibility()
        {
          events.visible = model->predictVisibility_func(expectation.x(), expectation.nonObs);
          return events.visible;
        }
        
        
        virtual bool predictAppearance(bool force = false)
        {
          if (force || !tasks.predictedApp)
          {
//JFR_DEBUG("predictAppearance");
            if (predictAppearance_func())
            {
              tasks.predictedApp = true;
              return true;
            }
            return false;
          }
          return true;
        }
        
        virtual bool updateDescriptor()
        {
          return landmarkPtr()->descriptorPtr->addObservation(this->shared_from_this());
        }
        virtual void updateVisibilityMap()
        {
          landmarkPtr()->visibilityMap.addObservation(this->shared_from_this());
        }
        
        virtual bool isDescriptorValid()
        {
          return landmarkPtr()->descriptorPtr->isPredictionValid(this->shared_from_this());
        }

        virtual bool predictAppearance_func() = 0;

        double getMatchScore() {
          return measurement.matchScore;
        }

        void update(bool correct_P = true) ;
#if 0
        virtual bool voteForKillingLandmark();
#endif

        virtual double computeLinearityScore() = 0; 

        virtual void transferInfoObs(observation_ptr_t & obs);

        virtual void desc_image(image::oimstream& os) const {}
        
    };

  }

}


#define ENABLE_LINK_TO_SENSOR_SPEC(className)            \
  protected:\
    sensor_spec_wptr_t sensorSpecWPtr;\
  public:\
    void linkTo##className( sensor_spec_ptr_t ptr )\
    {\
      sensorSpecWPtr = ptr;\
      ObservationAbstract::linkToSensor(ptr);\
    }


#define ENABLE_ACCESS_TO_SENSOR_SPEC(accessName)            \
    sensor_spec_ptr_t accessName##Ptr( void )\
    {\
      sensor_spec_ptr_t sptr = sensorSpecWPtr.lock();\
      if (!sptr) {\
        std::cerr << __FILE__ << ":" << __LINE__\
      << " ObsSpec::sensor threw weak" << std::endl;\
        throw "WEAK";\
      }\
      return sptr;\
    }



#endif // #ifndef __ObservationAbstract_H__
/*
 * Local variables:
 * mode: c++
 * indent-tabs-mode: t
 * tab-width: 2
 * c-basic-offset: 2
 * End:
 */