Replace StateBlock by some derived types

demos/demo_analytic_autodiff_factor.cpp

@@ -80,7 +80,7 @@ int main(int argc, char** argv)
     // Wolf problem
     ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2d);
     ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2d);
-    SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
+    SensorBasePtr sensor = new SensorBase("ODOM 2d", std::make_shared<StatePoint2d>(Eigen::VectorXd::Zero(2)), std::make_shared<StateAngle>(0), std::make_shared<StateParams2>(Eigen::VectorXd::Zero(2)), 2);
 
     // Ceres wrapper
     SolverCeres* ceres_manager_autodiff = new SolverCeres(wolf_problem_autodiff, ceres_options);

demos/hello_wolf/landmark_point_2d.cpp

@@ -27,12 +27,13 @@
  */
 
 #include "landmark_point_2d.h"
+#include "core/state_block/state_block_derived.h"
 
 namespace wolf
 {
 
 LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) :
-        LandmarkBase("LandmarkPoint2d", std::make_shared<StateBlock>(_xy))
+        LandmarkBase("LandmarkPoint2d", std::make_shared<StatePoint2d>(_xy))
 {
     setId(_id);
 }

src/sensor/sensor_odom_2d.cpp

@@ -20,17 +20,19 @@
 //
 //--------LICENSE_END--------
 #include "core/sensor/sensor_odom_2d.h"
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
 #include "core/state_block/state_angle.h"
 
 namespace wolf {
 
 SensorOdom2d::SensorOdom2d(Eigen::VectorXd _extrinsics, const ParamsSensorOdom2d& _intrinsics) :
-        SensorBase("SensorOdom2d", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2),
+        SensorBase("SensorOdom2d", std::make_shared<StatePoint2d>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2),
         k_disp_to_disp_(_intrinsics.k_disp_to_disp),
         k_rot_to_rot_(_intrinsics.k_rot_to_rot)
 {
     assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2d.");
+    getStateBlock('P')->setNonTransformable();
+    getStateBlock('O')->setNonTransformable();
     //
 }
 

src/sensor/sensor_odom_3d.cpp

@@ -28,14 +28,14 @@
 
 #include "core/sensor/sensor_odom_3d.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 {
 
 SensorOdom3d::SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorOdom3d& _intrinsics) :
-                        SensorBase("SensorOdom3d", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
+                        SensorBase("SensorOdom3d", std::make_shared<StatePoint3d>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
                         k_disp_to_disp_(_intrinsics.k_disp_to_disp),
                         k_disp_to_rot_(_intrinsics.k_disp_to_rot),
                         k_rot_to_rot_(_intrinsics.k_rot_to_rot),
@@ -43,6 +43,8 @@ SensorOdom3d::SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, const ParamsSe
                         min_rot_var_(_intrinsics.min_rot_var)
 {
     assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d.");
+    getStateBlock('P')->setNonTransformable();
+    getStateBlock('O')->setNonTransformable();
 
     noise_cov_ = (Eigen::Vector6d() << min_disp_var_, min_disp_var_, min_disp_var_, min_rot_var_, min_rot_var_, min_rot_var_).finished().asDiagonal();
     setNoiseCov(noise_cov_); // sets also noise_std_