featureModel.hpp

/* $Id$ */

#ifndef SLAM_FEATURE_MODEL_HPP
#define SLAM_FEATURE_MODEL_HPP

#include <map>

#include "jmath/jblas.hpp"
#include "jmath/gaussianVector.hpp"

#include "geom/t3dEuler.hpp"

#include "filter/observeModel.hpp"

#include "slam/observation.hpp"

namespace jafar {
  namespace slam {

    // forward declaration
    class BaseFeature;

    class FeatureModel {
      std::size_t m_sizeMergeState;
    public:

      jblas::mat Jx;

      jblas::mat Jframe;

      FeatureModel(std::size_t sizeRobotPose, std::size_t sizeState, std::size_t sizeMergeState) :
          m_sizeMergeState(sizeMergeState),
      Jx(sizeState, sizeState),
      Jframe(sizeState, sizeRobotPose)
          {}

      virtual ~FeatureModel() {}

      std::size_t sizeState() {return Jx.size1();}
      std::size_t sizeRobotPose() {return Jframe.size2();}
      std::size_t sizeMergeState() { return m_sizeMergeState; }

      virtual void toFrame(const jblas::vec& frame_, const jblas::vec& x, jblas::vec& xRes) = 0;

      virtual void toFrameJac(jblas::vec const& frame_, jblas::vec const& x_) = 0;

      virtual void fromFrame(jblas::vec const& frame_, jblas::vec const& x_, jblas::vec& xRes) = 0;
      virtual void fromFrame(jblas::vec const& frame_, jblas::vec const& x_, jblas::vec_range& xRes) = 0;

      virtual void fromFrameJac(const jblas::vec& frame_, const jblas::vec& x_) = 0;

 //     /** Function to apply contraints over features (this function is only implemented 
 //       for segment features)
 //       */
 //     virtual void fixFeature(jblas::vec_range& L) {};

      virtual void computeMergeState( const jblas::vec& x_, jblas::vec& mergeX_, jblas::mat& jac ) = 0;
    };






    class BaseFeatureObserveModel : public jafar::filter::BlockObserveModel {

    public:

      FeatureModel& featureModel;

      BaseFeatureObserveModel(FeatureModel& featureModel, std::size_t sizeObs_);

    BaseFeatureObserveModel(FeatureModel& featureModel_, std::size_t sizeObs_, std::size_t sizeObsInit_);
    BaseFeatureObserveModel(FeatureModel& featureModel_, std::size_t sizeObs_, std::size_t sizeObsInit_, std::size_t sizeInnovation_, std::size_t sizePrediction);

      virtual ~BaseFeatureObserveModel();

      std::size_t sizeRobotPose() const {return sizeState1();};
      std::size_t sizeState() const {return sizeState2();};

      virtual Observation::ObservationType typeObs() const = 0;
    
    jblas::sym_mat RInit;

      jafar::geom::T3D const& robotToSensor() const {return *m_robotToSensor;}

      void setRobotToSensor(jafar::geom::T3DEuler const& robotToSensor_);
    virtual jblas::sym_mat const& getR() const {
      if (robotToSensor().hasCov())
        return featureR;
      else
        return R;
      }

    
    std::size_t sizeObsInit(){
      return p_sizeObsInit;
    }
    
      virtual jblas::sym_mat const& getRInit() const {
      return RInit;
    }
    
      virtual void computeR(Observation const& obs);
    
    virtual void computeRInit(Observation const& obs);
    

      virtual void computeSensorR(Observation const& obs) {}
    
    
    virtual void computeSensorRInit(Observation const& obs) {
      computeSensorR(obs);
      RInit = R;
    } 

    
      virtual jblas::vec const& predictObservation(const jblas::vec_range& robotPose_,
               const jblas::vec_range& feature_);

      virtual void predictObservationJac(const jblas::vec_range& robotPose_,
           const jblas::vec_range& feature_);

      virtual void initInternalState(BaseFeature& feature, 
             jafar::geom::T3DEuler const& robotPose_, 
             Observation const& obs_) const {}

      virtual void updateInternalState(BaseFeature& feature,
               jafar::geom::T3DEuler const& robotPose_,
               Observation const& obs_) const {}

      jafar::jmath::GaussianVector zPred;


    protected:
      void computePredictObservationJac(const jblas::vec_range& robotPose_,
           const jblas::vec_range& feature_, jblas::mat& J1, jblas::mat& J2);
      jblas::mat JobsSensor;

      jafar::geom::T3D* m_robotToSensor;

      jblas::mat JobsRobotToSensor;

      jblas::sym_mat featureR;

    std::size_t p_sizeObsInit;
    
      virtual jblas::vec const& predictObservationInSensorFrame(jblas::vec const& featureInSensorFrame_) = 0;

      virtual void predictObservationInSensorFrameJac(jblas::vec const& featureInSensorFrame_) = 0;

    }; // class BaseFeatureObserveModel
    





    class FeatureObserveModel : public BaseFeatureObserveModel {
      
    public:

      jblas::mat JinvObs;

    FeatureObserveModel(FeatureModel& model, std::size_t sizeObs_);
    
    FeatureObserveModel(FeatureModel& model, std::size_t sizeObs_, std::size_t sizeObsInit_);
    FeatureObserveModel(FeatureModel& model, std::size_t sizeObs_, std::size_t sizeObsInit_, std::size_t sizeInnovation_, std::size_t sizePrediction_);
    
      virtual ~FeatureObserveModel();

//       jblas::vec inverseObservation(const jblas::vec& z_);
//       
//       void inverseObservationJac(const jblas::vec& z_);

      jblas::vec inverseObservation(Observation const& obs_);
      
      void inverseObservationJac(Observation const& obs_);

    virtual void postInitCovariance(jblas::sym_mat_range & P,jblas::mat const& Jfeature) {};


    protected:

//      /** inverse observation function in sensor frame, to be defined by user.
//       */
//      virtual jblas::vec inverseObservationInSensorFrame(const jblas::vec& z_) = 0;
//
//      /// jacobian of inverseObservationInSensorFrame()
//      virtual void inverseObservationInSensorFrameJac(const jblas::vec& z_) = 0;
      
      virtual jblas::vec inverseObservationInSensorFrame(Observation const& obs_) = 0;

      virtual void inverseObservationInSensorFrameJac(Observation const& obs_) = 0;

    }; // class FeatureObserveModel

  } // namespace slam
} // namespace jafar


#endif // SLAM_FEATURE_MODEL_HPP