Make state_block_vec of the exact size

This obliges to resize() it in derived classes if more state blocks are
needed.
See FrameIMU constructors for an example.

src/frame_base.cpp

@@ -13,7 +13,7 @@ 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_(6), // allow for 6 state blocks by default. Should be enough in all applications.
+            state_block_vec_(3), // allow for 6 state blocks by default. Resize in derived constructors if needed.
             frame_id_(++frame_id_count_),
             type_id_(NON_KEY_FRAME),
 			status_(ST_ESTIMATED),
@@ -22,9 +22,6 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
     state_block_vec_[0] = _p_ptr;
     state_block_vec_[1] = _o_ptr;
     state_block_vec_[2] = _v_ptr;
-    state_block_vec_[3] = nullptr;
-    state_block_vec_[4] = nullptr;
-    state_block_vec_[5] = nullptr;
     //
     if (isKey())
         std::cout << "constructed +KF" << id() << std::endl;
@@ -35,7 +32,7 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
 FrameBase::FrameBase(const FrameKeyType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
             NodeBase("FRAME", "BASE"),
             trajectory_ptr_(),
-            state_block_vec_(6), // allow for 6 state blocks by default. Should be enough in all applications.
+            state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
             frame_id_(++frame_id_count_),
             type_id_(_tp),
 			status_(ST_ESTIMATED),
@@ -44,9 +41,6 @@ FrameBase::FrameBase(const FrameKeyType & _tp, const TimeStamp& _ts, StateBlockP
     state_block_vec_[0] = _p_ptr;
     state_block_vec_[1] = _o_ptr;
     state_block_vec_[2] = _v_ptr;
-    state_block_vec_[3] = nullptr;
-    state_block_vec_[4] = nullptr;
-    state_block_vec_[5] = nullptr;
 
     if (isKey())
         std::cout << "constructed +KF" << id() << std::endl;

src/frame_base.h

@@ -97,6 +97,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
     protected:
         StateBlockPtr getStateBlockPtr(unsigned int _i) const;
         void setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr);
+        void resizeStateBlockVec(int _size);
     private:
         void removeStateBlocks();
 
@@ -259,6 +260,7 @@ inline StateBlockPtr FrameBase::getStateBlockPtr(unsigned int _i) const
 
 inline void FrameBase::setStateBlockPtr(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;
 }
 
@@ -280,6 +282,12 @@ inline CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
     return _capt_ptr;
 }
 
+inline void FrameBase::resizeStateBlockVec(int _size)
+{
+    if (_size > state_block_vec_.size())
+        state_block_vec_.resize(_size);
+}
+
 inline StateStatus FrameBase::getStatus() const
 {
     return status_;

src/frame_imu.cpp

@@ -11,6 +11,7 @@ FrameIMU::FrameIMU(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _v_
                    StateBlockPtr _ba_ptr, StateBlockPtr _bg_ptr) :
         FrameBase(_ts, _p_ptr, _q_ptr, _v_ptr)//,
 {
+    resizeStateBlockVec(5); // could have done push_back, but prefer more explicit locations for the StateBlocks
     setStateBlockPtr(3, std::make_shared<StateBlock>(3)); // acc bias
     setStateBlockPtr(4, std::make_shared<StateBlock>(3)); // gyro bias
     setType("IMU");
@@ -20,6 +21,7 @@ FrameIMU::FrameIMU(const FrameKeyType& _tp, const TimeStamp& _ts, StateBlockPtr
                    StateQuaternionPtr _q_ptr, StateBlockPtr _ba_ptr, StateBlockPtr _bg_ptr) :
         FrameBase(_tp, _ts, _p_ptr, _q_ptr, _v_ptr)
 {
+    resizeStateBlockVec(5); // could have done push_back, but prefer more explicit locations for the StateBlocks
     setStateBlockPtr(3, std::make_shared<StateBlock>(3)); // acc bias
     setStateBlockPtr(4, std::make_shared<StateBlock>(3)); // gyro bias
     setType("IMU");
@@ -28,6 +30,7 @@ FrameIMU::FrameIMU(const FrameKeyType& _tp, const TimeStamp& _ts, StateBlockPtr
 FrameIMU::FrameIMU(const FrameKeyType& _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) :
         FrameBase(_tp, _ts, std::make_shared<StateBlock>(3), std::make_shared<StateQuaternion>(), std::make_shared<StateBlock>(3))
 {
+    resizeStateBlockVec(5); // could have done push_back, but prefer more explicit locations for the StateBlocks
     setStateBlockPtr(3, std::make_shared<StateBlock>(3)); // acc bias
     setStateBlockPtr(4, std::make_shared<StateBlock>(3)); // gyro bias
     assert(_x.size() == 16 && "Wrong vector size! Must be 16.");

src/landmark_base.cpp

@@ -12,15 +12,13 @@ unsigned int LandmarkBase::landmark_id_count_ = 0;
 LandmarkBase::LandmarkBase(const LandmarkType & _tp, const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
             NodeBase("LANDMARK", _type),
             map_ptr_(),
-            state_block_vec_(4), // allow for 4 state blocks by default. Should be enough in all applications.
+            state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed.
             landmark_id_(++landmark_id_count_),
             type_id_(_tp),
             status_(LANDMARK_CANDIDATE)
 {
     state_block_vec_[0] = _p_ptr;
     state_block_vec_[1] = _o_ptr;
-    state_block_vec_[2] = nullptr;
-    state_block_vec_[3] = nullptr;
     std::cout << "constructed  +L" << id() << std::endl;
 }
                 

src/sensor_base.cpp

@@ -10,7 +10,7 @@ SensorBase::SensorBase(const SensorType& _tp, const std::string& _type, StateBlo
                        const unsigned int _noise_size, const bool _extr_dyn) :
         NodeBase("SENSOR", _type),
         hardware_ptr_(),
-        state_block_vec_(6), // allow for 6 state blocks by default. Should be enough in all applications.
+        state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
         sensor_id_(++sensor_id_count_), // simple ID factory
         type_id_(_tp),
         extrinsic_dynamic_(_extr_dyn),
@@ -20,9 +20,6 @@ SensorBase::SensorBase(const SensorType& _tp, const std::string& _type, StateBlo
     state_block_vec_[0] = _p_ptr;
     state_block_vec_[1] = _o_ptr;
     state_block_vec_[2] = _intr_ptr;
-    state_block_vec_[3] = nullptr;
-    state_block_vec_[4] = nullptr;
-    state_block_vec_[5] = nullptr;
     std::cout << "constructed  +S" << id() << std::endl;
     //
 }
@@ -31,7 +28,7 @@ SensorBase::SensorBase(const SensorType & _tp, const std::string& _type, StateBl
                        const Eigen::VectorXs & _noise_std, const bool _extr_dyn) :
         NodeBase("SENSOR", _type),
         hardware_ptr_(),
-        state_block_vec_(6), // allow for 6 state blocks by default. Should be enough in all applications.
+        state_block_vec_(6), // allow for 3 state blocks by default. Resize in derived constructors if needed.
         sensor_id_(++sensor_id_count_), // simple ID factory
         type_id_(_tp),
         extrinsic_dynamic_(_extr_dyn),
@@ -41,9 +38,6 @@ SensorBase::SensorBase(const SensorType & _tp, const std::string& _type, StateBl
     state_block_vec_[0] = _p_ptr;
     state_block_vec_[1] = _o_ptr;
     state_block_vec_[2] = _intr_ptr;
-    state_block_vec_[3] = nullptr;
-    state_block_vec_[4] = nullptr;
-    state_block_vec_[5] = nullptr;
     noise_cov_.setZero();
     for (unsigned int i = 0; i < _noise_std.size(); i++)
         noise_cov_(i, i) = noise_std_(i) * noise_std_(i);