HippocampusI.C

00001 /*!@file Hippocampus.C maintains the current thought location of the robot */
00002 //This modules invovles in the perception
00003 
00004 //////////////////////////////////////////////////////////////////// //
00005 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00006 // University of Southern California (USC) and the iLab at USC.         //
00007 // See http://iLab.usc.edu for information about this project.          //
00008 // //////////////////////////////////////////////////////////////////// //
00009 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00010 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00011 // in Visual Environments, and Applications'' by Christof Koch and      //
00012 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00013 // pending; application number 09/912,225 filed July 23, 2001; see      //
00014 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00015 // //////////////////////////////////////////////////////////////////// //
00016 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00017 //                                                                      //
00018 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00019 // redistribute it and/or modify it under the terms of the GNU General  //
00020 // Public License as published by the Free Software Foundation; either  //
00021 // version 2 of the License, or (at your option) any later version.     //
00022 //                                                                      //
00023 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00024 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00025 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00026 // PURPOSE.  See the GNU General Public License for more details.       //
00027 //                                                                      //
00028 // You should have received a copy of the GNU General Public License    //
00029 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00030 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00031 // Boston, MA 02111-1307 USA.                                           //
00032 // //////////////////////////////////////////////////////////////////// //
00033 //
00034 // Primary maintainer for this file: Lior Elazary <lelazary@yahoo.com>
00035 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Robots/RobotBrain/HippocampusI.C $
00036 // $Id: HippocampusI.C 10794 2009-02-08 06:21:09Z itti $
00037 //
00038 
00039 #include "Component/ModelManager.H"
00040 #include "Media/FrameSeries.H"
00041 #include "Transport/FrameInfo.H"
00042 #include "Raster/GenericFrame.H"
00043 #include "Image/Image.H"
00044 #include "Image/DrawOps.H"
00045 #include "Image/lapack.H"
00046 #include "Image/MatrixOps.H"
00047 #include "Image/MathOps.H"
00048 #include "GUI/ImageDisplayStream.H"
00049 #include "GUI/DebugWin.H"
00050 #include "Robots/RobotBrain/HippocampusI.H"
00051 #include <math.h>
00052 
00053 
00054 // ######################################################################
00055 HippocampusI::HippocampusI(OptionManager& mgr,
00056     const std::string& descrName, const std::string& tagName) :
00057   ModelComponent(mgr, descrName, tagName),
00058   itsCurrentState(new RobotSimEvents::StateMessage),
00059   itsCurrentGPSPos(new RobotSimEvents::GPSMessage),
00060   itsCurrentMotion(new RobotSimEvents::MotionMessage),
00061   itsCurrentLandmarks(new RobotSimEvents::LandmarksMessage),
00062   itsLastLandmarkState(new RobotSimEvents::StateMessage),
00063   itsLmDistanceMax(0),
00064   itsNumParticles(300),
00065   itsRangeVariance(50),
00066   itsBearingVariance(.5),
00067   itsFrame(0)
00068 {
00069 
00070   itsOfs = nub::ref<OutputFrameSeries>(new OutputFrameSeries(mgr));
00071   addSubComponent(itsOfs);
00072 
00073   //Starting position
00074   itsCurrentState->xPos = 0;
00075   itsCurrentState->yPos = 0;
00076   itsCurrentState->orientation = 0;
00077 
00078   itsLastLandmarkState->xPos = itsCurrentState->xPos;
00079   itsLastLandmarkState->yPos = itsCurrentState->yPos;
00080   itsLastLandmarkState->orientation = itsCurrentState->orientation;
00081 
00082 
00083   itsCurrentGPSPos->xPos = 0;
00084   itsCurrentGPSPos->yPos = 0;
00085   itsCurrentGPSPos->orientation = 0;
00086 
00087   itsCurrentMotion->distance = 0;
00088   itsCurrentMotion->angle = 0;
00089 
00090   itsMap = Image<PixRGB<byte> > (800, 800, ZEROS);
00091 
00092   //Initialize the particles to be at (0,0,0) with equal weight
00093   itsParticles.resize(itsNumParticles);
00094   for(int i=0; i<itsNumParticles; i++)
00095   {
00096     itsParticles.at(i).x     = itsCurrentState->xPos;
00097     itsParticles.at(i).y     = itsCurrentState->yPos;
00098     itsParticles.at(i).theta = itsCurrentState->orientation;
00099     itsParticles.at(i).w     = 1/itsNumParticles;
00100   }
00101 
00102   //init the landmarks
00103   //addLandmark(Landmark(1,  609.6*2, 609.6*7));
00104   //addLandmark(Landmark(2,  609.6*1, 609.6*5));
00105   //addLandmark(Landmark(3,  609.6*1, 609.6*3));
00106   //addLandmark(Landmark(4,  609.6*2, 609.6*2));
00107   //addLandmark(Landmark(5,  609.6*3, 609.6*4));
00108   //addLandmark(Landmark(6,  609.6*4.1, 609.6*2.2));
00109   //addLandmark(Landmark(7,  609.6*5, 609.6*4));
00110   //addLandmark(Landmark(8,  609.6*6, 609.6*4));
00111   //addLandmark(Landmark(9,  609.6*8, 609.6*4));
00112   //addLandmark(Landmark(10, 609.6*8, 609.6*3));
00113   //addLandmark(Landmark(11, 609.6*8, 609.6*6));
00114   //addLandmark(Landmark(12, 609.6*6, 609.6*7));
00115   //addLandmark(Landmark(13, 609.6*4, 609.6*7));
00116 
00117   //addLandmark(Landmark(1,  609.6*2, 609.6*-1));
00118   //addLandmark(Landmark(2,  609.6*4, 609.6*2));
00119   //addLandmark(Landmark(3,  609.6*4, 609.6*0));
00120   //addLandmark(Landmark(4,  609.6*3, 609.6*-1));
00121 
00122   //11_19/Path1
00123   //addLandmark(Landmark("L1",  609.6*2, 609.6*-0.75));
00124   //addLandmark(Landmark("L2",  609.6*-1, 609.6*1));
00125   //addLandmark(Landmark("L3",  609.6*-1, 609.6*4.2));
00126   //addLandmark(Landmark("L4",  609.6*-0.3, 609.6*-1));
00127   //addLandmark(Landmark("L5",  609.6*1, 609.6*5.5));
00128   //addLandmark(Landmark("L6",  609.6*-1.2, 609.6*3));
00129   //addLandmark(Landmark("L7",  609.6*-1.2, 609.6*2));
00130   //addLandmark(Landmark("L8",  609.6*1, 609.6*4));
00131   //addLandmark(Landmark("L9",  609.6*2, 609.6*3));
00132   //addLandmark(Landmark("L10", 609.6*0, 609.6*4));
00133   //addLandmark(Landmark("L11", 609.6*4, 609.6*0));
00134   //addLandmark(Landmark("L12", 609.6*2.2, 609.6*4));
00135   //addLandmark(Landmark("L14", 609.6*2.9, 609.6*6));
00136   //addLandmark(Landmark("L13", 609.6*-1, 609.6*0));
00137 
00138 
00139   //11_21/Path1
00140   Image<double> cov(2,2,ZEROS);
00141   addLandmark(Landmark(string("L1"),  609.6*3,    609.6*-1,   cov));
00142   addLandmark(Landmark(string("L2"),  609.6*-1,   609.6*4,    cov));
00143   addLandmark(Landmark(string("L3"),  609.6*4,    609.6*0,    cov));
00144   addLandmark(Landmark(string("L4"),  609.6*-1.5, 609.6*2,    cov));
00145   addLandmark(Landmark(string("L5"),  609.6*3.5,  609.6*3,    cov));
00146   addLandmark(Landmark(string("L6"),  609.6*1,    609.6*4,    cov));
00147   addLandmark(Landmark(string("L7"),  609.6*3.5,  609.6*1,    cov));
00148   addLandmark(Landmark(string("L8"),  609.6*1,    609.6*5.5,  cov));
00149   addLandmark(Landmark(string("L9"),  609.6*-1.5, 609.6*3,    cov));
00150   addLandmark(Landmark(string("L10"), 609.6*2,    609.6*3,    cov));
00151   addLandmark(Landmark(string("L11"), 609.6*-0.8, 609.6*5,    cov));
00152   addLandmark(Landmark(string("L12"), 609.6*-0.5, 609.6*-1,   cov));
00153   addLandmark(Landmark(string("L13"), 609.6*1,    609.6*-1,   cov));
00154   addLandmark(Landmark(string("L14"), 609.6*2.5,  609.6*6,    cov));
00155 
00156 
00157 }
00158 
00159 // ######################################################################
00160 HippocampusI::~HippocampusI()
00161 {
00162   SimEventsUtils::unsubscribeSimEvents(itsTopicsSubscriptions, itsObjectPrx);
00163 }
00164 
00165 
00166 // ######################################################################
00167 void HippocampusI::init(Ice::CommunicatorPtr ic, Ice::ObjectAdapterPtr adapter)
00168 {
00169 
00170   Ice::ObjectPtr hippPtr = this;
00171   itsObjectPrx = adapter->add(hippPtr,
00172       ic->stringToIdentity("Hippocampus"));
00173 
00174 
00175   IceStorm::TopicPrx topicPrx;
00176 
00177   itsTopicsSubscriptions.push_back(SimEventsUtils::TopicInfo("GPSMessageTopic", topicPrx));
00178   itsTopicsSubscriptions.push_back(SimEventsUtils::TopicInfo("MotionMessageTopic", topicPrx));
00179   itsTopicsSubscriptions.push_back(SimEventsUtils::TopicInfo("LandmarksMessageTopic", topicPrx));
00180 
00181   SimEventsUtils::initSimEvents(ic, itsObjectPrx, itsTopicsSubscriptions);
00182 
00183   itsEventsPub = RobotSimEvents::EventsPrx::uncheckedCast(
00184       SimEventsUtils::getPublisher(ic, "StateMessageTopic")
00185       );
00186 
00187   IceUtil::ThreadPtr hippThread = this;
00188   hippThread->start();
00189 
00190   usleep(10000);
00191 }
00192 
00193 
00194 // ######################################################################
00195 void HippocampusI::run()
00196 {
00197   sleep(1);
00198 
00199   while(1)
00200   {
00201     evolve();
00202     usleep(100000);
00203   }
00204 }
00205 
00206 // ######################################################################
00207 void HippocampusI::evolve()
00208 {
00209   //calculate the new state from the particles (mean)
00210   float xPos = 0, yPos = 0, orientation = 0;
00211   for(int i=0; i<itsNumParticles; i++)
00212   {
00213     xPos += itsParticles.at(i).x;
00214     yPos += itsParticles.at(i).y;
00215     orientation += itsParticles.at(i).theta;
00216   }
00217   xPos        /= itsParticles.size();
00218   yPos        /= itsParticles.size();
00219   orientation /= itsParticles.size();
00220 
00221   itsCurrentState->xPos = xPos;
00222   itsCurrentState->yPos = yPos;
00223   itsCurrentState->orientation = orientation;
00224 
00225   LDEBUG("%f %f %f\n",
00226       itsCurrentState->xPos,
00227       itsCurrentState->yPos,
00228       itsCurrentState->orientation);
00229 
00230   itsEventsPub->updateMessage(itsCurrentState);
00231 }
00232 
00233 // ######################################################################
00234 void HippocampusI::updateMessage(const RobotSimEvents::EventMessagePtr& eMsg, const Ice::Current&)
00235 {
00236   //Get a gps message
00237   if(eMsg->ice_isA("::RobotSimEvents::GPSMessage"))
00238   {
00239     RobotSimEvents::GPSMessagePtr gpsMsg = RobotSimEvents::GPSMessagePtr::dynamicCast(eMsg);
00240     itsCurrentGPSPos->xPos = gpsMsg->xPos;
00241     itsCurrentGPSPos->yPos = gpsMsg->yPos;
00242     itsCurrentGPSPos->orientation = gpsMsg->orientation;
00243   }
00244 
00245   if(eMsg->ice_isA("::RobotSimEvents::MotionMessage"))
00246   {
00247     RobotSimEvents::MotionMessagePtr motionMsg = RobotSimEvents::MotionMessagePtr::dynamicCast(eMsg);
00248     itsCurrentMotion->distance = motionMsg->distance;
00249     itsCurrentMotion->angle = motionMsg->angle;
00250 
00251     updateParticleMotion(itsCurrentMotion);
00252     itsFrame++;
00253 
00254    // printf("%i\t-1\t%f\t%f\n", itsFrame++,
00255    //     itsCurrentMotion->distance,
00256    //     itsCurrentMotion->angle);
00257 
00258   }
00259 
00260   if(eMsg->ice_isA("::RobotSimEvents::LandmarksMessage"))
00261   {
00262 
00263     RobotSimEvents::LandmarksMessagePtr landmarksMsg = RobotSimEvents::LandmarksMessagePtr::dynamicCast(eMsg);
00264     //updateParticleObservation(landmarksMsg);
00265     updateParticleSlamObservation(landmarksMsg);
00266 
00267     itsFrame++;
00268    // for(uint li=0; li<landmarksMsg->landmarks.size(); li++)
00269    // {
00270 
00271    //   printf("%i\t%s\t%f\t%f\n",itsFrame,
00272    //       landmarksMsg->landmarks[li].name.c_str(),
00273    //       landmarksMsg->landmarks[li].range,
00274    //       landmarksMsg->landmarks[li].bearing);
00275    // }
00276 
00277    /////// for(uint i=0; i<itsParticles.size(); i++)
00278    /////// {
00279    ///////   LINFO("%i %f\n",
00280    ///////       i, itsParticles[i].w);
00281    /////// }
00282     ////Save the current landmarks for latter display
00283     itsCurrentLandmarks->landmarks = landmarksMsg->landmarks;
00284 
00285     //find the max particle
00286     int maxP = 0; float maxVal=itsParticles[0].w;
00287     for(uint i=1; i<itsParticles.size(); i++)
00288     {
00289       if (itsParticles[i].w > maxVal)
00290       {
00291         maxVal = itsParticles[i].w;
00292         maxP = i;
00293       }
00294     }
00295 
00296     itsLandmarkDB = itsParticles[maxP].landmarksDB;
00297 
00298 
00299     displayMap();
00300     resampleParticles();
00301 
00302   }
00303   fflush(stdout);
00304 }
00305 
00306 // ######################################################################
00307 void HippocampusI::updateParticleMotion(RobotSimEvents::MotionMessagePtr newMotion)
00308 {
00309   /* Needed
00310      double time; //time for this newMotion, because each sampling is different
00311      double commandF; //forward or backward
00312      double commandS; //steer
00313      */
00314   double alpha[4] = {0.002, 0, 0.2, 0};
00315 
00316   // check out values
00317   //printf("%f %f %f %f %f\n", newMotion->distance, newMotion->angle,
00318   //    itsParticles.at(0).x, itsParticles.at(0).y, itsParticles.at(0).theta);
00319 
00320   //Move each particle according to the motion model, plus some noise
00321   if(abs(newMotion->angle)<10 && abs(newMotion->distance)<25) {
00322     itsParticlesMutex.lock();
00323 
00324     for(int i=0; i<itsNumParticles; i++)
00325     {
00326       //odometry data
00327       float theta = itsParticles.at(i).theta;
00328       float dx = newMotion->distance*cos(theta);
00329       float dy = newMotion->distance*sin(theta);
00330 
00331 //      itsParticles.at(i).x     += dx;
00332 //      itsParticles.at(i).y     += dy;
00333 //      itsParticles.at(i).theta += newMotion->angle*M_PI/180;
00334 //
00335       float rot1  = atan2(dy, dx) - theta;
00336       float trans = sqrt(pow(dx,2)+pow(dy,2));
00337       float rot2  = (theta+newMotion->angle*M_PI/180) - theta - rot1;
00338 
00339       //noise
00340       float nr1 = rot1  - randomDoubleFromNormal(alpha[0]*abs(rot1)+alpha[1]*abs(trans));      //rot1
00341       float ntr = trans - randomDoubleFromNormal(alpha[2]*abs(trans)+alpha[3]*abs(rot1+rot2)); //trans
00342       float nr2 = rot2  - randomDoubleFromNormal(alpha[0]*abs(rot2)+alpha[1]*abs(trans));      //rot2
00343 
00344       //new particle
00345       itsParticles.at(i).x     += ntr*cos(theta+nr1);
00346       itsParticles.at(i).y     += ntr*sin(theta+nr1);
00347       itsParticles.at(i).theta += nr1 + nr2;
00348 
00349       //Bound the angle to -PI <-> PI
00350       if(itsParticles.at(i).theta > M_PI)
00351       {
00352         itsParticles.at(i).theta -= 2*M_PI;
00353 
00354       }
00355       if(itsParticles.at(i).theta < M_PI)
00356       {
00357         itsParticles.at(i).theta += 2*M_PI;
00358       }
00359     }
00360     itsParticlesMutex.unlock();
00361   }
00362 
00363 }
00364 
00365 // ######################################################################
00366 void HippocampusI::updateParticleSlamObservation(RobotSimEvents::LandmarksMessagePtr landmarksMsg)
00367 {
00368   itsParticlesMutex.lock();
00369   float sum = 0;
00370   std::vector<Particle>::iterator particleIt =
00371     itsParticles.begin();
00372 
00373   for(; particleIt != itsParticles.end(); particleIt++)
00374   {
00375     for(uint li=0; li<landmarksMsg->landmarks.size(); li++)
00376     {
00377 
00378       std::string landmarkName = landmarksMsg->landmarks[li].name;
00379       float range = landmarksMsg->landmarks[li].range*1000;
00380       float bearing = landmarksMsg->landmarks[li].bearing;
00381 
00382       //Check to make sure we know about this landmark
00383       std::map<std::string, Landmark>::iterator landIt =
00384         particleIt->landmarksDB.find(landmarkName);
00385 
00386       //Landmark currLand = landIt->second;
00387 
00388       Image<double>  sensorNoise(2,2, ZEROS);
00389       sensorNoise.setVal(0,0, squareOf(500)); //Range noise
00390       sensorNoise.setVal(1,1, squareOf(5*M_PI/180)); //bearing noise
00391 
00392 
00393       //If we don't have the landmark in our database, then let's add it.
00394       if(landIt == particleIt->landmarksDB.end())
00395       {
00396         //initalize landmark mean as u_j = g-1(z,x);
00397 
00398         double landmarkXPos = particleIt->x +  range*cos(bearing + particleIt->theta);
00399         double landmarkYPos = particleIt->y +  range*sin(bearing + particleIt->theta);
00400 
00401         //initalize covariance
00402         double dx = landmarkXPos - particleIt->x;
00403         double dy = landmarkYPos - particleIt->y;
00404         double d2 = squareOf(dx) + squareOf(dy);
00405         double d = sqrt(d2);
00406 
00407 
00408         Image<double> G(2,2, NO_INIT);
00409         G.setVal(0,0, dx/d); G.setVal(1,0, dy/d);
00410         G.setVal(0,1, -dy/d2); G.setVal(1,1, dx/d2);
00411 
00412         Image<double> Ginv = matrixInv(G);
00413 
00414 
00415         Image<double> cov = matrixMult(matrixMult(Ginv,sensorNoise),transpose(Ginv));
00416 
00417         //add the landmark to the DB
00418         Landmark lm(landmarkName, landmarkXPos, landmarkYPos, cov);
00419         particleIt->landmarksDB[landmarkName] = lm;
00420         particleIt->w *= 0.00001;
00421       }
00422       else {
00423 
00424         //Use the Cholesky factorization for the EKF update
00425 
00426         //we have the landmark calculate the change in landmark position
00427         //predict the mesurment z=g(u,x)
00428         double landmarkXPos = landIt->second.posX;
00429         double landmarkYPos = landIt->second.posY;
00430 
00431         double dx = landmarkXPos - particleIt->x;
00432         double dy = landmarkYPos - particleIt->y;
00433         double d2 = squareOf(dx) + squareOf(dy);
00434         double d = sqrt(d2);
00435 
00436         double beliefRange = d;
00437         double beliefBearing = atan2(dy, dx) - particleIt->theta;
00438 
00439         //Limit the angle between -pi to pi
00440         if (beliefBearing < M_PI)
00441         {
00442           beliefBearing += 2*M_PI;
00443         }
00444         if (beliefBearing > M_PI)
00445         {
00446           beliefBearing -= 2*M_PI;
00447         }
00448 
00449 
00450         Image<double> G(2,2, NO_INIT);
00451         G.setVal(0,0, dx/d); G.setVal(1,0, dy/d);
00452         G.setVal(0,1, -dy/d2); G.setVal(1,1, dx/d2);
00453 
00454         Image<double> innov(1,2,NO_INIT);
00455         innov.setVal(0,0, range - beliefRange);
00456         innov.setVal(0,1, bearing - beliefBearing);
00457 
00458 
00459         Image<double> PHt = matrixMult(landIt->second.cov,transpose(G));
00460         Image<double> S = matrixMult(G, PHt) + sensorNoise;
00461 
00462         //Make symmetric
00463         S = S+transpose(S);
00464         S = S *0.5;
00465 
00466         Image<double> SChol = lapack::dpotrf(&S);
00467         SChol.setVal(0,1,0); //Set the bottom left to 0 ?
00468 
00469         Image<double> SCholInv = matrixInv(SChol);
00470 
00471 
00472         Image<double> W1 = matrixMult(PHt, SCholInv);
00473         Image<double> K = matrixMult(W1, transpose(SCholInv));
00474 
00475 
00476         Image<double> dLandmarkPos = matrixMult(K,innov);
00477         landIt->second.posX += dLandmarkPos.getVal(0,0);
00478         landIt->second.posY += dLandmarkPos.getVal(0,1);
00479 
00480         landIt->second.cov -= matrixMult(W1, transpose(W1));
00481 
00482 
00483         Image<double> Q = matrixMult(matrixMult(G, landIt->second.cov),transpose(G)) + sensorNoise;
00484 
00485         Image<double> diffSq = matrixMult(matrixMult(transpose(innov), matrixInv(Q)), innov);
00486 
00487         particleIt->w *= exp(-0.5*diffSq.getVal(0,0)) + 0.001;
00488 
00489       }
00490     }
00491 
00492     sum += particleIt->w;
00493 
00494   }
00495 
00496   //normalize
00497   for(uint i=0; i<itsParticles.size(); i++)
00498     itsParticles.at(i).w /= sum;
00499 
00500   itsParticlesMutex.unlock();
00501 }
00502 
00503 // ######################################################################
00504 void HippocampusI::updateParticleObservation(RobotSimEvents::LandmarksMessagePtr landmarksMsg)
00505 {
00506   itsParticlesMutex.lock();
00507   float sum = 0;
00508   for(int particle=0; particle<itsNumParticles; particle++)
00509   {
00510     for(uint li=0; li<landmarksMsg->landmarks.size(); li++)
00511     {
00512       //Check to make sure we know about this landmark
00513       std::map<std::string, Landmark>::iterator landIt = itsLandmarkDB.find(landmarksMsg->landmarks.at(li).name);
00514 
00515       //If we don't have the landmark in our database, then let's add it.
00516       if(landIt == itsLandmarkDB.end())
00517       {
00518         //addLandmark(landmarksMsg->landmarks.at(li));
00519         continue;
00520       }
00521 
00522       Landmark currLand = landIt->second;
00523 
00524       float bearing =  landmarksMsg->landmarks[li].bearing;
00525       float range   =  landmarksMsg->landmarks[li].range*1000;
00526 
00527       float beliefRange    = sqrt(pow(currLand.posY - itsParticles.at(particle).y, 2) +
00528                                   pow(currLand.posX - itsParticles.at(particle).x, 2));
00529       float beliefBearing  = atan2(currLand.posY - itsParticles.at(particle).y,
00530                                    currLand.posX - itsParticles.at(particle).x)
00531                              - itsParticles.at(particle).theta;
00532 
00533       if (beliefBearing < M_PI)
00534         beliefBearing += 2*M_PI;
00535       if (beliefBearing > M_PI)
00536         beliefBearing -= 2*M_PI;
00537 
00538       float innovRange   = range   - beliefRange;
00539       float innovBearing = bearing - beliefBearing;
00540 
00541 
00542       //TODO:Make this a 2D gaussian
00543       float prob = exp(-0.5*pow(innovRange,   2) / itsRangeVariance  ) + .0001;
00544       prob =   exp(-0.5*pow(innovBearing, 2) / itsBearingVariance) + .0001;
00545 
00546       itsParticles.at(particle).w = prob;
00547       //LINFO("%i %f\n", particle, prob);
00548     }
00549     sum += itsParticles.at(particle).w;
00550   }
00551 
00552   //normalize
00553   for(uint i=0; i<itsParticles.size(); i++)
00554     itsParticles.at(i).w /= sum;
00555 
00556   itsParticlesMutex.unlock();
00557 }
00558 
00559 // ######################################################################
00560 void HippocampusI::addLandmark(Landmark lm)
00561 {
00562   //Initalize the landmark position with a fixed range
00563   LINFO("Adding Landmark %s pos", lm.name.c_str());
00564   itsGTLandmarkDB[lm.name] = lm;
00565 }
00566 
00567 
00568 
00569 // ######################################################################
00570 void HippocampusI::resampleParticles()
00571 {
00572   //Use a roulette wheel to probabilistically duplicate particles with high weights,
00573   //and discard those with low weights
00574   itsParticlesMutex.lock();
00575 
00576   std::vector<Particle> newParticles;
00577 
00578   //Calculate a Cumulative Distribution Function for our particle weights
00579   std::vector<float> CDF;
00580   CDF.resize(itsParticles.size());
00581 
00582   CDF.at(0) = itsParticles.at(0).w;
00583   for(uint i=1; i<CDF.size(); i++)
00584     CDF.at(i) = CDF.at(i-1) + itsParticles.at(i).w;
00585 
00586   uint i = 0;
00587   float u = randomDouble()* 1.0/float(itsParticles.size());
00588 
00589   for(uint j=0; j < itsParticles.size(); j++)
00590   {
00591     while(u > CDF.at(i))
00592       i++;
00593 
00594     Particle p = itsParticles.at(i);
00595     p.w     = 1.0/float(itsParticles.size());
00596 
00597     newParticles.push_back(p);
00598 
00599     u += 1.0/float(itsParticles.size());
00600   }
00601 
00602   itsParticles = newParticles;
00603 
00604   itsParticlesMutex.unlock();
00605 }
00606 
00607 // ######################################################################
00608 void HippocampusI::displayMap()
00609 {
00610   int circRadius = 10;
00611   Image<PixRGB<byte> > tmpMap = itsMap;
00612 
00613 
00614  // //Graph the average distance between known landmarks, and their ground truthed positions
00615  // if(itsGTLandmarkDB.size() > 0) {
00616  //   std::map<std::string, Landmark>::iterator lIt = itsLandmarkDB.begin();
00617  //   float lmDistance = 0.0;
00618  //   for(;lIt != itsLandmarkDB.end(); lIt++)
00619  //   {
00620  //     //Make sure we have a ground truth of the found landmark
00621  //     std::map<std::string, Landmark>::iterator matchIt = itsGTLandmarkDB.find(lIt->second.name);
00622  //     if(matchIt != itsLandmarkDB.end())
00623  //     {
00624  //       lmDistance += sqrt(
00625  //           squareOf(matchIt->second.posX - lIt->second.posX) +
00626  //           squareOf(matchIt->second.posY - lIt->second.posY)
00627  //           );
00628  //     }
00629  //   }
00630  //   if(lmDistance > itsLmDistanceMax)
00631  //     itsLmDistanceMax = lmDistance;
00632 
00633  //   itsLmDistanceHistory.push_back(lmDistance);
00634  //
00635  //   Image<PixRGB<byte> > distanceGraph = linePlot(itsLmDistanceHistory, tmpMap.getWidth(), 150, 0, itsLmDistanceMax, "t");
00636  //   itsOfs->writeRGB(distanceGraph, "DistanceGraph", FrameInfo("DistanceGraph", SRC_POS));
00637  // }
00638 
00639   Image<float> probMap(itsMap.getDims(), ZEROS);
00640 
00641   //Draw the State on the map;
00642   float scale = 1.0/7.0;
00643   Point2D<int> offset((int)(609.6*3.0*scale), (int)(609.6*2.0*scale));
00644 
00645   Point2D<int> p1 = Point2D<int>(scale*609.6*0   , scale*609.6*0) + offset;
00646   Point2D<int> p2 = Point2D<int>(scale*609.6*3   , scale*609.6*0) + offset;
00647   Point2D<int> p3 = Point2D<int>(scale*609.6*3   , scale*609.6*5) + offset;
00648   Point2D<int> p4 = Point2D<int>(scale*609.6*-0.5, scale*609.6*5) + offset;
00649   Point2D<int> p5 = Point2D<int>(scale*609.6*-0.5, scale*609.6*0) + offset;
00650 
00651   drawLine(tmpMap, p1, p2, PixRGB<byte>(50,50,50));
00652   drawLine(tmpMap, p2, p3, PixRGB<byte>(50,50,50));
00653   drawLine(tmpMap, p3, p4, PixRGB<byte>(50,50,50));
00654   drawLine(tmpMap, p4, p5, PixRGB<byte>(50,50,50));
00655   drawLine(tmpMap, p5, p1, PixRGB<byte>(50,50,50));
00656 
00657   //Draw the particles on the map
00658   for(uint particle=0; particle<itsParticles.size(); particle++) {
00659     Point2D<int> particlePos( itsParticles.at(particle).x*scale,
00660         itsParticles.at(particle).y*scale);
00661     float prob = itsParticles.at(particle).w;
00662     if (probMap.coordsOk(offset + particlePos))
00663         probMap.setVal(offset + particlePos, prob);
00664   }
00665 
00666   //Draw the ground truth landmarks on the map
00667   std::map<std::string, Landmark>::iterator landIt = itsGTLandmarkDB.begin();
00668   for(;landIt != itsGTLandmarkDB.end(); ++landIt)
00669   {
00670     Point2D<int> lmPos(
00671         (*landIt).second.posX * scale,
00672         (*landIt).second.posY * scale
00673         );
00674     drawCross(tmpMap, lmPos + offset, PixRGB<byte>(0,100, 0));
00675 
00676   }
00677 
00678 
00679   //Draw the leading particle's landmarks on the map
00680   landIt = itsLandmarkDB.begin();
00681   for(;landIt != itsLandmarkDB.end(); ++landIt)
00682   {
00683     Point2D<int> lmPos(
00684         (*landIt).second.posX * scale,
00685         (*landIt).second.posY * scale
00686         );
00687 
00688     if(tmpMap.coordsOk(offset+lmPos) &&
00689         tmpMap.coordsOk(offset+lmPos+Point2D<int>(landIt->second.name.size()*10, circRadius*2.5)))
00690     {
00691       Image<PixRGB<byte> > labelImage(landIt->second.name.size()*10, circRadius*2.5, ZEROS);
00692 
00693       drawCircle(labelImage, Point2D<int>(circRadius,circRadius), circRadius, PixRGB<byte>(0,0,150),0);
00694 
00695       writeText(
00696           labelImage,
00697           Point2D<int>(circRadius/2,circRadius/2),
00698           landIt->second.name.c_str(),
00699           PixRGB<byte>(75,75,255),
00700           PixRGB<byte>(0,0,0),
00701           SimpleFont::FIXED(6),
00702           true
00703           );
00704 
00705       labelImage = flipVertic(labelImage);
00706       inplacePaste(tmpMap, labelImage, offset+lmPos-Point2D<int>(circRadius, circRadius));
00707     }
00708   }
00709 
00710   //draw the lines to the landmark
00711   for(uint li=0; li<itsCurrentLandmarks->landmarks.size(); li++)
00712   {
00713     float bearing = itsCurrentLandmarks->landmarks[li].bearing;
00714     float range = (itsCurrentLandmarks->landmarks[li].range*1000);
00715 
00716     float posX = itsCurrentState->xPos + (range*cos(itsCurrentState->orientation + bearing));
00717     float posY = itsCurrentState->yPos + (range*sin(itsCurrentState->orientation + bearing));
00718 
00719     drawLine(tmpMap,
00720         offset + Point2D<int>((int)(itsCurrentState->xPos*scale),
00721           ((int)(itsCurrentState->yPos*scale))),
00722         offset + Point2D<int>((int)(posX*scale), ((int)(posY*scale))),
00723         PixRGB<byte>(0,0,255));
00724   }
00725 
00726   Point2D<int> posOnMap((int)(itsCurrentState->xPos*scale),
00727       (int)( itsCurrentState->yPos*scale));
00728 
00729   //Draw the robot
00730   if (itsMap.coordsOk(offset + posOnMap))
00731   {
00732     itsMap.setVal(offset + posOnMap, PixRGB<byte>(255,0,0));
00733     drawDisk(tmpMap, offset + posOnMap, 10, PixRGB<byte>(100,200,100));
00734 
00735     Point2D<int> robotAng(int(cos(itsCurrentState->orientation)*9),
00736                           int(sin(itsCurrentState->orientation)*9));
00737 
00738     drawLine(tmpMap, offset + posOnMap, offset + posOnMap + robotAng, PixRGB<byte>(0,0,0), 2);
00739   }
00740 
00741   inplaceNormalize(probMap, 0.0F, 255.0F);
00742 
00743   //Print out the frame number and the believed robot position
00744   printf("%d %d %d ", itsFrame, posOnMap.i, posOnMap.j);
00745   std::map<std::string, Landmark>::iterator gtIt = itsGTLandmarkDB.begin();
00746   for(;gtIt != itsGTLandmarkDB.end(); gtIt++) {
00747     //Print out the name of this landmark
00748     printf("%s ", gtIt->second.name.c_str());
00749 
00750     //See if our current landmark DB knows about this ground truth
00751     std::map<std::string, Landmark>::iterator lIt = itsLandmarkDB.find(gtIt->second.name);
00752 
00753     if(lIt == itsLandmarkDB.end())
00754     {
00755       //If it does not, then output zeros for the position
00756       printf("0 0 ");
00757     }
00758     else
00759     {
00760       //If it does, then print out the believed position of this landmark
00761       printf("%f %f ", lIt->second.posX, lIt->second.posY);
00762     }
00763   }
00764   printf("\n");
00765 
00766 
00767   itsOfs->writeRGB(flipVertic(tmpMap), "Map", FrameInfo("Map", SRC_POS));
00768   itsOfs->writeRGB(flipVertic(toRGB(probMap)), "ProbMap", FrameInfo("Map", SRC_POS));
00769 }
00770 
Generated on Sun May 8 08:41:31 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3