Localization.C

00001 #include "Robots/SeaBeeIII/Localization.H"
00002 #include "Transport/FrameInfo.H"
00003 #include "Image/ColorOps.H"
00004 
00005 // ######################################################################
00006 /**
00007  *        \fun LocaLization Constructor function
00008  *        Initializes the Localization class
00009  *
00010  *
00011  *
00012  */
00013         Localization::Localization(OptionManager& mgr, const std::string& descrName, const std::string& tagName) :
00014           RobotBrainComponent(mgr, descrName, tagName),
00015           itsOfs(new OutputFrameSeries(mgr)),
00016           itsRunning(true),
00017           itsNumParticles(1000),
00018           itsKeepParticles(1000),
00019           generator(42u),
00020           generate_drift(generator, dist_type(M_DRIFT,SIGMA_DRIFT)),
00021           generate_trans(generator, dist_type(M_TRANS,SIGMA_TRANS))
00022         {
00023                 //// The images used for plotting the particles
00024                 iMap = Image<float > (2000,2000,ZEROS);
00025                 originalMap = Image<float > (2000,2000,ZEROS);
00026 
00027                 addSubComponent(itsOfs);
00028                 initPoints(itsNumParticles);
00029         }
00030 /**
00031  *        \fun registerTopics
00032  *        For registering the messages that we want to listen to and publish
00033  *
00034  *
00035  *
00036  */
00037 void Localization::registerTopics()
00038 {
00039   LINFO("Registering Localization Message");
00040   //this->registerPublisher("RetinaMessageTopic");
00041   this->registerSubscription("ObstacleMessageTopic");
00042   this->registerSubscription("OrientationMessageTopic");
00043 }
00044 /**
00045  *        \fun initPoints
00046  *        initialize the points in the image
00047  *
00048  *
00049  *
00050  */
00051 void Localization::initPoints(const short int no = 1000){
00052 
00053                 pList = vector<pParticle>();
00054 
00055                 pParticle p;
00056                 for (int i = 0 ; i < no ; i++){
00057                         p = pParticle(1000,0);
00058                         p.setProb(1.0);
00059                         pList.push_back(p);
00060                 }
00061                 itsNumParticles = no;
00062         }
00063 /**
00064  *        \fun getNormalizer
00065  *        Returns the total probability of all the particles
00066  *
00067  *
00068  *
00069  */
00070         float Localization::getNormalizer(){
00071 
00072                 float totalProb = 0.0;
00073 
00074                 for (int i = 0 ; i < itsNumParticles ; i++){
00075                         totalProb += pList[i].getProb();
00076                         iMap.setVal( pList[i].p.j, pList[i].p.i, 255*pList[i].getProb() );
00077                 }
00078 
00079                 return totalProb;
00080         }
00081 /**
00082  *        \fun evolve
00083  *
00084  */        void Localization::evolve()
00085         {
00086                 return;
00087         }
00088 
00089 // ######################################################################
00090 /**
00091  *        \fun updateMessage
00092  *        Returns the total probability of all the particles
00093  *
00094  *
00095  *
00096  */
00097 void Localization::updateMessage(const RobotSimEvents::EventMessagePtr& eMsg,
00098     const Ice::Current&)
00099 {
00100   if(eMsg->ice_isA("::RobotSimEvents::OrientationMessage"))
00101   {
00102 
00103           RobotSimEvents::OrientationMessagePtr oMsg = RobotSimEvents::OrientationMessagePtr::dynamicCast(eMsg);
00104         LINFO("Message is a camera config message: orientation of sub = %d, sub running status = %d", oMsg->orientation, oMsg->running);
00105 
00106         // First check if the population has depleted or not ... if it has then resample
00107         if(itsNumParticles < BETA * INIT_PARTICLES)
00108                 resample(INIT_PARTICLES);
00109 
00110         predict(oMsg);
00111 
00112   }else if(eMsg->ice_isA("::RobotSimEvents::ObstacleMessage")) {
00113 
00114     RobotSimEvents::ObstacleMessagePtr oMsg = RobotSimEvents::ObstacleMessagePtr::dynamicCast(eMsg);
00115     LINFO("Message is a camera config message: camera1 distance = %d, orientation = %d", oMsg->distanceOfObstacle1, oMsg->orientation1);
00116 
00117     update(oMsg);
00118 
00119   }
00120 }
00121 /**
00122  *        \fun predict
00123  *        predict the new position based on the updates received from the compass
00124  *
00125  *
00126  *
00127  */
00128 
00129 void Localization::predict(RobotSimEvents::OrientationMessagePtr oMsg){
00130 // This function will contain the code for predicting the next position of points using gaussian distribution in
00131 // for the error in their movement
00132 
00133 
00134         if(oMsg->running){
00135         // sub is moving forward ... let's predict the sub's position with the model that we have
00136                 // so we will use orientation from the message and forward the points in that direction according to the time elapsed
00137 
00138                 time_t newtime,elapsedTime;
00139                 time(&newtime);
00140                 elapsedTime = newtime - timestamp ;
00141 
00142                 if( elapsedTime == 0 )
00143                         return;
00144                 else {
00145                         //clear the image
00146                         iMap = Image<float > (2000,2000,ZEROS);
00147 
00148                         // First retrieve the distance travelled in current orientation
00149                         int distance = elapsedTime * BOT_SPEED;
00150                         int changeInOrientation = curOrientation - oMsg->orientation;
00151 
00152                         float Edrift=0, Etrans=0;
00153                         // Now let's run the particles through the prediction equations to see their new positions
00154                         // There are two sources of error for forward movement. One is distance and the other one is orientation
00155                         // We will have to model errors in both of them.
00156                         for (std::vector<class pParticle>::iterator iter = pList.begin(); iter != pList.end() ; iter++ )
00157                         {
00158                                 ///  First erase the current position of the point
00159                                 iMap.setVal((iter->p).j, (iter->p).i, 0.0 );
00160                                 // Do the update on each particles using the equation
00161                                 // SIGMA_TRANS = SIGMA_TRANS * sqrt(EXPERIMENT_DIST) // EXPERIMENT_DIST in centimeters preferable
00162                                 // SIGMA_DRIFT = SIGMA_DRIFT * sqrt(EXPERIMENT_DIST/2.0)
00163                                 // We will consider SIGMA_TRANS and SIGMA_DRIFT on a unit distance
00164                                 // here rand_N is a random number generator, yet to be implemented
00165                                 Etrans = generate_trans() * distance; //rand_N (M_TRANS * distance, SIGMA_TRANS * distance);
00166 
00167                                 //This is for a completely different purpose. here we know that it we rotated certain degrees... but this thing just models any error
00168                                 // in the measures taken by compass ... so set the distributions accordingly
00169                                 Edrift = generate_drift() * changeInOrientation; //rand_N (M_DRIFT * changeInOrientation, SIGMA_DRIFT * changeInOrientation);
00170 
00171                                 // First adjust for the drift
00172                                 iter->orientation = iter->orientation + Edrift;
00173                                 // Now adjust for the error in distance travelled
00174                                 (iter->p).i = (iter->p).i + (distance+Etrans)*cos(iter->orientation);
00175                                 (iter->p).j = (iter->p).j + (distance+Etrans)*sin(iter->orientation);
00176                                 // One more iteration to calculate the drift in orientation
00177 
00178                                 Edrift = generate_drift() * changeInOrientation; //rand_N (M_DRIFT * distance, SIGMA_TRANS * distance);
00179 
00180                                 iter->orientation = iter->orientation + Edrift;
00181                                 // set the point in the image
00182                                 iMap.setVal( (iter->p).j, (iter->p).i, 255*iter->getProb() );
00183                         }
00184                 }
00185 
00186         } else {
00187                 // if it is possible to be not moving and still changing the orientation then do add an orientation change loop here.
00188 
00189                 // if we are not moving, then just update the timestamp... it is just a message to say that we are not moving right now.
00190                         time(&timestamp);
00191         }
00192 
00193         //
00194         getCurrentEstimate();
00195         iMap.setVal( maxPoint.p.j, maxPoint.p.i, 255);
00196 
00197         // we have estimate in maxpoint ... now plot it on the map
00198 
00199         // write the image out to the map
00200         itsOfs->writeRGB(iMap, "iMap", FrameInfo("IMap", SRC_POS));
00201         itsOfs->writeRGB(luminance(iMap), "B/W_iMap", FrameInfo("B/W_IMap", SRC_POS));
00202         itsOfs->updateNext();
00203 }
00204 
00205 /**
00206  *        \fun update
00207  *        update the position stats according to the obstacle position
00208  *
00209  */
00210 void Localization::update(RobotSimEvents::ObstacleMessagePtr oMsg ){
00211 // change the ObstacleMessageTopic to LandmarkMessageTopic
00212 // First we will obtain the point based on obstacle message
00213 Point2D p = maxPoint;
00214 p.j = p.j + sin(oMsg->orientation) * distanceOfObstacle;
00215 p.i = p.i + cos(oMsg->orientation) * distanceOfObstacle;
00216 //maxPoint.p.j maxPoint.p.i
00217 
00218 
00219 }
00220 /**
00221  *        \fn void Localization::resample(int no)
00222  *        resample the population and remove the unrelevant particles
00223  *
00224  */
00225 void Localization::resample(int no){
00226 
00227 // Calculate the mean of the population
00228         if(no != 0)
00229          itsKeepParticles = no;
00230 
00231         float totalProb=getNormalizer();
00232         float mean = totalProb/itsNumParticles;
00233         float stddev = 0.0 ;
00234         float minProb = 0.0 ;
00235         for (std::vector<class pParticle>::iterator iter = pList.begin(); iter != pList.end() ; iter++ )
00236         {
00237                 stddev += ( iter->prob - mean ) * ( iter->prob - mean );
00238                 if ( iter -> prob < minProb )
00239                         minProb = iter->prob;
00240 
00241         }
00242         stddev = stddev / itsNumParticles;
00243         stddev = sqrt ( stddev );
00244 
00245         // Now since we have standard deviation and mean at hand we can calculate what particles to say good bye to
00246         // if particles' probability is less than mean - ( (stddev + (mean - minProb - stddev)) / 2 ) and not greater than 0.01, then we will discard them
00247 
00248         for (std::vector<class pParticle>::iterator iter = pList.begin(); iter != pList.end() ; iter++ )
00249         {
00250                 if( iter -> prob < 0.01 &&  ( iter -> prob < mean - (stddev + (mean - minProb - stddev ) )/2.0 ) )
00251                 {
00252                         pList.erase(iter);
00253                         itsNumParticles --;
00254                 }
00255 
00256         }
00257 
00258         std::sort(pList.begin(), pList.end(), greater<pParticle>() );
00259 
00260 }
00261 /**
00262  *        \fun getCurrentEstimate
00263  *        Return the current estimate
00264  *
00265  */
00266 class pParticle& Localization::getCurrentEstimate(){
00267                 // We can use several method for estimating position.
00268                 // This implementation is using max
00269                         float maxProb = 0;
00270                         std::vector<class pParticle>::iterator iterEst;
00271                         for (std::vector<class pParticle>::iterator iter = pList.begin(); iter != pList.end() ; iter++ )
00272                         {
00273                                 if(iter->prob > maxProb)
00274                                 {
00275                                         maxProb = iter->prob;
00276                                         iterEst = iter;
00277                                 }
00278                         }
00279                         return *iterEst;
00280                 // Here I have used weighted mean
00281 /*
00282                         float maxProb = 0;
00283                         float x=0,y=0;
00284                         for (std::vector<class pParticle>::iterator *iter = pList.begin(); iter != pList.end() ; iter++ )
00285                         {
00286                                 x += (iter->p).x * iter->prob;
00287                                 y += (iter->p).y * iter->prob;
00288                         }
00289                         return ( new pParticle(x,y) );
00290 */
00291 
00292 }
00293 
00294 /**
00295  *        \fun ~Localization()
00296  *        Destructor for the Localization class.
00297  *        Returns the total probability of all the particles
00298  *
00299  */
00300 Localization::~Localization()
00301 {
00302         // delete all the left out particles from memory
00303         pList.clear();
00304 }
00305 
00306 
00307 
00308 //Image<float>::iterator itr = a.beginw(), stop = a.endw();
00309 //while (itr != stop)
00310   //*itr++ = 42;
00311 
00312 /*
00313 for(Image<float>::iterator itr = a.beginw(), stop = a.endw();
00314     itr != stop;
00315     ++itr)
00316   {
00317     *itr = 42;
00318   }
00319 
00320 */
00321 
00322 /*                struct timeval newtime;
00323                 long double elapsedTime;
00324 
00325                 gettimeofday (&newtime,NULL);
00326                    elapsedTime = newtime.tv_usec - timestamp.tv_usec;
00327                 elapsedTime = elapsedTime / 1000000.0; //convert into seconds
00328 
00329                 printf("%LF - ",elapsedTime);
00330 
00331                 if (elapsedTime * BOT_SPEED)*/
00332 
Generated on Sun May 8 08:41:38 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3