orca-robotics
INTRODUCTION
Overview
Download and Install
Documentation
REPOSITORY
Interfaces
Drivers
Libraries
Utilities
Software Map
DEVELOPER
Dashboard
PEOPLE
Contributors
Users
Project
Download
Mailing lists
features.h
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; 00028 00029 std::string featureTypeToString( int featureType ); 00030 00036 class FeatureObs 00037 { 00038 public: 00039 virtual ~FeatureObs() {} 00040 00041 virtual std::string toString() const=0; 00042 00043 virtual int featureType() const=0; 00044 virtual double pFalsePositive() const=0; 00045 virtual double pTruePositive() const=0; 00046 00047 // Allow sanity checks for debugging 00048 virtual bool isSane() const=0; 00049 }; 00050 inline std::ostream &operator<<( std::ostream &s, const FeatureObs &o ) 00051 { return s << o.toString(); } 00052 00054 bool isSane( double pFalsePositive, double pTruePositive ); 00055 00061 class PointFeatureObs : public virtual FeatureObs 00062 { 00063 public: 00064 PointFeatureObs( int featureType, 00065 double pFalsePositive, 00066 double pTruePositive, 00067 double range, 00068 double bearing, 00069 double rangeSd, 00070 double bearingSd ) 00071 : featureType_(featureType), 00072 pFalsePositive_(pFalsePositive), 00073 pTruePositive_(pTruePositive), 00074 range_(range), 00075 bearing_(bearing), 00076 rangeSd_(rangeSd), 00077 bearingSd_(bearingSd) 00078 { assert(rangeSd>=0 && bearingSd>=0); } 00079 00080 // const access 00081 double range() const { return range_; } 00082 double bearing() const { return bearing_; } 00083 double rangeSd() const { return rangeSd_; } 00084 double bearingSd() const { return bearingSd_; } 00085 00086 std::string toString() const; 00087 00088 int featureType() const { return featureType_; } 00089 double pFalsePositive() const { return pFalsePositive_; } 00090 double pTruePositive() const { return pTruePositive_; } 00091 00092 bool isSane() const; 00093 00094 private: 00095 int featureType_; 00096 double pFalsePositive_; 00097 double pTruePositive_; 00098 00099 double range_; 00100 double bearing_; 00101 double rangeSd_; 00102 double bearingSd_; 00103 }; 00104 00110 class PoseFeatureObs : public virtual FeatureObs 00111 { 00112 public: 00113 PoseFeatureObs( int featureType, 00114 double pFalsePositive, 00115 double pTruePositive, 00116 double range, 00117 double bearing, 00118 double orientation, 00119 double rangeSd, 00120 double bearingSd, 00121 double orientationSd ) 00122 : featureType_(featureType), 00123 pFalsePositive_(pFalsePositive), 00124 pTruePositive_(pTruePositive), 00125 range_(range), 00126 bearing_(bearing), 00127 orientation_(orientation), 00128 rangeSd_(rangeSd), 00129 bearingSd_(bearingSd), 00130 orientationSd_(orientationSd) 00131 { assert(rangeSd>=0 && bearingSd>=0); } 00132 00133 // const access 00134 double range() const { return range_; } 00135 double bearing() const { return bearing_; } 00136 double orientation() const { return orientation_; } 00137 double rangeSd() const { return rangeSd_; } 00138 double bearingSd() const { return bearingSd_; } 00139 double orientationSd() const { return orientationSd_; } 00140 00141 std::string toString() const; 00142 00143 int featureType() const { return featureType_; } 00144 double pFalsePositive() const { return pFalsePositive_; } 00145 double pTruePositive() const { return pTruePositive_; } 00146 00147 bool isSane() const; 00148 00149 private: 00150 int featureType_; 00151 double pFalsePositive_; 00152 double pTruePositive_; 00153 00154 double range_; 00155 double bearing_; 00156 double orientation_; 00157 double rangeSd_; 00158 double bearingSd_; 00159 double orientationSd_; 00160 }; 00161 00174 class LineFeatureObs : public virtual FeatureObs 00175 { 00176 public: 00177 LineFeatureObs( int featureType, double pFalsePositive, double pTruePositive, 00178 double rangeStart, double bearingStart, 00179 double rangeEnd, double bearingEnd, 00180 bool startSighted, bool endSighted, 00181 double rhoSd, double alphaSd ); 00182 00183 // const geom2d::PolarPoint &start() const { return start_; } 00184 // const geom2d::PolarPoint &end() const { return end_; } 00185 00186 double rangeStart() const { return start_.range(); } 00187 double bearingStart() const { return start_.bearing(); } 00188 double rangeEnd() const { return end_.range(); } 00189 double bearingEnd() const { return end_.bearing(); } 00190 00191 bool startSighted() const { return startSighted_; } 00192 bool endSighted() const { return endSighted_; } 00193 00194 // polar observation of infinite line 00195 double rho() const { return rho_; } 00196 double alpha() const { return alpha_; } 00197 00198 // uncertainty 00199 double rhoSd() const { return rhoSd_; } 00200 double alphaSd() const { return alphaSd_; } 00201 00202 std::string toString() const; 00203 00204 virtual int featureType() const { return featureType_; } 00205 virtual double pFalsePositive() const { return pFalsePositive_; } 00206 virtual double pTruePositive() const { return pTruePositive_; } 00207 00208 bool isSane() const; 00209 00210 private: 00211 00212 int featureType_; 00213 double pFalsePositive_; 00214 double pTruePositive_; 00215 00216 geom2d::PolarPoint start_, end_; 00217 00218 bool startSighted_; 00219 bool endSighted_; 00220 00221 // obs of infinite line 00222 double rho_, alpha_; 00223 double rhoSd_, alphaSd_; 00224 }; 00225 00226 } 00227 00228 #endif
