Orca Robotics

00001 #ifndef LASERFEATURES_FEATURES_H
00002 #define LASERFEATURES_FEATURES_H
00003 
00004 #include <hydrogeom2d/geom2d.h>
00005 #include <assert.h>
00006 
00007 namespace hydrofeatureobs {
00008 
00009 // Define a few feature types.  These are defined as int's rather than enum, so
00010 // users can define their own.
00011 
00013 const int LASERREFLECTOR   = 0;
00015 const int FOREGROUNDPOINT  = 1;
00017 const int DOOR             = 2;
00019 const int CORNER           = 3;
00021 const int LINE             = 4;
00023 const int INTERNALCORNER   = 5;
00025 const int EXTERNALCORNER   = 6;
00027 const int VEHICLEPOSE      = 7;
00029 const int GPSFIX           = 8;
00030 
00031 std::string featureTypeToString( int featureType );
00032 
00038 class FeatureObs
00039 {
00040 public:
00041     virtual ~FeatureObs() {}
00042 
00043     virtual std::string toString()  const=0;
00044 
00045     virtual int    featureType()    const=0;
00046     virtual double pFalsePositive() const=0;
00047     virtual double pTruePositive()  const=0;
00048 
00049     // Allow sanity checks for debugging
00050     virtual bool isSane() const=0;
00051 };
00052 inline std::ostream &operator<<( std::ostream &s, const FeatureObs &o )
00053 { return s << o.toString(); }
00054 
00056 bool isSane( double pFalsePositive, double pTruePositive );
00057 
00063 class PointFeatureObs : public virtual FeatureObs
00064 {
00065 public:
00066     PointFeatureObs( int featureType,
00067                      double pFalsePositive,
00068                      double pTruePositive,
00069                      double range,
00070                      double bearing,
00071                      double rangeSd,
00072                      double bearingSd )
00073         : featureType_(featureType),
00074           pFalsePositive_(pFalsePositive),
00075           pTruePositive_(pTruePositive),
00076           range_(range),
00077           bearing_(bearing),
00078           rangeSd_(rangeSd),
00079           bearingSd_(bearingSd)
00080         { assert(rangeSd>=0 && bearingSd>=0); }
00081 
00082     // const access
00083     double range()   const { return range_; }
00084     double bearing() const { return bearing_; }
00085     double rangeSd()    const { return rangeSd_; }
00086     double bearingSd()  const { return bearingSd_; }
00087 
00088     std::string toString() const;
00089 
00090     int    featureType()    const { return featureType_; }
00091     double pFalsePositive() const { return pFalsePositive_; }
00092     double pTruePositive()  const { return pTruePositive_; }
00093 
00094     bool isSane() const;
00095 
00096 private:
00097     int   featureType_;
00098     double pFalsePositive_;
00099     double pTruePositive_;
00100 
00101     double range_;
00102     double bearing_;
00103     double rangeSd_;
00104     double bearingSd_;
00105 };
00106 
00112 class PoseFeatureObs : public virtual FeatureObs
00113 {
00114 public:
00115     PoseFeatureObs( int featureType,
00116                     double pFalsePositive,
00117                     double pTruePositive,
00118                     double range,
00119                     double bearing,
00120                     double orientation,
00121                     double rangeSd,
00122                     double bearingSd,
00123                     double orientationSd )
00124         : featureType_(featureType),
00125           pFalsePositive_(pFalsePositive),
00126           pTruePositive_(pTruePositive),
00127           range_(range),
00128           bearing_(bearing),
00129           orientation_(orientation),
00130           rangeSd_(rangeSd),
00131           bearingSd_(bearingSd),
00132           orientationSd_(orientationSd)
00133         { assert(rangeSd>=0 && bearingSd>=0); }
00134 
00135     // const access
00136     double range()       const { return range_; }
00137     double bearing()     const { return bearing_; }
00138     double orientation() const { return orientation_; }
00139     double rangeSd()        const { return rangeSd_; }
00140     double bearingSd()      const { return bearingSd_; }
00141     double orientationSd() const { return orientationSd_; }
00142 
00143     std::string toString() const;
00144 
00145     int    featureType()    const { return featureType_; }
00146     double pFalsePositive() const { return pFalsePositive_; }
00147     double pTruePositive()  const { return pTruePositive_; }
00148 
00149     bool isSane() const;
00150 
00151 private:
00152     int   featureType_;
00153     double pFalsePositive_;
00154     double pTruePositive_;
00155 
00156     double range_;
00157     double bearing_;
00158     double orientation_;
00159     double rangeSd_;
00160     double bearingSd_;
00161     double orientationSd_;
00162 };
00163 
00176 class LineFeatureObs : public virtual FeatureObs
00177 {
00178 public:
00179     LineFeatureObs( int featureType, double pFalsePositive, double pTruePositive,
00180                     double rangeStart, double bearingStart,
00181                     double rangeEnd, double bearingEnd,
00182                     bool startSighted, bool endSighted,
00183                     double rhoSd, double alphaSd );
00184 
00185 //     const geom2d::PolarPoint &start() const { return start_; }
00186 //     const geom2d::PolarPoint &end()   const { return end_; }
00187 
00188     double rangeStart()   const { return start_.range(); }
00189     double bearingStart() const { return start_.bearing(); }
00190     double rangeEnd()     const { return end_.range(); }
00191     double bearingEnd()   const { return end_.bearing(); }
00192 
00193     bool startSighted() const { return startSighted_; }
00194     bool endSighted() const { return endSighted_; }
00195 
00196     // polar observation of infinite line
00197     double rho()   const { return rho_; }
00198     double alpha() const { return alpha_; }
00199 
00200     // uncertainty
00201     double rhoSd()   const { return rhoSd_; }
00202     double alphaSd() const { return alphaSd_; }
00203 
00204     std::string toString() const;
00205 
00206     virtual int    featureType()    const { return featureType_; }
00207     virtual double pFalsePositive() const { return pFalsePositive_; }
00208     virtual double pTruePositive()  const { return pTruePositive_; }
00209 
00210     bool isSane() const;
00211 
00212 private:
00213 
00214     int   featureType_;
00215     double pFalsePositive_;
00216     double pTruePositive_;
00217 
00218     geom2d::PolarPoint start_, end_;
00219 
00220     bool startSighted_;
00221     bool endSighted_;
00222 
00223     // obs of infinite line
00224     double rho_, alpha_;
00225     double rhoSd_, alphaSd_;
00226 };
00227 
00228 }
00229 
00230 #endif
features.h
