segmentFeature.hpp

/* $Id$ */

#ifndef SLAM_SEGMENT_FEATURE_HPP
#define SLAM_SEGMENT_FEATURE_HPP

#include "boost/tuple/tuple.hpp"

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

#include "geom/t3d.hpp"

#include "filter/constraintModel.hpp"

#include "filter/predictModel.hpp"

#include "slam/eulerTools.hpp"
#include "slam/observation.hpp"
#include "slam/bearingOnlyFeature.hpp"
#include "camera/cameraPinhole.hpp"

namespace jafar {
namespace slam {

  class BearingOnlySlam;

  class SegmentObservation : public Observation {
  
  public:

  jblas::vec2 ext1;
  jblas::vec2 ext2;

  SegmentObservation(ObservationType type_, unsigned int _robotId = 0) :
    Observation(type_, _robotId),
    ext1(), ext2()
  {
        JFR_ASSERT( type_ == SEGMENT_IMAGE or type_ == SEGMENTAP_IMAGE or type_ == SEGMENTID_IMAGE or type == SEGMENTID_EXT_IMAGE or type == SEGMENT_STEREOIMAGE, "Not a segment type");
    }
  
  SegmentObservation(SegmentObservation const& obs) :
    Observation(obs),
    ext1(obs.ext1),
    ext2(obs.ext2)
  {}

  virtual ~SegmentObservation() {}

  void setExtremities(jblas::vec2 const& ext1_, jblas::vec2 const& ext2_) {
    ext1.assign(ext1_);
    ext2.assign(ext2_);
  }

  }; // SegmentObservation

    std::ostream& operator <<(std::ostream& s, const jafar::slam::SegmentObservation& o_);

    class StereoSegmentObservation : public Observation {
    
    public:

    jblas::vec3 ext1;
    jblas::vec3 ext2;

    StereoSegmentObservation(unsigned int _robotId = 0) :
        Observation(SEGMENT_STEREOIMAGE, _robotId),
        ext1(), ext2()
    {}
    
    StereoSegmentObservation(StereoSegmentObservation const& obs) :
        Observation(obs),
        ext1(obs.ext1),
        ext2(obs.ext2)
    {}

    virtual ~StereoSegmentObservation() {}

    void setExtremities(jblas::vec3 const& ext1_, jblas::vec3 const& ext2_) {
        ext1.assign(ext1_);
        ext2.assign(ext2_);
    }

    }; // StereoSegmentObservation


    std::ostream& operator <<(std::ostream& s, const jafar::slam::StereoSegmentObservation& o_);

  class SegmentUConstraint : public jafar::filter::JacobianStrongConstraintModel {

  public:

  SegmentUConstraint(double alpha, int maxNbTimesApplied) :
  JacobianStrongConstraintModel(6, 1.0, alpha, maxNbTimesApplied)
  {
  // some terms are always zero
  J.clear();
  }

  ~SegmentUConstraint() {}

  double estimateValue(jblas::vec_range const& x0) const {
  using namespace ublas;
  return norm_2(project(x0, range(3,6)));
  }

  void estimateValueJac(jblas::vec_range const& x0) {
  using namespace ublas;
  jblas::mat_range J_r(J, range(0,1), range(3,6));
  jmath::ublasExtra::norm_2Jac<3>(project(x0, range(3,6)), J_r);
  }

  };

  class SegmentPluckerConstraint : public jafar::filter::JacobianStrongConstraintModel {

  public:

  SegmentPluckerConstraint(double alpha, int maxNbTimesApplied) :
  JacobianStrongConstraintModel(6, 0.0, alpha, maxNbTimesApplied) {}

  ~SegmentPluckerConstraint() {}

  double estimateValue(jblas::vec_range const& x0) const {
  using namespace ublas;
  p_estimatedValue = inner_prod(project(x0, range(0,3)), project(x0, range(3,6)));
  return p_estimatedValue;
  }

  void estimateValueJac(jblas::vec_range const& x0) {
  using namespace ublas;
  jmath::ublasExtra::inner_prodJac<3>(project(x0, range(0,3)), project(x0, range(3,6)), J);
  }

//       double weakeningVariance() {
// //   p_R = p_R0;
//  return p_R;
//       }

  };

    
    class SegmentFeatureContraintModel :
      public jafar::filter::JacobianBlockPredictModel {
      
    public:

      SegmentFeatureContraintModel();
      ~SegmentFeatureContraintModel() {}

      void fixFeature(jblas::vec_range& L);
      void fixFeatureJac(jblas::vec_range& L, jblas::mat& JfL);

      void predict(jblas::vec_range& x_r);

    }; // class SegmentFeatureContraintModel 


  class SegmentFeature : public BaseFeature {

  public:

  std::string debugObs;

  double s1;
  double s2;

  bool ENLARGE;

//       /// to keep extremities information, used for the display
//       SegmentObservation referenceObservation;

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

  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;
//       void applyConstraints();

  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;}
  
  //jblas::vec homogenousToEuclideanPlucker(jblas::vec const& hPlucker);

  protected:

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

  private:

  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;

  }; // SegmentFeature


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

  class SegmentFeatureModel : public FeatureModel {

  public:

  SegmentFeatureModel();
  ~SegmentFeatureModel();


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

  void fromFrame(const jblas::vec& frame_, const jblas::vec& x_, jblas::vec& xRes);
  void fromFrame(const jblas::vec& frame_, const jblas::vec& x_, jblas::vec_range& xRes);
  void fromFrameJac(const jblas::vec& frame_, const jblas::vec& x_);
    //void fixFeature(jblas::vec& L);
    void computeMergeState( const jblas::vec& x_, jblas::vec& mergeX_, jblas::mat& jac  );
  
  }; // class SegmentFeatureModel
  
  class ImageEuclideanPluckerFeatureObserveModel : public BearingOnlyFeatureObserveModel {

  protected:

  jafar::camera::CameraPinhole camera;
  jblas::mat33 Pl;
  jblas::mat33 Plinv;

  void pluckerLineInit(jblas::vec const& z_, double d_, double phi_, double sg_, jblas::vec& pluckerLine) const;

  void pluckerLineInitJac(jblas::vec const& z_, double d_, double phi_, double sg_, jblas::mat& J) const;

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

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

  jblas::vec const& predictObservationInImageFrame(jblas::vec3 const& l_);
  void predictObservationInImageFrameJac(jblas::vec3 const& l_, 
            jblas::mat& Jl) const;

  boost::tuple<double,double> computeExtremitiesAbscissa(SegmentFeature 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::sym_mat p_pixCov;
  
  
  virtual void computeSensorR(Observation const& obs);
  
  // virtual void computeSensorRVariable(Observation const& obs);

  double computeBaselineInSensorFrame(jblas::vec_range const& deltaPose, Observation const& obsRef) const {return 1.0;}

  public:
  
  double depthAlpha;
  double depthBeta;

  double phiMin;
  double phiMax;

  double phiSigma;

  double phiKSigma;


  ImageEuclideanPluckerFeatureObserveModel(SegmentFeatureModel& featureModel_, 
          jafar::camera::CameraPinhole const& camera_,
          double depthAlpha_, double depthKSigma_,
          double phiMin_, double phiMax_,
          double phiSigma_,
          double phiKSigma_);

  ~ImageEuclideanPluckerFeatureObserveModel();

  Observation::ObservationType typeObs() const {return Observation::SEGMENT_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;

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

  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);
  static jblas::vec2 extToLine(jblas::vec const& ext1,jblas::vec const& ext2);
  
  static void extToLineJac(jblas::vec const& ext1,jblas::vec const& ext2, jblas::mat& J);

  friend std::string boSegFeatureInitState(InitFeature const& f_, BearingOnlySlam& slam_);

  }; // class ImageEuclideanPluckerFeatureObserveModel
  
  

    
  class ImagePluckerFeatureObserveModel : public FeatureObserveModel {
      
    protected:
      SegmentFeatureModel& segmentFeatureModel;
      
      jafar::camera::CameraPinhole camera;
      jblas::mat33 Pl;
      jblas::mat33 Plinv;
      
      jblas::vec2 beta;
      jblas::mat22 covar_beta;
      
      jblas::mat JbBeta;
      
    
      virtual jblas::vec const& homogeneousToImageModel(jblas::vec3 const& l_) = 0;
      
      virtual void homogeneousToImageModelJac(jblas::vec3 const& l_, 
          jblas::mat& Jl) const = 0;
      
      
      virtual jblas::vec3 imageModelToHomogeneous(jblas::vec const& rt_) = 0;
      
      virtual void  imageModelToHomogeneousJac(jblas::vec const& rt_, jblas::mat& HMrt) = 0;
      
      virtual void  imageModelToHomogeneousJac(jblas::vec const& rt_, jblas::vec3& l, jblas::mat& HMrt) = 0;
      
      
      jblas::vec3 planeBaseToDirVector(jblas::vec3 const& an_, jblas::vec2 const& beta_);
      
      void planeBaseToDirVectorJac(jblas::vec_range const& an_, jblas::vec2 const&  beta_, jblas::vec_range& b, jblas::mat33& B_a);
      
      
      boost::tuple<double,double> computeExtremitiesAbscissa(SegmentFeature 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::sym_mat p_pixCov;


      
    public:
        
      ImagePluckerFeatureObserveModel(SegmentFeatureModel& featureModel_, jafar::camera::CameraPinhole const& camera_);
      ~ImagePluckerFeatureObserveModel();
      
      Observation::ObservationType typeObs() const {return Observation::SEGMENT_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 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);

      
      void postInitCovariance(jblas::sym_mat_range & P, jblas::mat const& Jx);
      
      void setup(double beta0 = 0, double beta1 = 0, double sigma_beta = 0.5);
      
      
  }; // class ImagePluckerFeatureObserveModel


  
  
        class RhoThetaImagePluckerFeatureObserveModel :
        public ImagePluckerFeatureObserveModel {
    
        protected:

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

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

      
        virtual jblas::vec const& homogeneousToImageModel(jblas::vec3 const& l_);
        
        virtual void homogeneousToImageModelJac(jblas::vec3 const& l_, 
                jblas::mat& Jl) const;
        
        
        virtual jblas::vec3 imageModelToHomogeneous(jblas::vec const& rt_);
        
        virtual void  imageModelToHomogeneousJac(jblas::vec const& rt_, jblas::vec3& l, jblas::mat& HMrt);
    
        virtual void  imageModelToHomogeneousJac(jblas::vec const& rt_, jblas::mat& HMrt);
    
//        /** Retro-project Plucker line
//        */
//        virtual jblas::vec inverseObservationInSensorFrame(jblas::vec const& z_);
//
//        /** Retro-project Plucker line -- output and Jacobian
//        */
//        virtual void inverseObservationInSensorFrameJac(jblas::vec const& z_);

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

        virtual void inverseObservationInSensorFrameJac(Observation const& obs_);
    
        public:
    
        RhoThetaImagePluckerFeatureObserveModel(SegmentFeatureModel& featureModel_, jafar::camera::CameraPinhole const& camera_);
        ~RhoThetaImagePluckerFeatureObserveModel();
    
        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);
    

        }; // class RhoThetaImagePluckerFeatureObserveModel

    
    
        class ScaledHomogeneousImagePluckerFeatureObserveModel :
      public ImagePluckerFeatureObserveModel {
        
protected:
        
        jblas::vec const& predictObservationInSensorFrame( jblas::vec const& feature_);
        
        void predictObservationInSensorFrameJac( jblas::vec const& feature_);
        
        
        virtual jblas::vec const& homogeneousToImageModel(jblas::vec3 const& l_);
        
        virtual void homogeneousToImageModelJac(jblas::vec3 const& l_, 
                            jblas::mat& Jl) const;
        
        
        virtual jblas::vec3 imageModelToHomogeneous(jblas::vec const& sh_);
        
        
        virtual void  imageModelToHomogeneousJac(jblas::vec const& sh_, 
                             jblas::vec3& l, 
                             jblas::mat& HM_sh);
        
        
        virtual void  imageModelToHomogeneousJac(jblas::vec const& sh_,
                             jblas::mat& HM_sh);
        
        
//        /** Retro-project Plucker line
//          */
//        virtual jblas::vec inverseObservationInSensorFrame(jblas::vec const& z_);
//        
//        
//        /** Retro-project Plucker line -- output and Jacobian
//          */
//        virtual void inverseObservationInSensorFrameJac(jblas::vec const& z_);

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


                virtual void inverseObservationInSensorFrameJac(Observation const& obs_);
        
public:
          
        ScaledHomogeneousImagePluckerFeatureObserveModel(SegmentFeatureModel& featureModel_, jafar::camera::CameraPinhole const& camera_);
        
        ~ScaledHomogeneousImagePluckerFeatureObserveModel();
        
        virtual jblas::vec const& computeInnovation(jblas::vec const& z_, 
                              jblas::vec const& zPred_);
        
        virtual void computeSensorR(Observation const& obs);
        

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

        static void extToImageModelJac(jblas::vec const& ext1,
                         jblas::vec const& ext2, 
                         jblas::mat& H_e);
        
        static void extToImageModelJac(jblas::vec const& ext1,
                         jblas::vec const& ext2,
                         jblas::vec3 const& ref,
                         jblas::mat& HM_e);
        
        
        
      }; // class HomogeneousImagePluckerFeatureObserveModel
    

    class StereoImagePluckerFeatureObserveModel : public FeatureObserveModel {

    protected:

            SegmentFeatureModel& segmentFeatureModel;
            // stereo bench 
            jafar::camera::StereoBench stereoBench;
            // Plucker camera matrix
            jblas::mat33 Pl;
            // Plucker camera inverse matrix
            jblas::mat33 Plinv;

            jblas::vec3 planeBaseToDirVector(jblas::vec3 const& an_, jblas::vec2 const& beta_);
            
            void planeBaseToDirVectorJac(jblas::vec_range const& an_, jblas::vec2 const&  beta_, jblas::vec_range& b, jblas::mat33& B_a);
            
            
            boost::tuple<double,double> computeExtremitiesAbscissa(SegmentFeature const& segFeature_, 
                    geom::T3DEuler const& robotPose_,
                    StereoSegmentObservation const& obs_) const;
            
            void segmentExtremitiesPluckerLines(StereoSegmentObservation 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::sym_mat p_pixCov;
      
            double p_dispVar;
      
            jblas::vec const& predictObservationInSensorFrame(const jblas::vec& feature_);

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

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

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

             void inverseObservationInSensorFrameJac(Observation const& obs_);

    public: 
            StereoImagePluckerFeatureObserveModel(SegmentFeatureModel& featureModel_, jafar::camera::StereoBench const& stereoBench_);

            ~StereoImagePluckerFeatureObserveModel();

            Observation::ObservationType typeObs() const {return Observation::SEGMENT_STEREOIMAGE;}

            void setPixCov(jblas::vec const& pixCov_, double stabilizingFactor = 1.0);
      
      void setDispVar(double dispVar, 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 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);

            jblas::vec const& computeInnovation(jblas::vec const& z_, 
                                                            jblas::vec const& zPred_);
                
            void computeSensorR(Observation const& obs);
            
            jblas::vec2  extToRhoTheta(jblas::vec const& ext1,jblas::vec const& ext2);
      
      
            void extToRhoThetaJac(jblas::vec const& ext1,jblas::vec const& ext2, jblas::mat& J);


    }; // class StereoImagePointFeatureObserveModel
  
    
} // namespace slam
} // namespace jafar


#endif // SLAM_SEGMENT_FEATURE_HPP