29 #ifndef HECTOR_POSE_ESTIMATION_MEASUREMENT_UPDATE_H 30 #define HECTOR_POSE_ESTIMATION_MEASUREMENT_UPDATE_H 32 #include <boost/type_traits/is_base_of.hpp> 45 template <
class MeasurementModel>
48 enum { MeasurementDimension = MeasurementModel::MeasurementDimension };
50 typedef typename MeasurementModel::MeasurementVector
Vector;
51 typedef typename MeasurementModel::NoiseVariance
Variance;
54 : has_variance_(false)
59 : has_variance_(false)
64 : has_variance_(false)
69 : has_variance_(false)
73 template <
typename OtherDerived>
Update_(
const Eigen::MatrixBase<OtherDerived>& other)
75 , has_variance_(false)
79 virtual Vector &
operator=(Vector
const& y) { y_ = y;
return y_; }
80 virtual Vector &
operator=(
double y) { y_(0) = y;
return y_; }
82 virtual Vector
const &
getVector()
const {
return y_; }
86 virtual void setVariance(Variance
const& R) { R_ = R; has_variance_ =
true; }
98 template <
class ConcreteModel,
class Enable =
void>
102 typename ConcreteModel::MeasurementVector
const&
getVector(
const State&) {
return *
static_cast<typename ConcreteModel::MeasurementVector *
>(0); }
103 typename ConcreteModel::NoiseVariance
const&
getVariance(
const State&) {
return *
static_cast<typename ConcreteModel::NoiseVariance *
>(0); }
108 template <
class ConcreteModel>
109 class UpdateInspector<ConcreteModel, typename
boost::enable_if<boost::is_base_of<Update_<ConcreteModel>, typename Update<ConcreteModel>::type> >::type >
113 typename ConcreteModel::MeasurementVector
const&
getVector(
const State&) {
return update_.getVector(); }
114 typename ConcreteModel::NoiseVariance
const&
getVariance(
const State&) {
return update_.getVariance(); }
123 #endif // HECTOR_POSE_ESTIMATION_MEASUREMENT_UPDATE_H
virtual Vector & operator=(Vector const &y)
Update_< MeasurementModel > Type
Update_(const Eigen::MatrixBase< OtherDerived > &other)
ConcreteModel::NoiseVariance const & getVariance(const State &)
Update_(double x, double y, double z)
virtual bool hasVariance() const
virtual void setVariance(Variance const &R)
ConcreteModel::MeasurementVector const & getVector(const State &)
virtual Variance const & getVariance() const
MeasurementModel::MeasurementVector Vector
virtual ~MeasurementUpdate()
MeasurementModel::NoiseVariance Variance
UpdateInspector(const typename Update< ConcreteModel >::type &update)
const Update< ConcreteModel >::type & update_
virtual Vector & operator=(double y)
const Update< ConcreteModel >::type & update_
virtual Vector const & getVector() const
UpdateInspector(const typename Update< ConcreteModel >::type &update)
virtual bool hasVariance() const
ConcreteModel::NoiseVariance const & getVariance(const State &)
ConcreteModel::MeasurementVector const & getVector(const State &)
Update_< ConcreteModel > type