RG_Lane.H

00001 /*!@file Robots2/Beobot2/LaneFollowing/RG_Lane/RG_Lane.H Ice Module to Log data    */
00002 // //////////////////////////////////////////////////////////////////// //
00003 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00004 // University of Southern California (USC) and the iLab at USC.         //
00005 // See http://iLab.usc.edu for information about this project.          //
00006 // //////////////////////////////////////////////////////////////////// //
00007 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00008 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00009 // in Visual Environments, and Applications'' by Christof Koch and      //
00010 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00011 // pending; application number 09/912,225 filed July 23, 2001; see      //
00012 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00013 // //////////////////////////////////////////////////////////////////// //
00014 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00015 //                                                                      //
00016 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00017 // redistribute it and/or modify it under the terms of the GNU General  //
00018 // Public License as published by the Free Software Foundation; either  //
00019 // version 2 of the License, or (at your option) any later version.     //
00020 //                                                                      //
00021 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00022 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00023 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00024 // PURPOSE.  See the GNU General Public License for more details.       //
00025 //                                                                      //
00026 // You should have received a copy of the GNU General Public License    //
00027 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00028 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00029 // Boston, MA 02111-1307 USA.                                           //
00030 // //////////////////////////////////////////////////////////////////// //
00031 //
00032 // Primary maintainer for this file: Chin-Kai Chang <chinkaic@usc.edu>
00033 // $HeadURL: svn://ilab.usc.edu/trunk/saliency/src/Robots/Beobot2/LaneFollowing/RG_Lane/RG_Lane.H
00034 // $Id: RG_Lane.H 12962 2010-03-06 02:13:53Z irock $
00035 //
00036 
00037 #include "Component/ModelComponent.H"
00038 #include "Component/ModelParam.H"
00039 
00040 #include "Robots/RobotBrain/RobotBrainComponent.H"
00041 #include "Util/Timer.H"
00042 
00043 #include "Ice/RobotBrainObjects.ice.H"
00044 #include "Ice/RobotSimEvents.ice.H"
00045 #include <IceUtil/Thread.h>
00046 
00047 #include "Robots/Beobot2/BeoCommon.H"
00048 
00049 #include "Media/FrameSeries.H"
00050 
00051 #include "Image/Image.H"
00052 #include "Image/Point3D.H"
00053 #include "Raster/Raster.H"
00054 
00055 #include <vector>
00056 #include "Transport/FrameInfo.H"
00057 
00058 #ifndef RG_LANE
00059 #define RG_LANE
00060 
00061 typedef struct _RegionInformation
00062 {
00063   uint start;
00064   uint end;
00065 
00066   float minDistance;
00067   uint minIndex;
00068 }
00069 RegionInformation;
00070 
00071 class imageDB 
00072 {
00073         std::string path;//Full path such as "../data/corner/HNB/2010_12_34/image00000"
00074         int start;
00075         int end;
00076         int current;    
00077 public:
00078                 imageDB(){}
00079 
00080                 imageDB(std::string p,int s,int e){
00081                         start = s;
00082                         end = e;
00083                         path = p;
00084                         current = s;
00085                 }
00086                 Image<PixRGB<byte> > nextImg(){
00087                         std::string imgpath = sformat("%s%05d.ppm",path.c_str(),current);
00088                         LDEBUG("Image path is %s",imgpath.c_str());
00089                         if(current++ > end)
00090                                 current = start;                        
00091                         return Raster::ReadRGB(imgpath.c_str());
00092                 }
00093                 ~imageDB(){}
00094 };
00095 class kalman_filter
00096 {
00097                 float est_var,sensor_var;
00098                 float k;
00099                 float est_x;
00100         public:
00101                 kalman_filter(float ini_ev,float ini_sv){
00102                         est_var = ini_ev;
00103                         sensor_var = ini_sv;
00104                         k = est_var/(est_var + sensor_var);
00105                         est_x = 0;
00106                 }
00107                 float update(float measure){
00108                         if(est_x == 0){
00109                                         est_x = measure;
00110                                         return est_x;
00111                         }
00112                         est_x = est_x + 10*k*(measure - est_x);//update estimate x
00113                         est_var = est_var*sensor_var/(est_var+sensor_var);//update p
00114                         k = est_var/(est_var+sensor_var);//update k
00115                         return est_x;
00116                 }
00117 
00118 };
00119 
00120 class RG_Lane: public RobotBrainComponent
00121 {
00122 public:
00123 
00124   RG_Lane(OptionManager& mgr,
00125                 const std::string& descrName = "RG_Lane",
00126                 const std::string& tagName = "RG_Lane");
00127 
00128   ~RG_Lane();
00129 
00130   virtual void evolve();
00131 
00132   //! Get a message
00133   virtual void updateMessage (const RobotSimEvents::EventMessagePtr& eMsg, const Ice::Current&);
00134 
00135   virtual void registerTopics();
00136 
00137   void start1();
00138 
00139 private:
00140 
00141         void openDB(const std::string& path);
00142   void loadFrame();
00143 
00144   void loadDB(const std::string& path);
00145   void drawState();
00146   Image<PixRGB<byte> > itsCurrImg;
00147   Image<PixRGB<byte> > itsProcImg;
00148   nub::soft_ref<OutputFrameSeries> itsOfs;
00149         imageDB itsImageDB;
00150         kalman_filter itsKf;
00151         int itsKfx;
00152   //! Overall Display Image
00153   Image<PixRGB<byte> > itsDispImg;
00154 
00155 
00156   Timer itsTimer;
00157 };
00158 #endif
00159 
00160 // ######################################################################
00161 /* So things look consistent in everyone's emacs... */
00162 /* Local Variables: */
00163 /* indent-tabs-mode: nil */
00164 /* End: */
Generated on Sun May 8 08:41:17 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3