Improve assert messages

src/problem.cpp

@@ -193,26 +193,26 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, const Eigen::Vecto
     {
         case FRM_PO_2D:
         {
-//            assert(_frame_state.size() == 3 && "Wrong state vector size");
+            assert(_frame_state.size() == 3 && "Wrong state vector size. Use 3 for 2D pose.");
             return trajectory_ptr_->addFrame(std::make_shared<FrameBase>(_frame_key_type, _time_stamp, std::make_shared<StateBlock>(_frame_state.head(2)),
                                   std::make_shared<StateBlock>(_frame_state.tail(1))));
         }
         case FRM_PO_3D:
         {
-//            assert(_frame_state.size() == 7 && "Wrong state vector size");
+            assert(_frame_state.size() == 7 && "Wrong state vector size. Use 7 for 3D pose.");
             return trajectory_ptr_->addFrame(std::make_shared<FrameBase>(_frame_key_type, _time_stamp, std::make_shared<StateBlock>(_frame_state.head(3)),
                                   std::make_shared<StateQuaternion>(_frame_state.tail(4))));
         }
         case FRM_POV_3D:
         {
-//            assert(_frame_state.size() == 10 && "Wrong state vector size");
+            assert(_frame_state.size() == 10 && "Wrong state vector size. Use 10 for 3D pose and velocity.");
             return trajectory_ptr_->addFrame(std::make_shared<FrameBase>(_frame_key_type, _time_stamp, std::make_shared<StateBlock>(_frame_state.head(3)),
                                   std::make_shared<StateQuaternion>(_frame_state.segment<4>(3)),
                                   std::make_shared<StateBlock>(_frame_state.tail(3))));
         }
         case FRM_PQVBB_3D:
         {
-//            assert(_frame_state.size() == 16 && "Wrong state vector size");
+            assert(_frame_state.size() == 16 && "Wrong state vector size. Use 16 for 3D pose, velocity, and IMU biases.");
             return trajectory_ptr_->addFrame(std::make_shared<FrameIMU>(_frame_key_type, _time_stamp, _frame_state));
         }
         default: