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