diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp index e46557c44f1a0f6195eb28a531dd439a80951bbf..0e4695e5a8c82ebe74ae1c67c5fc37cfc396ecea 100644 --- a/hello_wolf/hello_wolf.cpp +++ b/hello_wolf/hello_wolf.cpp @@ -226,9 +226,9 @@ int main() sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! for (auto kf : problem->getTrajectory()->getFrameList()) if (kf->isKeyOrAux()) - for (auto sb : kf->getStateBlockVec()) - if (sb && !sb->isFixed()) - sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! + for (auto& pair_key_sb : kf->getStateBlockMap()) + if (!pair_key_sb.second->isFixed()) + pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT ! for (auto lmk : problem->getMap()->getLandmarkList()) for (auto sb : lmk->getStateBlockVec()) if (sb && !sb->isFixed()) diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/hello_wolf/hello_wolf_autoconf.cpp index bc53460f2410f7985fd02d73502c79bec9b2842d..5bc4c42b94f13f552f498a86f1ad1143d0782c48 100644 --- a/hello_wolf/hello_wolf_autoconf.cpp +++ b/hello_wolf/hello_wolf_autoconf.cpp @@ -219,9 +219,9 @@ int main() sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! for (auto kf : problem->getTrajectory()->getFrameList()) if (kf->isKeyOrAux()) - for (auto sb : kf->getStateBlockVec()) - if (sb && !sb->isFixed()) - sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! + for (auto& pair_key_sb : kf->getStateBlockMap()) + if (pair_key_sb.second && !pair_key_sb.second->isFixed()) + pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT ! for (auto lmk : problem->getMap()->getLandmarkList()) for (auto sb : lmk->getStateBlockVec()) if (sb && !sb->isFixed()) diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index aed6ff774ed52c5d0d4023188515823feca7ffab..a27970fa474f50bbb27de5d384457e09be24dce0 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -7,28 +7,30 @@ namespace wolf{ class TrajectoryBase; class CaptureBase; class StateBlock; + +/** \brief Enumeration of frame types + */ +typedef enum +{ + KEY = 2, ///< key frame. It plays at optimizations (estimated). + AUXILIARY = 1, ///< auxiliary frame. It plays at optimizations (estimated). + NON_ESTIMATED = 0 ///< regular frame. It does not play at optimization. +} FrameType; } //Wolf includes #include "core/common/wolf.h" #include "core/common/time_stamp.h" #include "core/common/node_base.h" +#include "core/state_block/has_state_blocks.h" //std includes namespace wolf { -/** \brief Enumeration of frame types - */ -typedef enum -{ - KEY = 2, ///< key frame. It plays at optimizations (estimated). - AUXILIARY = 1, ///< auxiliary frame. It plays at optimizations (estimated). - NON_ESTIMATED = 0 ///< regular frame. It does not play at optimization. -} FrameType; //class FrameBase -class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase> +class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<FrameBase> { friend CaptureBase; friend FactorBase; @@ -37,7 +39,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase TrajectoryBaseWPtr trajectory_ptr_; CaptureBasePtrList capture_list_; FactorBasePtrList constrained_by_list_; - std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order: Position, Orientation, Velocity. +// std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order: Position, Orientation, Velocity. static unsigned int frame_id_count_; @@ -93,21 +95,21 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase TimeStamp getTimeStamp() const; void getTimeStamp(TimeStamp& _ts) const; - // State blocks - public: - const std::vector<StateBlockPtr>& getStateBlockVec() const; - std::vector<StateBlockPtr>& getStateBlockVec(); - protected: - StateBlockPtr getStateBlock(unsigned int _i) const; - void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr); - void resizeStateBlockVec(unsigned int _size); - - public: - StateBlockPtr getP() const; - StateBlockPtr getO() const; +// // State blocks +// public: +// const std::vector<StateBlockPtr>& getStateBlockVec() const; +// std::vector<StateBlockPtr>& getStateBlockVec(); +// protected: +// StateBlockPtr getStateBlock(unsigned int _i) const; +// void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr); +// void resizeStateBlockVec(unsigned int _size); +// +// public: +// StateBlockPtr getP() const; +// StateBlockPtr getO() const; StateBlockPtr getV() const; - void setP(const StateBlockPtr _p_ptr); - void setO(const StateBlockPtr _o_ptr); +// void setP(const StateBlockPtr _p_ptr); +// void setO(const StateBlockPtr _o_ptr); void setV(const StateBlockPtr _v_ptr); protected: void registerNewStateBlocks(); @@ -115,10 +117,10 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase virtual void setProblem(ProblemPtr _problem) final; // Fixed / Estimated - public: - void fix(); - void unfix(); - bool isFixed() const; +// public: +// void fix(); +// void unfix(); +// bool isFixed() const; // States public: @@ -210,55 +212,55 @@ inline TimeStamp FrameBase::getTimeStamp() const return time_stamp_; } -inline const std::vector<StateBlockPtr>& FrameBase::getStateBlockVec() const -{ - return state_block_vec_; -} - -inline std::vector<StateBlockPtr>& FrameBase::getStateBlockVec() -{ - return state_block_vec_; -} - -inline StateBlockPtr FrameBase::getP() const -{ - return state_block_vec_[0]; -} -inline void FrameBase::setP(const StateBlockPtr _p_ptr) -{ - state_block_vec_[0] = _p_ptr; -} - -inline StateBlockPtr FrameBase::getO() const -{ - return state_block_vec_[1]; -} -inline void FrameBase::setO(const StateBlockPtr _o_ptr) -{ - state_block_vec_[1] = _o_ptr; -} +//inline const std::vector<StateBlockPtr>& FrameBase::getStateBlockVec() const +//{ +// return state_block_vec_; +//} + +//inline std::vector<StateBlockPtr>& FrameBase::getStateBlockVec() +//{ +// return state_block_vec_; +//} + +//inline StateBlockPtr FrameBase::getP() const +//{ +// return state_block_vec_[0]; +//} +//inline void FrameBase::setP(const StateBlockPtr _p_ptr) +//{ +// state_block_vec_[0] = _p_ptr; +//} + +//inline StateBlockPtr FrameBase::getO() const +//{ +// return state_block_vec_[1]; +//} +//inline void FrameBase::setO(const StateBlockPtr _o_ptr) +//{ +// state_block_vec_[1] = _o_ptr; +//} inline StateBlockPtr FrameBase::getV() const { - return state_block_vec_[2]; + return getStateBlock("V"); } inline void FrameBase::setV(const StateBlockPtr _v_ptr) { - state_block_vec_[2] = _v_ptr; + setStateBlock("V", _v_ptr); } -inline StateBlockPtr FrameBase::getStateBlock(unsigned int _i) const -{ - assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!"); - return state_block_vec_[_i]; -} - -inline void FrameBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr) -{ - assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!"); - state_block_vec_[_i] = _sb_ptr; -} +//inline StateBlockPtr FrameBase::getStateBlock(unsigned int _i) const +//{ +// assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!"); +// return state_block_vec_[_i]; +//} +// +//inline void FrameBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr) +//{ +// assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!"); +// state_block_vec_[_i] = _sb_ptr; +//} inline TrajectoryBasePtr FrameBase::getTrajectory() const { @@ -270,11 +272,11 @@ inline const CaptureBasePtrList& FrameBase::getCaptureList() const return capture_list_; } -inline void FrameBase::resizeStateBlockVec(unsigned int _size) -{ - if (_size > state_block_vec_.size()) - state_block_vec_.resize(_size); -} +//inline void FrameBase::resizeStateBlockVec(unsigned int _size) +//{ +// if (_size > state_block_vec_.size()) +// state_block_vec_.resize(_size); +//} inline unsigned int FrameBase::getHits() const { diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h index 19fa2e37da4f9897e1310efa5923b99e28cc8919..c84d345a96d6856e3911f145209c0db0bd8a33eb 100644 --- a/include/core/state_block/has_state_blocks.h +++ b/include/core/state_block/has_state_blocks.h @@ -29,14 +29,14 @@ class HasStateBlocks // Some typical shortcuts -- not all should be coded here, see notes below. StateBlockPtr getP() const; StateBlockPtr getO() const; - StateBlockPtr getV() const; // This probably not here but in derived classes only - StateBlockPtr getW() const; // This probably not here but in derived classes only - StateBlockPtr getIntrinsics() const; // This probably not here but in derived classes only +// StateBlockPtr getV() const; // This probably not here but in derived classes only +// StateBlockPtr getW() const; // This probably not here but in derived classes only +// StateBlockPtr getIntrinsics() const; // This probably not here but in derived classes only void setP(const StateBlockPtr _p_ptr); void setO(const StateBlockPtr _o_ptr); - void setV(const StateBlockPtr _v_ptr); // This probably not here but in derived classes only - void setW(const StateBlockPtr _w_ptr); // This probably not here but in derived classes only - void setIntrinsics(const StateBlockPtr _i_ptr); // This probably not here but in derived classes only +// void setV(const StateBlockPtr _v_ptr); // This probably not here but in derived classes only +// void setW(const StateBlockPtr _w_ptr); // This probably not here but in derived classes only +// void setIntrinsics(const StateBlockPtr _i_ptr); // This probably not here but in derived classes only public: // These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this. @@ -47,6 +47,10 @@ class HasStateBlocks protected: StateBlockPtr setStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb); StateBlockPtr getStateBlock(const std::string& _sb_type) const; + unsigned int removeStateBlock(const std::string& _sb_type) + { + return state_block_map_.erase(_sb_type); + } // Emplace derived state blocks (angle, quaternion, etc). template<typename SB, typename ... Args> @@ -100,7 +104,7 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string& } template<typename ... Args> -inline StateBlockPtr HasStateBlocks::emplaceStateBlock<StateBlock>(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor) +inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor) { assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type!"); std::shared_ptr<StateBlock> sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...); @@ -110,8 +114,9 @@ inline StateBlockPtr HasStateBlocks::emplaceStateBlock<StateBlock>(const std::st inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const std::string& _sb_type) const { - if (state_block_map_.count(_sb_type)) - return state_block_map_.at(_sb_type); + auto it = state_block_map_.find(_sb_type); + if (it != state_block_map_.end()) + return it->second; else return nullptr; } @@ -126,20 +131,20 @@ inline wolf::StateBlockPtr HasStateBlocks::getO() const return getStateBlock("O"); } -inline wolf::StateBlockPtr HasStateBlocks::getV() const -{ - return getStateBlock("V"); -} - -inline wolf::StateBlockPtr HasStateBlocks::getW() const -{ - return getStateBlock("W"); -} - -inline wolf::StateBlockPtr HasStateBlocks::getIntrinsics() const -{ - return getStateBlock("I"); -} +//inline wolf::StateBlockPtr HasStateBlocks::getV() const +//{ +// return getStateBlock("V"); +//} +// +//inline wolf::StateBlockPtr HasStateBlocks::getW() const +//{ +// return getStateBlock("W"); +//} +// +//inline wolf::StateBlockPtr HasStateBlocks::getIntrinsics() const +//{ +// return getStateBlock("I"); +//} inline void HasStateBlocks::setP(const StateBlockPtr _p_ptr) { @@ -151,42 +156,42 @@ inline void HasStateBlocks::setO(const StateBlockPtr _o_ptr) setStateBlock("O", _o_ptr); } -inline void HasStateBlocks::setV(const StateBlockPtr _v_ptr) -{ - setStateBlock("V", _v_ptr); -} - -inline void HasStateBlocks::setW(const StateBlockPtr _w_ptr) -{ - setStateBlock("W", _w_ptr); -} - -inline void HasStateBlocks::setIntrinsics(const StateBlockPtr _i_ptr) -{ - setStateBlock("I", _i_ptr); -} +//inline void HasStateBlocks::setV(const StateBlockPtr _v_ptr) +//{ +// setStateBlock("V", _v_ptr); +//} +// +//inline void HasStateBlocks::setW(const StateBlockPtr _w_ptr) +//{ +// setStateBlock("W", _w_ptr); +//} +// +//inline void HasStateBlocks::setIntrinsics(const StateBlockPtr _i_ptr) +//{ +// setStateBlock("I", _i_ptr); +//} inline void HasStateBlocks::fix() { - for (auto sbp : state_block_map_) - if (sbp.second != nullptr) - sbp.second->fix(); + for (auto pair_key_sbp : state_block_map_) + if (pair_key_sbp.second != nullptr) + pair_key_sbp.second->fix(); } inline void HasStateBlocks::unfix() { - for (auto sbp : state_block_map_) - if (sbp != nullptr) - sbp->unfix(); + for (auto pair_key_sbp : state_block_map_) + if (pair_key_sbp.second != nullptr) + pair_key_sbp.second->unfix(); } inline bool HasStateBlocks::isFixed() const { bool fixed = true; - for (auto sb : state_block_map_) + for (auto pair_key_sbp : state_block_map_) { - if (sb.second) - fixed &= sb.second->isFixed(); + if (pair_key_sbp.second) + fixed &= pair_key_sbp.second->isFixed(); } return fixed; } diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index 7d56eefa61707d3aa78d251c893bf37fef689c46..5e11a2d4f5bda019bd7cf96698371d5812d61f00 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -90,9 +90,11 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks //frame state blocks for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList()) if (fr_ptr->isKeyOrAux()) - for (auto sb : fr_ptr->getStateBlockVec()) - if (sb) - all_state_blocks.push_back(sb); + for (const auto& pair_key_sb : fr_ptr->getStateBlockMap()) + { + const auto& sb = pair_key_sb.second; + all_state_blocks.push_back(sb); + } // landmark state blocks for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList()) @@ -115,16 +117,16 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks // first create a vector containing all state blocks for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList()) if (fr_ptr->isKeyOrAux()) - for (auto sb : fr_ptr->getStateBlockVec()) - if (sb) - for(auto sb2 : fr_ptr->getStateBlockVec()) - if (sb2) - { - state_block_pairs.emplace_back(sb, sb2); - double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2)); - if (sb == sb2) - break; - } + for (auto pair_key_sb : fr_ptr->getStateBlockMap()) + for(auto pair_key_sb2 : fr_ptr->getStateBlockMap()) + { + const auto& sb = pair_key_sb.second; + const auto& sb2 = pair_key_sb2.second; + state_block_pairs.emplace_back(sb, sb2); + double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2)); + if (sb == sb2) + break; + } // landmark state blocks for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList()) diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index d4202bc490276d0bc8fc858ca6ec54645948619b..3269ff36bba7009ab85a954a4e78f7e92f17764b 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -14,33 +14,45 @@ unsigned int FrameBase::frame_id_count_ = 0; FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) : NodeBase("FRAME", "Base"), trajectory_ptr_(), - state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed. +// state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed. frame_id_(++frame_id_count_), type_(NON_ESTIMATED), time_stamp_(_ts) { - state_block_vec_[0] = _p_ptr; - state_block_vec_[1] = _o_ptr; - state_block_vec_[2] = _v_ptr; + if (_p_ptr) + setStateBlock("P", _p_ptr); + if (_o_ptr) + setStateBlock("O", _o_ptr); + if (_v_ptr) + setStateBlock("V", _v_ptr); +// state_block_vec_[0] = _p_ptr; +// state_block_vec_[1] = _o_ptr; +// state_block_vec_[2] = _v_ptr; } FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) : NodeBase("FRAME", "Base"), trajectory_ptr_(), - state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed. +// state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed. frame_id_(++frame_id_count_), type_(_tp), time_stamp_(_ts) { - state_block_vec_[0] = _p_ptr; - state_block_vec_[1] = _o_ptr; - state_block_vec_[2] = _v_ptr; + if (_p_ptr) + setStateBlock("P", _p_ptr); + if (_o_ptr) + setStateBlock("O", _o_ptr); + if (_v_ptr) + setStateBlock("V", _v_ptr); +// state_block_vec_[0] = _p_ptr; +// state_block_vec_[1] = _o_ptr; +// state_block_vec_[2] = _v_ptr; } FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) : NodeBase("FRAME", "Base"), trajectory_ptr_(), - state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed. +// state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed. frame_id_(++frame_id_count_), type_(_tp), time_stamp_(_ts) @@ -50,20 +62,22 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2D!"); StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) ); StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((Scalar) _x(2) ) ); - StateBlockPtr v_ptr ( nullptr ); - state_block_vec_[0] = p_ptr; - state_block_vec_[1] = o_ptr; - state_block_vec_[2] = v_ptr; + setStateBlock("P", p_ptr); + setStateBlock("O", o_ptr); +// state_block_vec_[0] = p_ptr; +// state_block_vec_[1] = o_ptr; +// state_block_vec_[2] = v_ptr; this->setType("PO 2D"); }else if(_frame_structure.compare("PO") == 0 and _dim == 3){ // auto _x = Eigen::VectorXs::Zero(7); assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for 3D!"); StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) ); StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) ); - StateBlockPtr v_ptr ( nullptr ); - state_block_vec_[0] = p_ptr; - state_block_vec_[1] = o_ptr; - state_block_vec_[2] = v_ptr; + setStateBlock("P", p_ptr); + setStateBlock("O", o_ptr); +// state_block_vec_[0] = p_ptr; +// state_block_vec_[1] = o_ptr; +// state_block_vec_[2] = v_ptr; this->setType("PO 3D"); }else if(_frame_structure.compare("POV") == 0 and _dim == 3){ // auto _x = Eigen::VectorXs::Zero(10); @@ -71,15 +85,19 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) ); StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3) ) ); StateBlockPtr v_ptr ( std::make_shared<StateBlock> (_x.tail <3> ( ) ) ); - state_block_vec_[0] = p_ptr; - state_block_vec_[1] = o_ptr; - state_block_vec_[2] = v_ptr; + setStateBlock("P", p_ptr); + setStateBlock("O", o_ptr); + setStateBlock("V", v_ptr); +// state_block_vec_[0] = p_ptr; +// state_block_vec_[1] = o_ptr; +// state_block_vec_[2] = v_ptr; this->setType("POV 3D"); }else{ std::cout << _frame_structure << " ^^ " << _dim << std::endl; throw std::runtime_error("Unknown frame structure"); } + } FrameBase::~FrameBase() @@ -129,26 +147,41 @@ void FrameBase::registerNewStateBlocks() { if (getProblem() != nullptr) { - for (StateBlockPtr sbp : getStateBlockVec()) - if (sbp != nullptr) - getProblem()->notifyStateBlock(sbp,ADD); + for (auto pair_key_sbp : getStateBlockMap()) + if (pair_key_sbp.second != nullptr) + getProblem()->notifyStateBlock(pair_key_sbp.second,ADD); } } void FrameBase::removeStateBlocks() { - for (unsigned int i = 0; i < state_block_vec_.size(); i++) + for (auto pair_key_sb : getStateBlockMap()) { - StateBlockPtr sbp = getStateBlock(i); + auto& key = pair_key_sb.first; + auto& sbp = pair_key_sb.second; if (sbp != nullptr) { if (getProblem() != nullptr) { getProblem()->notifyStateBlock(sbp,REMOVE); } - setStateBlock(i, nullptr); + removeStateBlock(key); } } + + +// for (unsigned int i = 0; i < state_block_map_.size(); i++) +// { +// StateBlockPtr sbp = getStateBlock(i); +// if (sbp != nullptr) +// { +// if (getProblem() != nullptr) +// { +// getProblem()->notifyStateBlock(sbp,REMOVE); +// } +// setStateBlock(i, nullptr); +// } +// } } void FrameBase::setNonEstimated() @@ -194,37 +227,39 @@ void FrameBase::setAux() } } -void FrameBase::fix() -{ - for (auto sbp : state_block_vec_) - if (sbp != nullptr) - sbp->fix(); -} - -void FrameBase::unfix() -{ - for (auto sbp : state_block_vec_) - if (sbp != nullptr) - sbp->unfix(); -} - -bool FrameBase::isFixed() const -{ - bool fixed = true; - for (auto sb : getStateBlockVec()) - { - if (sb) - fixed &= sb->isFixed(); - } - return fixed; -} +//void FrameBase::fix() +//{ +// for (auto sbp : state_block_vec_) +// if (sbp != nullptr) +// sbp->fix(); +//} +// +//void FrameBase::unfix() +//{ +// for (auto sbp : state_block_vec_) +// if (sbp != nullptr) +// sbp->unfix(); +//} +// +//bool FrameBase::isFixed() const +//{ +// bool fixed = true; +// for (auto sb : getStateBlockVec()) +// { +// if (sb) +// fixed &= sb->isFixed(); +// } +// return fixed; +//} void FrameBase::setState(const Eigen::VectorXs& _state) { - int size = 0; - for(unsigned int i = 0; i<state_block_vec_.size(); i++){ - size += (state_block_vec_[i]==nullptr ? 0 : state_block_vec_[i]->getSize()); - } + int size = getSize(); +// for (const auto & pair_key_sb : getStateBlockMap()) +// size += pair_key_sb.second->getSize(); +// for(unsigned int i = 0; i<state_block_vec_.size(); i++){ +// size += (state_block_vec_[i]==nullptr ? 0 : state_block_vec_[i]->getSize()); +// } //State Vector size must be lower or equal to frame state size : // example : PQVBB -> if we initialize only position and orientation due to use of processorOdom3D @@ -234,11 +269,11 @@ void FrameBase::setState(const Eigen::VectorXs& _state) const unsigned int _st_size = _state.size(); //initialize the FrameBase StateBlocks while StateBlocks list's end not reached and input state_size can go further - for (StateBlockPtr sb : state_block_vec_) - if (sb && (index < _st_size)) + for (auto& pair_key_sb : getStateBlockMap()) + if (index < _st_size) { - sb->setState(_state.segment(index, sb->getSize()), isKeyOrAux()); // do not notify if state block is not estimated by the solver - index += sb->getSize(); + pair_key_sb.second->setState(_state.segment(index, pair_key_sb.second->getSize()), isKeyOrAux()); // do not notify if state block is not estimated by the solver + index += pair_key_sb.second->getSize(); } } @@ -253,28 +288,21 @@ Eigen::VectorXs FrameBase::getState() const void FrameBase::getState(Eigen::VectorXs& _state) const { - SizeEigen size = 0; - for (StateBlockPtr sb : state_block_vec_) - if (sb) - size += sb->getSize(); - - _state = Eigen::VectorXs(size); + _state.resize(getSize()); SizeEigen index = 0; - for (StateBlockPtr sb : state_block_vec_) - if (sb) - { - _state.segment(index,sb->getSize()) = sb->getState(); - index += sb->getSize(); - } + for (const auto& pair_key_sb : getStateBlockMap()) + { + _state.segment(index,pair_key_sb.second->getSize()) = pair_key_sb.second->getState(); + index += pair_key_sb.second->getSize(); + } } unsigned int FrameBase::getSize() const { unsigned int size = 0; - for (auto st : state_block_vec_) - if (st) - size += st->getSize(); + for (const auto& pair_key_sb : getStateBlockMap()) + size += pair_key_sb.second->getSize(); return size; } diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index c4b9677728e442846d422071719e4ec22b3c37d7..364808ae965963ca2e9d547f55bbab8113dda235 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -630,33 +630,29 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& bool success(true); int i = 0, j = 0; - const auto& state_bloc_vec = _frame_ptr->getStateBlockVec(); + const auto& state_bloc_map = _frame_ptr->getStateBlockMap(); // computing size SizeEigen sz = 0; - for (const auto& sb : state_bloc_vec) - if (sb) - sz += sb->getLocalSize(); + for (const auto& pair_key_sb : state_bloc_map) + sz += pair_key_sb.second->getLocalSize(); // resizing _covariance = Eigen::MatrixXs(sz, sz); // filling covariance - for (const auto& sb_i : state_bloc_vec) + for (const auto& pair_key_sb_i : state_bloc_map) { - if (sb_i) + const auto& sb_i = pair_key_sb_i.second; + j = 0; + for (const auto& pair_key_sb_j : state_bloc_map) { - j = 0; - for (const auto& sb_j : state_bloc_vec) - { - if (sb_j) - { - success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j); - j += sb_j->getLocalSize(); - } - } - i += sb_i->getLocalSize(); + const auto& sb_j = pair_key_sb_j.second; + success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j); + j += sb_j->getLocalSize(); } + i += sb_i->getLocalSize(); + } return success; } @@ -916,9 +912,11 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c if (state_blocks) { cout << " sb:"; - for (auto sb : F->getStateBlockVec()) - if (sb != nullptr) - cout << " " << (sb->isFixed() ? "Fix" : "Est"); + for (const auto& pair_key_sb : F->getStateBlockMap()) + { + const auto& sb = pair_key_sb.second; + cout << " " << (sb->isFixed() ? "Fix" : "Est"); + } cout << endl; } if (depth >= 2) @@ -1141,8 +1139,9 @@ bool Problem::check(int verbose_level) const cout << (F->isKeyOrAux() ? (F->isKey() ? " KF" : " EF") : " F") << F->id() << " @ " << F.get() << endl; cout << " -> P @ " << F->getProblem().get() << endl; cout << " -> T @ " << F->getTrajectory().get() << endl; - for (auto sb : F->getStateBlockVec()) + for (const auto& pair_key_sb : F->getStateBlockMap()) { + const auto& sb = pair_key_sb.second; cout << " sb @ " << sb.get(); if (sb) { @@ -1344,7 +1343,7 @@ bool Problem::check(int verbose_level) const } } // find in own Frame - found = found || (std::find(F->getStateBlockVec().begin(), F->getStateBlockVec().end(), sb) != F->getStateBlockVec().end()); + found = found || (std::find_if(F->getStateBlockMap().begin(), F->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != F->getStateBlockMap().end()); // find in own Capture found = found || (std::find(C->getStateBlockVec().begin(), C->getStateBlockVec().end(), sb) != C->getStateBlockVec().end()); // find in own Sensor @@ -1352,7 +1351,7 @@ bool Problem::check(int verbose_level) const found = found || (std::find(S->getStateBlockVec().begin(), S->getStateBlockVec().end(), sb) != S->getStateBlockVec().end()); // find in constrained Frame if (Fo) - found = found || (std::find(Fo->getStateBlockVec().begin(), Fo->getStateBlockVec().end(), sb) != Fo->getStateBlockVec().end()); + found = found || (std::find_if(Fo->getStateBlockMap().begin(), Fo->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != Fo->getStateBlockMap().end()); // find in constrained Capture if (Co) found = found || (std::find(Co->getStateBlockVec().begin(), Co->getStateBlockVec().end(), sb) != Co->getStateBlockVec().end()); @@ -1361,7 +1360,7 @@ bool Problem::check(int verbose_level) const { // find in constrained feature's Frame FrameBasePtr foF = fo->getFrame(); - found = found || (std::find(foF->getStateBlockVec().begin(), foF->getStateBlockVec().end(), sb) != foF->getStateBlockVec().end()); + found = found || (std::find_if(foF->getStateBlockMap().begin(), foF->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != foF->getStateBlockMap().end()); // find in constrained feature's Capture CaptureBasePtr foC = fo->getCapture(); found = found || (std::find(foC->getStateBlockVec().begin(), foC->getStateBlockVec().end(), sb) != foC->getStateBlockVec().end()); diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index 03f991c07c4a144f560a3759c4c081cdd2410ed1..639715e0754a5bc7d9167b2619a85cc1ea43d281 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -39,7 +39,7 @@ TEST(FrameBase, StateBlocks) { FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); - ASSERT_EQ(F->getStateBlockVec().size(), (unsigned int) 3); + ASSERT_EQ(F->getStateBlockMap().size(),(unsigned int) 2); ASSERT_EQ(F->getP()->getState().size(),(unsigned int) 2); ASSERT_EQ(F->getO()->getState().size(),(unsigned int) 1); ASSERT_EQ(F->getV(), nullptr); @@ -142,7 +142,7 @@ TEST(FrameBase, GetSetState) // Set the state, check that state blocks hold the current states F.setState(x); - ASSERT_TRUE((p - F.getP()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL)); + ASSERT_MATRIX_APPROX(p, F.getP()->getState(), Constants::EPS_SMALL); ASSERT_TRUE((q - F.getO()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL)); ASSERT_TRUE((v - F.getV()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));