segmentInvDepthInovFeature.hpp

    /* $Id$ */

#ifndef SLAM_SEGMENT_INV_DEPTH_INOV_FEATURE_HPP
#define SLAM_SEGMENT_INV_DEPTH_INOV_FEATURE_HPP

#include "jmath/jblas.hpp"

#include "camera/cameraBarreto.hpp"

#include "slam/feature.hpp"
#include "slam/featureModel.hpp"
#include "slam/segmentFeature.hpp"

    namespace jafar {
  namespace slam {
    
    class SegInvDepthFeatureModel;

    class ImageSegInvDepthInovFeatureObserveModel : public FeatureObserveModel {
        jblas::mat Jobs1Tmp;
        jblas::mat Jobs2Tmp;
      protected:

        double m_id0;
        double m_sigmaId0;

        jafar::camera::CameraPinhole camera;

        SegInvDepthFeatureModel& segInvDepthFeatureModel;

        jblas::vec const& predictObservationInSensorFrame(const jblas::vec& feature_);

        void predictObservationInSensorFrameJac(const jblas::vec& feature_);

//       jblas::vec inverseObservationInSensorFrame(jblas::vec const& z_);
        // 
//       void inverseObservationInSensorFrameJac(jblas::vec const& z_);

        jblas::vec inverseObservationInSensorFrame(Observation const& obs_);

        void inverseObservationInSensorFrameJac(Observation const& obs_);
        
        jblas::sym_mat p_pixCov;

      public:

        ImageSegInvDepthInovFeatureObserveModel(SegInvDepthFeatureModel& featureModel_, jafar::camera::CameraPinhole const& camera_);

        ~ImageSegInvDepthInovFeatureObserveModel();

        Observation::ObservationType typeObs() const {return Observation::SEGMENTID_IMAGE;}
      
        void setPixCov(jblas::vec const& pixCov_, double stabilizingFactor = 1.0);

      //void initStateInSensorFrame(InitFeature& feature_,
  //        Observation const& obs_,
        //                          double dMin_, double dMax_);
        
        void initInternalState(BaseFeature& feature, jafar::geom::T3DEuler const& robotPose_, Observation const& obs_) const;
      
        void updateInternalState(BaseFeature& feature, jafar::geom::T3DEuler const& robotPose_, Observation const& obs_) const;
        
        void postInitCovariance(jblas::sym_mat_range & P, jblas::mat const& Jfeature);
        void predictObservationJac(const jblas::vec_range& robotPose_, const jblas::vec_range& feature_);
        
        void predictExtObs(jblas::vec_range const& pose, jblas::sym_mat_range const& poseCov,
                           jblas::vec3 const& ext, jblas::sym_mat const& extCov,
                           jblas::vec& zPredExt, jblas::sym_mat& zPredExtCov);
        void predictExtObsJac(jblas::vec_range const& pose, jblas::vec3 const Ext, jblas::vec& eInImage,  jblas::mat& JeR, jblas::mat& JeExt);

        
        virtual jblas::vec const& computeInnovation(jblas::vec const& z_, jblas::vec const& zPred_);

        static jblas::vec2 extToImageModel(jblas::vec const& ext1,jblas::vec const& ext2);

        static void extToImageModelJac(jblas::vec const& ext1, jblas::vec const& ext2, jblas::mat& J);

    
        virtual void computeSensorR(Observation const& obs);

        void setup(double id0 = 0.5, double sigmaId0 = 0.5);


    }; // class ImageSegInvDepthFeatureObserveModel

  }}

#endif // SLAM_SEG_INV_DEPTH_FEATURE_HPP