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/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 {
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);
00117 sigma = 0;
00118 for (unsigned int i = 0; i < 3; i++)
00119 sigma(i + 1, i + 1) = pow(size_[i], 2);
00120 return sigma;
00121 }
00122
00123 }