00001 //================================================================================================= 00002 // Copyright (c) 2013, 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_FILTER_H 00030 #define HECTOR_POSE_ESTIMATION_FILTER_H 00031 00032 #include <hector_pose_estimation/state.h> 00033 #include <hector_pose_estimation/types.h> 00034 00035 #include <map> 00036 00037 namespace hector_pose_estimation { 00038 00039 class Filter { 00040 public: 00041 Filter(); 00042 virtual ~Filter(); 00043 00044 virtual std::string getType() const = 0; 00045 00046 virtual bool init(PoseEstimation& estimator); 00047 virtual void cleanup(); 00048 virtual void reset(); 00049 00050 virtual const State& state() const { return state_; } 00051 virtual State& state() { return state_; } 00052 00053 virtual bool predict(const Systems& systems, double dt); 00054 virtual bool predict(const SystemPtr& system, double dt); 00055 virtual bool doPredict(double dt); 00056 00057 virtual bool correct(const Measurements& measurements); 00058 virtual bool correct(const MeasurementPtr& measurement); 00059 virtual bool doCorrect(); 00060 00061 // struct Predictor {}; 00062 template <class ConcreteModel> struct Predictor_ /* : public Predictor */ { 00063 Predictor_(Filter *filter, ConcreteModel *model) : filter_(filter), model_(model) { reset(); } 00064 virtual bool predict(double dt) = 0; 00065 virtual void reset() { init_ = true; } 00066 00067 Filter *base() { return filter_; } 00068 const Filter *base() const { return filter_; } 00069 00070 State& state() { return filter_->state(); } 00071 const State& state() const { return filter_->state(); } 00072 00073 typename ConcreteModel::SubState& sub() { return model_->state(filter_->state()); } 00074 const typename ConcreteModel::SubState& sub() const { return model_->state(filter_->state()); } 00075 00076 template <typename Derived> typename Derived::template Predictor_<ConcreteModel> *derived() { return dynamic_cast<typename Derived::template Predictor_<ConcreteModel> *>(this); } 00077 template <typename Derived> const typename Derived::template Predictor_<ConcreteModel> *derived() const { return dynamic_cast<const typename Derived::template Predictor_<ConcreteModel> *>(this); } 00078 00079 protected: 00080 Filter *filter_; 00081 ConcreteModel *model_; 00082 bool init_; 00083 }; 00084 00085 // class Corrector {}; 00086 template <class ConcreteModel> struct Corrector_ /* : public Corrector */ { 00087 Corrector_(Filter *filter, ConcreteModel *model) : filter_(filter), model_(model) { reset(); } 00088 virtual bool correct(const typename ConcreteModel::MeasurementVector& y, const typename ConcreteModel::NoiseVariance& R) = 0; 00089 virtual void reset() { init_ = true; } 00090 00091 Filter *base() { return filter_; } 00092 const Filter *base() const { return filter_; } 00093 00094 State& state() { return filter_->state(); } 00095 const State& state() const { return filter_->state(); } 00096 00097 typename ConcreteModel::SubState& sub() { return model_->sub(filter_->state()); } 00098 const typename ConcreteModel::SubState& sub() const { return model_->sub(filter_->state()); } 00099 00100 template <typename Derived> typename Derived::template Corrector_<ConcreteModel> *derived() { return dynamic_cast<typename Derived::template Corrector_<ConcreteModel> *>(this); } 00101 template <typename Derived> const typename Derived::template Corrector_<ConcreteModel> *derived() const { return dynamic_cast<const typename Derived::template Corrector_<ConcreteModel> *>(this); } 00102 00103 protected: 00104 Filter *filter_; 00105 ConcreteModel *model_; 00106 bool init_; 00107 }; 00108 00109 template <typename Derived> Derived *derived() { return dynamic_cast<Derived *>(this); } 00110 template <typename Derived> const Derived *derived() const { return dynamic_cast<const Derived *>(this); } 00111 00112 template <typename Derived> 00113 struct Factory { 00114 Factory(Derived *filter) : filter_(filter) {} 00115 // template <class ConcreteModel> boost::shared_ptr<typename Derived::template Predictor<ConcreteModel> > addPredictor(ConcreteModel *model) { return boost::make_shared<typename Derived::template Predictor_<ConcreteModel> >(filter_, model); } 00116 // template <class ConcreteModel> boost::shared_ptr<typename Derived::template Corrector<ConcreteModel> > addCorrector(ConcreteModel *model) { return boost::make_shared<typename Derived::template Corrector_<ConcreteModel> >(filter_, model); } 00117 template <class ConcreteModel> boost::shared_ptr<Predictor_<ConcreteModel> > addPredictor(ConcreteModel *model) { return boost::shared_static_cast<Predictor_<ConcreteModel> >(boost::make_shared<typename Derived::template Predictor_<ConcreteModel> >(filter_, model)); } 00118 template <class ConcreteModel> boost::shared_ptr<Corrector_<ConcreteModel> > addCorrector(ConcreteModel *model) { return boost::shared_static_cast<Corrector_<ConcreteModel> >(boost::make_shared<typename Derived::template Corrector_<ConcreteModel> >(filter_, model)); } 00119 00120 private: 00121 Derived *filter_; 00122 }; 00123 template <typename Derived> static Factory<Derived> factory(Derived *filter) { return Factory<Derived>(filter); } 00124 00125 protected: 00126 State state_; 00127 }; 00128 00129 } // namespace hector_pose_estimation 00130 00131 #endif // HECTOR_POSE_ESTIMATION_FILTER_H