$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_UPDATE_H 00030 #define HECTOR_POSE_ESTIMATION_MEASUREMENT_UPDATE_H 00031 00032 #include <boost/type_traits/is_base_of.hpp> 00033 00034 namespace hector_pose_estimation { 00035 00036 class MeasurementUpdate 00037 { 00038 public: 00039 MeasurementUpdate() {} 00040 virtual ~MeasurementUpdate() {} 00041 00042 virtual bool hasCovariance() const { return false; } 00043 }; 00044 00045 template <class MeasurementModel> 00046 class Update_ : public MeasurementUpdate { 00047 public: 00048 typedef Update_<MeasurementModel> Type; 00049 typedef typename MeasurementModel::MeasurementVector Vector; 00050 typedef typename MeasurementModel::NoiseCovariance Covariance; 00051 00052 Update_() 00053 : y_(MeasurementModel::MeasurementDimension) 00054 , has_covariance_(false) 00055 {} 00056 Update_(Vector const& y) 00057 : y_(MeasurementModel::MeasurementDimension) 00058 , has_covariance_(false) 00059 { 00060 setValue(y); 00061 } 00062 Update_(double y) 00063 : y_(MeasurementModel::MeasurementDimension) 00064 , has_covariance_(false) 00065 { 00066 setValue(y); 00067 } 00068 virtual ~Update_() {} 00069 00070 virtual void setValue(Vector const& y) { y_ = y; } 00071 virtual void setValue(double y) { y_(1) = y; } 00072 00073 virtual void setCovariance(Covariance const& R) { R_ = R; has_covariance_ = true; } 00074 virtual Vector const &getVector() const { return y_; } 00075 virtual Covariance const &getCovariance() const { return R_; } 00076 00077 virtual Vector &operator=(Vector const& y) { setValue(y); return y_; } 00078 virtual Vector &operator=(double y) { setValue(y); return y_; } 00079 00080 virtual bool hasCovariance() const { return has_covariance_; } 00081 00082 protected: 00083 Vector y_; 00084 Covariance R_; 00085 bool has_covariance_; 00086 }; 00087 00088 namespace internal { 00089 template <class ConcreteModel, class ConcreteUpdate = Update_<ConcreteModel>, class Enable = void> 00090 struct UpdateInspector { 00091 static typename ConcreteModel::MeasurementVector const& getVector(const ConcreteUpdate &update, const ConcreteModel*) { return *static_cast<typename ConcreteModel::MeasurementVector *>(0); } 00092 static typename ConcreteModel::NoiseCovariance const& getCovariance(const ConcreteUpdate &update, const ConcreteModel*) { return *static_cast<typename ConcreteModel::NoiseCovariance *>(0); } 00093 }; 00094 00095 template <class ConcreteModel, class ConcreteUpdate> 00096 struct UpdateInspector<ConcreteModel, ConcreteUpdate, typename boost::enable_if<boost::is_base_of<Update_<ConcreteModel>,ConcreteUpdate> >::type > 00097 { 00098 static typename ConcreteModel::MeasurementVector const& getVector(const ConcreteUpdate &update, const ConcreteModel*) { return update.getVector(); } 00099 static typename ConcreteModel::NoiseCovariance const& getCovariance(const ConcreteUpdate &update, const ConcreteModel*) { return update.getCovariance(); } 00100 }; 00101 } 00102 00103 } // namespace hector_pose_estimation 00104 00105 #endif // HECTOR_POSE_ESTIMATION_MEASUREMENT_UPDATE_H