Skip to content
Snippets Groups Projects
Commit 9b7b5adb authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

remove all commented code related to state_block_vec_

parent 370f9060
No related branches found
No related tags found
1 merge request!323Resolve "New data structure for storing stateblocks"
Pipeline #4352 failed
This commit is part of merge request !323. Comments created here will be created in the context of that merge request.
...@@ -40,7 +40,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha ...@@ -40,7 +40,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
CaptureBasePtrList capture_list_; CaptureBasePtrList capture_list_;
FactorBasePtrList constrained_by_list_; FactorBasePtrList constrained_by_list_;
std::string structure_; std::string structure_;
// std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order: Position, Orientation, Velocity.
static unsigned int frame_id_count_; static unsigned int frame_id_count_;
...@@ -97,32 +96,14 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha ...@@ -97,32 +96,14 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
void getTimeStamp(TimeStamp& _ts) const; void getTimeStamp(TimeStamp& _ts) const;
// // State blocks // // State blocks
// public: 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; StateBlockPtr getV() const;
// void setP(const StateBlockPtr _p_ptr);
// void setO(const StateBlockPtr _o_ptr);
void setV(const StateBlockPtr _v_ptr); void setV(const StateBlockPtr _v_ptr);
protected: protected:
void registerNewStateBlocks(); void registerNewStateBlocks();
void removeStateBlocks(); void removeStateBlocks();
virtual void setProblem(ProblemPtr _problem) final; virtual void setProblem(ProblemPtr _problem) final;
// Fixed / Estimated
// public:
// void fix();
// void unfix();
// bool isFixed() const;
// States // States
public: public:
void setState(const Eigen::VectorXs& _state); void setState(const Eigen::VectorXs& _state);
...@@ -214,34 +195,6 @@ inline TimeStamp FrameBase::getTimeStamp() const ...@@ -214,34 +195,6 @@ inline TimeStamp FrameBase::getTimeStamp() const
return time_stamp_; 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 StateBlockPtr FrameBase::getV() const inline StateBlockPtr FrameBase::getV() const
{ {
return getStateBlock("V"); return getStateBlock("V");
...@@ -252,18 +205,6 @@ inline void FrameBase::setV(const StateBlockPtr _v_ptr) ...@@ -252,18 +205,6 @@ inline void FrameBase::setV(const StateBlockPtr _v_ptr)
setStateBlock("V", _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 TrajectoryBasePtr FrameBase::getTrajectory() const inline TrajectoryBasePtr FrameBase::getTrajectory() const
{ {
return trajectory_ptr_.lock(); return trajectory_ptr_.lock();
...@@ -274,12 +215,6 @@ inline const CaptureBasePtrList& FrameBase::getCaptureList() const ...@@ -274,12 +215,6 @@ inline const CaptureBasePtrList& FrameBase::getCaptureList() const
return capture_list_; return capture_list_;
} }
//inline void FrameBase::resizeStateBlockVec(unsigned int _size)
//{
// if (_size > state_block_vec_.size())
// state_block_vec_.resize(_size);
//}
inline unsigned int FrameBase::getHits() const inline unsigned int FrameBase::getHits() const
{ {
return constrained_by_list_.size(); return constrained_by_list_.size();
......
...@@ -15,7 +15,6 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _ ...@@ -15,7 +15,6 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
NodeBase("FRAME", "Base"), NodeBase("FRAME", "Base"),
trajectory_ptr_(), trajectory_ptr_(),
structure_(""), structure_(""),
// state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
frame_id_(++frame_id_count_), frame_id_(++frame_id_count_),
type_(NON_ESTIMATED), type_(NON_ESTIMATED),
time_stamp_(_ts) time_stamp_(_ts)
...@@ -35,16 +34,12 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _ ...@@ -35,16 +34,12 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
setStateBlock("V", _v_ptr); setStateBlock("V", _v_ptr);
structure_ += "V"; structure_ += "V";
} }
// 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) : FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
NodeBase("FRAME", "Base"), NodeBase("FRAME", "Base"),
trajectory_ptr_(), trajectory_ptr_(),
structure_(""), structure_(""),
// state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
frame_id_(++frame_id_count_), frame_id_(++frame_id_count_),
type_(_tp), type_(_tp),
time_stamp_(_ts) time_stamp_(_ts)
...@@ -64,41 +59,29 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr ...@@ -64,41 +59,29 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
setStateBlock("V", _v_ptr); setStateBlock("V", _v_ptr);
structure_ += "V"; structure_ += "V";
} }
// 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) : FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) :
NodeBase("FRAME", "Base"), NodeBase("FRAME", "Base"),
trajectory_ptr_(), trajectory_ptr_(),
structure_(_frame_structure), structure_(_frame_structure),
// state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
frame_id_(++frame_id_count_), frame_id_(++frame_id_count_),
type_(_tp), type_(_tp),
time_stamp_(_ts) time_stamp_(_ts)
{ {
if(_frame_structure.compare("PO") == 0 and _dim == 2){ if(_frame_structure.compare("PO") == 0 and _dim == 2){
// auto _x = Eigen::VectorXs::Zero(3);
assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2D!"); assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2D!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) ); StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((Scalar) _x(2) ) ); StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((Scalar) _x(2) ) );
setStateBlock("P", p_ptr); setStateBlock("P", p_ptr);
setStateBlock("O", o_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"); this->setType("PO 2D");
}else if(_frame_structure.compare("PO") == 0 and _dim == 3){ }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!"); assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for 3D!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) ); StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) ); StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) );
setStateBlock("P", p_ptr); setStateBlock("P", p_ptr);
setStateBlock("O", o_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"); this->setType("PO 3D");
}else if(_frame_structure.compare("POV") == 0 and _dim == 3){ }else if(_frame_structure.compare("POV") == 0 and _dim == 3){
// auto _x = Eigen::VectorXs::Zero(10); // auto _x = Eigen::VectorXs::Zero(10);
...@@ -109,9 +92,6 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c ...@@ -109,9 +92,6 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
setStateBlock("P", p_ptr); setStateBlock("P", p_ptr);
setStateBlock("O", o_ptr); setStateBlock("O", o_ptr);
setStateBlock("V", v_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"); this->setType("POV 3D");
}else{ }else{
std::cout << _frame_structure << " ^^ " << _dim << std::endl; std::cout << _frame_structure << " ^^ " << _dim << std::endl;
...@@ -231,37 +211,10 @@ void FrameBase::setAux() ...@@ -231,37 +211,10 @@ 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::setState(const Eigen::VectorXs& _state) void FrameBase::setState(const Eigen::VectorXs& _state)
{ {
int size = getSize(); int size = getSize();
//State Vector size must be lower or equal to frame state size :
// example : POV -> if we initialize only position and orientation due to use of processorOdom3D
assert(_state.size() == size && "In FrameBase::setState wrong state size"); assert(_state.size() == size && "In FrameBase::setState wrong state size");
unsigned int index = 0; unsigned int index = 0;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment