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/uniform_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 UniformVector::UniformVector(const Vector3& mu, const Vector3& size)
00048 : Pdf<Vector3> ( 1 ),
00049 mu_(mu),
00050 size_(size)
00051 {
00052 for (unsigned int i=0; i<3; i++)
00053 assert(size_[i] > 0);
00054
00055 probability_ = 1 / (size_[0]*2 * size_[1]*2 * size_[2]*2);
00056 }
00057
00058
00059 UniformVector::~UniformVector(){}
00060
00061 UniformVector* UniformVector::Clone() const
00062 {
00063 return new UniformVector(mu_, size_);
00064 }
00065
00066 std::ostream& operator<< (std::ostream& os, const UniformVector& g)
00067 {
00068 os << "Mu :\n" << g.ExpectedValueGet() << endl
00069 << "Size :\n" << g.CovarianceGet() << endl;
00070 return os;
00071 }
00072
00073
00074
00075 Probability UniformVector::ProbabilityGet(const Vector3& input) const
00076 {
00077 for (unsigned int i=0; i<3; i++){
00078 if (input[i] < (mu_[0] - (size_[0]))) return 0;
00079 if (input[i] > (mu_[0] + (size_[0]))) return 0;
00080 }
00081 return probability_;
00082 }
00083
00084
00085 bool
00086 UniformVector::SampleFrom (vector<Sample<Vector3> >& list_samples, const int num_samples, int method, void * args) const
00087 {
00088 list_samples.resize(num_samples);
00089 vector<Sample<Vector3> >::iterator sample_it = list_samples.begin();
00090 for (sample_it=list_samples.begin(); sample_it!=list_samples.end(); sample_it++)
00091 SampleFrom( *sample_it, method, args);
00092
00093 return true;
00094 }
00095
00096
00097 bool
00098 UniformVector::SampleFrom (Sample<Vector3>& one_sample, int method, void * args) const
00099 {
00100 one_sample.ValueSet( Vector3( ((runif() - 0.5) * 2 * size_[0]) + mu_[0],
00101 ((runif() - 0.5) * 2 * size_[1]) + mu_[1],
00102 ((runif() - 0.5) * 2 * size_[2]) + mu_[2]));
00103 return true;
00104 }
00105
00106
00107 Vector3
00108 UniformVector::ExpectedValueGet ( ) const
00109 {
00110 return mu_;
00111 }
00112
00113 SymmetricMatrix
00114 UniformVector::CovarianceGet () const
00115 {
00116 SymmetricMatrix sigma(3); sigma = 0;
00117 for (unsigned int i=0; i<3; i++)
00118 sigma(i+1,i+1) = pow(size_[i],2);
00119 return sigma;
00120 }
00121
00122 }