$search
00001 //================================================================================================= 00002 // Copyright (c) 2011, Johannes Meyer, TU Darmstadt 00003 // All rights reserved. 00004 00005 // Redistribution and use in source and binary forms, with or without 00006 // modification, are permitted provided that the following conditions are met: 00007 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Flight Systems and Automatic Control group, 00013 // TU Darmstadt, nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00027 //================================================================================================= 00028 00029 #ifndef HECTOR_POSE_ESTIMATION_MEASUREMENT_H 00030 #define HECTOR_POSE_ESTIMATION_MEASUREMENT_H 00031 00032 #include <bfl/filter/kalmanfilter.h> 00033 #include "measurement_model.h" 00034 #include "measurement_update.h" 00035 #include "queue.h" 00036 00037 namespace hector_pose_estimation { 00038 00039 class PoseEstimation; 00040 00041 class Measurement 00042 { 00043 public: 00044 Measurement(const std::string& name); 00045 virtual ~Measurement(); 00046 00047 virtual const std::string& getName() const { return name_; } 00048 void setName(const std::string& name) { name_ = name; } 00049 00050 virtual MeasurementModel* getModel() const { return 0; } 00051 00052 virtual bool init(); 00053 virtual void cleanup(); 00054 virtual void reset(); 00055 00056 virtual SystemStatus getStatusFlags() const { return status_flags_; } 00057 virtual bool active(const SystemStatus& status) { return enabled(); } 00058 00059 virtual ParameterList& parameters() { return parameters_; } 00060 virtual const ParameterList& parameters() const { return parameters_; } 00061 00062 virtual void add(const MeasurementUpdate &update); 00063 virtual bool update(PoseEstimation &estimator, const MeasurementUpdate &update) = 0; 00064 virtual void process(PoseEstimation &estimator); 00065 00066 bool enabled() const { return enabled_; } 00067 void enable() { enabled_ = true; } 00068 void disable() { enabled_ = false; } 00069 00070 void increase_timer(double dt); 00071 void updated(); 00072 bool timedout() const; 00073 00074 protected: 00075 virtual Queue& queue() = 0; 00076 void updateInternal(PoseEstimation &estimator, ColumnVector const& y); 00077 00078 virtual bool onInit() { return true; } 00079 virtual void onReset() { } 00080 virtual void onCleanup() { } 00081 00082 protected: 00083 std::string name_; 00084 ParameterList parameters_; 00085 SystemStatus status_flags_; 00086 00087 bool enabled_; 00088 double min_interval_; 00089 00090 double timeout_; 00091 double timer_; 00092 }; 00093 00094 typedef boost::shared_ptr<Measurement> MeasurementPtr; 00095 00096 template <class ConcreteModel, class ConcreteUpdate = typename Update_<ConcreteModel>::Type > 00097 class Measurement_ : public Measurement { 00098 public: 00099 typedef ConcreteModel Model; 00100 typedef ConcreteUpdate Update; 00101 static const unsigned int MeasurementDimension = Model::MeasurementDimension; 00102 typedef typename Model::MeasurementVector MeasurementVector; 00103 typedef typename Model::NoiseCovariance NoiseCovariance; 00104 00105 Measurement_(const std::string& name) 00106 : Measurement(name) 00107 , model_(new Model) 00108 { 00109 parameters_.add(model_->parameters()); 00110 } 00111 00112 Measurement_(Model *model, const std::string& name) 00113 : Measurement(name) 00114 , model_(model) 00115 { 00116 parameters_.add(model_->parameters()); 00117 } 00118 00119 virtual ~Measurement_() { 00120 } 00121 00122 virtual bool init() { return model_->init() && Measurement::init(); } 00123 virtual void cleanup() { model_->cleanup(); Measurement::cleanup(); } 00124 virtual void reset() { model_->reset(); Measurement::reset(); } 00125 00126 virtual Model* getModel() const { return model_.get(); } 00127 virtual bool active(const SystemStatus& status) { return enabled() && model_->applyStatusMask(status); } 00128 00129 virtual MeasurementVector const& getVector(const Update &update) { return internal::UpdateInspector<ConcreteModel,ConcreteUpdate>::getVector(update, getModel()); } 00130 virtual NoiseCovariance const& getCovariance(const Update &update) { return update.hasCovariance() ? internal::UpdateInspector<ConcreteModel,ConcreteUpdate>::getCovariance(update, getModel()) : static_cast<NoiseCovariance const&>(model_->AdditiveNoiseSigmaGet()); } 00131 virtual void setNoiseCovariance(NoiseCovariance const& sigma); 00132 00133 virtual bool update(PoseEstimation &estimator, const MeasurementUpdate &update); 00134 00135 protected: 00136 boost::shared_ptr<Model> model_; 00137 00138 Queue_<Update> queue_; 00139 virtual Queue& queue() { return queue_; } 00140 00141 virtual bool beforeUpdate(PoseEstimation &estimator, const Update &update) { return true; } 00142 virtual void afterUpdate(PoseEstimation &estimator) { } 00143 }; 00144 00145 template <class ConcreteModel, class ConcreteUpdate> 00146 bool Measurement_<ConcreteModel, ConcreteUpdate>::update(PoseEstimation &estimator, const MeasurementUpdate &update_) 00147 { 00148 if (!enabled()) return false; 00149 if (min_interval_ > 0.0 && timer_ < min_interval_) return false; 00150 00151 try { 00152 Update const &update = dynamic_cast<Update const &>(update_); 00153 if (!beforeUpdate(estimator, update)) return false; 00154 00155 if (update.hasCovariance()) setNoiseCovariance(getCovariance(update)); 00156 updateInternal(estimator, getVector(update)); 00157 } catch(std::bad_cast& e) { 00158 std::cerr << "In measurement " << getName() << ": " << e.what() << std::endl; 00159 } 00160 00161 afterUpdate(estimator); 00162 return true; 00163 } 00164 00165 template <class ConcreteModel, class ConcreteUpdate> 00166 void Measurement_<ConcreteModel, ConcreteUpdate>::setNoiseCovariance(Measurement_<ConcreteModel, ConcreteUpdate>::NoiseCovariance const& sigma) 00167 { 00168 model_->AdditiveNoiseSigmaSet(sigma); 00169 } 00170 00171 } // namespace hector_pose_estimation 00172 00173 #endif // HECTOR_POSE_ESTIMATION_MEASUREMENT_H