added dim_ and getDim() in problem

src/problem.cpp

@@ -36,25 +36,26 @@ Problem::Problem(const std::string& _frame_structure) :
         trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
         map_ptr_(std::make_shared<MapBase>()),
         processor_motion_ptr_(),
-        state_size_(0),
-        state_cov_size_(0),
         prior_is_set_(false)
 {
     if (_frame_structure == "PO 2D")
     {
         state_size_ = 3;
         state_cov_size_ = 3;
+        dim_ = 2;
     }
 
     else if (_frame_structure == "PO 3D")
     {
         state_size_ = 7;
         state_cov_size_ = 6;
+        dim_ = 3;
     }
     else if (_frame_structure == "POV 3D")
     {
         state_size_ = 10;
         state_cov_size_ = 9;
+        dim_ = 3;
     }
     else std::runtime_error(
             "Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
@@ -315,6 +316,11 @@ void Problem::getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) co
     _cov_size = state_cov_size_;
 }
 
+SizeEigen Problem::getDim() const
+{
+    return dim_;
+}
+
 Eigen::VectorXs Problem::zeroState()
 {
     Eigen::VectorXs state = Eigen::VectorXs::Zero(getFrameStructureSize());

src/problem.h

@@ -42,7 +42,7 @@ class Problem : public std::enable_shared_from_this<Problem>
         ProcessorMotionPtr  processor_motion_ptr_;
         StateBlockList      state_block_list_;
         std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
-        SizeEigen state_size_, state_cov_size_;
+        SizeEigen state_size_, state_cov_size_, dim_;
         std::map<ConstraintBasePtr, Notification> constraint_notification_map_;
         std::map<StateBlockPtr, Notification> state_block_notification_map_;
         bool prior_is_set_;
@@ -58,6 +58,7 @@ class Problem : public std::enable_shared_from_this<Problem>
         // Properties -----------------------------------------
         SizeEigen getFrameStructureSize() const;
         void getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const;
+        SizeEigen getDim() const;
 
         // Hardware branch ------------------------------------
         HardwareBasePtr getHardwarePtr();