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 #include "people_tracking_filter/gaussian_pos_vel.h"
00039 #include <wrappers/rng/rng.h>
00040 #include <cmath>
00041 #include <cassert>
00042
00043 using namespace tf;
00044
00045 namespace BFL
00046 {
00047 GaussianPosVel::GaussianPosVel(const StatePosVel& mu, const StatePosVel& sigma)
00048 : Pdf<StatePosVel> (1),
00049 mu_(mu),
00050 sigma_(sigma),
00051 gauss_pos_(mu.pos_, sigma.pos_),
00052 gauss_vel_(mu.vel_, sigma.vel_)
00053 {}
00054
00055
00056 GaussianPosVel::~GaussianPosVel() {}
00057
00058 GaussianPosVel* GaussianPosVel::Clone() const
00059 {
00060 return new GaussianPosVel(mu_, sigma_);
00061 }
00062
00063 std::ostream& operator<< (std::ostream& os, const GaussianPosVel& g)
00064 {
00065 os << "\nMu pos :\n" << g.ExpectedValueGet().pos_ << endl
00066 << "\nMu vel :\n" << g.ExpectedValueGet().vel_ << endl
00067 << "\nSigma:\n" << g.CovarianceGet() << endl;
00068 return os;
00069 }
00070
00071
00072 Probability GaussianPosVel::ProbabilityGet(const StatePosVel& input) const
00073 {
00074 return gauss_pos_.ProbabilityGet(input.pos_) * gauss_vel_.ProbabilityGet(input.vel_);
00075 }
00076
00077
00078 bool
00079 GaussianPosVel::SampleFrom(vector<Sample<StatePosVel> >& list_samples, const int num_samples, int method, void * args) const
00080 {
00081 list_samples.resize(num_samples);
00082 vector<Sample<StatePosVel> >::iterator sample_it = list_samples.begin();
00083 for (sample_it = list_samples.begin(); sample_it != list_samples.end(); sample_it++)
00084 SampleFrom(*sample_it, method, args);
00085
00086 return true;
00087 }
00088
00089
00090 bool
00091 GaussianPosVel::SampleFrom(Sample<StatePosVel>& one_sample, int method, void * args) const
00092 {
00093 one_sample.ValueSet(StatePosVel(Vector3(rnorm(mu_.pos_[0], sigma_.pos_[0]*dt_),
00094 rnorm(mu_.pos_[1], sigma_.pos_[1]*dt_),
00095 rnorm(mu_.pos_[2], sigma_.pos_[2]*dt_)),
00096 Vector3(rnorm(mu_.vel_[0], sigma_.vel_[0]*dt_),
00097 rnorm(mu_.vel_[1], sigma_.vel_[1]*dt_),
00098 rnorm(mu_.vel_[2], sigma_.vel_[2]*dt_))));
00099 return true;
00100 }
00101
00102
00103 StatePosVel
00104 GaussianPosVel::ExpectedValueGet() const
00105 {
00106 return mu_;
00107 }
00108
00109 SymmetricMatrix
00110 GaussianPosVel::CovarianceGet() const
00111 {
00112 SymmetricMatrix sigma(6);
00113 sigma = 0;
00114 for (unsigned int i = 0; i < 3; i++)
00115 {
00116 sigma(i + 1, i + 1) = pow(sigma_.pos_[i], 2);
00117 sigma(i + 4, i + 4) = pow(sigma_.vel_[i], 2);
00118 }
00119 return sigma;
00120 }
00121
00122 }