Main MRPT website > C++ reference
MRPT logo

PF_implementations.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 
00029 #ifndef PF_implementations_H
00030 #define PF_implementations_H
00031 
00032 #include <mrpt/utils/stl_extensions.h>
00033 #include <mrpt/bayes/CParticleFilterData.h>
00034 #include <mrpt/random.h>
00035 #include <mrpt/slam/CActionCollection.h>
00036 #include <mrpt/slam/CActionRobotMovement3D.h>
00037 #include <mrpt/slam/CActionRobotMovement2D.h>
00038 #include <mrpt/slam/TKLDParams.h>
00039 
00040 #include <mrpt/math/distributions.h>  // chi2inv
00041 
00042 #include <mrpt/slam/PF_implementations_data.h>
00043 
00044 #include <mrpt/slam/link_pragmas.h>
00045 
00046 
00047 /** \file PF_implementations.h
00048   *  This file contains the implementations of the template members declared in mrpt::slam::PF_implementation
00049   */
00050 
00051 namespace mrpt
00052 {
00053         namespace slam
00054         {
00055                 using namespace std;
00056                 using namespace mrpt::utils;
00057                 using namespace mrpt::random;
00058                 using namespace mrpt::poses;
00059                 using namespace mrpt::bayes;
00060                 using namespace mrpt::math;
00061 
00062                 /** Auxiliary method called by PF implementations: return true if we have both action & observation,
00063                   *   otherwise, return false AND accumulate the odometry so when we have an observation we didn't lose a thing.
00064                   *   On return=true, the "m_movementDrawer" member is loaded and ready to draw samples of the increment of pose since last step.
00065                   *  This method is smart enough to accumulate CActionRobotMovement2D or CActionRobotMovement3D, whatever comes in.
00066                   */
00067                 template <class PARTICLE_TYPE,class MYSELF>
00068                 template <class BINTYPE>
00069                 bool PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_gatherActionsCheckBothActObs(
00070                         const CActionCollection * actions,
00071                         const CSensoryFrame             * sf )
00072                 {
00073                         MYSELF *me = static_cast<MYSELF*>(this);
00074 
00075                         if (actions!=NULL)      // A valid action?
00076                         {
00077                                 {
00078                                         CActionRobotMovement2DPtr       robotMovement2D = actions->getBestMovementEstimation();
00079                                         if (robotMovement2D.present())
00080                                         {
00081                                                 if (m_accumRobotMovement3DIsValid) THROW_EXCEPTION("Mixing 2D and 3D actions is not allowed.")
00082 
00083                                                 if (!m_accumRobotMovement2DIsValid)
00084                                                 {               // First time:
00085                                                         robotMovement2D->poseChange->getMean( m_accumRobotMovement2D.rawOdometryIncrementReading );
00086                                                         m_accumRobotMovement2D.motionModelConfiguration = robotMovement2D->motionModelConfiguration;
00087                                                 }
00088                                                 else  m_accumRobotMovement2D.rawOdometryIncrementReading += robotMovement2D->poseChange->getMeanVal();
00089 
00090                                                 m_accumRobotMovement2DIsValid = true;
00091                                         }
00092                                         else // If there is no 2D action, look for a 3D action:
00093                                         {
00094                                                 CActionRobotMovement3DPtr       robotMovement3D = actions->getActionByClass<CActionRobotMovement3D>();
00095                                                 if (robotMovement3D)
00096                                                 {
00097                                                         if (m_accumRobotMovement2DIsValid) THROW_EXCEPTION("Mixing 2D and 3D actions is not allowed.")
00098 
00099                                                         if (!m_accumRobotMovement3DIsValid)
00100                                                                         m_accumRobotMovement3D = robotMovement3D->poseChange;
00101                                                         else    m_accumRobotMovement3D += robotMovement3D->poseChange;
00102                                                         // This "+=" takes care of all the Jacobians, etc... You MUST love C++!!! ;-)
00103 
00104                                                         m_accumRobotMovement3DIsValid = true;
00105                                                 }
00106                                                 else
00107                                                         return false; // We have no actions...
00108                                         }
00109                                 }
00110                         }
00111 
00112                         const bool SFhasValidObservations = (sf==NULL) ? false : PF_SLAM_implementation_doWeHaveValidObservations(me->m_particles,sf);
00113 
00114                         // All the things we need?
00115                         if (! ((m_accumRobotMovement2DIsValid || m_accumRobotMovement3DIsValid) && SFhasValidObservations))
00116                                 return false;
00117 
00118                         // Since we're gonna return true, load the pose-drawer:
00119                         // Take the accum. actions as input:
00120                         if (m_accumRobotMovement3DIsValid)
00121                         {
00122                                 m_movementDrawer.setPosePDF( m_accumRobotMovement3D );  // <--- Set mov. drawer
00123                                 m_accumRobotMovement3DIsValid = false; // Reset odometry for next iteration
00124                         }
00125                         else
00126                         {
00127                                 CActionRobotMovement2D  theResultingRobotMov;
00128                                 theResultingRobotMov.computeFromOdometry(
00129                                         m_accumRobotMovement2D.rawOdometryIncrementReading,
00130                                         m_accumRobotMovement2D.motionModelConfiguration );
00131 
00132                                 m_movementDrawer.setPosePDF( theResultingRobotMov.poseChange );  // <--- Set mov. drawer
00133                                 m_accumRobotMovement2DIsValid = false; // Reset odometry for next iteration
00134                         }
00135                         return true;
00136                 } // end of PF_SLAM_implementation_gatherActionsCheckBothActObs
00137 
00138                 /** A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFOptimal" (optimal sampling with rejection sampling approximation),
00139                   *  common to both localization and mapping.
00140                   *
00141                   * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
00142                   *
00143                   *  This method implements optimal sampling with a rejection sampling-based approximation of the true posterior.
00144                   *  For details, see the papers:
00145                   *
00146                   *  J.-L. Blanco, J. González, and J.-A. Fernández-Madrigal,
00147                   *    "An Optimal Filtering Algorithm for Non-Parametric Observation Models in
00148                   *     Robot Localization," in Proc. IEEE International Conference on Robotics
00149                   *     and Automation (ICRA'08), 2008, pp. 461–466.
00150                   */
00151                 template <class PARTICLE_TYPE,class MYSELF>
00152                 template <class BINTYPE>
00153                 void PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_pfAuxiliaryPFOptimal(
00154                         const CActionCollection * actions,
00155                         const CSensoryFrame             * sf,
00156                         const CParticleFilter::TParticleFilterOptions &PF_options,
00157                         const TKLDParams &KLD_options)
00158                 {
00159                         // Standard and Optimal AuxiliaryPF actually have a shared implementation body:
00160                         PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(actions,sf,PF_options,KLD_options, true /*Optimal PF*/ );
00161                 }
00162 
00163 
00164                 /** A generic implementation of the PF method "pfStandardProposal" (standard proposal distribution, that is, a simple SIS particle filter),
00165                   *  common to both localization and mapping.
00166                   *
00167                   * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
00168                   */
00169                 template <class PARTICLE_TYPE,class MYSELF>
00170                 template <class BINTYPE>
00171                 void PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_pfStandardProposal(
00172                         const CActionCollection * actions,
00173                         const CSensoryFrame             * sf,
00174                         const CParticleFilter::TParticleFilterOptions &PF_options,
00175                         const TKLDParams &KLD_options)
00176                 {
00177                         MRPT_START
00178                         typedef std::set<BINTYPE,typename BINTYPE::lt_operator>         TSetStateSpaceBins;
00179 
00180                         MYSELF *me = static_cast<MYSELF*>(this);
00181 
00182                         // In this method we don't need the "PF_SLAM_implementation_gatherActionsCheckBothActObs" machinery,
00183                         //  since prediction & update are two independent stages well separated for this algorithm.
00184 
00185                         // --------------------------------------------------------------------------------------
00186                         //  Prediction: Simply draw samples from the motion model
00187                         // --------------------------------------------------------------------------------------
00188                         if (actions)
00189                         {
00190                                 // Find a robot movement estimation:
00191                                 CPose3D                         motionModelMeanIncr;
00192                                 {
00193                                         CActionRobotMovement2DPtr       robotMovement2D = actions->getBestMovementEstimation();
00194                                         // If there is no 2D action, look for a 3D action:
00195                                         if (robotMovement2D.present())
00196                                         {
00197                                                 m_movementDrawer.setPosePDF( robotMovement2D->poseChange );
00198                                                 motionModelMeanIncr = robotMovement2D->poseChange->getMeanVal();
00199                                         }
00200                                         else
00201                                         {
00202                                                 CActionRobotMovement3DPtr       robotMovement3D = actions->getActionByClass<CActionRobotMovement3D>();
00203                                                 if (robotMovement3D)
00204                                                 {
00205                                                         m_movementDrawer.setPosePDF( robotMovement3D->poseChange );
00206                                                         robotMovement3D->poseChange.getMean( motionModelMeanIncr );
00207                                                 }
00208                                                 else { THROW_EXCEPTION("Action list does not contain any CActionRobotMovement2D or CActionRobotMovement3D object!"); }
00209                                         }
00210                                 }
00211 
00212                                 // Update particle poses:
00213                                 if ( !PF_options.adaptiveSampleSize )
00214                                 {
00215                                         const size_t M = me->m_particles.size();
00216                                         // -------------------------------------------------------------
00217                                         // FIXED SAMPLE SIZE
00218                                         // -------------------------------------------------------------
00219                                         CPose3D incrPose;
00220                                         for (size_t i=0;i<M;i++)
00221                                         {
00222                                                 // Generate gaussian-distributed 2D-pose increments according to mean-cov:
00223                                                 m_movementDrawer.drawSample( incrPose );
00224                                                 CPose3D finalPose = CPose3D(*getLastPose(i)) + incrPose;
00225 
00226                                                 // Update the particle with the new pose: this part is caller-dependant and must be implemented there:
00227                                                 PF_SLAM_implementation_custom_update_particle_with_new_pose( me->m_particles[i].d, TPose3D(finalPose) );
00228                                         }
00229                                 }
00230                                 else
00231                                 {
00232                                         // -------------------------------------------------------------
00233                                         //   ADAPTIVE SAMPLE SIZE
00234                                         // Implementation of Dieter Fox's KLD algorithm
00235                                         //  31-Oct-2006 (JLBC): First version
00236                                         //  19-Jan-2009 (JLBC): Rewriten within a generic template
00237                                         // -------------------------------------------------------------
00238                                         TSetStateSpaceBins stateSpaceBins;
00239 
00240                                         size_t Nx = KLD_options.KLD_minSampleSize;
00241                                         const double delta_1 = 1.0 - KLD_options.KLD_delta;
00242                                         const double epsilon_1 = 0.5 / KLD_options.KLD_epsilon;
00243 
00244                                         // Prepare data for executing "fastDrawSample"
00245                                         me->prepareFastDrawSample(PF_options);
00246 
00247                                         // The new particle set:
00248                                         std::vector<TPose3D>  newParticles;
00249                                         vector_double         newParticlesWeight;
00250                                         std::vector<size_t>   newParticlesDerivedFromIdx;
00251 
00252                                         CPose3D  increment_i;
00253                                         size_t N = 1;
00254 
00255                                         do      // THE MAIN DRAW SAMPLING LOOP
00256                                         {
00257                                                 // Draw a robot movement increment:
00258                                                 m_movementDrawer.drawSample( increment_i );
00259 
00260                                                 // generate the new particle:
00261                                                 const size_t drawn_idx = me->fastDrawSample(PF_options);
00262                                                 const CPose3D newPose = CPose3D(*getLastPose(drawn_idx)) + increment_i;
00263                                                 const TPose3D newPose_s = newPose;
00264 
00265                                                 // Add to the new particles list:
00266                                                 newParticles.push_back( newPose_s );
00267                                                 newParticlesWeight.push_back(0);
00268                                                 newParticlesDerivedFromIdx.push_back(drawn_idx);
00269 
00270                                                 // Now, look if the particle falls in a new bin or not:
00271                                                 // --------------------------------------------------------
00272                                                 BINTYPE p;
00273                                                 KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>(p,KLD_options, me->m_particles[drawn_idx].d, &newPose_s);
00274 
00275                                                 if (stateSpaceBins.find( p )==stateSpaceBins.end())
00276                                                 {
00277                                                         // It falls into a new bin:
00278                                                         // Add to the stateSpaceBins:
00279                                                         stateSpaceBins.insert( p );
00280 
00281                                                         // K = K + 1
00282                                                         size_t  K = stateSpaceBins.size();
00283                                                         if ( K>1) //&& newParticles.size() > options.KLD_minSampleSize )
00284                                                         {
00285                                                                 // Update the number of m_particles!!
00286                                                                 Nx =  round(epsilon_1 * math::chi2inv(delta_1,K-1));
00287                                                                 //printf("k=%u \tn=%u \tNx:%u\n", k, newParticles.size(), Nx);
00288                                                         }
00289                                                 }
00290                                                 N = newParticles.size();
00291                                         } while (       N < max(Nx,(size_t)KLD_options.KLD_minSampleSize) &&
00292                                                                 N < KLD_options.KLD_maxSampleSize );
00293 
00294                                         // ---------------------------------------------------------------------------------
00295                                         // Substitute old by new particle set:
00296                                         //   Old are in "m_particles"
00297                                         //   New are in "newParticles", "newParticlesWeight","newParticlesDerivedFromIdx"
00298                                         // ---------------------------------------------------------------------------------
00299                                         this->PF_SLAM_implementation_replaceByNewParticleSet(
00300                                                 me->m_particles,
00301                                                 newParticles,newParticlesWeight,newParticlesDerivedFromIdx );
00302 
00303                                 } // end adaptive sample size
00304                         }
00305 
00306                         if (sf)
00307                         {
00308                                 const size_t M = me->m_particles.size();
00309                                 //      UPDATE STAGE
00310                                 // ----------------------------------------------------------------------
00311                                 // Compute all the likelihood values & update particles weight:
00312                                 for (size_t i=0;i<M;i++)
00313                                 {
00314                                         const TPose3D  *partPose= getLastPose(i); // Take the particle data:
00315                                         CPose3D  partPose2 = CPose3D(*partPose);
00316                                         const double obs_log_likelihood = PF_SLAM_computeObservationLikelihoodForParticle(PF_options,i,*sf,partPose2);
00317                                         me->m_particles[i].log_w += obs_log_likelihood * PF_options.powFactor;
00318                                 } // for each particle "i"
00319 
00320                                 // Normalization of weights is done outside of this method automatically.
00321                         }
00322 
00323                         MRPT_END
00324                 }  // end of PF_SLAM_implementation_pfStandardProposal
00325 
00326                 /** A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFStandard" (Auxiliary particle filter with the standard proposal),
00327                   *  common to both localization and mapping.
00328                   *
00329                   * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
00330                   *
00331                   *  This method is described in the paper:
00332                   *   Pitt, M.K.; Shephard, N. (1999). "Filtering Via Simulation: Auxiliary Particle Filters".
00333                   *    Journal of the American Statistical Association 94 (446): 590–591. doi:10.2307/2670179.
00334                   *
00335                   */
00336                 template <class PARTICLE_TYPE,class MYSELF>
00337                 template <class BINTYPE>
00338                 void PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_pfAuxiliaryPFStandard(
00339                         const CActionCollection * actions,
00340                         const CSensoryFrame             * sf,
00341                         const CParticleFilter::TParticleFilterOptions &PF_options,
00342                         const TKLDParams &KLD_options)
00343                 {
00344                         // Standard and Optimal AuxiliaryPF actually have a shared implementation body:
00345                         PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(actions,sf,PF_options,KLD_options, false /*APF*/ );
00346                 }
00347 
00348                 /*---------------------------------------------------------------
00349                                         PF_SLAM_particlesEvaluator_AuxPFOptimal
00350                  ---------------------------------------------------------------*/
00351                 template <class PARTICLE_TYPE,class MYSELF>
00352                 template <class BINTYPE>
00353                 double  PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_particlesEvaluator_AuxPFOptimal(
00354                         const CParticleFilter::TParticleFilterOptions &PF_options,
00355                         const CParticleFilterCapable    *obj,
00356                         size_t                                  index,
00357                         const void                              *action,
00358                         const void                              *observation )
00359                 {
00360                         MRPT_START
00361 
00362                         //const PF_implementation<PARTICLE_TYPE,MYSELF> *myObj = reinterpret_cast<const PF_implementation<PARTICLE_TYPE,MYSELF>*>( obj );
00363                         const MYSELF *me = static_cast<const MYSELF*>(obj);
00364 
00365                         // Compute the quantity:
00366                         //     w[i]·p(zt|z^{t-1},x^{[i],t-1})
00367                         // As the Monte-Carlo approximation of the integral over all posible $x_t$.
00368                         // --------------------------------------------
00369                         double  indivLik, maxLik= -1e300;
00370                         CPose3D maxLikDraw;
00371                         size_t  N = PF_options.pfAuxFilterOptimal_MaximumSearchSamples;
00372                         ASSERT_(N>1)
00373 
00374                         const CPose3D oldPose = *me->getLastPose(index);
00375                         vector_double   vectLiks(N,0);          // The vector with the individual log-likelihoods.
00376                         CPose3D                 drawnSample;
00377                         for (size_t q=0;q<N;q++)
00378                         {
00379                                 me->m_movementDrawer.drawSample(drawnSample);
00380                                 CPose3D x_predict = oldPose + drawnSample;
00381 
00382                                 // Estimate the mean...
00383                                 indivLik = me->PF_SLAM_computeObservationLikelihoodForParticle(
00384                                         PF_options,
00385                                         index,
00386                                         *static_cast<const CSensoryFrame*>(observation),
00387                                         x_predict );
00388 
00389                                 MRPT_CHECK_NORMAL_NUMBER(indivLik);
00390                                 vectLiks[q] = indivLik;
00391                                 if ( indivLik > maxLik )
00392                                 {       // Keep the maximum value:
00393                                         maxLikDraw      = drawnSample;
00394                                         maxLik          = indivLik;
00395                                 }
00396                         }
00397 
00398                         // This is done to avoid floating point overflow!!
00399                         //      average_lik    =      \sum(e^liks)   * e^maxLik  /     N
00400                         // log( average_lik  ) = log( \sum(e^liks) ) + maxLik   - log( N )
00401                         double avrgLogLik = math::averageLogLikelihood( vectLiks );
00402 
00403                         // Save into the object:
00404                         me->m_pfAuxiliaryPFOptimal_estimatedProb[index] = avrgLogLik; // log( accum / N );
00405                         me->m_pfAuxiliaryPFOptimal_maxLikelihood[index] = maxLik;
00406 
00407                         if (PF_options.pfAuxFilterOptimal_MLE)
00408                                 me->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[index] = maxLikDraw;
00409 
00410                         // and compute the resulting probability of this particle:
00411                         // ------------------------------------------------------------
00412                         return me->m_particles[index].log_w + me->m_pfAuxiliaryPFOptimal_estimatedProb[index];
00413 
00414                         MRPT_END
00415                 } // end of PF_SLAM_particlesEvaluator_AuxPFOptimal
00416 
00417 
00418                 /**  Compute w[i]·p(z_t | mu_t^i), with mu_t^i being
00419                   *    the mean of the new robot pose
00420                   *
00421                   * \param action MUST be a "const CPose3D*"
00422                   * \param observation MUST be a "const CSensoryFrame*"
00423                   */
00424                 template <class PARTICLE_TYPE,class MYSELF>
00425                 template <class BINTYPE>
00426                 double  PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_particlesEvaluator_AuxPFStandard(
00427                         const CParticleFilter::TParticleFilterOptions &PF_options,
00428                         const CParticleFilterCapable    *obj,
00429                         size_t                                  index,
00430                         const void                              *action,
00431                         const void                              *observation )
00432                 {
00433                         MRPT_START
00434 
00435                         //const PF_implementation<PARTICLE_TYPE,MYSELF> *myObj = reinterpret_cast<const PF_implementation<PARTICLE_TYPE,MYSELF>*>( obj );
00436                         const MYSELF *myObj = static_cast<const MYSELF*>(obj);
00437 
00438                         // Take the previous particle weight:
00439                         const double cur_logweight = myObj->m_particles[index].log_w;
00440                         const CPose3D oldPose = *myObj->getLastPose(index);
00441 
00442                         if (!PF_options.pfAuxFilterStandard_FirstStageWeightsMonteCarlo)
00443                         {
00444                                 // Just use the mean:
00445                                 // , take the mean of the posterior density:
00446                                 CPose3D  x_predict;
00447                                 x_predict.composeFrom( oldPose, *static_cast<const CPose3D*>(action) );
00448 
00449                                 // and compute the obs. likelihood:
00450                                 // --------------------------------------------
00451                                 myObj->m_pfAuxiliaryPFStandard_estimatedProb[index] = myObj->PF_SLAM_computeObservationLikelihoodForParticle(
00452                                         PF_options, index,
00453                                         *static_cast<const CSensoryFrame*>(observation), x_predict );
00454 
00455                                 // Combined log_likelihood: Previous weight * obs_likelihood:
00456                                 return cur_logweight + myObj->m_pfAuxiliaryPFStandard_estimatedProb[index];
00457                         }
00458                         else
00459                         {
00460                                 // Do something similar to in Optimal sampling:
00461                                 // Compute the quantity:
00462                                 //     w[i]·p(zt|z^{t-1},x^{[i],t-1})
00463                                 // As the Monte-Carlo approximation of the integral over all posible $x_t$.
00464                                 // --------------------------------------------
00465                                 double  indivLik, maxLik= -1e300;
00466                                 CPose3D maxLikDraw;
00467                                 size_t  N = PF_options.pfAuxFilterOptimal_MaximumSearchSamples;
00468                                 ASSERT_(N>1)
00469 
00470                                 vector_double   vectLiks(N,0);          // The vector with the individual log-likelihoods.
00471                                 CPose3D         drawnSample;
00472                                 for (size_t q=0;q<N;q++)
00473                                 {
00474                                         myObj->m_movementDrawer.drawSample(drawnSample);
00475                                         CPose3D x_predict = oldPose + drawnSample;
00476 
00477                                         // Estimate the mean...
00478                                         indivLik = myObj->PF_SLAM_computeObservationLikelihoodForParticle(
00479                                                 PF_options,
00480                                                 index,
00481                                                 *static_cast<const CSensoryFrame*>(observation),
00482                                                 x_predict );
00483 
00484                                         MRPT_CHECK_NORMAL_NUMBER(indivLik);
00485                                         vectLiks[q] = indivLik;
00486                                         if ( indivLik > maxLik )
00487                                         {       // Keep the maximum value:
00488                                                 maxLikDraw      = drawnSample;
00489                                                 maxLik          = indivLik;
00490                                         }
00491                                 }
00492 
00493                                 // This is done to avoid floating point overflow!!
00494                                 //      average_lik    =      \sum(e^liks)   * e^maxLik  /     N
00495                                 // log( average_lik  ) = log( \sum(e^liks) ) + maxLik   - log( N )
00496                                 double avrgLogLik = math::averageLogLikelihood( vectLiks );
00497 
00498                                 // Save into the object:
00499                                 myObj->m_pfAuxiliaryPFStandard_estimatedProb[index] = avrgLogLik; // log( accum / N );
00500 
00501                                 myObj->m_pfAuxiliaryPFOptimal_maxLikelihood[index] = maxLik;
00502                                 if (PF_options.pfAuxFilterOptimal_MLE)
00503                                         myObj->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[index] = maxLikDraw;
00504 
00505                                 // and compute the resulting probability of this particle:
00506                                 // ------------------------------------------------------------
00507                                 return cur_logweight + myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index];
00508                         }
00509                         MRPT_END
00510                 }
00511 
00512                 // USE_OPTIMAL_SAMPLING:
00513                 //   true -> PF_SLAM_implementation_pfAuxiliaryPFOptimal
00514                 //  false -> PF_SLAM_implementation_pfAuxiliaryPFStandard
00515                 template <class PARTICLE_TYPE,class MYSELF>
00516                 template <class BINTYPE>
00517                 void PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(
00518                         const CActionCollection * actions,
00519                         const CSensoryFrame             * sf,
00520                         const CParticleFilter::TParticleFilterOptions &PF_options,
00521                         const TKLDParams &KLD_options,
00522                         const bool USE_OPTIMAL_SAMPLING  )
00523                 {
00524                         MRPT_START
00525                         typedef std::set<BINTYPE,typename BINTYPE::lt_operator>         TSetStateSpaceBins;
00526 
00527                         MYSELF *me = static_cast<MYSELF*>(this);
00528 
00529                         const size_t M = me->m_particles.size();
00530 
00531                         // ----------------------------------------------------------------------
00532                         //        We can execute optimal PF only when we have both, an action, and
00533                         //     a valid observation from which to compute the likelihood:
00534                         //   Accumulate odometry/actions until we have a valid observation, then
00535                         //    process them simultaneously.
00536                         // ----------------------------------------------------------------------
00537                         if (!PF_SLAM_implementation_gatherActionsCheckBothActObs<BINTYPE>(actions,sf))
00538                                 return; // Nothing we can do here...
00539                         // OK, we have m_movementDrawer loaded and observations...let's roll!
00540 
00541 
00542                         // -------------------------------------------------------------------------------
00543                         //              0) Common part:  Prepare m_particles "draw" and compute "fastDrawSample"
00544                         // -------------------------------------------------------------------------------
00545                         // We need the (aproximate) maximum likelihood value for each
00546                         //  previous particle [i]:
00547                         //     max{ p( z^t | data^[i], x_(t-1)^[i], u_(t) ) }
00548                         //
00549 
00550                         m_pfAuxiliaryPFOptimal_maxLikelihood.assign(M, INVALID_LIKELIHOOD_VALUE);
00551                         m_pfAuxiliaryPFOptimal_maxLikDrawnMovement.resize(M);
00552 //                      if (USE_OPTIMAL_SAMPLING)
00553                                 m_pfAuxiliaryPFOptimal_estimatedProb.resize(M);
00554 //                      else
00555                                 m_pfAuxiliaryPFStandard_estimatedProb.resize(M);
00556 
00557                         // Pass the "mean" robot movement to the "weights" computing function:
00558                         CPose3D meanRobotMovement;
00559                         m_movementDrawer.getSamplingMean3D(meanRobotMovement);
00560 
00561                         // Prepare data for executing "fastDrawSample"
00562                         typedef PF_implementation<PARTICLE_TYPE,MYSELF> TMyClass; // Use this longer declaration to avoid errors in old GCC.
00563                         CParticleFilterCapable::TParticleProbabilityEvaluator funcOpt = &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFOptimal<BINTYPE>;
00564                         CParticleFilterCapable::TParticleProbabilityEvaluator funcStd = &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFStandard<BINTYPE>;
00565 
00566                         me->prepareFastDrawSample(
00567                                 PF_options,
00568                                 USE_OPTIMAL_SAMPLING ? funcOpt : funcStd,
00569                                 &meanRobotMovement,
00570                                 sf );
00571 
00572                         // For USE_OPTIMAL_SAMPLING=1,  m_pfAuxiliaryPFOptimal_maxLikelihood is now computed.
00573 
00574                         if (USE_OPTIMAL_SAMPLING && PF_options.verbose)
00575                         {
00576                                 printf("[prepareFastDrawSample] max      (log) = %10.06f\n",  math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb) );
00577                                 printf("[prepareFastDrawSample] max-mean (log) = %10.06f\n", -math::mean(m_pfAuxiliaryPFOptimal_estimatedProb) + math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb) );
00578                                 printf("[prepareFastDrawSample] max-min  (log) = %10.06f\n", -math::minimum(m_pfAuxiliaryPFOptimal_estimatedProb) + math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb) );
00579                         }
00580 
00581                         // Now we have the vector "m_fastDrawProbability" filled out with:
00582                         //               w[i]·p(zt|z^{t-1},x^{[i],t-1},X)
00583                         //  where,
00584                         //
00585                         //  =========== For USE_OPTIMAL_SAMPLING = true ====================
00586                         //  X is the robot pose prior (as implemented in
00587                         //  the aux. function "PF_SLAM_particlesEvaluator_AuxPFOptimal"),
00588                         //  and also the "m_pfAuxiliaryPFOptimal_maxLikelihood" filled with the maximum lik. values.
00589                         //
00590                         //  =========== For USE_OPTIMAL_SAMPLING = false ====================
00591                         //  X is a single point close to the mean of the robot pose prior (as implemented in
00592                         //  the aux. function "PF_SLAM_particlesEvaluator_AuxPFStandard").
00593                         //
00594                         vector<TPose3D>                 newParticles;
00595                         vector<double>                  newParticlesWeight;
00596                         vector<size_t>                  newParticlesDerivedFromIdx;
00597 
00598                         // We need the (aproximate) maximum likelihood value for each
00599                         //  previous particle [i]:
00600                         //
00601                         //     max{ p( z^t | data^[i], x_(t-1)^[i], u_(t) ) }
00602                         //
00603                         if (PF_options.pfAuxFilterOptimal_MLE)
00604                                 m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed.assign(M, false);
00605 
00606                         const double            maxMeanLik = math::maximum(
00607                                 USE_OPTIMAL_SAMPLING ?
00608                                         m_pfAuxiliaryPFOptimal_estimatedProb :
00609                                         m_pfAuxiliaryPFStandard_estimatedProb );
00610 
00611                         if ( !PF_options.adaptiveSampleSize )
00612                         {
00613                                 // ----------------------------------------------------------------------
00614                                 //                                              1) FIXED SAMPLE SIZE VERSION
00615                                 // ----------------------------------------------------------------------
00616                                 newParticles.resize(M);
00617                                 newParticlesWeight.resize(M);
00618                                 newParticlesDerivedFromIdx.resize(M);
00619 
00620                                 const bool doResample = me->ESS() < PF_options.BETA;
00621 
00622                                 for (size_t i=0;i<M;i++)
00623                                 {
00624                                         size_t k;
00625 
00626                                         // Generate a new particle:
00627                                         //   (a) Draw a "t-1" m_particles' index:
00628                                         // ----------------------------------------------------------------
00629                                         if (doResample)
00630                                                         k = me->fastDrawSample(PF_options);             // Based on weights of last step only!
00631                                         else    k = i;
00632 
00633                                         // Do one rejection sampling step:
00634                                         // ---------------------------------------------
00635                                         CPose3D         newPose;
00636                                         double          newParticleLogWeight;
00637                                         PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
00638                                                 USE_OPTIMAL_SAMPLING,doResample,maxMeanLik,
00639                                                 k,
00640                                                 sf,PF_options,
00641                                                 newPose, newParticleLogWeight);
00642 
00643                                         // Insert the new particle
00644                                         newParticles[i] = newPose;
00645                                         newParticlesDerivedFromIdx[i] = k;
00646                                         newParticlesWeight[i] = newParticleLogWeight;
00647 
00648                                 } // for i
00649                         } // end fixed sample size
00650                         else
00651                         {
00652                                 // -------------------------------------------------------------------------------------------------
00653                                 //                                                              2) ADAPTIVE SAMPLE SIZE VERSION
00654                                 //
00655                                 //      Implementation of Dieter Fox's KLD algorithm
00656                                 //              JLBC (3/OCT/2006)
00657                                 // -------------------------------------------------------------------------------------------------
00658                                 // The new particle set:
00659                                 newParticles.clear();
00660                                 newParticlesWeight.clear();
00661                                 newParticlesDerivedFromIdx.clear();
00662 
00663                                 // ------------------------------------------------------------------------------
00664                                 // 2.1) PRELIMINARY STAGE: Build a list of pairs<TPathBin,vector_uint> with the
00665                                 //      indexes of m_particles that fall into each multi-dimensional-path bins
00666                                 //      //The bins will be saved into "stateSpaceBinsLastTimestep", and the list
00667                                 //      //of corresponding m_particles (in the last timestep), in "stateSpaceBinsLastTimestepParticles"
00668                                 //  - Added JLBC (01/DEC/2006)
00669                                 // ------------------------------------------------------------------------------
00670                                 TSetStateSpaceBins                      stateSpaceBinsLastTimestep;
00671                                 std::vector<vector_uint>        stateSpaceBinsLastTimestepParticles;
00672                                 typename MYSELF::CParticleList::iterator                partIt;
00673                                 unsigned int    partIndex;
00674 
00675                                 printf( "[FIXED_SAMPLING] Computing...");
00676                                 for (partIt = me->m_particles.begin(),partIndex=0; partIt!=me->m_particles.end(); ++partIt,++partIndex)
00677                                 {
00678                                         // Load the bin from the path data:
00679                                         BINTYPE p;
00680                                         KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>(p, KLD_options,partIt->d );
00681 
00682                                         // Is it a new bin?
00683                                         typename TSetStateSpaceBins::iterator posFound=stateSpaceBinsLastTimestep.find(p);
00684                                         if ( posFound == stateSpaceBinsLastTimestep.end() )
00685                                         {       // Yes, create a new pair <bin,index_list> in the list:
00686                                                 stateSpaceBinsLastTimestep.insert( p );
00687                                                 stateSpaceBinsLastTimestepParticles.push_back( vector_uint(1,partIndex) );
00688                                         }
00689                                         else
00690                                         { // No, add the particle's index to the existing entry:
00691                                                 const size_t idx = std::distance(stateSpaceBinsLastTimestep.begin(),posFound);
00692                                                 stateSpaceBinsLastTimestepParticles[idx].push_back( partIndex );
00693                                         }
00694                                 }
00695                                 printf( "done (%u bins in t-1)\n",(unsigned int)stateSpaceBinsLastTimestep.size());
00696 
00697                                 // ------------------------------------------------------------------------------
00698                                 // 2.2)    THE MAIN KLD-BASED DRAW SAMPLING LOOP
00699                                 // ------------------------------------------------------------------------------
00700                                 double          delta_1 = 1.0 - KLD_options.KLD_delta;
00701                                 double          epsilon_1 = 0.5 / KLD_options.KLD_epsilon;
00702                                 bool            doResample = me->ESS() < 0.5;
00703                                 //double        maxLik = math::maximum(m_pfAuxiliaryPFOptimal_maxLikelihood); // For normalization purposes only
00704 
00705                                 // The desired dynamic number of m_particles (to be modified dynamically below):
00706                                 const size_t  minNumSamples_KLD = max((size_t)KLD_options.KLD_minSampleSize, (size_t)round(KLD_options.KLD_minSamplesPerBin*stateSpaceBinsLastTimestep.size()) );
00707                                 size_t Nx = minNumSamples_KLD ;
00708 
00709                                 const size_t Np1 = me->m_particles.size();
00710                                 vector_size_t oldPartIdxsStillNotPropragated(Np1);  // Use a list since we'll use "erase" a lot here.
00711                                 for (size_t k=0;k<Np1;k++) oldPartIdxsStillNotPropragated[k]=k; //.push_back(k);
00712 
00713                                 const size_t Np = stateSpaceBinsLastTimestepParticles.size();
00714                                 vector_size_t permutationPathsAuxVector(Np);
00715                                 for (size_t k=0;k<Np;k++) permutationPathsAuxVector[k]=k;
00716 
00717                                 // Instead of picking randomly from "permutationPathsAuxVector", we can shuffle it now just once,
00718                                 // then pick in sequence from the tail and resize the container:
00719                                 std::random_shuffle(
00720                                         permutationPathsAuxVector.begin(),
00721                                         permutationPathsAuxVector.end(),
00722                                         mrpt::random::random_generator_for_STL );
00723 
00724                                 size_t k = 0;
00725                                 size_t N = 0;
00726 
00727                                 TSetStateSpaceBins              stateSpaceBins;
00728 
00729                                 do // "N" is the index of the current "new particle":
00730                                 {
00731                                         // Generate a new particle:
00732                                         //
00733                                         //   (a) Propagate the last set of m_particles, and only if the
00734                                         //       desired number of m_particles in this step is larger,
00735                                         //       perform a UNIFORM sampling from the last set. In this way
00736                                         //       the new weights can be computed in the same way for all m_particles.
00737                                         // ---------------------------------------------------------------------------
00738                                         if (doResample)
00739                                         {
00740                                                 k = me->fastDrawSample(PF_options);             // Based on weights of last step only!
00741                                         }
00742                                         else
00743                                         {
00744                                                 // Assure that at least one particle per "discrete-path" is taken (if the
00745                                                 //  number of samples allows it)
00746                                                 if (permutationPathsAuxVector.size())
00747                                                 {
00748                                                         const size_t idxBinSpacePath = *permutationPathsAuxVector.rbegin();
00749                                                         permutationPathsAuxVector.resize(permutationPathsAuxVector.size()-1);
00750 
00751                                                         const size_t idx = randomGenerator.drawUniform32bit() % stateSpaceBinsLastTimestepParticles[idxBinSpacePath].size();
00752                                                         k = stateSpaceBinsLastTimestepParticles[idxBinSpacePath][idx];
00753                                                         ASSERT_(k<me->m_particles.size());
00754 
00755                                                         // Also erase it from the other permutation vector list:
00756                                                         oldPartIdxsStillNotPropragated.erase(std::find(oldPartIdxsStillNotPropragated.begin(),oldPartIdxsStillNotPropragated.end(),k));
00757                                                 }
00758                                                 else
00759                                                 {
00760                                                         // Select a particle from the previous set with a UNIFORM distribution,
00761                                                         // in such a way we will assign each particle the updated weight accounting
00762                                                         // for its last weight.
00763                                                         // The first "old_N" m_particles will be drawn using a uniform random selection
00764                                                         // without repetitions:
00765                                                         //
00766                                                         // Select a index from "oldPartIdxsStillNotPropragated" and remove it from the list:
00767                                                         if (oldPartIdxsStillNotPropragated.size())
00768                                                         {
00769                                                                 const size_t idx = randomGenerator.drawUniform32bit() % oldPartIdxsStillNotPropragated.size();
00770                                                                 vector_size_t::iterator it = oldPartIdxsStillNotPropragated.begin() + idx; //advance(it,idx);
00771                                                                 k = *it;
00772                                                                 oldPartIdxsStillNotPropragated.erase(it);
00773                                                         }
00774                                                         else
00775                                                         {
00776                                                                 // N>N_old -> Uniformly draw index:
00777                                                                 k = randomGenerator.drawUniform32bit() % me->m_particles.size();
00778                                                         }
00779                                                 }
00780                                         }
00781 
00782                                         // Do one rejection sampling step:
00783                                         // ---------------------------------------------
00784                                         CPose3D         newPose;
00785                                         double          newParticleLogWeight;
00786                                         PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
00787                                                 USE_OPTIMAL_SAMPLING,doResample,maxMeanLik,
00788                                                 k,
00789                                                 sf,PF_options,
00790                                                 newPose, newParticleLogWeight);
00791 
00792                                         // Insert the new particle
00793                                         newParticles.push_back( newPose );
00794                                         newParticlesDerivedFromIdx.push_back( k );
00795                                         newParticlesWeight.push_back(newParticleLogWeight);
00796 
00797                                         // ----------------------------------------------------------------
00798                                         // Now, the KLD-sampling dynamic sample size stuff:
00799                                         //  look if the particle's PATH falls into a new bin or not:
00800                                         // ----------------------------------------------------------------
00801                                         BINTYPE p;
00802                                         const TPose3D  newPose_s = newPose;
00803                                         KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>( p,KLD_options, me->m_particles[k].d, &newPose_s );
00804 
00805                                         // -----------------------------------------------------------------------------
00806                                         // Look for the bin "p" into "stateSpaceBins": If it is not yet into the set,
00807                                         //  then we may increase the desired particle number:
00808                                         // -----------------------------------------------------------------------------
00809 
00810                                         // Found?
00811                                         if ( stateSpaceBins.find(p)==stateSpaceBins.end() )
00812                                         {
00813                                                 // It falls into a new bin: add to the stateSpaceBins:
00814                                                 stateSpaceBins.insert( p );
00815 
00816                                                 // K = K + 1
00817                                                 int K = stateSpaceBins.size();
00818                                                 if ( K>1 )
00819                                                 {
00820                                                         // Update the number of m_particles!!
00821                                                         Nx = (size_t) (epsilon_1 * math::chi2inv(delta_1,K-1));
00822                                                         //printf("k=%u \tn=%u \tNx:%u\n", k, newParticles.size(), Nx);
00823                                                 }
00824                                         }
00825 
00826                                         N = newParticles.size();
00827 
00828                                 } while ((  N < KLD_options.KLD_maxSampleSize &&
00829                                                         N < max(Nx,minNumSamples_KLD)) ||
00830                                                         (permutationPathsAuxVector.size() && !doResample) );
00831 
00832                                 printf("\n[ADAPTIVE SAMPLE SIZE]  #Bins: %u \t #Particles: %u \t Nx=%u\n", static_cast<unsigned>(stateSpaceBins.size()),static_cast<unsigned>(N), (unsigned)Nx);
00833                         } // end adaptive sample size
00834 
00835 
00836                         // ---------------------------------------------------------------------------------
00837                         // Substitute old by new particle set:
00838                         //   Old are in "m_particles"
00839                         //   New are in "newParticles", "newParticlesWeight","newParticlesDerivedFromIdx"
00840                         // ---------------------------------------------------------------------------------
00841                         this->PF_SLAM_implementation_replaceByNewParticleSet(
00842                                 me->m_particles,
00843                                 newParticles,newParticlesWeight,newParticlesDerivedFromIdx );
00844 
00845 
00846                         // In this PF_algorithm, we must do weight normalization by ourselves:
00847                         me->normalizeWeights();
00848 
00849                         MRPT_END
00850                 } // end of PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal
00851 
00852 
00853                 /* ------------------------------------------------------------------------
00854                                                         PF_SLAM_aux_perform_one_rejection_sampling_step
00855                    ------------------------------------------------------------------------ */
00856                 template <class PARTICLE_TYPE,class MYSELF>
00857                 template <class BINTYPE>
00858                 void PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_aux_perform_one_rejection_sampling_step(
00859                         const bool              USE_OPTIMAL_SAMPLING,
00860                         const bool              doResample,
00861                         const double    maxMeanLik,
00862                         size_t    k, // The particle from the old set "m_particles[]"
00863                         const CSensoryFrame             * sf,
00864                         const CParticleFilter::TParticleFilterOptions &PF_options,
00865                         CPose3D                 & out_newPose,
00866                         double                  & out_newParticleLogWeight)
00867                 {
00868                         MYSELF *me = static_cast<MYSELF*>(this);
00869 
00870                         // ADD-ON: If the 'm_pfAuxiliaryPFOptimal_estimatedProb[k]' is **extremelly** low relative to the other m_particles,
00871                         //  resample only this particle with a copy of another one, uniformly:
00872                         while ( ( (USE_OPTIMAL_SAMPLING ? m_pfAuxiliaryPFOptimal_estimatedProb[k] : m_pfAuxiliaryPFStandard_estimatedProb[k] )
00873                                                 -maxMeanLik) < -PF_options.max_loglikelihood_dyn_range )
00874                         {
00875                                 // Select another 'k' uniformly:
00876                                 k = randomGenerator.drawUniform32bit() % me->m_particles.size();
00877                                 if (PF_options.verbose) cout << "[PF_implementation] Warning: Discarding very unlikely particle" << endl;
00878                         }
00879 
00880                         const CPose3D oldPose = *getLastPose(k);        // Get the current pose of the k'th particle
00881 
00882                         //   (b) Rejection-sampling: Draw a new robot pose from x[k],
00883                         //       and accept it with probability p(zk|x) / maxLikelihood:
00884                         // ----------------------------------------------------------------
00885                         double poseLogLik;
00886                         if ( PF_SLAM_implementation_skipRobotMovement() )
00887                         {
00888                                 // The first robot pose in the SLAM execution: All m_particles start
00889                                 // at the same point (this is the lowest bound of subsequent uncertainty):
00890                                 out_newPose = oldPose;
00891                                 poseLogLik = 0;
00892                         }
00893                         else
00894                         {
00895                                 CPose3D movementDraw;
00896                                 if (!USE_OPTIMAL_SAMPLING)
00897                                 {       // APF:
00898                                         m_movementDrawer.drawSample( movementDraw );
00899                                         out_newPose.composeFrom(oldPose, movementDraw); // newPose = oldPose + movementDraw;
00900                                         // Compute likelihood:
00901                                         poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(PF_options, k,*sf,out_newPose);
00902                                 }
00903                                 else
00904                                 {       // Optimal APF with rejection sampling:
00905                                         // Rejection-sampling:
00906                                         double acceptanceProb;
00907                                         int             timeout = 0;
00908                                         const int       maxTries=10000;
00909                                         double      bestTryByNow_loglik= -std::numeric_limits<double>::max();
00910                                         TPose3D         bestTryByNow_pose;
00911                                         do
00912                                         {
00913                                                 // Draw new robot pose:
00914                                                 if (PF_options.pfAuxFilterOptimal_MLE && !m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k])
00915                                                 {       // No! first take advantage of a good drawn value, but only once!!
00916                                                         m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k] = true;
00917                                                         movementDraw = CPose3D( m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[k] );
00918                                                 }
00919                                                 else
00920                                                 {
00921                                                         // Draw new robot pose:
00922                                                         m_movementDrawer.drawSample( movementDraw );
00923                                                 }
00924 
00925                                                 out_newPose.composeFrom(oldPose, movementDraw); // out_newPose = oldPose + movementDraw;
00926 
00927                                                 // Compute acceptance probability:
00928                                                 poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(PF_options, k,*sf,out_newPose);
00929 
00930                                                 if (poseLogLik>bestTryByNow_loglik)
00931                                                 {
00932                                                         bestTryByNow_loglik = poseLogLik;
00933                                                         bestTryByNow_pose = out_newPose;
00934                                                 }
00935 
00936                                                 double ratioLikLik = std::exp( poseLogLik - m_pfAuxiliaryPFOptimal_maxLikelihood[k] );
00937                                                 acceptanceProb = std::min( 1.0, ratioLikLik );
00938 
00939                                                 if ( ratioLikLik > 1)
00940                                                 {
00941                                                         m_pfAuxiliaryPFOptimal_maxLikelihood[k] = poseLogLik; //  :'-( !!!
00942                                                         //acceptanceProb = 0;           // Keep searching or keep this one?
00943                                                 }
00944                                         } while ( ++timeout<maxTries && acceptanceProb < randomGenerator.drawUniform(0.0,0.999) );
00945 
00946                                         if (timeout>=maxTries)
00947                                         {
00948                                                 out_newPose = bestTryByNow_pose;
00949                                                 poseLogLik = bestTryByNow_loglik;
00950                                                 if (PF_options.verbose) cout << "[PF_implementation] Warning: timeout in rejection sampling." << endl;
00951                                         }
00952 
00953                                 }
00954 
00955                                 // And its weight:
00956                                 if (USE_OPTIMAL_SAMPLING)
00957                                 {       // Optimal PF
00958                                         if (doResample)
00959                                                 out_newParticleLogWeight = 0;  // By definition of our optimal PF, all samples have identical weights.
00960                                         else
00961                                         {
00962                                                 const double weightFact = m_pfAuxiliaryPFOptimal_estimatedProb[k] * PF_options.powFactor;
00963                                                 out_newParticleLogWeight = me->m_particles[k].log_w + weightFact;
00964                                         }
00965                                 }
00966                                 else
00967                                 {       // APF:
00968                                         const double weightFact = (poseLogLik-m_pfAuxiliaryPFStandard_estimatedProb[k]) * PF_options.powFactor;
00969                                         if (doResample)
00970                                                         out_newParticleLogWeight = weightFact;
00971                                         else    out_newParticleLogWeight = weightFact + me->m_particles[k].log_w;
00972                                 }
00973 
00974                         }
00975                         // Done.
00976                 } // end PF_SLAM_aux_perform_one_rejection_sampling_step
00977 
00978 
00979         } // end namespace
00980 } // end namespace
00981 
00982 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011