doorextractor.h

00001 /*
00002  * Orca-Robotics Project: Components for robotics 
00003  *               http://orca-robotics.sf.net/
00004  * Copyright (c) 2004-2009 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 <hydroscanutil/scanutil.h>
00016 #include <hydroutil/context.h>
00017 
00018 namespace hydrolfextract {
00019 
00020 class DoorJamb {
00021 public:
00022     DoorJamb( const hydroscanutil::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 hydroscanutil::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         hydroscanutil::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<float>    &ranges,
00095                           const std::vector<DoorJamb> &possibleStarts,
00096                           const std::vector<DoorJamb> &possibleEnds,
00097                           std::vector<Door>           &actualDoors ) const;
00098 
00099     double minDoorWidthSq_;
00100     double maxDoorWidthSq_;
00101 
00102     Config             config_;
00103     hydroutil::Context context_;
00104 };
00105 
00109 class DoorFeatureExtractor {
00110 public:
00111 
00112     DoorFeatureExtractor( DoorExtractor::Config cfg,
00113                           double rangeSd,
00114                           double bearingSd,
00115                           double pFalsePositive,
00116                           double pTruePositive )
00117         : config_(cfg),
00118           rangeSd_(rangeSd),
00119           bearingSd_(bearingSd),
00120           pFalsePositive_(pFalsePositive),
00121           pTruePositive_(pTruePositive)
00122         {
00123             assert( pTruePositive_ > pFalsePositive_ ); 
00124             assert( rangeSd_ > 0 );
00125             assert( bearingSd_ > 0 );            
00126         }
00127 
00128     std::vector<hydrofeatureobs::PointFeatureObs*> extractFeatures( const std::vector<float> &ranges,
00129                                                                     const std::vector<Door>  &doors ) const;
00130 
00131 private:
00132 
00133     DoorExtractor::Config config_;
00134     double rangeSd_;
00135     double bearingSd_;
00136     double pFalsePositive_;
00137     double pTruePositive_;
00138 
00139 };
00140 
00141 }
00142 
00143 #endif