[skip ci] adding local reference state blocks

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();
 

src/problem/problem.cpp

@@ -29,6 +29,7 @@
 #include "core/factor/factor_quaternion_absolute.h"
 #include "core/state_block/state_quaternion.h"
 #include "core/state_block/state_angle.h"
+#include "core/state_block/state_block_derived.h"
 #include "core/tree_manager/factory_tree_manager.h"
 #include "core/tree_manager/tree_manager_base.h"
 #include "core/utils/loader.h"
@@ -37,7 +38,7 @@
 namespace wolf
 {
 
-Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map) :
+Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map, StateBlockPtr _local_reference_p, StateBlockPtr _local_reference_o) :
         tree_manager_(nullptr),
         hardware_ptr_(std::make_shared<HardwareBase>()),
         trajectory_ptr_(std::make_shared<TrajectoryBase>()),
@@ -67,6 +68,9 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr
     }
     else std::runtime_error(
             "Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
+
+    local_reference_ = Reference{_local_reference_p ? _local_reference_p : std::make_shared<StatePoint3d>(Eigen::Vector3d::Zero()), 
+                                 _local_reference_o ? _local_reference_o : std::make_shared<StateQuaternion>(Eigen::Quaterniond::Identity())};
 }
 
 void Problem::setup()
@@ -77,9 +81,9 @@ void Problem::setup()
         map_ptr_   -> setProblem(shared_from_this());
 }
 
-ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map)
+ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map, StateBlockPtr _local_reference_p, StateBlockPtr _local_reference_o)
 {
-    ProblemPtr p(new Problem(_frame_structure, _dim, _map)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
+    ProblemPtr p(new Problem(_frame_structure, _dim, _map, _local_reference_p, _local_reference_o)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
     p->setup();
     return p->shared_from_this();
 }