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