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