VectorHistField.C

00001 /*!@file Devices/VectorHistField.C implementation of vector histogram
00002 field algorithm routines */
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00005 // University of Southern California (USC) and the iLab at USC.         //
00006 // See http://iLab.usc.edu for information about this project.          //
00007 // //////////////////////////////////////////////////////////////////// //
00008 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00009 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00010 // in Visual Environments, and Applications'' by Christof Koch and      //
00011 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00012 // pending; application number 09/912,225 filed July 23, 2001; see      //
00013 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00014 // //////////////////////////////////////////////////////////////////// //
00015 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00016 //                                                                      //
00017 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00018 // redistribute it and/or modify it under the terms of the GNU General  //
00019 // Public License as published by the Free Software Foundation; either  //
00020 // version 2 of the License, or (at your option) any later version.     //
00021 //                                                                      //
00022 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00023 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00024 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00025 // PURPOSE.  See the GNU General Public License for more details.       //
00026 //                                                                      //
00027 // You should have received a copy of the GNU General Public License    //
00028 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00029 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00030 // Boston, MA 02111-1307 USA.                                           //
00031 // //////////////////////////////////////////////////////////////////// //
00032 //
00033 // Primary maintainer for this file: farhan baluch fbaluch@usc.edu
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Robots/LoBot/control/VectorHistField.C $
00035 // $Id: VectorHistField.C 12962 2010-03-06 02:13:53Z irock $
00036 //
00037 
00038 #include "VectorHistField.H"
00039 #include "Component/OptionManager.H"
00040 #include "Devices/Serial.H"
00041 #include "Image/DrawOps.H"
00042 #include "Image/Kernels.H"
00043 #include "Image/MathOps.H"
00044 #include "Image/ColorOps.H"
00045 #include "Image/CutPaste.H"
00046 #include <cmath>
00047 #include <iterator>
00048 
00049 // ######################################################################
00050 VectorHistField::VectorHistField(OptionManager& mgr, const std::string& descrName,
00051                          const std::string& tagName, const int width,
00052                          const int height) :
00053         VectorField(mgr, descrName, tagName,width,height)
00054 {
00055   itsDist=0;
00056   itsAng=0;
00057 }
00058 
00059 // ######################################################################
00060 VectorHistField::~VectorHistField()
00061 { }
00062 
00063 // ######################################################################
00064 Image<PixRGB<byte> > VectorHistField::updateField(std::vector<Point2D<float> > sensorData, Point2D<int> robotPos, float robotOrient, float robotDist,Point2D<int> goal,Image<geom::vec2f> itsObsTemplate)
00065 {
00066 
00067     geom::vec2f tmp,tmp2,goalVec;
00068     Image<float> final(itsField.getDims(),ZEROS);
00069         //Point2D<int> robotPos(itsField.getWidth()/2,itsField.getHeight() - 1);
00070         //in cell space so getHeight()-1 should move it up one cell
00071 
00072     robotPos.i = itsField.getWidth()/2;
00073     robotPos.j = (itsField.getHeight())-5;
00074         //robotPos.j = itsField.getHeight()/2;
00075 
00076     itsSensorData = sensorData;
00077     itsRobotPos = robotPos;
00078     itsRobotOrientation = robotOrient;
00079 
00080     goalVec.set_polar_rad((float)goal.i,geom::deg2rad((float)goal.j));
00081 
00082 
00083     itsGoalField.clear(goalVec);
00084 
00085     float hist=4.5F,gForce=3.0;
00086     Image<float> finblob(itsField.getDims(),ZEROS);
00087 
00088     scaleFieldBy(hist);
00089     scaleGoalFieldBy(gForce);
00090 
00091     //rotate added vectors depending on how the robot turned
00092     //rotateField(itsRobotOrientation);
00093     rotateGoalField(itsRobotOrientation);
00094 
00095     goalVec.rotate_deg(itsRobotOrientation);
00096     itsGoal.i = goalVec.length();
00097     itsGoal.j = goalVec.theta_deg();
00098 
00099 
00100     //lets create a goal field and add that to the mix
00101     itsField = itsField + itsGoalField;
00102 
00103     //shiftField((int)robotDist, (int)itsRobotOrientation);
00104 
00105     Image<geom::vec2f>::iterator aptrF;
00106 
00107     for (uint i=0;i<sensorData.size();i++)
00108     {
00109         tmp.set_polar_rad(sensorData[i].i,geom::deg2rad(sensorData[i].j));
00110 
00111           //make obstacle position relative to robot
00112         Point2D<int> center(tmp.x() + robotPos.i,robotPos.j - tmp.y());
00113 
00114         //LINFO("gauss %d center %d,%d", i,center.i, center.j);
00115         //paste the template at the location of obstacle and add to field
00116 
00117         geom::vec2f bg(0,0);
00118         itsField = itsField + shiftClean(itsObsTemplate,
00119                                 center.i-itsField.getWidth()/2,
00120                               center.j-itsField.getHeight()/2,bg);
00121     }
00122 
00123         //Image<geom::vec2f>::iterator aptrF;
00124     aptrF = itsField.beginw();
00125     while(aptrF != itsField.end())
00126     {
00127         tmp2 = *aptrF;
00128         tmp2/=(float)sensorData.size()+1; //taking an average
00129         tmp2.set_length(20.0);//(tmp2.length()+5.0);
00130         *aptrF++ = tmp2;
00131     }
00132 
00133     inplaceNormalize<float>(finblob, 0.0,150.0);
00134      return toRGB<byte>(finblob);
00135 }
00136 
00137 // ######################################################################
00138 Image<geom::vec2f> VectorHistField::obstacleTemplate(float sigma,float amp)
00139 {
00140 
00141   Image<geom::vec2f> result(itsField.getDims(),NO_INIT);
00142   Point2D<int> center(itsField.getWidth()/2, itsField.getHeight()/2);
00143   Image<float> blob = gaussianBlob<float>(itsField.getDims(),center,sigma,
00144                                                 sigma);
00145 
00146   inplaceNormalize<float>(blob, 0.0,amp);
00147   Image<geom::vec2f>::iterator aptrF = result.beginw();
00148   Image<float>::iterator blobPtr = blob.beginw();
00149 
00150   geom::vec2f tmp2;
00151   int idx = 0;
00152   float minAng = 0.0, maxAng=0.0;
00153   while(aptrF!=result.end())
00154     {
00155       Point2D<int> endPt;
00156       endPt.i = idx % itsField.getWidth();
00157       endPt.j = (int) idx/itsField.getHeight();
00158       float angle = atan2(center.j - endPt.j, center.i-endPt.i);
00159 
00160       if (angle < minAng)
00161           minAng = angle;
00162 
00163       if (angle > maxAng)
00164           maxAng = angle;
00165 
00166       tmp2.set_polar_rad(*blobPtr++,  angle);
00167 
00168       *aptrF++ = tmp2;
00169       idx++;
00170     }
00171 
00172   return result;
00173 
00174 }
00175 
00176 
00177 // ######################################################################
00178 void VectorHistField::shiftField(int dist,int ang)
00179 {
00180 
00181   float x = itsDist*sin(geom::deg2rad(itsAng));
00182   float y = itsDist*cos(geom::deg2rad(itsAng));
00183   int cellSize = 3000/30;  //grid size 30 max sonar reading 3000
00184   int cellHalf = cellSize/2;
00185 
00186   int shiftX = 0, shiftY=0;
00187   int north = 1; int south=-1;
00188   int east =1, west=-1;
00189   geom::vec2f bg;
00190   bg.set_polar_rad((float)itsGoal.i,geom::deg2rad((float)itsGoal.j));
00191 
00192   if (x < cellHalf && x > -cellHalf)
00193     {
00194       shiftX = 0;
00195       if(y > cellHalf)
00196         shiftY = north;                       //shift NORTH
00197       else if(y < -cellHalf)
00198         shiftY = south;                       //shift SOUTH
00199     }
00200 
00201 
00202   else if(y < cellHalf && y > -cellHalf)
00203     {
00204       shiftY = 0;
00205       if(x > cellHalf)
00206         shiftX = west;                        //shift WEST
00207       else if(x < -cellHalf)
00208         shiftX = east;                        //shift EAST
00209     }
00210 
00211 
00212   else if(y > cellHalf)
00213     {
00214       shiftY = north;
00215       if(x > cellHalf)
00216         shiftX = west;                         //shift NW
00217       else if(x < -cellHalf)
00218         shiftX = east;                         //shift NE
00219     }
00220 
00221 
00222 
00223   else if(y < -cellHalf)
00224     {
00225       shiftY = south;
00226       if(x > cellHalf)
00227         shiftX = west;                         //shift SW
00228       else if(x < -cellHalf)
00229         shiftX = east;                          //shift SE
00230     }
00231 
00232   if (shiftX !=0 | shiftY!=0)
00233     {
00234       LINFO("Shifting field %d,%d", shiftX,shiftY);
00235       itsField = shiftClean(itsField,shiftX, shiftY);
00236       itsDist = 0;
00237       itsAng = 0;
00238     }
00239   else
00240     {
00241       LINFO("No shift total dist=%d,total ang=%d", itsDist,itsAng);
00242       itsDist+=dist;
00243       itsAng+=ang;
00244 
00245     }
00246 }
00247 
00248 
00249 
00250 
00251 
00252 // ######################################################################
00253 /* So things look consistent in everyone's emacs... */
00254 /* Local Variables: */
00255 /* indent-tabs-mode: nil */
00256 /* End: */
Generated on Sun May 8 08:41:30 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3