measurement.h
Go to the documentation of this file.
1 //=================================================================================================
2 // Copyright (c) 2011, Johannes Meyer, TU Darmstadt
3 // All rights reserved.
4 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 // * Neither the name of the Flight Systems and Automatic Control group,
13 // TU Darmstadt, nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without
15 // specific prior written permission.
16 
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //=================================================================================================
28 
29 #ifndef HECTOR_POSE_ESTIMATION_MEASUREMENT_H
30 #define HECTOR_POSE_ESTIMATION_MEASUREMENT_H
31 
37 
38 namespace hector_pose_estimation {
39 
40 template <class ConcreteModel> class Measurement_;
41 
43 {
44 public:
45  Measurement(const std::string& name);
46  virtual ~Measurement();
47 
48  template <class ConcreteModel> static boost::shared_ptr<Measurement_<ConcreteModel> > create(ConcreteModel *model, const std::string& name);
49 
50  virtual const std::string& getName() const { return name_; }
51  void setName(const std::string& name) { name_ = name; }
52 
53  virtual MeasurementModel* getModel() const { return 0; }
54  virtual int getDimension() const { return 0; }
55 
56  virtual Filter *filter() const { return filter_; }
57  virtual void setFilter(Filter *filter) { filter_ = filter; }
58 
59  virtual bool init(PoseEstimation& estimator, State& state);
60  virtual void cleanup();
61  virtual void reset(State& state);
62 
63  virtual ParameterList& parameters() { return parameters_; }
64  virtual const ParameterList& parameters() const { return parameters_; }
65 
66  virtual void add(const MeasurementUpdate &update);
67  virtual bool process();
68  virtual bool update(const MeasurementUpdate &update);
69 
70  bool enabled() const { return enabled_; }
71  void enable() { enabled_ = true; }
72  void disable() { enabled_ = false; status_flags_ = 0; }
73 
74  virtual bool active(const State& state);
75  virtual SystemStatus getStatusFlags() const { return status_flags_; }
76 
77  void setTimeout(double timeout) { timeout_ = timeout; }
78  double getTimeout() const { return timeout_; }
79 
80  void setMinInterval(double min_interval) { min_interval_ = min_interval; }
81  double getMinInterval() const { return min_interval_; }
82 
83  void increase_timer(double dt);
84  bool timedout() const;
85 
86 protected:
87  virtual Queue& queue() = 0;
88  virtual bool updateImpl(const MeasurementUpdate &update) { return false; }
89 
90  virtual bool onInit(PoseEstimation& estimator) { return true; } // currently unsed...
91  virtual void onReset() { }
92  virtual void onCleanup() { } // currently unsed...
93 
94 protected:
95  std::string name_;
98 
99  bool enabled_;
101 
102  double timeout_;
103  double timer_;
104 
106 };
107 
108 template <class ConcreteModel>
109 class Measurement_ : public Measurement {
110 public:
111  typedef ConcreteModel Model;
113 
114  enum { MeasurementDimension = Model::MeasurementDimension };
115  typedef typename Model::MeasurementVector MeasurementVector;
116  typedef typename Model::NoiseVariance NoiseVariance;
117 
118  Measurement_(const std::string& name)
119  : Measurement(name)
120  , model_(new Model)
121  {
122  parameters_.add(model_->parameters());
123  }
124 
125  Measurement_(Model *model, const std::string& name)
126  : Measurement(name)
127  , model_(model)
128  {
129  parameters_.add(model_->parameters());
130  }
131 
132  virtual ~Measurement_() {
133  }
134 
135  virtual Model* getModel() const { return model_.get(); }
136  virtual int getDimension() const { return MeasurementDimension; }
137 
138  virtual Filter *filter() const { return corrector_ ? corrector_->base() : 0; }
139  virtual const boost::shared_ptr< Filter::Corrector_<Model> >& corrector() const { return corrector_; }
140  virtual void setFilter(Filter *filter = 0); // implemented in filter/set_filter.h
141 
142  virtual bool init(PoseEstimation& estimator, State& state) {
143  if (!Measurement::init(estimator, state)) return false;
144  model_->getMeasurementNoise(R_, state, true);
145  return true;
146  }
147 
148  virtual void reset(State& state) {
149  model_->getMeasurementNoise(R_, state, true);
150  Measurement::reset(state);
151  if (corrector()) corrector()->reset();
152  }
153 
154  virtual MeasurementVector const& getVector(const Update &update, const State &state) {
155  const MeasurementVector *fixed = getModel()->getFixedMeasurementVector();
156  if (fixed) return *fixed;
157  return traits::UpdateInspector<ConcreteModel>(update).getVector(state);
158  }
159 
160  virtual NoiseVariance const& getVariance(const Update &update, const State &state) {
161  if (update.hasVariance()) return traits::UpdateInspector<ConcreteModel>(update).getVariance(state);
162  model_->getMeasurementNoise(R_, state, false);
163  return R_;
164  }
165 
166  virtual void setNoiseVariance(NoiseVariance const& R) {
167 // if (!R_) R_.reset(new NoiseVariance);
168  R_ = R;
169  }
170 
171 protected:
172  virtual bool updateImpl(const MeasurementUpdate &update);
173  virtual bool prepareUpdate(State &state, const Update &update) { return getModel()->prepareUpdate(state, update); }
174  virtual void afterUpdate(State &state) { getModel()->afterUpdate(state); }
175 
176 protected:
178  NoiseVariance R_;
179 
181  virtual Queue& queue() { return queue_; }
182 
184 };
185 
186 template <class ConcreteModel>
187 boost::shared_ptr<Measurement_<ConcreteModel> > Measurement::create(ConcreteModel *model, const std::string& name)
188 {
189  return boost::make_shared<Measurement_<ConcreteModel> >(model, name);
190 }
191 
192 } // namespace hector_pose_estimation
193 
194 #include "measurement.inl"
195 
196 #endif // HECTOR_POSE_ESTIMATION_MEASUREMENT_H
static boost::shared_ptr< Measurement_< ConcreteModel > > create(ConcreteModel *model, const std::string &name)
Definition: measurement.h:187
virtual void afterUpdate(State &state)
Definition: measurement.h:174
virtual bool update(const MeasurementUpdate &update)
virtual void afterUpdate(State &state)
virtual NoiseVariance const & getVariance(const Update &update, const State &state)
Definition: measurement.h:160
virtual void reset(State &state)
Definition: measurement.h:148
virtual bool hasVariance() const
virtual bool active(const State &state)
Definition: measurement.cpp:76
virtual const std::string & getName() const
Definition: measurement.h:50
virtual MeasurementModel * getModel() const
Definition: measurement.h:53
Measurement_(const std::string &name)
Definition: measurement.h:118
virtual void setNoiseVariance(NoiseVariance const &R)
Definition: measurement.h:166
virtual ParameterList & parameters()
Definition: measurement.h:63
virtual bool onInit(PoseEstimation &estimator)
Definition: measurement.h:90
Measurement(const std::string &name)
Definition: measurement.cpp:35
void setName(const std::string &name)
Definition: measurement.h:51
virtual Filter * filter() const
Definition: measurement.h:56
virtual bool prepareUpdate(State &state, const Update &update)
Definition: measurement.h:173
ParameterList & add(const std::string &key, T &value, const T &default_value)
Definition: parameters.h:148
boost::shared_ptr< Model > model_
Definition: measurement.h:177
virtual void reset(State &state)
Definition: measurement.cpp:66
virtual bool prepareUpdate(State &state, const MeasurementUpdate &update)
virtual bool updateImpl(const MeasurementUpdate &update)
Definition: measurement.h:88
virtual SystemStatus getStatusFlags() const
Definition: measurement.h:75
Model::MeasurementVector MeasurementVector
Definition: measurement.h:115
virtual bool init(PoseEstimation &estimator, State &state)
Definition: measurement.h:142
virtual const ParameterList & parameters() const
Definition: measurement.h:64
void setTimeout(double timeout)
Definition: measurement.h:77
virtual int getDimension() const
Definition: measurement.h:54
Model::NoiseVariance NoiseVariance
Definition: measurement.h:116
virtual Filter * filter() const
Definition: measurement.h:138
unsigned int SystemStatus
Definition: types.h:70
Measurement_(Model *model, const std::string &name)
Definition: measurement.h:125
traits::Update< ConcreteModel >::type Update
Definition: measurement.h:112
virtual bool init(PoseEstimation &estimator, State &state)
Definition: measurement.cpp:53
virtual const boost::shared_ptr< Filter::Corrector_< Model > > & corrector() const
Definition: measurement.h:139
virtual Model * getModel() const
Definition: measurement.h:135
virtual int getDimension() const
Definition: measurement.h:136
boost::shared_ptr< Filter::Corrector_< Model > > corrector_
Definition: measurement.h:183
virtual void setFilter(Filter *filter)
Definition: measurement.h:57
virtual void add(const MeasurementUpdate &update)
Definition: measurement.cpp:91
ConcreteModel::NoiseVariance const & getVariance(const State &)
void setMinInterval(double min_interval)
Definition: measurement.h:80
virtual MeasurementVector const & getVector(const Update &update, const State &state)
Definition: measurement.h:154


hector_pose_estimation_core
Author(s): Johannes Meyer
autogenerated on Thu Feb 18 2021 03:29:30