Fix classes to new StateBlock abstract class

demos/hello_wolf/sensor_range_bearing.cpp

@@ -28,6 +28,7 @@
 
 #include "sensor_range_bearing.h"
 #include "core/state_block/state_angle.h"
+#include "core/state_block/state_block_derived.h"
 
 namespace wolf
 {
@@ -36,7 +37,7 @@ WOLF_PTR_TYPEDEFS(SensorRangeBearing);
 
 SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std) :
         SensorBase("SensorRangeBearing",
-                   std::make_shared<StateBlock>(_extrinsics.head<2>(), true),
+                   std::make_shared<StatePoint2d>(_extrinsics.head<2>(), true),
                    std::make_shared<StateAngle>(_extrinsics(2), true),
                    nullptr,
                    _noise_std)

src/landmark/landmark_base.cpp

@@ -23,9 +23,10 @@
 #include "core/landmark/landmark_base.h"
 #include "core/factor/factor_base.h"
 #include "core/map/map_base.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
 #include "core/state_block/state_angle.h"
 #include "core/state_block/state_quaternion.h"
+#include "core/state_block/factory_state_block.h"
 #include "core/common/factory.h"
 #include "core/yaml/yaml_conversion.h"
 
@@ -216,7 +217,7 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
     Eigen::VectorXd pos         = _node["position"]         .as< Eigen::VectorXd  >();
     bool            pos_fixed   = _node["position fixed"]   .as< bool >();
 
-    StateBlockPtr pos_sb = std::make_shared<StateBlock>(pos, pos_fixed);
+    StateBlockPtr pos_sb = FactoryStateBlock::create("P", pos, pos_fixed);
     StateBlockPtr ori_sb = nullptr;
 
     if (_node["orientation"])

src/sensor/sensor_diff_drive.cpp

@@ -27,21 +27,23 @@
  */
 
 #include "core/sensor/sensor_diff_drive.h"
+#include "core/state_block/state_block_derived.h"
 #include "core/state_block/state_angle.h"
 
 namespace wolf
 {
 
-SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
-                                 ParamsSensorDiffDrivePtr _intrinsics) :
-        SensorBase("SensorDiffDrive",
-                   std::make_shared<StateBlock>(_extrinsics.head(2), true),
-                   std::make_shared<StateAngle>(_extrinsics(2), true),
-                   std::make_shared<StateBlock>(3, false, nullptr, false), 2),
-                   params_diff_drive_(_intrinsics)
+SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics, ParamsSensorDiffDrivePtr _intrinsics)
+    : SensorBase("SensorDiffDrive",
+                 std::make_shared<StatePoint2d>(_extrinsics.head(2), true),
+                 std::make_shared<StateAngle>(_extrinsics(2), true),
+                 std::make_shared<StateParams3>(
+                     Vector3d(_intrinsics->radius_left, _intrinsics->radius_right, _intrinsics->wheel_separation),
+                     false),
+                 2),
+      params_diff_drive_(_intrinsics)
 {
     radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution;
-    getIntrinsic()->setState(Eigen::Vector3d(_intrinsics->radius_left,_intrinsics->radius_right,_intrinsics->wheel_separation));
     unfixIntrinsics();
 
     if(params_diff_drive_->set_intrinsics_prior)
@@ -53,7 +55,6 @@ SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
     Eigen::Vector2d noise_sigma; noise_sigma << sigma_rev, sigma_rev;
 
     setNoiseStd(noise_sigma);
-   
 }
 
 SensorDiffDrive::~SensorDiffDrive()

src/sensor/sensor_pose.cpp

@@ -28,14 +28,14 @@
 
 #include "core/sensor/sensor_pose.h"
 
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
 #include "core/state_block/state_quaternion.h"
 #include "core/math/rotations.h"
 
 namespace wolf {
 
 SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& _intrinsics) :
-                        SensorBase("SensorPose", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
+                        SensorBase("SensorPose", std::make_shared<StatePoint3d>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
                         std_p_(_intrinsics.std_p),
                         std_o_(_intrinsics.std_o)
 {