Draft: Resolve "Problem local_reference_"

Closes #479

include/core/problem/problem.h

@@ -99,11 +99,19 @@ class Problem : public std::enable_shared_from_this<Problem>
         Reference local_reference_;
 
       private:  // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
-        Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !!
+        Problem(const std::string& _frame_structure, 
+                SizeEigen _dim, 
+                MapBasePtr _map, 
+                StateBlockPtr _local_reference_p = nullptr, 
+                StateBlockPtr _local_reference_o = nullptr); // USE create() below !!
         void setup();
 
     public:
-        static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map = std::make_shared<MapBase>()); // USE THIS AS A CONSTRUCTOR!
+        static ProblemPtr create(const std::string& _frame_structure,
+                                 SizeEigen _dim,
+                                 MapBasePtr _map = std::make_shared<MapBase>(),
+                                 StateBlockPtr _local_reference_p = nullptr,
+                                 StateBlockPtr _local_reference_o = nullptr); // USE THIS AS A CONSTRUCTOR!
         static ProblemPtr autoSetup(ParamsServer &_server);
         virtual ~Problem();