Orca Robotics

00001 /*
00002  * Orca-Robotics Project: Components for robotics 
00003  *               http://orca-robotics.sf.net/
00004  * Copyright (c) 2004-2008 Alex Brooks
00005  *
00006  * This copy of Orca is licensed to you under the terms described in
00007  * the LICENSE file included in this distribution.
00008  *
00009  */
00010 #ifndef HYDROLFEXTRACT_DOOREXTRACTOR__H
00011 #define HYDROLFEXTRACT_DOOREXTRACTOR__H
00012 
00013 #include <vector>
00014 #include <hydrofeatureobs/features.h>
00015 #include <hydrolaserfeatureextract/scanutil.h>
00016 #include <hydroutil/context.h>
00017 
00018 namespace hydrolfextract {
00019 
00020 class DoorJamb {
00021 public:
00022     DoorJamb( const ScannerConfig      &scannerConfig,
00023               const std::vector<float> &ranges,
00024               int                       pReturnI );
00025 
00026     int returnI;
00027     double range;
00028     double bearing;
00029     double x;
00030     double y;
00031 };
00032 inline std::ostream &operator<<( std::ostream &s, const DoorJamb &d )
00033 { return s << "[i,r,b = "<<d.returnI<<", "<<d.range<<", "<<d.bearing*180.0/M_PI<<"deg]"; }
00034 
00035 class Door {
00036 public:
00037     Door( const DoorJamb &doorStart, const DoorJamb &doorEnd )
00038         : start(doorStart),
00039           end(doorEnd)
00040         {}
00041 
00042     DoorJamb start;
00043     DoorJamb end;
00044 };
00045 std::string toString( const Door &door );
00046 inline std::ostream &operator<<( std::ostream &s, const Door &d )
00047 { return s << toString(d); }
00048 
00052 class DoorExtractor
00053 {
00054 
00055 public: 
00056 
00057     struct Config {
00058         
00059         Config( const ScannerConfig &scannerCfg )
00060             : scannerConfig(scannerCfg),
00061               minDoorWidth(0.9),
00062               maxDoorWidth(1.2),
00063               minDeltaRangeThruDoor(2.0),
00064               minReturnsThruDoor(3),
00065               minReturnsBesideDoor(2),
00066               maxDeltaRangeWallBesideDoor(0.3)
00067             {}
00068 
00069         ScannerConfig scannerConfig;
00070         double minDoorWidth;
00071         double maxDoorWidth;
00072         double minDeltaRangeThruDoor;
00073         int    minReturnsThruDoor;
00074         int    minReturnsBesideDoor;
00075         double maxDeltaRangeWallBesideDoor;        
00076 
00077         bool isSane() const;
00078     };
00079 
00081 
00082     DoorExtractor( const Config &c, const hydroutil::Context &context );
00083 
00084     std::vector<Door> extractDoors( const std::vector<float> &ranges ) const;
00085     
00086     const Config &config() const { return config_; }
00087 
00088 private: 
00089 
00090     void findPossibleDoorJambs( const std::vector<float> &ranges,
00091                                 std::vector<DoorJamb>    &possibleStarts,
00092                                 std::vector<DoorJamb>    &possibleEnds ) const;
00093 
00094     void findActualDoors( const std::vector<DoorJamb> &possibleStarts,
00095                           const std::vector<DoorJamb> &possibleEnds,
00096                           std::vector<Door>           &actualDoors ) const;
00097 
00098     double minDoorWidthSq_;
00099     double maxDoorWidthSq_;
00100 
00101     Config             config_;
00102     hydroutil::Context context_;
00103 };
00104 
00108 class DoorFeatureExtractor {
00109 public:
00110 
00111     DoorFeatureExtractor( DoorExtractor::Config cfg,
00112                           double rangeSd,
00113                           double bearingSd,
00114                           double pFalsePositive,
00115                           double pTruePositive )
00116         : config_(cfg),
00117           rangeSd_(rangeSd),
00118           bearingSd_(bearingSd),
00119           pFalsePositive_(pFalsePositive),
00120           pTruePositive_(pTruePositive)
00121         {
00122             assert( pTruePositive_ > pFalsePositive_ ); 
00123             assert( rangeSd_ > 0 );
00124             assert( bearingSd_ > 0 );            
00125         }
00126 
00127     std::vector<hydrofeatureobs::PointFeatureObs*> extractFeatures( const std::vector<float> &ranges,
00128                                                                     const std::vector<Door>  &doors ) const;
00129 
00130 private:
00131 
00132     DoorExtractor::Config config_;
00133     double rangeSd_;
00134     double bearingSd_;
00135     double pFalsePositive_;
00136     double pTruePositive_;
00137 
00138 };
00139 
00140 }
00141 
00142 #endif
doorextractor.h
