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/measmodel_vector.h"
00038
00039 using namespace std;
00040 using namespace BFL;
00041 using namespace tf;
00042
00043 static const unsigned int NUM_MEASMODEL_VECTOR_COND_ARGS = 1;
00044 static const unsigned int DIM_MEASMODEL_VECTOR = 3;
00045
00046
00047
00048 MeasPdfVector::MeasPdfVector(const Vector3& sigma)
00049 : ConditionalPdf<Vector3, Vector3>(DIM_MEASMODEL_VECTOR, NUM_MEASMODEL_VECTOR_COND_ARGS),
00050 meas_noise_(Vector3(0, 0, 0), sigma)
00051 {}
00052
00053
00054
00055 MeasPdfVector::~MeasPdfVector()
00056 {}
00057
00058
00059
00060 Probability
00061 MeasPdfVector::ProbabilityGet(const Vector3& measurement) const
00062 {
00063 return meas_noise_.ProbabilityGet(measurement - ConditionalArgumentGet(0));
00064 }
00065
00066
00067
00068 bool
00069 MeasPdfVector::SampleFrom(Sample<Vector3>& one_sample, int method, void *args) const
00070 {
00071 cerr << "MeasPdfVector::SampleFrom Method not applicable" << endl;
00072 assert(0);
00073 return false;
00074 }
00075
00076
00077
00078
00079 Vector3
00080 MeasPdfVector::ExpectedValueGet() const
00081 {
00082 cerr << "MeasPdfVector::ExpectedValueGet Method not applicable" << endl;
00083 Vector3 result;
00084 assert(0);
00085 return result;
00086 }
00087
00088
00089
00090
00091 SymmetricMatrix
00092 MeasPdfVector::CovarianceGet() const
00093 {
00094 cerr << "MeasPdfVector::CovarianceGet Method not applicable" << endl;
00095 SymmetricMatrix Covar(DIM_MEASMODEL_VECTOR);
00096 assert(0);
00097 return Covar;
00098 }
00099
00100
00101 void
00102 MeasPdfVector::CovarianceSet(const MatrixWrapper::SymmetricMatrix& cov)
00103 {
00104 Vector3 cov_vec(sqrt(cov(1, 1)), sqrt(cov(2, 2)), sqrt(cov(3, 3)));
00105 meas_noise_.sigmaSet(cov_vec);
00106 }
00107
00108
00109