GistSal_Navigation.H

00001 /*!@file Robots2/Beobot2/Navigation/GistSal_Navigation/GistSal_Navigation.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: Christian Siagian <siagian@usc.edu>
00033 // $HeadURL: svn://ilab.usc.edu/trunk/saliency/src/Robots/Beobot2/Navigation/GistSal_Navigation/GistSal_Navigation.H
00034 // $Id: GistSal_Navigation.H 12962 2010-03-06 02:13:53Z irock $
00035 //
00036 
00037 #ifndef GISTSAL_NAVIGATIONI_H
00038 #define GISTSAL_NAVIGATIONI_H
00039 
00040 #include "Component/ModelComponent.H"
00041 #include "Component/ModelParam.H"
00042 
00043 #include "Robots/RobotBrain/RobotBrainComponent.H"
00044 #include "Util/Timer.H"
00045 
00046 #include "Ice/RobotBrainObjects.ice.H"
00047 #include "Ice/RobotSimEvents.ice.H"
00048 #include <IceUtil/Thread.h>
00049 
00050 #include "Robots/Beobot2/BeoCommon.H"
00051 
00052 #include "Media/FrameSeries.H"
00053 
00054 #include "Image/Image.H"
00055 #include "Image/Point3D.H"
00056 #include "GUI/SimpleMeter.H"
00057 
00058 #include "Beobot/Environment.H"
00059 #include "Beobot/GSlocalizer.H"
00060 
00061 #include <vector>
00062 #include "SIFT/Keypoint.H"
00063 #include "SIFT/VisualObject.H"
00064 #include "SIFT/VisualObjectDB.H"
00065 #include "SIFT/VisualObjectMatch.H"
00066 #include "Transport/FrameInfo.H"
00067 
00068 #include "Beobot/SalientRegionTracker.H"
00069 
00070 // ######################################################################
00071 
00072 struct LandmarkDBMatch
00073 {
00074   LandmarkDBMatch() { };
00075 
00076   LandmarkDBMatch
00077   (rutz::shared_ptr<VisualObject> inVo,
00078    Point2D<int> inVoOffset,
00079    GSlocJobData inDbi,
00080    Point2D<int> inDbOffset,
00081    rutz::shared_ptr<VisualObjectMatch> inMatchRes):
00082     vo(inVo),
00083     voOffset(inVoOffset),
00084     dbi(inDbi),
00085     dbOffset(inDbOffset),
00086     matchRes(inMatchRes)
00087   { }
00088 
00089   //! visual object that is being matched
00090   rutz::shared_ptr<VisualObject> vo;
00091   Point2D<int> voOffset;
00092 
00093   //! index of the matched database salient region
00094   GSlocJobData dbi;
00095   Point2D<int> dbOffset;
00096 
00097   //! matching statistics
00098   rutz::shared_ptr<VisualObjectMatch> matchRes;
00099 };
00100 
00101 // ######################################################################
00102 //! Vision Navigation using Gist and Salient Regions
00103 class GistSal_Navigation : public RobotBrainComponent
00104 {
00105 public:
00106 
00107   GistSal_Navigation(OptionManager& mgr,
00108                 const std::string& descrName = "GistSal_Navigation",
00109                 const std::string& tagName = "GistSal_Navigation");
00110 
00111   //! set the environment - has to be done
00112   void setEnvironment(rutz::shared_ptr<Environment> env);
00113 
00114   ~GistSal_Navigation();
00115 
00116   virtual void evolve();
00117 
00118   //! Get a message
00119   virtual void updateMessage
00120   (const RobotSimEvents::EventMessagePtr& eMsg,
00121    const Ice::Current&);
00122 
00123   virtual void registerTopics();
00124 
00125   void start1();
00126 
00127 private:
00128 
00129   uint getKeypointMatches();
00130   uint projectForward
00131   (std::vector<rutz::shared_ptr<LandmarkDBMatch> > &landmarkDBMatches,
00132    nub::soft_ref<SalientRegionTracker> tracker);
00133 
00134   void computeErrorDifference();
00135   bool estimateProgressToDestination();
00136   void recover(bool foundSomething);
00137   void navigation();
00138 
00139   void updateMotorPID(double tran, double rot,double error);
00140   void updateMotor(double tran,double rot);
00141 
00142   void drawState();
00143 
00144   void updatePosition(double turn);
00145   Image<PixRGB<byte> > drawInfoImg();
00146 
00147   Image<PixRGB<byte> > itsCurrImg;
00148   Image<PixRGB<byte> > itsProcImg;
00149 
00150   IceUtil::Mutex its_input_mutex;           //!< locking current input info
00151   IceUtil::Mutex its_Curr_Mtr_mutex;        //!< locking motor related vars
00152   IceUtil::Mutex its_Curr_Dbr_mutex;        //!< locking landmark DB results
00153   IceUtil::Mutex its_Curr_Loc_mutex;        //!< locking current location
00154   IceUtil::Mutex its_tracker_mutex;         //!< locking tracker states
00155   IceUtil::Mutex its_Curr_CornerMtr_mutex;  //!< locking corner motor command
00156 
00157   nub::soft_ref<OutputFrameSeries> itsOfs;
00158 
00159   //! Overall Display Image
00160   Image<PixRGB<byte> > itsDispImg;
00161 
00162   //! all the environment information
00163   rutz::shared_ptr<Environment> itsEnvironment;
00164 
00165   //! from its environment: topological map
00166   rutz::shared_ptr<TopologicalMap> itsTopologicalMap;
00167 
00168   //! from its environment: visual landmark database
00169   rutz::shared_ptr<LandmarkDB> itsLandmarkDB;
00170 
00171   Timer  itsTimer;
00172   int    itsCurrImgID;
00173   int    itsPrevProcImgID;
00174 
00175   //! Motor related states
00176   double itsRcTransSpeed;
00177   double itsRcRotSpeed;
00178   int    itsRemoteMode;
00179 
00180   double itsTransSpeed;
00181   double itsRotSpeed;
00182   int    itsDir;               // current direction
00183 
00184   float itsTurnAngle;
00185   float itsJunctionDist;
00186 
00187   double itsCornerTransSpeed;
00188   double itsCornerRotSpeed;
00189   int itsCornerProcStatus;
00190 
00191   double itsXError;
00192   double itsYError;
00193   double itsError;
00194 
00195   double itsIState;
00196   double itsDState;
00197 
00198   double itsPGain;
00199   double itsIGain;
00200   double itsDGain;
00201 
00202   uint itsReverseCount;
00203   bool itsReverseDir;
00204 
00205   double itsDistToGoal;
00206 
00207   //! tracker related modules and states
00208   Timer  itsLastFoundTimer;
00209   nub::soft_ref<SalientRegionTracker> itsSearchTracker;
00210   nub::soft_ref<SalientRegionTracker> itsNewMovementTracker;
00211   nub::soft_ref<SalientRegionTracker> itsCurrMovementTracker;
00212   bool itsResetSearchTracker;
00213 
00214   std::vector<Point2D<int> > itsCurrSalPoints;
00215   std::vector<Rectangle >    itsCurrSalRects;
00216   ImageSet<float> itsCurrCmap;
00217 
00218   //! incoming matched salient regions
00219   std::vector<rutz::shared_ptr<LandmarkDBMatch> > itsCurrLandmarkDBMatches;
00220   std::vector<rutz::shared_ptr<LandmarkDBMatch> > itsNewLandmarkDBMatches;
00221 
00222   //! path planning
00223   uint  itsDesiredSegmentLocation;
00224   float itsDesiredSegmentLengthTraveled;
00225 
00226   uint  itsCurrentSegmentLocation;
00227   float itsCurrentSegmentLengthTraveled;
00228 
00229   std::vector<int> itsMoves;
00230   bool itsInGoal;
00231 };
00232 #endif
00233 
00234 // ######################################################################
00235 /* So things look consistent in everyone's emacs... */
00236 /* Local Variables: */
00237 /* indent-tabs-mode: nil */
00238 /* End: */
Generated on Sun May 8 08:41:19 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3