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 #ifndef HECTOR_POSE_ESTIMATION_MEASUREMENT_MODEL_H
00030 #define HECTOR_POSE_ESTIMATION_MEASUREMENT_MODEL_H
00031
00032 #include <hector_pose_estimation/model.h>
00033 #include <hector_pose_estimation/substate.h>
00034 #include <hector_pose_estimation/input.h>
00035
00036 namespace hector_pose_estimation {
00037
00038 class MeasurementModel : public Model {
00039 public:
00040 virtual ~MeasurementModel() {}
00041
00042 virtual bool init(PoseEstimation& estimator, Measurement &measurement, State& state) { return true; }
00043 virtual int getDimension() const = 0;
00044
00045 virtual SystemStatus getStatusFlags() { return SystemStatus(0); }
00046 virtual bool active(const State& state) { return !(state.getSystemStatus() & STATUS_ALIGNMENT); }
00047
00048 virtual bool prepareUpdate(State& state, const MeasurementUpdate& update) { return true; }
00049 virtual void afterUpdate(State& state) {}
00050 };
00051
00052 template <class Derived, int _Dimension> class MeasurementModel_;
00053
00054 namespace traits {
00055
00056 template <class Derived, int _Dimension = Derived::MeasurementDimension>
00057 struct MeasurementModel {
00058 enum { MeasurementDimension = _Dimension };
00059 typedef typename ColumnVector_<MeasurementDimension>::type MeasurementVector;
00060 typedef typename SymmetricMatrix_<MeasurementDimension>::type NoiseVariance;
00061 typedef typename Matrix_<MeasurementDimension,Dynamic>::type MeasurementMatrix;
00062 typedef typename Matrix_<State::Covariance::RowsAtCompileTime,MeasurementDimension>::type GainMatrix;
00063 typedef typename ColumnVector_<State::Covariance::RowsAtCompileTime>::type UpdateVector;
00064
00065 enum { InputDimension = traits::Input<Derived>::Dimension };
00066 typedef typename traits::Input<Derived>::Type InputType;
00067 typedef typename traits::Input<Derived>::Vector InputVector;
00068 typedef typename Matrix_<MeasurementDimension,InputDimension>::type InputMatrix;
00069 };
00070
00071 #define MEASUREMENT_MODEL_TRAIT(Derived, _Dimension) \
00072 typedef typename traits::MeasurementModel<Derived, _Dimension> trait; \
00073 \
00074 enum { MeasurementDimension = _Dimension }; \
00075 typedef typename trait::MeasurementVector MeasurementVector; \
00076 typedef typename trait::NoiseVariance NoiseVariance; \
00077 typedef typename trait::MeasurementMatrix MeasurementMatrix; \
00078 typedef typename trait::GainMatrix GainMatrix; \
00079 typedef typename trait::UpdateVector UpdateVector; \
00080 \
00081 enum { InputDimension = trait::InputDimension }; \
00082 typedef typename trait::InputType InputType; \
00083 typedef typename trait::InputVector InputVector; \
00084 typedef typename trait::InputMatrix InputMatrix; \
00085
00086 }
00087
00088 template <class Derived, int _Dimension>
00089 class MeasurementModel_ : public MeasurementModel {
00090 public:
00091 MEASUREMENT_MODEL_TRAIT(Derived, _Dimension)
00092 virtual ~MeasurementModel_() {}
00093
00094 virtual int getDimension() const { return trait::MeasurementDimension; }
00095
00096 Derived *derived() { return static_cast<Derived *>(this); }
00097 const Derived *derived() const { return static_cast<const Derived *>(this); }
00098
00099 virtual void getExpectedValue(MeasurementVector& y_pred, const State& state);
00100 virtual void getStateJacobian(MeasurementMatrix& C, const State& state, bool init = true);
00101 virtual void getInputJacobian(InputMatrix& D, const State& state, bool init = true);
00102 virtual void getMeasurementNoise(NoiseVariance& R, const State& state, bool init = true);
00103
00104 virtual void limitError(MeasurementVector& error) {}
00105
00106 virtual const MeasurementVector* getFixedMeasurementVector() const { return 0; }
00107 };
00108
00109 template <class Derived, int _Dimension>
00110 void MeasurementModel_<Derived, _Dimension>::getExpectedValue(MeasurementVector& y_pred, const State& state)
00111 {
00112 y_pred.setZero();
00113 }
00114
00115 template <class Derived, int _Dimension>
00116 void MeasurementModel_<Derived, _Dimension>::getStateJacobian(MeasurementMatrix& C, const State& state, bool init)
00117 {
00118 if (init) C.setZero();
00119 }
00120
00121 template <class Derived, int _Dimension>
00122 void MeasurementModel_<Derived, _Dimension>::getInputJacobian(InputMatrix& D, const State& state, bool init)
00123 {
00124 if (init) D.setZero();
00125 }
00126
00127 template <class Derived, int _Dimension>
00128 void MeasurementModel_<Derived, _Dimension>::getMeasurementNoise(NoiseVariance& R, const State& state, bool init)
00129 {
00130 if (init) R.setZero();
00131 }
00132
00133 }
00134
00135 #endif // HECTOR_POSE_ESTIMATION_MEASUREMENT_MODEL_H