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 #include "cob_people_tracking_filter/gaussian_vector.h"
00038 #include <wrappers/rng/rng.h>
00039 #include <cmath>
00040 #include <cassert>
00041
00042 using namespace tf;
00043
00044 namespace BFL
00045 {
00046 GaussianVector::GaussianVector(const Vector3& mu, const Vector3& sigma)
00047 : Pdf<Vector3> ( 1 ),
00048 mu_(mu),
00049 sigma_(sigma),
00050 sigma_changed_(true)
00051 {
00052 for (unsigned int i=0; i<3; i++)
00053 assert(sigma[i] > 0);
00054 }
00055
00056
00057 GaussianVector::~GaussianVector(){}
00058
00059
00060 std::ostream& operator<< (std::ostream& os, const GaussianVector& g)
00061 {
00062 os << "Mu :\n" << g.ExpectedValueGet() << endl
00063 << "Sigma:\n" << g.CovarianceGet() << endl;
00064 return os;
00065 }
00066
00067 void GaussianVector::sigmaSet( const Vector3& sigma )
00068 {
00069 sigma_ = sigma;
00070 sigma_changed_ = true;
00071 }
00072
00073 Probability GaussianVector::ProbabilityGet(const Vector3& input) const
00074 {
00075 if (sigma_changed_){
00076 sigma_changed_ = false;
00077
00078 for (unsigned int i=0; i<3; i++)
00079 sigma_sq_[i] = 2 * sigma_[i] * sigma_[i];
00080
00081 sqrt_ = 1/ sqrt(M_PI*M_PI*M_PI* sigma_sq_[0] * sigma_sq_[1] * sigma_sq_[2]);
00082 }
00083
00084 Vector3 diff = input - mu_;
00085 return sqrt_ * exp( - (diff[0]*diff[0]/sigma_sq_[0])
00086 - (diff[1]*diff[1]/sigma_sq_[1])
00087 - (diff[2]*diff[2]/sigma_sq_[2]) );
00088 }
00089
00090
00091 bool
00092 GaussianVector::SampleFrom (vector<Sample<Vector3> >& list_samples, const int num_samples, int method, void * args) const
00093 {
00094 list_samples.resize(num_samples);
00095 vector<Sample<Vector3> >::iterator sample_it = list_samples.begin();
00096 for (sample_it=list_samples.begin(); sample_it!=list_samples.end(); sample_it++)
00097 SampleFrom( *sample_it, method, args);
00098
00099 return true;
00100 }
00101
00102
00103 bool
00104 GaussianVector::SampleFrom (Sample<Vector3>& one_sample, int method, void * args) const
00105 {
00106 one_sample.ValueSet( Vector3(rnorm(mu_[0], sigma_[0]),
00107 rnorm(mu_[1], sigma_[1]),
00108 rnorm(mu_[2], sigma_[2])));
00109 return true;
00110 }
00111
00112
00113 Vector3
00114 GaussianVector::ExpectedValueGet ( ) const
00115 {
00116 return mu_;
00117 }
00118
00119 SymmetricMatrix
00120 GaussianVector::CovarianceGet () const
00121 {
00122 SymmetricMatrix sigma(3); sigma = 0;
00123 for (unsigned int i=0; i<3; i++)
00124 sigma(i+1,i+1) = pow(sigma_[i],2);
00125 return sigma;
00126 }
00127
00128 GaussianVector*
00129 GaussianVector::Clone() const
00130 {
00131 return new GaussianVector(mu_, sigma_);
00132 }
00133
00134 }