foregroundextractor.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_FOREGROUND_EXTRACTOR_H
00011 #define HYDROLFEXTRACT_FOREGROUND_EXTRACTOR_H
00012 
00013 #include <vector>
00014 #include <hydrofeatureobs/features.h>
00015 #include <hydroscanutil/scanutil.h>
00016 #include <hydrolaserfeatureextract/polefinder.h>
00017 
00018 namespace hydrolfextract {
00019 
00020 struct AnnotatedPole {
00021     AnnotatedPole( const Pole &p, double pDistToClutter )
00022         : pole(p), distToClutter(pDistToClutter)
00023         {}
00024 
00025     Pole   pole;
00026     double distToClutter;
00027 };
00028 std::string toString( const AnnotatedPole &p );
00029 
00030 class ForegroundExtractor
00031 {
00032 
00033 public: 
00034 
00035     struct Config {
00036         Config( const hydroscanutil::ScannerConfig &scannerCfg )
00037             : scannerConfig(scannerCfg),
00038               minForegroundWidth(0.1),
00039               maxForegroundWidth(0.5),
00040               minForegroundBackgroundSeparation(1.0),
00041               clearForegroundBackgroundSeparation(4.0),
00042               reliableScannerRange(5.0)
00043             {}
00044 
00045         hydroscanutil::ScannerConfig scannerConfig;
00046         double minForegroundWidth;
00047         double maxForegroundWidth;
00048         double minForegroundBackgroundSeparation;
00049         double clearForegroundBackgroundSeparation;
00050         double reliableScannerRange;
00051 
00052         bool isSane() const;
00053     };
00054 
00055     ForegroundExtractor( const Config &config )
00056         : config_(config)
00057         { assert( config.isSane() ); }
00058 
00059     std::vector<AnnotatedPole> extractForegroundPoints( const std::vector<float> &ranges,
00060                                                         bool debug=false ) const;
00061 
00062     const Config &config() const { return config_; }
00063     
00064 private: 
00065 
00066     mutable std::vector<Pole> possiblePoles_;
00067 
00068     Config config_;
00069 };
00070 std::string toString( const ForegroundExtractor::Config &config );
00071 
00072 class ForegroundFeatureExtractor
00073 {
00074 
00075 public: 
00076 
00077     ForegroundFeatureExtractor( const ForegroundExtractor::Config &config,
00078                                 double rangeSd,
00079                                 double bearingSd,
00080                                 double pFalsePositive,
00081                                 double pTruePositive )
00082         : config_(config),
00083           rangeSd_(rangeSd),
00084           bearingSd_(bearingSd),
00085           pFalsePositive_(pFalsePositive),
00086           pTruePositive_(pTruePositive)
00087         {}
00088 
00089     std::vector<hydrofeatureobs::PointFeatureObs*> extractFeatures( const std::vector<float>          &ranges,
00090                                                                     const std::vector<AnnotatedPole>  &fgPoints ) const;
00091 
00092 private: 
00093 
00094     ForegroundExtractor::Config config_;
00095 
00096     double rangeSd_;
00097     double bearingSd_;
00098     double pFalsePositive_;
00099     double pTruePositive_;
00100 };
00101 
00102 //
00103 // A collection of dodgy hacks for determining pTruePositive/pFalsePositive.
00104 //
00105 void
00106 calcForegroundPointReliability( const std::vector<float>          &ranges,
00107                                 const AnnotatedPole               &fgPoint,
00108                                 const ForegroundExtractor::Config &config,
00109                                 double                             basePTruePositive,
00110                                 double                             basePFalsePositive,
00111                                 double                            &pTruePositive,
00112                                 double                            &pFalsePositive );
00113 
00114 } // namespace
00115 
00116 #endif