Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include "srs_people_tracking_filter/gaussian_pos_vel.h"
00040 #include <wrappers/rng/rng.h>
00041 #include <cmath>
00042 #include <cassert>
00043
00044 using namespace tf;
00045
00046 namespace BFL
00047 {
00048 GaussianPosVel::GaussianPosVel (const StatePosVel& mu, const StatePosVel& sigma)
00049 : Pdf<StatePosVel> ( 1 ),
00050 mu_(mu),
00051 sigma_(sigma),
00052 gauss_pos_(mu.pos_, sigma.pos_),
00053 gauss_vel_(mu.vel_, sigma.vel_)
00054 {}
00055
00056
00057 GaussianPosVel::~GaussianPosVel(){}
00058
00059 GaussianPosVel* GaussianPosVel::Clone() const
00060 {
00061 return new GaussianPosVel(mu_, sigma_);
00062 }
00063
00064 std::ostream& operator<< (std::ostream& os, const GaussianPosVel& g)
00065 {
00066 os << "\nMu pos :\n" << g.ExpectedValueGet().pos_ << endl
00067 << "\nMu vel :\n" << g.ExpectedValueGet().vel_ << endl
00068 << "\nSigma:\n" << g.CovarianceGet() << endl;
00069 return os;
00070 }
00071
00072
00073 Probability GaussianPosVel::ProbabilityGet(const StatePosVel& input) const
00074 {
00075 return gauss_pos_.ProbabilityGet(input.pos_) * gauss_vel_.ProbabilityGet(input.vel_);
00076 }
00077
00078
00079 bool
00080 GaussianPosVel::SampleFrom (vector<Sample<StatePosVel> >& list_samples, const int num_samples, int method, void * args) const
00081 {
00082 list_samples.resize(num_samples);
00083 vector<Sample<StatePosVel> >::iterator sample_it = list_samples.begin();
00084 for (sample_it=list_samples.begin(); sample_it!=list_samples.end(); sample_it++)
00085 SampleFrom( *sample_it, method, args);
00086
00087 return true;
00088 }
00089
00090
00091 bool
00092 GaussianPosVel::SampleFrom (Sample<StatePosVel>& one_sample, int method, void * args) const
00093 {
00094 one_sample.ValueSet( StatePosVel(Vector3(rnorm(mu_.pos_[0], sigma_.pos_[0]*dt_),
00095 rnorm(mu_.pos_[1], sigma_.pos_[1]*dt_),
00096 rnorm(mu_.pos_[2], sigma_.pos_[2]*dt_)),
00097 Vector3(rnorm(mu_.vel_[0], sigma_.vel_[0]*dt_),
00098 rnorm(mu_.vel_[1], sigma_.vel_[1]*dt_),
00099 rnorm(mu_.vel_[2], sigma_.vel_[2]*dt_))) );
00100 return true;
00101 }
00102
00103
00104 StatePosVel
00105 GaussianPosVel::ExpectedValueGet ( ) const
00106 {
00107 return mu_;
00108 }
00109
00110 SymmetricMatrix
00111 GaussianPosVel::CovarianceGet () const
00112 {
00113 SymmetricMatrix sigma(6); sigma = 0;
00114 for (unsigned int i=0; i<3; i++){
00115 sigma(i+1,i+1) = pow(sigma_.pos_[i],2);
00116 sigma(i+4,i+4) = pow(sigma_.vel_[i],2);
00117 }
00118 return sigma;
00119 }
00120
00121 }