sensor.hpp

/* $Id$ */

#ifndef SIMU_SENSOR_HPP
#define SIMU_SENSOR_HPP

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

#include "camera/cameraPinhole.hpp"
#include "camera/cameraBarreto.hpp"

#include "slam/feature.hpp"
#include "slam/segmentFeature.hpp"
#include "slam/observation.hpp"
#include "simu/environment.hpp"

namespace jafar {
  namespace slam {
    class SegmentObservationsSelector;
  }
  namespace simu {

    class Sensor {

     public:

      jblas::vec z;
      jblas::vec noiseCov;

      Sensor(const jblas::vec& noiseCov_,
       bool doAddNoise = false, unsigned int randomSeed = 1,
       double noiseFactor = 1.0, unsigned int sensorId_ = 0) :
  z(noiseCov_.size()),
  noiseCov(noiseCov_),
  p_doAddNoise(doAddNoise),
  noise(jblas::zero_vec(noiseCov_.size()), noiseFactor*noiseFactor*noiseCov_, randomSeed),
  sensorId(sensorId_)
      {}

      Sensor(const jblas::vec& noiseCov_, std::size_t zSize,
         bool doAddNoise = false, unsigned int randomSeed = 1,
         double noiseFactor = 1.0, unsigned int sensorId_ = 0) :
    z(zSize),
    noiseCov(noiseCov_),
    p_doAddNoise(doAddNoise),
    noise(jblas::zero_vec(noiseCov_.size()), noiseFactor*noiseFactor*noiseCov_, randomSeed),
    sensorId(sensorId_)
      {}


      virtual ~Sensor() {}
   public:
      jblas::vec noiseStdDev() const;

    protected:

      bool p_doAddNoise;

      jafar::jmath::MultiDimNormalDistribution noise;
      unsigned int sensorId;
    };
    
    class SensorRobot : public Sensor {
      public:
      double range;
      double apertureH;
      double apertureV;
      const Robot& robot;
        SensorRobot( Robot& _robot, double range_, double apertureH_, double apertureV_, const jblas::vec& noiseCov_,
       bool doAddNoise = false, unsigned int randomSeed = 1,
       double noiseFactor = 1.0) : Sensor(noiseCov_, doAddNoise, randomSeed, noiseFactor),
        range(range_), apertureH(apertureH_), apertureV(apertureV_), robot(_robot)
      {}
      bool isInRange( const Robot& _robot2 ) const;
      jafar::slam::Observation* observe( const Robot& _robot2);
    };

    class SensorPoint : public Sensor {

    public:

      SensorPoint(const jblas::vec& noiseCov_,
      bool doAddNoise = false, unsigned int randomSeed = 1,
      double noiseFactor = 1.0, unsigned int sensorId_ = 0) :
  Sensor(noiseCov_, doAddNoise, randomSeed, noiseFactor, sensorId_)
      {}

      virtual ~SensorPoint() {}

      virtual bool isInRange( const jblas::vec& sensor_, const Point& feature_) const = 0;

      virtual jafar::slam::Observation* observe(const jblas::vec& sensor_,
            const Point& feature_, unsigned int _robotId) = 0;
    };

//     std::ostream& operator <<(std::ostream& s, const jafar::simu::SensorPoint& sensor_);

    class SensorPointSimple : public SensorPoint {

    public:

      double range;
      double apertureH;
      double apertureV;

      SensorPointSimple(double range_, double apertureH_, double apertureV_, const jblas::vec& noiseCov_,
      bool doAddNoise = false, unsigned int randomSeed = 1,
      double noiseFactor = 1.0, unsigned int sensorId_ = 0) : 
  SensorPoint(noiseCov_, doAddNoise, randomSeed, noiseFactor, sensorId_),
  range(range_), apertureH(apertureH_), apertureV(apertureV_)
      {}

      ~SensorPointSimple() {}

      double getApertureH() const {return apertureH;}

      double getApertureV() const {return apertureV;}

      bool isInRange(const jblas::vec& sensor_, const Point& feature_) const;

    };

    class SensorPointCartesian : public SensorPointSimple {
      
    public :

      SensorPointCartesian(double dRange_, double thetaRange_, double phiRange_, const jblas::vec& noiseCov_,
         bool doAddNoise = false, unsigned int randomSeed = 1, double noiseFactor = 1.0, unsigned int sensorId_ = 0);
      ~SensorPointCartesian();

      jafar::slam::Observation* observe(const jblas::vec& sensor_, 
          const Point& feature_, unsigned int _robotId);
    };

    class SensorPointPolar : public SensorPointSimple {
      
    public :

      SensorPointPolar(double dRange_, double thetaRange_,double phiRange_, const jblas::vec& noiseCov_,
           bool doAddNoise = false, unsigned int randomSeed = 1,
           double noiseFactor = 1.0, unsigned int sensorId_ = 0);
      ~SensorPointPolar();

      jafar::slam::Observation* observe(const jblas::vec& sensor_,
          const Point& feature_, unsigned int _robotId);
    };

    class SensorPointBearing : public SensorPointSimple {
      
    public :

      SensorPointBearing(double dRange_, double thetaRange_,double phiRange_, const jblas::vec& noiseCov_,
       bool doAddNoise = false, unsigned int randomSeed = 1,
       double noiseFactor = 1.0, unsigned int sensorId_ = 0);
      ~SensorPointBearing();

      jafar::slam::Observation* observe(const jblas::vec& sensor_,
          const Point& feature_, unsigned int _robotId);

      friend std::ostream& operator <<(std::ostream& s, const SensorPointSimple& sensor_);
    };

    std::ostream& operator <<(std::ostream& s, const SensorPointSimple& sensor_);

    class SensorPointImage : public SensorPoint {
      
    public :

      jafar::camera::CameraPinhole camera;
      double range;

      SensorPointImage(jafar::camera::CameraPinhole const& camera_,
           double dRange_,
           const jblas::vec& noiseCov_,
           bool doAddNoise = false, unsigned int randomSeed = 1,
           double noiseFactor = 1.0, unsigned int sensorId_ = 0);
      
      ~SensorPointImage();

      double getApertureH() const {return camera.getApertureU();}

      double getApertureV() const {return camera.getApertureV();}

      bool isInRange(const jblas::vec& sensor_, const Point& feature_) const;

      jafar::slam::Observation* observe(const jblas::vec& sensor_,
          const Point& feature_, unsigned int _robotId);

    protected :

      friend std::ostream& operator <<(std::ostream& s, const SensorPointImage& sensor_);

    };

    std::ostream& operator <<(std::ostream& s, const SensorPointImage& sensor_);


    class SensorPointOmniImage : public SensorPoint {
      
    public :

      jafar::camera::CameraParabolicBarreto camera;
      double range;

      SensorPointOmniImage(jafar::camera::CameraParabolicBarreto const& camera_,
         double dRange_,
         const jblas::vec& noiseCov_,
         bool doAddNoise = false, unsigned int randomSeed = 1,
         double noiseFactor = 1.0, unsigned int sensorId_ = 0);
      
      ~SensorPointOmniImage();

      bool isInRange(const jblas::vec& sensor_, const Point& feature_) const;

      jafar::slam::Observation* observe(const jblas::vec& sensor_,
          const Point& feature_, unsigned int _robotId);

    protected :

      friend std::ostream& operator <<(std::ostream& s, const SensorPointOmniImage& sensor_);

    };

    std::ostream& operator <<(std::ostream& s, const SensorPointOmniImage& sensor_);

    class SensorSegmentImage : public Sensor {

    public :

      double minSegmentSize;

      jafar::camera::CameraPinhole camera;
      double range;

      slam::Observation::ObservationType type;

      SensorSegmentImage(jafar::camera::CameraPinhole const& camera_,
       double dRange_,
       jblas::vec const& noiseCov_,
       double minSegmentSize_,
       bool doAddNoise = false, unsigned int randomSeed = 1,
       double noiseFactor = 1.0, unsigned int sensorId_ = 0, 
             slam::Observation::ObservationType type_ = slam::Observation::SEGMENT_IMAGE);

      SensorSegmentImage(jafar::camera::CameraPinhole const& camera_,
                       double dRange_,
                       jblas::vec const& noiseCov_,
                       std::size_t zSize_,
                       double minSegmentSize_,
                       bool doAddNoise=false, unsigned int randomSeed = 1,
                       double noiseFactor = 1.0, unsigned int sensorId_ = 0, 
             slam::Observation::ObservationType type_ = slam::Observation::SEGMENT_IMAGE);

      virtual ~SensorSegmentImage();

//       /// @return horizontal aperture in radian
//       double getApertureU() const {return camera.getApertureU();}

//       /// @return vertical aperture in radian
//       double getApertureV() const {return camera.getApertureV();}

      virtual bool isInRange(jblas::vec const& sensor_, Segment const& seg_) const;   

//       bool isPixelInImage(jblas::vec2 const& pix_) const;

      virtual jafar::slam::Observation* observe(jblas::vec const& sensor_,
          Segment const& seg_, unsigned int _robotId);

          void setSegmentObservationsSelector(slam::SegmentObservationsSelector* _selector);
    protected :
      jafar::slam::SegmentObservationsSelector* m_obsevationsSelector;

      virtual bool findVisibleExtremities(jblas::vec3 const& ext1_, jblas::vec3 const& ext2_,
          jblas::vec2& pix1, jblas::vec2& pix2,
          double step = 0.1) const;


      friend std::ostream& operator <<(std::ostream& s, const SensorSegmentImage& sensor_);

    };

    std::ostream& operator <<(std::ostream& s, const jafar::simu::SensorSegmentImage& sensor_);

    class SensorSegmentStereoImage : public SensorSegmentImage {

    public :

      jafar::camera::StereoBench stereoBench;

      jafar::jmath::NormalDistribution dispNoise;
 
      SensorSegmentStereoImage(jafar::camera::StereoBench const& stereoBench_,
             double dRange_,
             jblas::vec const& noiseCov_,
             double const& noiseDisp_,
             double minSegmentSize_,
             bool doAddNoise = false, unsigned int randomSeed = 1,
             double noiseFactor = 1.0, unsigned int sensorId_ = 0);

      ~SensorSegmentStereoImage();

      bool isInRange(jblas::vec const& sensor_, Segment const& seg_) const;   

      jafar::slam::Observation* observe(jblas::vec const& sensor_,
                    Segment const& seg_, unsigned int _robotId);

    protected :

      bool findVisibleExtremities(jblas::vec3 const& ext1_, jblas::vec3 const& ext2_,
                  jblas::vec2& pix1, jblas::vec2& pix2, double& disp1, double& disp2,
                  double step = 0.1) const;



    friend std::ostream& operator <<(std::ostream& s, const SensorSegmentStereoImage& sensor_);

    };

    std::ostream& operator <<(std::ostream& s, const jafar::simu::SensorSegmentImage& sensor_);

  }
}


#endif // SIMU_SENSOR_HPP