Addressing issue #158

src/frame_base.cpp

@@ -177,11 +177,7 @@ void FrameBase::setState(const Eigen::VectorXs& _state)
 
 Eigen::VectorXs FrameBase::getState() const
 {
-    SizeEigen size = 0;
-    for (StateBlockPtr sb : state_block_vec_)
-        if (sb)
-            size += sb->getSize();
-    Eigen::VectorXs state(size);
+    Eigen::VectorXs state;
 
     getState(state);
 
@@ -195,10 +191,9 @@ void FrameBase::getState(Eigen::VectorXs& _state) const
         if (sb)
             size += sb->getSize();
 
-    assert(_state.size() == size && "Wrong state vector size");
+    _state = Eigen::VectorXs(size);
 
     SizeEigen index = 0;
-
     for (StateBlockPtr sb : state_block_vec_)
         if (sb)
         {

src/landmark/landmark_base.cpp

@@ -122,11 +122,7 @@ void LandmarkBase::removeStateBlocks()
 
 Eigen::VectorXs LandmarkBase::getState() const
 {
-    SizeEigen size = 0;
-    for (StateBlockPtr sb : state_block_vec_)
-        if (sb)
-            size += sb->getSize();
-    Eigen::VectorXs state(size);
+    Eigen::VectorXs state;
 
     getState(state);
 
@@ -140,10 +136,9 @@ void LandmarkBase::getState(Eigen::VectorXs& _state) const
         if (sb)
             size += sb->getSize();
 
-    assert(_state.size() == size && "Wrong state vector size");
+    _state = Eigen::VectorXs(size);
 
     SizeEigen index = 0;
-
     for (StateBlockPtr sb : state_block_vec_)
         if (sb)
         {

src/problem.cpp

@@ -249,8 +249,6 @@ Eigen::VectorXs Problem::getCurrentState()
 
 void Problem::getCurrentState(Eigen::VectorXs& state)
 {
-    assert(state.size() == getFrameStructureSize() && "Problem::getCurrentState: bad state size");
-
     if (processor_motion_ptr_ != nullptr)
         processor_motion_ptr_->getCurrentState(state);
     else if (trajectory_ptr_->getLastKeyFrame() != nullptr)
@@ -261,8 +259,6 @@ void Problem::getCurrentState(Eigen::VectorXs& state)
 
 void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
 {
-    assert(state.size() == getFrameStructureSize() && "Problem::getCurrentState: bad state size");
-
     if (processor_motion_ptr_ != nullptr)
     {
         processor_motion_ptr_->getCurrentState(state);
@@ -279,8 +275,6 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
 
 void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& state)
 {
-    assert(state.size() == getFrameStructureSize() && "Problem::getStateAtTimeStamp: bad state size");
-
     // try to get the state from processor_motion if any, otherwise...
     if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, state))
     {