29 #ifndef HECTOR_POSE_ESTIMATION_SUBSTATE_H 30 #define HECTOR_POSE_ESTIMATION_SUBSTATE_H 53 template <
int _VectorDimension,
int _CovarianceDimension>
class initializer;
58 enum { VectorDimension = _VectorDimension };
59 enum { CovarianceDimension = _CovarianceDimension };
60 initializer(
State& state) : index_(state.getVector().rows()), covariance_index_(state.getCovariance().rows()) {
61 assert(index_ + VectorDimension <= MaxVectorSize);
62 state.
x().conservativeResize(index_ + VectorDimension);
63 assert(covariance_index_ + CovarianceDimension <= MaxMatrixRowsCols);
64 state.
P().conservativeResize(covariance_index_ + CovarianceDimension, covariance_index_ + CovarianceDimension);
73 enum { VectorDimension = Dynamic };
74 enum { CovarianceDimension = Dynamic };
83 template <
int _VectorDimension,
int _CovarianceDimension = _VectorDimension>
92 typedef Block<State::Covariance,CovarianceDimension,CovarianceDimension>
CovarianceBlock;
118 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()); }
125 template <
typename VectorType> VectorBlock<VectorType,VectorDimension>
segment(VectorType &vector) {
return VectorBlock<VectorType,VectorDimension>(vector, index_,
getVectorDimension()); }
126 template <
typename MatrixType> Block<MatrixType,CovarianceDimension,CovarianceDimension>
block(MatrixType &matrix) {
return Block<MatrixType,CovarianceDimension,CovarianceDimension>(matrix, covariance_index_, covariance_index_,
getCovarianceDimension(),
getCovarianceDimension()); }
127 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()); }
128 template <
typename MatrixType> Block<MatrixType,CovarianceDimension,MatrixType::ColsAtCompileTime>
rows(MatrixType &matrix) {
return Block<MatrixType,CovarianceDimension,MatrixType::ColsAtCompileTime>(matrix, covariance_index_, 0,
getCovarianceDimension(), matrix.cols()); }
129 template <
typename MatrixType> Block<MatrixType,MatrixType::RowsAtCompileTime,CovarianceDimension>
cols(MatrixType &matrix) {
return Block<MatrixType,MatrixType::RowsAtCompileTime,CovarianceDimension>(matrix, 0, covariance_index_, matrix.rows(),
getCovarianceDimension()); }
138 , dimension_(vector_dimension)
139 , covariance_dimension_(covariance_dimension)
151 template <
int _VectorDimension,
int _CovarianceDimension>
161 template <
int _VectorDimension,
int _CovarianceDimension>
166 template <
int _VectorDimension,
int _CovarianceDimension>
168 return addSubState<_VectorDimension, _CovarianceDimension>(0, name);
171 template <
int _VectorDimension,
int _CovarianceDimension>
174 if (!name.empty()) substate = getSubState<_VectorDimension, _CovarianceDimension>(name);
176 if (substates_by_name_.count(name))
return substate;
178 substates_.push_back(boost::static_pointer_cast<SubState>(substate));
179 if (!name.empty()) substates_by_name_[name] =
SubStateWPtr(boost::static_pointer_cast<SubState>(substate));
182 substates_by_model_[model] =
SubStateWPtr(boost::static_pointer_cast<SubState>(substate));
189 #endif // HECTOR_POSE_ESTIMATION_SUBSTATE_H
boost::shared_ptr< SubState_< SubVectorDimension, SubCovarianceDimension > > getSubState(const Model *model) const
boost::shared_ptr< SubState_< SubVectorDimension, SubCovarianceDimension > > addSubState(const std::string &name=std::string())
int getVectorDimension() const
Block< const State::Covariance, CovarianceDimension, OtherSubState::CovarianceDimension > getCrossVariance(const OtherSubState &other) const
Block< MatrixType, CovarianceDimension, CovarianceDimension > block(MatrixType &matrix)
int getCovarianceDimension() const
initializer(State &state)
ConstVectorSegment getVector() const
int getVectorIndex() const
Block< MatrixType, MatrixType::RowsAtCompileTime, CovarianceDimension > cols(MatrixType &matrix)
int getVectorDimension() const
int getCovarianceDimension() const
int getCovarianceIndex() const
virtual const Covariance & getCovariance() const
Eigen::DenseIndex IndexType
ConstCovarianceBlock getCovariance() const
virtual int getCovarianceDimension() const =0
virtual int getVectorIndex() const =0
virtual IndexType getCovarianceDimension() const
virtual const Vector & getVector() const
Block< MatrixType, CovarianceDimension, OtherSubState::CovarianceDimension > block(MatrixType &matrix, const OtherSubState &other)
boost::shared_ptr< SubState_< _VectorDimension, _CovarianceDimension > > Ptr
const IndexType covariance_index_
VectorBlock< const Vector, Size > getSegment(IndexType start) const
virtual int getVectorDimension() const =0
Block< const State::Covariance, CovarianceDimension, CovarianceDimension > ConstCovarianceBlock
ColumnVector_< VectorDimension > Vector
Block< State::Covariance, CovarianceDimension, CovarianceDimension > CovarianceBlock
VectorBlock< State::Vector, VectorDimension > VectorSegment
Block< MatrixType, CovarianceDimension, MatrixType::ColsAtCompileTime > rows(MatrixType &matrix)
BaseState(State &state, int vector_dimension, int covariance_dimension)
boost::shared_ptr< SubState > SubStatePtr
const IndexType covariance_index_
const IndexType covariance_dimension_
Block< const State::Covariance, Dynamic, CovarianceDimension > ConstCrossVarianceBlock
const IndexType dimension_
VectorBlock< const State::Vector, VectorDimension > ConstVectorSegment
VectorBlock< VectorType, VectorDimension > segment(VectorType &vector)
virtual int getCovarianceIndex() const =0
initializer(State &state)
Block< State::Covariance, Dynamic, CovarianceDimension > CrossVarianceBlock
boost::weak_ptr< SubState > SubStateWPtr