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