segmentAnchorPluckerFeature.hpp

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

#ifndef SLAM_SEGMENT_ANCHOR_PLUCKER_FEATURE_HPP
#define SLAM_SEGMENT_ANCHOR_PLUCKER_FEATURE_HPP

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

namespace jafar {
namespace slam {
    
    class SegAnchorPluckerFeatureModel;

    class SegmentAnchorFeature : public BaseFeature {

      public:

        std::string debugObs;

        double s1;
        double s2;

        bool ENLARGE;

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

    SegAnchorPluckerFeatureModel* segapmodel;

        jblas::vec_range const& x0() const {return *p_x0;}
        jblas::vec_range& x0() {return *p_x0;}
        jblas::vec_range const& u() const {return *p_u;}
        jblas::vec_range& u() {return *p_u;}
        jblas::vec_range const& n() const {return *p_n;}
        jblas::vec_range& n() {return *p_n;}


        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;

        static void setConstraintAlpha(double alpha) {p_constraintAlpha = alpha;}
        static double getConstraintAlpha() {return p_constraintAlpha;}
        static void setConstraintMaxNbTimesApplied(int maxNbTimesApplied) {p_constraintMaxNbTimesApplied = maxNbTimesApplied;}
        static int getConstraintMaxNbTimesApplied() {return p_constraintMaxNbTimesApplied;}
        static void setFlagPlucklerConstraint(bool pluckerConstraint) {ENABLE_PLUCKER_CONSTRAINT=pluckerConstraint;}


      protected:

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

      private:

        jblas::vec_range* p_x0;

        jblas::vec_range* p_n;

        jblas::vec_range* p_u;


        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;

        static double p_constraintAlpha;
        static int p_constraintMaxNbTimesApplied;
        static bool ENABLE_PLUCKER_CONSTRAINT;


    }; // SegmentAnchorFeature


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

    class SegAnchorPluckerFeatureModel : public FeatureModel {
      public:
 
        jblas::mat JunanchorPl;

        SegAnchorPluckerFeatureModel();
        ~SegAnchorPluckerFeatureModel();


        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 computePlucker(jblas::vec const& x, jblas::vec& L);
        void computePluckerJac(jblas::vec const& x);
        void computePluckerJac(jblas::vec const& x, jblas::vec& L, jblas::mat& L_al);
        
        void computeMergeState( const jblas::vec& x_, jblas::vec& mergeX_ , jblas::mat& jac );

        void computeAnchorPlucker(jblas::vec const& L, jblas::vec const& x0, jblas::vec& aL);
        void computeAnchorPluckerJac(jblas::vec const& L, jblas::vec const& x0, jblas::vec& aL, jblas::mat& AL_l, jblas::mat& AL_x0);
        

    }; // class SegAnchorPluckerFeatureModel


    class ImageAnchorPluckerFeatureObserveModel : public FeatureObserveModel {

      protected:

        SegAnchorPluckerFeatureModel& segAnchorPluckerFeatureModel;
        
        jafar::camera::CameraPinhole camera;
        jblas::mat33 Pl;
        jblas::mat33 Plinv;
        
        jblas::vec2 beta;
        jblas::mat22 covar_beta;
        
        jblas::mat JbBeta;
        
        
        boost::tuple<double,double> computeExtremitiesAbscissa(SegmentAnchorFeature const& segFeature_, 
                geom::T3DEuler const& robotPose_,
                SegmentObservation const& obs_) const;
        
        void segmentExtremitiesPluckerLines(SegmentObservation const& segObs_, 
                        jblas::vec& lineExt1Sensor, 
                        jblas::vec& lineExt2Sensor) const;
        
        static boost::tuple<double, double, double> pluckerLinesDistance(jblas::vec const& line1_, 
                        jblas::vec const& line2_);

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

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

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

        void inverseObservationInSensorFrameJac(Observation const& obs_);

        jblas::sym_mat p_pixCov;

      public:

        ImageAnchorPluckerFeatureObserveModel(SegAnchorPluckerFeatureModel& featureModel_, jafar::camera::CameraPinhole const& camera_);

        ~ImageAnchorPluckerFeatureObserveModel();

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

        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& Jx);
        
        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 beta0 = 0, double beta1 = 0, double sigma_beta = 0.5);
        


    }; // class ImageAnchorPluckerFeatureObserveModel

  }}

#endif // SLAM_SEG_INV_DEPTH_FEATURE_HPP