Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
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 = typename Update_<ConcreteModel>::Type, class Enable = void>
00090 struct UpdateInspector {
00091 static typename ConcreteModel::MeasurementVector const& getVector(const ConcreteUpdate &, const ConcreteModel*) {
00092 throw std::runtime_error("You have to implement the UpdateInspector template class for your update type if you do not inherit from the Update_<MeasurementModel> class.");
00093 }
00094 static typename ConcreteModel::NoiseCovariance const& getCovariance(const ConcreteUpdate &, const ConcreteModel*) {
00095 throw std::runtime_error("You have to implement the UpdateInspector template class for your update type if you do not inherit from the Update_<MeasurementModel> class.");
00096 }
00097 };
00098
00099 template <class ConcreteModel, class ConcreteUpdate>
00100 struct UpdateInspector<ConcreteModel, ConcreteUpdate, typename boost::enable_if<boost::is_base_of<typename Update_<ConcreteModel>::Type,ConcreteUpdate> >::type >
00101 {
00102 static typename ConcreteModel::MeasurementVector const& getVector(const ConcreteUpdate &update, const ConcreteModel*) { return update.getVector(); }
00103 static typename ConcreteModel::NoiseCovariance const& getCovariance(const ConcreteUpdate &update, const ConcreteModel*) { return update.getCovariance(); }
00104 };
00105 }
00106
00107 }
00108
00109 #endif // HECTOR_POSE_ESTIMATION_MEASUREMENT_UPDATE_H