Beobot2_GistSalLocalizerMaster.H

Go to the documentation of this file.
00001 /*!@file                                                                */
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: Christian Siagian <siagian@usc.edu>
00033 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Robots/Beobot2/Localization/Beobot2_GistSalLocalizerMaster.H $
00034 // $Id: Beobot2_GistSalLocalizerMaster.H 12962 2010-03-06 02:13:53Z irock $
00035 //
00036 
00037 
00038 #ifndef BEOBOT2_GISTSALLOCALIZERMASTERI_H
00039 #define BEOBOT2_GISTSALLOCALIZERMASTERI_H
00040 
00041 #include "Component/ModelComponent.H"
00042 #include "Component/ModelParam.H"
00043 #include "Robots/RobotBrain/RobotBrainComponent.H"
00044 #include "GUI/XWinManaged.H"
00045 
00046 #include "Beobot/GSlocalizer.H"
00047 #include "Beobot/Landmark.H"
00048 #include "SIFT/Histogram.H"
00049 #include "Beobot/GSparticle.H"
00050 #include "Beobot/Environment.H"
00051 #include "Util/Timer.H"
00052 
00053 #include "Ice/RobotBrainObjects.ice.H"
00054 #include "Ice/BeobotEvents.ice.H"
00055 #include "Ice/IceImageUtils.H"
00056 #include <IceUtil/Thread.h>
00057 
00058 
00059 class Beobot2_GistSalLocalizerMasterI : public RobotBrainComponent
00060 {
00061 public:
00062 
00063   // ######################################################################
00064   /*! @name Constructors and Destructors */
00065   //@{
00066 
00067   Beobot2_GistSalLocalizerMasterI(OptionManager& mgr,
00068       const std::string& descrName = "Beobot2_GistSalLocalizerMaster",
00069       const std::string& tagName = "Beobot2_GistSalLocalizerMaster");
00070 
00071   ~Beobot2_GistSalLocalizerMasterI();
00072 
00073   //! Get started. See ModelComponent.
00074   virtual void start1();
00075 
00076   virtual void evolve();
00077 
00078   //!Get a message
00079   virtual void updateMessage(const RobotSimEvents::EventMessagePtr& eMsg,
00080       const Ice::Current&);
00081 
00082   virtual void registerTopics();
00083 
00084 
00085 //   //! set the prefix of file to save data  - has to be done
00086 //   void setSavePrefix(std::string prefix);
00087 
00088   //! set the environment - has to be done
00089   void setEnvironment(rutz::shared_ptr<Environment> env);
00090 
00091   //@}
00092 
00093 //   // ######################################################################
00094 //   //! @name Access functions
00095 //   //@{
00096 
00097 //   //! get number of objects compared in the search
00098 //   uint getNumObjectSearch(uint index);
00099 
00100 //   //! get the environment information
00101 //   rutz::shared_ptr<Environment> getEnvironment();
00102 
00103 //   //! set the window to display results
00104 //   void setWindow(rutz::shared_ptr<XWinManaged> inputWin);
00105 
00106 //   //! get the input image
00107 //   Image<PixRGB<byte> > getInputImage();
00108 
00109 //   //! get the number of objects inputted
00110 //   uint getNumInputObject();
00111 
00112 //   //! get the visual object that we try to match
00113 //   rutz::shared_ptr<VisualObject> getInputVO(uint index);
00114 
00115 //   //! get the input gist
00116 //   Image<double> getInputGist();
00117 
00118 //   //! get the visual object match for the found object
00119 //   rutz::shared_ptr<VisualObjectMatch> getVOmatch(uint index);
00120 
00121 //   //! get the segment number of the object match found
00122 //   uint getSegmentNumberMatch(uint index);
00123 
00124 //   //! get the length traveled of the object match found
00125 //   float getLengthTraveledMatch(uint index);
00126 
00127 //   //! get the object offset of the visual object
00128 //   //! that we try to match
00129 //   Point2D<int> getInputObjOffset(uint index);
00130 
00131 //   //! get the last input frame number
00132 //   int getInputFnum();
00133 
00134 //   //! get the last input frame number where search is started
00135 //   int getSearchInputFnum();
00136 
00137 //   //! get the segment histogram from the segment classifier
00138 //   rutz::shared_ptr<Histogram> getSegmentHistogram();
00139 
00140 //   //! get our geographical location
00141 //   Point2D<int> getLocation();
00142 
00143 //   //! get our segment location
00144 //   uint getSegmentLocation();
00145 
00146 //   //! get the length traveled within the segment
00147 //   float getSegmentLengthTraveled();
00148 
00149 //   //! set ground truth
00150 //   void setGroundTruth(uint snum, float ltrav);
00151 
00152 //   //! get ground truth
00153 //   void getGroundTruth(uint &snum, float &ltrav);
00154 
00155   //@}
00156 
00157   // ######################################################################
00158   /*! @name member functions */
00159   //@{
00160 
00161   //! initialize the localization particles
00162   void initParticles(std::string belFName = std::string(""));
00163 
00164 //   //! get the belief particles (usually for recovering crashes)
00165 //   std::vector<GSparticle> getBeliefParticles();
00166 
00167 //   //! check if the serach is finished
00168 //   bool outputReady();
00169 
00170 //   //! return the result of the matching search
00171 //   bool isMatchFound(uint index);
00172 
00173 //   //! input the image, visual object and gist feature for search
00174 //   //! also add the odometry change
00175   void input();
00176 //   void input(Image<PixRGB<byte> > ima,
00177 //              std::vector<rutz::shared_ptr<VisualObject> > inputVO,
00178 //              std::vector<Point2D<int> > inputObjOffset,
00179 //              int inputFnum, Image<double> cgist,
00180 //              float dx = -1.0F, float dy = -1.0F);
00181 
00182   //! the belief histogram for segment only localization
00183   rutz::shared_ptr<Histogram> getSegmentBeliefHistogram();
00184 
00185 //   //! stop search by cleaning up the queue
00186 //   //! NOTE: this is a hard stop (blocking operation)
00187 //   //!       may take time (500ms, or even longer)
00188 //   void stopSearch();
00189 
00190 //   //! stop search by flipping a stop-search bit
00191 //   //! NOTE: this is a soft/non-blocking operation
00192 //   void stopSearch2();
00193 
00194   //! update belief using the input just processed
00195   //! update our likely location
00196   void updateBelief();
00197 
00198   //! move the object from the previous location
00199   void actionUpdateBelief();
00200 
00201   //! update belief using the segment prediction
00202   void segmentUpdateBelief();
00203 
00204   //! update belief using all the objects found
00205   void objectUpdateBelief();
00206 
00207   //! update belief using object 'index'
00208   void objectUpdateBelief(uint index);
00209 
00210   //! set the most likely location
00211   void setLocation();
00212 
00213   //! save the localizer results
00214   void saveLocalizerResults();
00215 
00216   //! get the belief image (it is put on top of a map)
00217   Image<PixRGB<byte> > getBeliefImage(uint w, uint h, int &scale);
00218 
00219   Image<PixRGB<byte> > getDisplayImage();
00220   Image<PixRGB<byte> > getSalImage
00221   (Image<PixRGB<byte> > ima,
00222    std::vector<rutz::shared_ptr<VisualObject> > inputVO,
00223    std::vector<Point2D<int> > objOffset,
00224    std::vector<bool> found);
00225 
00226   // set the number of workers that will be
00227   void setNumWorkers(uint numWorkers);
00228 
00229   inline void setSavePrefix(std::string prefix);
00230 
00231 //   //! get motor signal
00232 //   /*! motor signal can be used (using PID, for example) to obtain
00233 //     motor command. Motor signal is a delta signal (in image coordinate)
00234 //     of where the robot should go to get to the goal state.
00235 //    */
00236 //   Point2D<int> getMotorSignal();
00237 
00238   //@}
00239 
00240 private:
00241 
00242 
00243   // ######################################################################
00244   /*! @name private functions */
00245   //@{
00246 
00247   //! set the segment and object search priority for landmark DB
00248   void setSearchPriority();
00249 
00250   //! add the search order preference randomly
00251   void addRandomPriority();
00252 
00253   //! add the search order preference based on segment
00254   void addSegmentPriority();
00255 
00256   //! add the search order preference based on saliency match
00257   void addSaliencyPriority();
00258 
00259   //! add the search order preference based on current belief location
00260   void addLocationPriority();
00261 
00262 //   //! get the match
00263 //   GSlocJobData getMatch(uint index);
00264 
00265 //   //@}
00266 
00267 //   //!  file prefix to save data
00268 //   std::string itsSavePrefix;
00269 
00270   //! localization particles for beliefs
00271   std::vector<GSparticle> itsBeliefParticles;
00272   std::vector<Point2D<int> > itsBeliefLocations;
00273 
00274   //! all the environment information
00275   rutz::shared_ptr<Environment> itsEnvironment;
00276 
00277   //! from its environment: topological map
00278   rutz::shared_ptr<TopologicalMap> itsTopologicalMap;
00279 
00280   //! from its environment: visual landmark database
00281   rutz::shared_ptr<LandmarkDB> itsLandmarkDB;
00282 
00283   //! the input image, visual objects, and gist
00284   Image<PixRGB<byte> > itsInputImage;
00285   std::vector<rutz::shared_ptr<VisualObject> > itsInputVO;
00286 //   std::vector<bool> itsVOKeypointsComputed;
00287   std::vector<Point2D<int> > itsInputObjOffset;
00288   Image<double> itsInputGist;
00289 
00290   //! ground truth information - default to (0,0.0)
00291   uint  itsSnumGT;
00292   float itsLtravGT;
00293 
00294   //! the current robot movement
00295   float itsRobotDx;
00296   float itsRobotDy;
00297 
00298   //! segment histogram from the classifier
00299   rutz::shared_ptr<Histogram> itsSegmentHistogram;
00300 
00301   //! result of search
00302   std::vector<Image<PixRGB<byte> > > itsVOmatchImage;
00303   std::vector<GSlocJobData> itsLmkMatch;
00304   std::vector<uint> itsSegNumMatch;
00305   std::vector<uint> itsLmkNumMatch;
00306   std::vector<uint> itsVobNumMatch;
00307   std::vector<float> itsLenTravMatch;
00308   std::vector<bool> itsMatchFound;
00309   std::vector<uint> itsNumObjectSearch;
00310 
00311   //! resulting geographic location
00312   uint itsSegmentLocation;
00313   float itsSegmentLengthTraveled;
00314   Point2D<int> itsLocation;
00315 
00316   //! job queue and number of jobs to do
00317   //! Note: they are on jobLock
00318   std::list<GSlocJobData> itsJobQueue;
00319 //   bool itsIsQueueSorted;         //!< note if the queue is sorted
00320   uint itsNumJobsProcessed;      //!< number of jobs that has been processed
00321   uint itsLastSuccessfulJob;     //!< job index last found
00322   uint itsNumObjectFound;        //!< number of objects found
00323   uint itsNumJobs;               //!< original number of jobs
00324 //   bool itsStopSearch;            //!< stop search request
00325 
00326   uint itsNumWorkers;            //!< the number of threads that are working
00327   uint itsNumBusyWorkers;
00328 
00329   //! segment histogram from the belief particles
00330   rutz::shared_ptr<Histogram> itsSegmentBeliefHistogram;
00331 
00332 //   //! especially for input
00333 //   bool itsOutputReady2;
00334 
00335   std::string                     itsSavePrefix;
00336 
00337   //! locks
00338   IceUtil::Mutex its_job_queue_mutex;        //!< locking jobQueue
00339   IceUtil::Mutex its_fnum_mutex;             //!< locking frame number
00340   IceUtil::Mutex its_num_busy_workers_mutex; //!< locking itsOutputReady2
00341   IceUtil::Mutex its_results_mutex;          //!< locking results
00342   IceUtil::Mutex its_input_info_mutex;       //!< locking the input information
00343   IceUtil::Mutex its_gist_mutex;             //!< locking gist features
00344   IceUtil::Mutex its_particles_mutex;        //!< locking belief particles
00345 
00346   rutz::shared_ptr<XWinManaged> itsInputWin;
00347   rutz::shared_ptr<XWinManaged> itsResultWin;
00348 
00349   Timer itsSearchTimer;
00350   std::vector<float> itsTimes;
00351   bool itsAbort;
00352 
00353   int itsInputFnum;
00354   int itsSearchInputFnum;
00355   bool itsStopSent;
00356 };
00357 
00358 // ######################################################################
00359 // Implementation for Beobot2_GisSalLocalizerMaster inline functions
00360 // ######################################################################
00361 inline void Beobot2_GistSalLocalizerMasterI::setSavePrefix(std::string prefix)
00362 { itsSavePrefix = prefix; }
00363 
00364 
00365 
00366 #endif
00367 
00368 // ######################################################################
00369 /* So things look consistent in everyone's emacs... */
00370 /* Local Variables: */
00371 /* indent-tabs-mode: nil */
00372 /* End: */
Generated on Sun May 8 08:41:18 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3