segmentInvDepthFeature.hpp

    /* $Id: segInvDepthFeature.hpp 3374 2008-12-01 18:17:38Z tvidal $ */

#ifndef SLAM_SEGMENT_INV_DEPTH_FEATURE_HPP
#define SLAM_SEGMENT_INV_DEPTH_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 SegmentIDFeature : public BaseFeature {

      public:

        std::string debugObs;

        double s1;
        double s2;

        bool ENLARGE;

        SegmentIDFeature(unsigned int id, FeatureModel& model, std::size_t sizeObs, Observation::ObservationType typeObs_);
        virtual ~SegmentIDFeature();

        SegInvDepthFeatureModel* segidmodel;

        void setFlagEnlarge() {ENLARGE=true;}

        virtual void setState(jblas::vec& x_, jblas::sym_mat& P_);

        jblas::vec3 getExt1() const;
        jblas::sym_mat getExt1Cov() const;
        jblas::vec3 getExt2() const;
        jblas::sym_mat getExt2Cov() const;
        jblas::mat getExt1Jac() const;
        jblas::mat getExt2Jac() const;

      protected:

        virtual void writeLogHeader(jafar::kernel::DataLogger& log) const;
        virtual void writeLogData(jafar::kernel::DataLogger& log) const;

      private:

        void computeExt(double s_, jblas::vec3& ext) const;

        void computeExtJac(double s_, jblas::mat& J) const;

        void computeExtCov(double s_, jblas::sym_mat& extCov) const;

    }; // SegmentIDFeature


    std::ostream& operator <<(std::ostream& s, const jafar::slam::SegmentIDFeature& f);

    class SegInvDepthFeatureModel : public FeatureModel {
      public:

        jblas::mat J3dPoints;

        SegInvDepthFeatureModel();
        ~SegInvDepthFeatureModel();


        void toFrame(jblas::vec const& frame_, jblas::vec const& x_, jblas::vec& xRes);

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

        void fromFrame(jblas::vec const& frame_, jblas::vec const& x_, jblas::vec& xRes);
        void fromFrame(jblas::vec const& frame_, jblas::vec const& x_, jblas::vec_range& xRes);
      
        void fromFrameJac(const jblas::vec& frame_, const jblas::vec& x_);

        void compute3dPoints(jblas::vec const& x, jblas::vec& p, jblas::vec& q);
        void compute3dPointsJac(jblas::vec const& x);
        void compute3dPointsJac(jblas::vec const& x, jblas::vec& p, jblas::vec& q, jblas::mat& Pt_id);
        //void compute3dLine(jblas::vec const& x, jblas::vec& pt1, jblas::vec& pt2);
        //void compute3dLineJac(jblas::vec const& x, jblas::vec& pt1, jblas::vec& pt2);
        void computeMergeState( const jblas::vec& x_, jblas::vec& mergeX_ , jblas::mat& jac );

    }; // class SegInvDepthFeatureModel


    class ImageSegInvDepthFeatureObserveModel : public FeatureObserveModel {

      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:

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

        ~ImageSegInvDepthFeatureObserveModel();

        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 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);

        virtual void computeSensorRInit(Observation const& obs);


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


    }; // class ImageSegInvDepthFeatureObserveModel

  }}

#endif // SLAM_SEG_INV_DEPTH_FEATURE_HPP