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; }
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 MeasurementVector const& getVector(const Update &update, const State &state) {
00143 const MeasurementVector *fixed = getModel()->getFixedMeasurementVector();
00144 if (fixed) return *fixed;
00145 return traits::UpdateInspector<ConcreteModel>(update).getVector(state);
00146 }
00147
00148 virtual NoiseVariance const& getVariance(const Update &update, const State &state) {
00149 if (update.hasVariance()) return traits::UpdateInspector<ConcreteModel>(update).getVariance(state);
00150
00151 bool init = false;
00152 if (!R_) {
00153 R_.reset(new NoiseVariance);
00154 init = true;
00155 }
00156 model_->getMeasurementNoise(*R_, state, init);
00157 return *R_;
00158 }
00159
00160 virtual void setNoiseVariance(NoiseVariance const& R) {
00161 if (!R_) R_.reset(new NoiseVariance);
00162 *R_ = R;
00163 }
00164
00165 virtual void clearNoiseVariance() {
00166 R_.reset();
00167 }
00168
00169 protected:
00170 virtual bool updateImpl(const MeasurementUpdate &update);
00171 virtual bool prepareUpdate(State &state, const Update &update) { return getModel()->prepareUpdate(state, update); }
00172 virtual void afterUpdate(State &state) { getModel()->afterUpdate(state); }
00173
00174 protected:
00175 boost::shared_ptr<Model> model_;
00176 boost::shared_ptr<NoiseVariance> R_;
00177
00178 Queue_<Update> queue_;
00179 virtual Queue& queue() { return queue_; }
00180
00181 boost::shared_ptr< Filter::Corrector_<Model> > corrector_;
00182 };
00183
00184 template <class ConcreteModel>
00185 boost::shared_ptr<Measurement_<ConcreteModel> > Measurement::create(ConcreteModel *model, const std::string& name)
00186 {
00187 return boost::make_shared<Measurement_<ConcreteModel> >(model, name);
00188 }
00189
00190 }
00191
00192 #include "measurement.inl"
00193
00194 #endif // HECTOR_POSE_ESTIMATION_MEASUREMENT_H