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_H
00030 #define HECTOR_POSE_ESTIMATION_MEASUREMENT_H
00031
00032 #include <hector_pose_estimation/measurement_model.h>
00033 #include <hector_pose_estimation/measurement_update.h>
00034 #include <hector_pose_estimation/types.h>
00035 #include <hector_pose_estimation/queue.h>
00036 #include <hector_pose_estimation/filter.h>
00037
00038 namespace hector_pose_estimation {
00039
00040 template <class ConcreteModel> class Measurement_;
00041
00042 class Measurement
00043 {
00044 public:
00045 Measurement(const std::string& name);
00046 virtual ~Measurement();
00047
00048 template <class ConcreteModel> static boost::shared_ptr<Measurement_<ConcreteModel> > create(ConcreteModel *model, const std::string& name);
00049
00050 virtual const std::string& getName() const { return name_; }
00051 void setName(const std::string& name) { name_ = name; }
00052
00053 virtual MeasurementModel* getModel() const { return 0; }
00054 virtual int getDimension() const { return 0; }
00055
00056 virtual Filter *filter() const { return filter_; }
00057 virtual void setFilter(Filter *filter) { filter_ = filter; }
00058
00059 virtual bool init(PoseEstimation& estimator, State& state);
00060 virtual void cleanup();
00061 virtual void reset(State& state);
00062
00063 virtual ParameterList& parameters() { return parameters_; }
00064 virtual const ParameterList& parameters() const { return parameters_; }
00065
00066 virtual void add(const MeasurementUpdate &update);
00067 virtual bool process();
00068 virtual bool update(const MeasurementUpdate &update);
00069
00070 bool enabled() const { return enabled_; }
00071 void enable() { enabled_ = true; }
00072 void disable() { enabled_ = false; status_flags_ = 0; }
00073
00074 virtual bool active(const State& state);
00075 virtual SystemStatus getStatusFlags() const { return status_flags_; }
00076
00077 void setTimeout(double timeout) { timeout_ = timeout; }
00078 double getTimeout() const { return timeout_; }
00079
00080 void setMinInterval(double min_interval) { min_interval_ = min_interval; }
00081 double getMinInterval() const { return min_interval_; }
00082
00083 void increase_timer(double dt);
00084 bool timedout() const;
00085
00086 protected:
00087 virtual Queue& queue() = 0;
00088 virtual bool updateImpl(const MeasurementUpdate &update) { return false; }
00089
00090 virtual bool onInit(PoseEstimation& estimator) { return true; }
00091 virtual void onReset() { }
00092 virtual void onCleanup() { }
00093
00094 protected:
00095 std::string name_;
00096 ParameterList parameters_;
00097 SystemStatus status_flags_;
00098
00099 bool enabled_;
00100 double min_interval_;
00101
00102 double timeout_;
00103 double timer_;
00104
00105 Filter *filter_;
00106 };
00107
00108 template <class ConcreteModel>
00109 class Measurement_ : public Measurement {
00110 public:
00111 typedef ConcreteModel Model;
00112 typedef typename traits::Update<ConcreteModel>::type Update;
00113
00114 enum { MeasurementDimension = Model::MeasurementDimension };
00115 typedef typename Model::MeasurementVector MeasurementVector;
00116 typedef typename Model::NoiseVariance NoiseVariance;
00117
00118 Measurement_(const std::string& name)
00119 : Measurement(name)
00120 , model_(new Model)
00121 {
00122 parameters_.add(model_->parameters());
00123 }
00124
00125 Measurement_(Model *model, const std::string& name)
00126 : Measurement(name)
00127 , model_(model)
00128 {
00129 parameters_.add(model_->parameters());
00130 }
00131
00132 virtual ~Measurement_() {
00133 }
00134
00135 virtual Model* getModel() const { return model_.get(); }
00136 virtual int getDimension() const { return MeasurementDimension; }
00137
00138 virtual Filter *filter() const { return corrector_ ? corrector_->base() : 0; }
00139 virtual const boost::shared_ptr< Filter::Corrector_<Model> >& corrector() const { return corrector_; }
00140 virtual void setFilter(Filter *filter = 0);
00141
00142 virtual bool init(PoseEstimation& estimator, State& state) {
00143 if (!Measurement::init(estimator, state)) return false;
00144 model_->getMeasurementNoise(R_, state, true);
00145 return true;
00146 }
00147
00148 virtual void reset(State& state) {
00149 model_->getMeasurementNoise(R_, state, true);
00150 Measurement::reset(state);
00151 if (corrector()) corrector()->reset();
00152 }
00153
00154 virtual MeasurementVector const& getVector(const Update &update, const State &state) {
00155 const MeasurementVector *fixed = getModel()->getFixedMeasurementVector();
00156 if (fixed) return *fixed;
00157 return traits::UpdateInspector<ConcreteModel>(update).getVector(state);
00158 }
00159
00160 virtual NoiseVariance const& getVariance(const Update &update, const State &state) {
00161 if (update.hasVariance()) return traits::UpdateInspector<ConcreteModel>(update).getVariance(state);
00162 model_->getMeasurementNoise(R_, state, false);
00163 return R_;
00164 }
00165
00166 virtual void setNoiseVariance(NoiseVariance const& R) {
00167
00168 R_ = R;
00169 }
00170
00171 protected:
00172 virtual bool updateImpl(const MeasurementUpdate &update);
00173 virtual bool prepareUpdate(State &state, const Update &update) { return getModel()->prepareUpdate(state, update); }
00174 virtual void afterUpdate(State &state) { getModel()->afterUpdate(state); }
00175
00176 protected:
00177 boost::shared_ptr<Model> model_;
00178 NoiseVariance R_;
00179
00180 Queue_<Update> queue_;
00181 virtual Queue& queue() { return queue_; }
00182
00183 boost::shared_ptr< Filter::Corrector_<Model> > corrector_;
00184 };
00185
00186 template <class ConcreteModel>
00187 boost::shared_ptr<Measurement_<ConcreteModel> > Measurement::create(ConcreteModel *model, const std::string& name)
00188 {
00189 return boost::make_shared<Measurement_<ConcreteModel> >(model, name);
00190 }
00191
00192 }
00193
00194 #include "measurement.inl"
00195
00196 #endif // HECTOR_POSE_ESTIMATION_MEASUREMENT_H