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 "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 {
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);
00124 sigma = 0;
00125 for (unsigned int i = 0; i < 3; i++)
00126 sigma(i + 1, i + 1) = pow(sigma_[i], 2);
00127 return sigma;
00128 }
00129
00130 GaussianVector*
00131 GaussianVector::Clone() const
00132 {
00133 return new GaussianVector(mu_, sigma_);
00134 }
00135
00136 }