Orca Robotics

00001 /*
00002  * Orca-Robotics Project: Components for robotics 
00003  *               http://orca-robotics.sf.net/
00004  * Copyright (c) 2004-2008 Alex Brooks, Alexei Makarenko, Tobias Kaupp,
00005  *               George Mathews, Stef Williams.
00006  *
00007  * This copy of Orca is licensed to you under the terms described in
00008  * the LICENSE file included in this distribution.
00009  *
00010  */
00011 #ifndef HYDROLFEXTRACT_REFLECTOREXTRACTOR__H
00012 #define HYDROLFEXTRACT_REFLECTOREXTRACTOR__H
00013 
00014 #include <vector>
00015 #include <hydrofeatureobs/features.h>
00016 #include <hydrolaserfeatureextract/scanutil.h>
00017 
00018 namespace hydrolfextract {
00019 
00020 class Reflector {
00021 public:
00022     int startI;
00023     int endI;
00024     int numReturns() const
00025         { return endI-startI+1; }
00026 };
00027 std::string toString( const Reflector &reflector );
00028 inline std::ostream &operator<<( std::ostream &s, const Reflector &r )
00029 { return s << toString(r); }
00030 
00034 class ReflectorExtractor
00035 {
00036 
00037 public: 
00038 
00039     struct Config {
00040         // Reasonable defaults
00041         Config( const ScannerConfig &scannerCfg )
00042             : scannerConfig(scannerCfg),
00043               maxDeltaRangeNearReflector(0.3),
00044               maxDeltaRangeWithinReflector(0.2),
00045               minReflectorBrightness(1)
00046             {}
00047 
00048         ScannerConfig scannerConfig;
00049         double maxDeltaRangeNearReflector;
00050         double maxDeltaRangeWithinReflector;
00051         int    minReflectorBrightness;
00052 
00053         bool isSane() const;
00054     };
00055 
00057 
00058     ReflectorExtractor( const Config &config )
00059         : config_(config)
00060         { assert( config.isSane() ); }
00061 
00062     std::vector<Reflector> extractReflectors( const std::vector<float>         &ranges,
00063                                               const std::vector<unsigned char> &intensities ) const;
00064 
00065     const Config &config() const { return config_; }
00066 
00067 private: 
00068 
00069     // 
00070     // Helper functions.  These assume that you're not asking for
00071     // info about the first or last return
00072     //
00073 
00074     // Returns the change in range between this return and the previous one.
00075     double deltaRange( const std::vector<float> &ranges,
00076                        int returnNum ) const
00077         { 
00078             assert( returnNum != 0 );
00079             return ranges[returnNum] - ranges[returnNum-1]; 
00080         }
00081 
00082     bool isSketchy( const std::vector<float> &ranges,
00083                     int returnNum ) const;
00084 
00085     mutable std::vector<Reflector> reflectors_;
00086 
00087     Config config_;    
00088 };
00089 
00093 class ReflectorFeatureExtractor {
00094 public:
00095     ReflectorFeatureExtractor( const ReflectorExtractor::Config &config,
00096                                double rangeSd,
00097                                double bearingSd,
00098                                double pFalsePositive,
00099                                double pTruePositive )
00100         : config_(config),
00101           rangeSd_(rangeSd),
00102           bearingSd_(bearingSd),
00103           pFalsePositive_(pFalsePositive),
00104           pTruePositive_(pTruePositive)
00105         { 
00106             assert( pTruePositive_ > pFalsePositive_ ); 
00107             assert( rangeSd_ > 0 );
00108             assert( bearingSd_ > 0 );
00109         }
00110 
00111     std::vector<hydrofeatureobs::PointFeatureObs*> extractFeatures( const std::vector<float>     &ranges,
00112                                                                     const std::vector<Reflector> &reflectors ) const;
00113 private:
00114 
00115     ReflectorExtractor::Config config_;
00116     double rangeSd_;
00117     double bearingSd_;
00118     double pFalsePositive_;
00119     double pTruePositive_;
00120 };
00121 
00123 double calcReflectorPFalsePositive( const std::vector<float> &ranges,
00124                                     int start,
00125                                     int end,
00126                                     double bestPossiblePFalsePositive,
00127                                     double maxDeltaRangeNearReflector );
00128 
00130 geom2d::PolarPoint
00131 calcObsRB( const std::vector<float> &ranges,
00132            const ScannerConfig      &scannerConfig,
00133            const Reflector          &reflector );                        
00134 
00135 } // namespace
00136 
00137 #endif
reflectorextractor.h
