ogsensormodel.h

00001 /*
00002  * Orca-Robotics Project: Components for robotics 
00003  *               http://orca-robotics.sf.net/
00004  * Copyright (c) 2004-2009 Alex Brooks, Alexei Makarenko, Tobias Kaupp
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 OG_SENSOR_MODEL_H
00011 #define OG_SENSOR_MODEL_H
00012 
00013 #include <vector>
00014 
00015 namespace laser2og
00016 {
00017 
00018 // A structure to describe the likelihood function of Laser-to-OG observation model.
00019 typedef struct
00020 {
00021     int size;
00022     double occupMax;
00023     double emptyMax;
00024     double rangeMax;
00025     double rangeStDev;
00026     double rangeStDevMax;
00027     double posStDevMax;
00028     double hedStDevMax;
00029     double angleIncrement;
00030 } OgLaserModelConfig; //og_laser_model_config_t;
00031 
00037 class OgSensorModel
00038 {
00039 public: 
00040     OgSensorModel( const OgLaserModelConfig & config );
00041     ~OgSensorModel();
00042     
00043     double likelihood( const double rangeToCell, const double rangeToReturn, const double posSD, const double headSD);
00044 
00045     // debug
00046     void print();
00047 
00048     double hedStDevMax() {return config_.hedStDevMax;};
00049     double posStDevMax() {return config_.posStDevMax;};
00050 
00051 private:
00052     // range to return
00053     std::vector<double> r_;
00054     // likelihood when return is within max range
00055     std::vector<double> pIn_;
00056     // likelihood when return is outside of max range
00057     std::vector<double> pOut_;   
00058     
00059     OgLaserModelConfig config_;
00060     
00061     void init();
00062 
00063 };
00064 
00065 } // namespace
00066 
00067 #endif