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