- Main Page
- Namespaces
- Classes
- Files
- Related Pages

# /Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/Trajectories.cpp

    00001 //==============================================================================
    00002 // Copyright (c) 2004           Model-based Embedded and Robotic Systems Group
    00003 // All Rights Reserved          Massachusetts Institute of Technology
    00004 //
    00005 // This software is provided as is. Model-based Embedded and Robotic Systems
    00006 // (MERS) group does not assume any responsibility.
    00007 //==============================================================================
    00008
    00009 #include "Trajectories.h"
    00010
    00011 #ifdef _MSC_VER
    00012 #pragma warning( disable : 4786 )
    00013 #endif
    00014
    00015 #include <iomanip>
    00016 #include "Estimate.h"
    00017
    00018 using namespace Mers::Estimation::Discrete;
    00019 using std::ostream;
    00020 using std::cerr;
    00021 using std::cout;
    00022 using std::setprecision;
    00023 using std::endl;
    00024
    00025
    00026         //-----------------------------------------------
    00027         //      CONSTRUCTOR
    00028         //-----------------------------------------------
    00029
    00030
    00031 Trajectories::Trajectories(const double p, unsigned short db)
    00032 : beliefProbability(0.0), uncertainty(p), debug(db), wasNormalized(false)
    00033 {}
    00034
    00035         //-----------------------------------------------
    00036         //      COPY CONSTRUCTOR
    00037         //-----------------------------------------------
    00038
    00039
    00040 Trajectories::Trajectories(const Trajectories &trajs)
    00041 : estimates(trajs.estimates), beliefProbability(trajs.beliefProbability),
    00042   uncertainty(trajs.uncertainty), debug(trajs.debug), wasNormalized(trajs.wasNormalized)
    00043 {}
    00044
    00045 Trajectories::Trajectories(const Trajectories * trajs)
    00046 : estimates(trajs->estimates), beliefProbability(trajs->beliefProbability),
    00047   uncertainty(trajs->uncertainty), debug(trajs->debug), wasNormalized(trajs->wasNormalized)
    00048 {}
    00049
    00050         //-----------------------------------------------
    00051         //      DESTRUCTOR
    00052         //-----------------------------------------------
    00053
    00054
    00055 Trajectories::~Trajectories()
    00056 {
    00057         Estimate * est;
    00058         while(!estimates.empty())
    00059         {
    00060                 est = estimates.back();
    00061                 delete est;
    00062                 estimates.pop_back();
    00063         }
    00064
    00065 }
    00066
    00067
    00068
    00069         //-----------------------------------------------
    00070         //      PUBLIC METHODS
    00071         //-----------------------------------------------
    00072
    00073 bool
    00074 Trajectories::addEstimate(Estimate * est)
    00075 {
    00076         if(wasNormalized)
    00077         {
    00078                 cout << "<Trajectories> WARNING: Trajectories were already normalized. Ignoring and deleting new estimate." << endl;
    00079                 delete est;
    00080                 return false;
    00081         }
    00082         if(est->getProbability() < 0.0 || est->getProbability() > 1.0)
    00083         {
    00084                 cerr << "<Trajectories> ERROR: Invalid estimate probability! Ignoring and deleting new estimate." << endl;
    00085                 delete est;
    00086                 return false;
    00087         }
    00088         if(est->getProbability() + beliefProbability > 1.0000001) // Add 1/10000000 to fix machine error problem
    00089         {
    00090                 cerr << "<Trajectories> ERROR: Probabiliy of belief state great than 1! ("
    00091                         << est->getProbability() + beliefProbability << "). Ignoring and deleting new estimate." << endl;
    00092                 cerr << "Estimates in belief state before error: " << endl;
    00093                 cerr << *this << endl;
    00094                 cerr << "New estimate that caused error: " << endl;
    00095                 cerr << *est << endl;
    00096                 delete est;
    00097                 return false;
    00098         }
    00099
    00100         if(!estimates.empty())
    00101         {
    00102                 // adde 1e-10 for machine error problems
    00103                 if(estimates.back()->getProbability()+1e-10 < est->getProbability()) {
    00104                         cout << "<Trajectories> WARNING: Estimates in belief state are not in best first order!" << endl;
    00105                         cout << "  Estimate " << *est << endl;
    00106                         cout << "  Added after " << *(estimates.back()) << endl;
    00107                 }
    00108         }
    00109
    00110         beliefProbability += est->getProbability();
    00111         estimates.push_back(est);
    00112         return true;
    00113 }
    00114
    00115
    00116 const Estimate *
    00117 Trajectories::getMostLikelyEstimate() const
    00118 {
    00119         if(estimates.size() < 1)
    00120                 return NULL;
    00121         return estimates.front();
    00122 }
    00123
    00124
    00125 const Estimate *
    00126 Trajectories::getEstimate(const unsigned int index) const
    00127 {
    00128         return estimates[index];
    00129 }
    00130
    00132
    00133 Estimate * Trajectories::getEstimateUnconst(const unsigned int index)
    00134 {
    00135         return estimates[index];
    00136 }
    00137
    00139
    00140 unsigned int
    00141 Trajectories::getNumberEstimates() const
    00142 {
    00143         return (unsigned int) estimates.size();
    00144 }
    00145
    00146
    00147 double
    00148 Trajectories::getBeliefUncertainty() const
    00149 {
    00150         return (1.0-beliefProbability);
    00151 }
    00152
    00153
    00154 double
    00155 Trajectories::getAccumulatedUncertainty() const
    00156 // Assumes that probably is normal (not negative log)
    00157 {
    00158         return (1.0-((1.0-uncertainty)*beliefProbability));
    00159 }
    00160
    00161
    00162 bool
    00163 Trajectories::normalize(void)
    00164 {
    00165         unsigned int i = 0;
    00166         if(wasNormalized)
    00167         {
    00168                 cout << "<Trajectories> WARNING: Trajectories were already normalized. Ignoring." << endl;
    00169                 return false;
    00170         }
    00171         for(i=0; i < getNumberEstimates(); i++)
    00172                 estimates[i]->setProbability(estimates[i]->getProbability()/beliefProbability);
    00173
    00174         wasNormalized = true;
    00175         return true;
    00176 }
    00177
    00178
    00179 void
    00180 Trajectories::setDebug(const unsigned short db)
    00181 {
    00182         debug = db;
    00183         return;
    00184 }
    00185
    00186
    00187 ostream&
    00188 Mers::Estimation::Discrete::operator<<(ostream& os, Trajectories const & trajs)
    00189 {
    00190         size_t numStates = trajs.estimates.size();
    00191         int p = os.precision();
    00192
    00193         for(unsigned int i=0; i<numStates; i++)
    00194         {
    00195                 os << *trajs.estimates[i];
    00196                 if(trajs.debug > 1)
    00197                         os << " from trajectory " << trajs.estimates[i]->getOriginatedTrajectoryNumber();
    00198                 os << endl;
    00199         }
    00200
    00201         if(trajs.debug > 0)
    00202         {
    00203                 os << " -- Total Trajectory Probablity: " << setprecision(4) << trajs.beliefProbability << setprecision(p);
    00204                 if(trajs.wasNormalized)
    00205                         os << " (Normalized to 1.0)";
    00206                 os << endl;
    00207         }
    00208         if(trajs.debug > 2)
    00209                 os << " -- Accumulated Uncertainty: " << setprecision(4) << trajs.getAccumulatedUncertainty() << setprecision(p) << endl;
    00210
    00211         return os;
    00212 }
    00213         //-----------------------------------------------
    00214         //      PRIVATE METHODS
    00215         //-----------------------------------------------
    00216