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 hasVariance() const { return false; } 00043 }; 00044 00045 template <class MeasurementModel> 00046 class Update_ : public MeasurementUpdate { 00047 public: 00048 enum { MeasurementDimension = MeasurementModel::MeasurementDimension }; 00049 typedef Update_<MeasurementModel> Type; 00050 typedef typename MeasurementModel::MeasurementVector Vector; 00051 typedef typename MeasurementModel::NoiseVariance Variance; 00052 00053 Update_() 00054 : has_variance_(false) 00055 { 00056 y_.setZero(); 00057 } 00058 Update_(Vector const& y) 00059 : has_variance_(false) 00060 { 00061 *this = y; 00062 } 00063 Update_(double y) 00064 : has_variance_(false) 00065 { 00066 *this = y; 00067 } 00068 Update_(double x, double y, double z) 00069 : has_variance_(false) 00070 { 00071 y_ = Vector(x, y, z); 00072 } 00073 template <typename OtherDerived> Update_(const Eigen::MatrixBase<OtherDerived>& other) 00074 : y_(other) 00075 , has_variance_(false) 00076 {} 00077 virtual ~Update_() {} 00078 00079 virtual Vector &operator=(Vector const& y) { y_ = y; return y_; } 00080 virtual Vector &operator=(double y) { y_(0) = y; return y_; } 00081 00082 virtual Vector const &getVector() const { return y_; } 00083 00084 virtual bool hasVariance() const { return has_variance_; } 00085 virtual Variance const &getVariance() const { return R_; } 00086 virtual void setVariance(Variance const& R) { R_ = R; has_variance_ = true; } 00087 00088 protected: 00089 Vector y_; 00090 Variance R_; 00091 bool has_variance_; 00092 }; 00093 00094 namespace traits { 00095 00096 template <class ConcreteModel> struct Update { typedef Update_<ConcreteModel> type; }; 00097 00098 template <class ConcreteModel, class Enable = void> 00099 class UpdateInspector { 00100 public: 00101 UpdateInspector(const typename Update<ConcreteModel>::type& update) : update_(update) {} 00102 typename ConcreteModel::MeasurementVector const& getVector(const State&) { return *static_cast<typename ConcreteModel::MeasurementVector *>(0); } 00103 typename ConcreteModel::NoiseVariance const& getVariance(const State&) { return *static_cast<typename ConcreteModel::NoiseVariance *>(0); } 00104 private: 00105 const typename Update<ConcreteModel>::type& update_; 00106 }; 00107 00108 template <class ConcreteModel> 00109 class UpdateInspector<ConcreteModel, typename boost::enable_if<boost::is_base_of<Update_<ConcreteModel>, typename Update<ConcreteModel>::type> >::type > 00110 { 00111 public: 00112 UpdateInspector(const typename Update<ConcreteModel>::type& update) : update_(update) {} 00113 typename ConcreteModel::MeasurementVector const& getVector(const State&) { return update_.getVector(); } 00114 typename ConcreteModel::NoiseVariance const& getVariance(const State&) { return update_.getVariance(); } 00115 private: 00116 const typename Update<ConcreteModel>::type& update_; 00117 }; 00118 00119 } // namespace traits 00120 00121 } // namespace hector_pose_estimation 00122 00123 #endif // HECTOR_POSE_ESTIMATION_MEASUREMENT_UPDATE_H