Localization.H

Go to the documentation of this file.
00001 /**
00002 * \file Localization.H
00003 * \brief Contains declaration for Localization class
00004 *
00005 *        Notes : Things to be done
00006 *                ->Make orientation relative to the reading taken at start instead of absolute
00007 *                        just introduce a message at the start time
00008 *                ->
00009 *
00010 *
00011 */
00012 
00013 #include "Component/ModelComponent.H"
00014 #include "Component/ModelParam.H"
00015 #include "Media/FrameSeries.H"
00016 
00017 #include "Raster/GenericFrame.H"
00018 #include "Image/Image.H"
00019 #include "GUI/XWinManaged.H"
00020 #include "GUI/ImageDisplayStream.H"
00021 #include "Image/Image.H"
00022 #include "Image/Pixels.H"
00023 #include "Robots/RobotBrain/RobotBrainComponent.H"
00024 #include "Util/Types.H"      // for byte
00025 #include <string>
00026 #include <vector>
00027 #include <math.h>
00028 
00029 
00030 #include "Ice/RobotBrainObjects.ice.H"
00031 #include "Ice/RobotSimEvents.ice.H"
00032 #include "Ice/IceImageUtils.H"
00033 #include <IceUtil/Thread.h>
00034 
00035 //////BOOST Libraries
00036 
00037 
00038 #include <boost/random/linear_congruential.hpp>
00039 #include <boost/random/normal_distribution.hpp>
00040 #include <boost/random/variate_generator.hpp>
00041 
00042 ////////////
00043 
00044 #ifndef LOCALIZATION_H
00045 #define LOCALIZATION_H
00046 
00047 /// \def BOT_SPEED 1
00048 /// \brief Some defintions for parameters that we are going to use in equations for predictions and updates using sensor data
00049 #define BOT_SPEED 1 //(pixels per 1 second)
00050 #define INIT_PARTICLES 1000  //(Kick off with 1000 particles)
00051 #define BETA 0.4 // max allowed depletion before resampling
00052 
00053 
00054 /// \def NORMAL_DIST(MEAN,SIGMA,X)
00055 /// \brief Equation of normal distribution
00056 #define NORMAL_DIST(MEAN,SIGMA,X) ((1.0/(SIGMA*sqrt(2*M_PI)))*pow(M_E,(-1.0*(X-MEAN)*(X-MEAN))/(2.0*SIGMA*SIGMA)))
00057 // Note: Fine tune these parameters to improve prediction performance
00058 // For best results use this constants from experiments on sub
00059 
00060 //Constants for forward movement
00061 
00062 //Constants for rotation
00063 #define M_DRIFT 0.35  // the mean rotation error per delta(theta) = 1
00064 #define SIGMA_DRIFT 5 // standard deviation on 360 degree rotation
00065 //
00066 #define M_TRANS 0.35  // the mean translation per delta(trans) = 1
00067 #define SIGMA_TRANS 5 // standard deviation on 1 unit translation
00068 //
00069 
00070 #include "pParticle.H"
00071 
00072 /**
00073 * \class Localization Localization.H "src/Robots/SeaBeeIII/Localization.H"
00074 * \brief contains functions for tracking the position of the sub using particle filter technic
00075 *
00076 */
00077 
00078 class Localization : public RobotBrainComponent
00079 {
00080 private:
00081         /// \brief pointer to OutputFrameSeries class
00082         nub::soft_ref<OutputFrameSeries>          itsOfs;
00083         /// \brief whether last time sub was running or not?
00084         bool itsRunning;
00085 
00086         /// \brief the image where positions are written out
00087         Image<float> iMap;
00088         /// \brief will contain the obstacles on the map
00089         Image<float> originalMap;
00090 
00091         /// \brief The list of particles
00092         vector<class pParticle> pList;
00093         /// \brief number of particles in the image right now
00094         int itsNumParticles;
00095         /// \brief minimum number of particles to be kept in image
00096         int itsKeepParticles;
00097 
00098         /// \brief current estimate of the method
00099         class pParticle maxPoint;
00100 
00101         /// \brief Memory for the message data that we have received
00102         /// Timestamp for estimation of the speed
00103         time_t timestamp;
00104 
00105         /// \private int curOrientation
00106         /// \brief The last orientation that we received
00107         int curOrientation;
00108 
00109         /// \typedef typedef boost::minstd_rand  base_gen_type
00110         /// \brief BOOST library stuff
00111         typedef boost::minstd_rand  base_gen_type;
00112         typedef boost::normal_distribution<>  dist_type;
00113         typedef boost::variate_generator<base_gen_type&, dist_type>  rgen_type;
00114 
00115         base_gen_type generator;
00116         rgen_type generate_drift;
00117         rgen_type generate_trans;
00118 
00119 
00120 public:
00121 
00122   Localization(OptionManager& mgr,
00123       const std::string& descrName = "Localization",
00124       const std::string& tagName = "Localization");
00125 
00126   ~Localization();
00127 
00128   virtual void evolve();
00129 
00130   //!Get a message
00131   virtual void updateMessage(const RobotSimEvents::EventMessagePtr& eMsg,
00132       const Ice::Current&);
00133 
00134   virtual void registerTopics();
00135 
00136   //
00137 
00138         void initPoints(const short int);
00139         float getNormalizer();
00140         float rand_N();
00141   //
00142   // Some of my functions
00143 
00144         void predict(RobotSimEvents::OrientationMessagePtr);
00145 
00146         void update(RobotSimEvents::ObstacleMessagePtr);
00147 
00148         void resample(int no =0);
00149 
00150         class pParticle& getCurrentEstimate();
00151   //
00152   // Debugging it
00153 
00154 };
00155 
00156 
00157 #endif
Generated on Sun May 8 08:41:38 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3