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_SUBSTATE_H
00030 #define HECTOR_POSE_ESTIMATION_SUBSTATE_H
00031
00032 #include <hector_pose_estimation/state.h>
00033
00034 namespace hector_pose_estimation {
00035
00036 class SubState
00037 {
00038 public:
00039 SubState(State& state) : state_(state) {}
00040 virtual ~SubState() {}
00041
00042 virtual int getVectorDimension() const = 0;
00043 virtual int getCovarianceDimension() const = 0;
00044 virtual int getVectorIndex() const = 0;
00045 virtual int getCovarianceIndex() const = 0;
00046
00047 virtual void reset() {}
00048 virtual void updated() {}
00049 virtual void normalize() {}
00050
00051 protected:
00052 State& state_;
00053 template <int _VectorDimension, int _CovarianceDimension> class initializer;
00054 };
00055
00056 template <int _VectorDimension, int _CovarianceDimension> class SubState::initializer {
00057 public:
00058 enum { VectorDimension = _VectorDimension };
00059 enum { CovarianceDimension = _CovarianceDimension };
00060 initializer(State& state) : index_(state.getVector().rows()), covariance_index_(state.getCovariance().rows()) {
00061 assert(index_ + VectorDimension <= MaxVectorSize);
00062 state.x().conservativeResize(index_ + VectorDimension);
00063 assert(covariance_index_ + CovarianceDimension <= MaxMatrixRowsCols);
00064 state.P().conservativeResize(covariance_index_ + CovarianceDimension);
00065 }
00066 protected:
00067 const IndexType index_;
00068 const IndexType covariance_index_;
00069 };
00070
00071 template <> class SubState::initializer<Dynamic,Dynamic> {
00072 public:
00073 enum { VectorDimension = Dynamic };
00074 enum { CovarianceDimension = Dynamic };
00075 initializer(State& state) : index_(0), covariance_index_(0)
00076 {}
00077 protected:
00078 const IndexType index_;
00079 const IndexType covariance_index_;
00080 };
00081 extern template class SubState::initializer<Dynamic,Dynamic>;
00082
00083 template <int _VectorDimension, int _CovarianceDimension = _VectorDimension>
00084 class SubState_ : public SubState, public SubState::initializer<_VectorDimension, _CovarianceDimension>
00085 {
00086 public:
00087 enum { VectorDimension = SubState::initializer<_VectorDimension, _CovarianceDimension>::VectorDimension };
00088 enum { CovarianceDimension = SubState::initializer<_VectorDimension, _CovarianceDimension>::CovarianceDimension };
00089 typedef ColumnVector_<VectorDimension> Vector;
00090
00091 typedef VectorBlock<State::Vector,VectorDimension> VectorSegment;
00092 typedef Block<State::Covariance,CovarianceDimension,CovarianceDimension> CovarianceBlock;
00093 typedef Block<State::Covariance,Dynamic,CovarianceDimension> CrossVarianceBlock;
00094
00095 typedef VectorBlock<const State::Vector,VectorDimension> ConstVectorSegment;
00096 typedef Block<const State::Covariance,CovarianceDimension,CovarianceDimension> ConstCovarianceBlock;
00097 typedef Block<const State::Covariance,Dynamic,CovarianceDimension> ConstCrossVarianceBlock;
00098
00099 typedef boost::shared_ptr<SubState_<_VectorDimension, _CovarianceDimension> > Ptr;
00100
00101 using SubState::initializer<_VectorDimension, _CovarianceDimension>::index_;
00102 using SubState::initializer<_VectorDimension, _CovarianceDimension>::covariance_index_;
00103
00104 public:
00105 SubState_(State& state)
00106 : SubState(state)
00107 , SubState::initializer<_VectorDimension, _CovarianceDimension>(state)
00108 {}
00109 virtual ~SubState_() {}
00110
00111 int getVectorDimension() const { return VectorDimension; }
00112 int getCovarianceDimension() const { return CovarianceDimension; }
00113 int getVectorIndex() const { return index_; }
00114 int getCovarianceIndex() const { return covariance_index_; }
00115
00116 ConstVectorSegment getVector() const { return ConstVectorSegment(state_.getVector(), index_, getVectorDimension()); }
00117 ConstCovarianceBlock getCovariance() const { return ConstCovarianceBlock(state_.getCovariance(), covariance_index_, covariance_index_, getCovarianceDimension(), getCovarianceDimension()); }
00118 template <typename OtherSubState> Block<const State::Covariance,CovarianceDimension,OtherSubState::CovarianceDimension> getCrossVariance(const OtherSubState &other) const { return Block<const State::Covariance,CovarianceDimension,OtherSubState::CovarianceDimension>(state_.getCovariance(), covariance_index_, covariance_index_, getCovarianceDimension(), other.getCovarianceDimension()); }
00119 template <int Size> VectorBlock<const Vector, Size> getSegment(IndexType start) const { return VectorBlock<const Vector, Size>(state_.getVector(), index_ + start); }
00120
00121 VectorSegment vector() { return VectorSegment(state_.x(), index_, getVectorDimension()); }
00122 CovarianceBlock P() { return CovarianceBlock(state_.P(), covariance_index_, covariance_index_, getCovarianceDimension(), getCovarianceDimension()); }
00123 CrossVarianceBlock P01() { return CrossVarianceBlock(state_.P(), 0, covariance_index_, state_.getCovarianceDimension(), getCovarianceDimension()); }
00124
00125 template <typename VectorType> VectorBlock<VectorType,VectorDimension> segment(VectorType &vector) { return VectorBlock<VectorType,VectorDimension>(vector, index_, getVectorDimension()); }
00126 template <typename MatrixType> Block<MatrixType,CovarianceDimension,CovarianceDimension> block(MatrixType &matrix) { return Block<MatrixType,CovarianceDimension,CovarianceDimension>(matrix, covariance_index_, covariance_index_, getCovarianceDimension(), getCovarianceDimension()); }
00127 template <typename MatrixType, typename OtherSubState> Block<MatrixType,CovarianceDimension,OtherSubState::CovarianceDimension> block(MatrixType &matrix, const OtherSubState &other) { return Block<MatrixType,CovarianceDimension,OtherSubState::CovarianceDimension>(matrix, covariance_index_, other.getCovarianceIndex(), getCovarianceDimension(), other.getCovarianceDimension()); }
00128 template <typename MatrixType> Block<MatrixType,CovarianceDimension,MatrixType::ColsAtCompileTime> rows(MatrixType &matrix) { return Block<MatrixType,CovarianceDimension,MatrixType::ColsAtCompileTime>(matrix, covariance_index_, 0, getCovarianceDimension(), matrix.cols()); }
00129 template <typename MatrixType> Block<MatrixType,MatrixType::RowsAtCompileTime,CovarianceDimension> cols(MatrixType &matrix) { return Block<MatrixType,MatrixType::RowsAtCompileTime,CovarianceDimension>(matrix, 0, covariance_index_, matrix.rows(), getCovarianceDimension()); }
00130 };
00131 extern template class SubState_<Dynamic,Dynamic>;
00132
00133 class BaseState : public SubState_<Dynamic,Dynamic>
00134 {
00135 public:
00136 BaseState(State& state, int vector_dimension, int covariance_dimension)
00137 : SubState_<Dynamic,Dynamic>(state)
00138 , dimension_(vector_dimension)
00139 , covariance_dimension_(covariance_dimension)
00140 {}
00141 virtual ~BaseState() {}
00142
00143 int getVectorDimension() const { return dimension_; }
00144 int getCovarianceDimension() const { return covariance_dimension_; }
00145
00146 private:
00147 const IndexType dimension_;
00148 const IndexType covariance_dimension_;
00149 };
00150
00151 template <int _VectorDimension, int _CovarianceDimension>
00152 boost::shared_ptr<SubState_<_VectorDimension, _CovarianceDimension> > State::getSubState(const Model *model) const {
00153 return boost::dynamic_pointer_cast<SubState_<_VectorDimension, _CovarianceDimension> >(substates_by_model_.count(model) ? substates_by_model_.at(model).lock() : SubStatePtr());
00154 }
00155
00156
00157
00158
00159
00160
00161 template <int _VectorDimension, int _CovarianceDimension>
00162 boost::shared_ptr<SubState_<_VectorDimension, _CovarianceDimension> > State::getSubState(const std::string& name) const {
00163 return boost::dynamic_pointer_cast<SubState_<_VectorDimension, _CovarianceDimension> >(substates_by_name_.count(name) ? substates_by_name_.at(name).lock() : SubStatePtr());
00164 }
00165
00166 template <int _VectorDimension, int _CovarianceDimension>
00167 boost::shared_ptr<SubState_<_VectorDimension, _CovarianceDimension> > State::addSubState(const std::string& name) {
00168 return addSubState<_VectorDimension, _CovarianceDimension>(0, name);
00169 }
00170
00171 template <int _VectorDimension, int _CovarianceDimension>
00172 boost::shared_ptr<SubState_<_VectorDimension, _CovarianceDimension> > State::addSubState(const Model *model, const std::string& name) {
00173 boost::shared_ptr<SubState_<_VectorDimension, _CovarianceDimension> > substate;
00174 if (!name.empty()) substate = getSubState<_VectorDimension, _CovarianceDimension>(name);
00175 if (!substate) {
00176 if (substates_by_name_.count(name)) return substate;
00177 substate.reset(new SubState_<_VectorDimension, _CovarianceDimension>(*this));
00178 substates_.push_back(boost::static_pointer_cast<SubState>(substate));
00179 if (!name.empty()) substates_by_name_[name] = SubStateWPtr(boost::static_pointer_cast<SubState>(substate));
00180 }
00181 if (model) {
00182 substates_by_model_[model] = SubStateWPtr(boost::static_pointer_cast<SubState>(substate));
00183 }
00184 return substate;
00185 }
00186
00187 }
00188
00189 #endif // HECTOR_POSE_ESTIMATION_SUBSTATE_H