diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index f128ae15169450a1091b2b065d97f27053b71d97..e5fd2a71de378e2722ffdd656dee9bfe05c636f3 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -40,7 +40,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
CaptureBasePtrList capture_list_;
FactorBasePtrList constrained_by_list_;
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_;
@@ -97,32 +96,14 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
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;
+ public:
StateBlockPtr getV() const;
-// void setP(const StateBlockPtr _p_ptr);
-// void setO(const StateBlockPtr _o_ptr);
void setV(const StateBlockPtr _v_ptr);
protected:
void registerNewStateBlocks();
void removeStateBlocks();
virtual void setProblem(ProblemPtr _problem) final;
- // Fixed / Estimated
-// public:
-// void fix();
-// void unfix();
-// bool isFixed() const;
-
// States
public:
void setState(const Eigen::VectorXs& _state);
@@ -214,34 +195,6 @@ 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 StateBlockPtr FrameBase::getV() const
{
return getStateBlock("V");
@@ -252,18 +205,6 @@ inline void FrameBase::setV(const StateBlockPtr _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
{
return trajectory_ptr_.lock();
@@ -274,12 +215,6 @@ 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 unsigned int FrameBase::getHits() const
{
return constrained_by_list_.size();
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 446ced4af7c50af5d13e3ab68053c47f9a556f82..b7d8b539afa88ad546dcb06ce96d9668f3f3c1d8 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -15,7 +15,6 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
NodeBase("FRAME", "Base"),
trajectory_ptr_(),
structure_(""),
-// 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)
@@ -35,16 +34,12 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
setStateBlock("V", _v_ptr);
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) :
NodeBase("FRAME", "Base"),
trajectory_ptr_(),
structure_(""),
-// 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)
@@ -64,41 +59,29 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
setStateBlock("V", _v_ptr);
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) :
NodeBase("FRAME", "Base"),
trajectory_ptr_(),
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_),
type_(_tp),
time_stamp_(_ts)
{
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!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((Scalar) _x(2) ) );
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> ( ) ) );
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);
@@ -109,9 +92,6 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
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;
@@ -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)
{
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");
unsigned int index = 0;