use derived state blocks

src/capture/capture_inertial_kinematics.cpp

@@ -23,6 +23,8 @@
 #include "bodydynamics/capture/capture_inertial_kinematics.h"
 #include "bodydynamics/sensor/sensor_inertial_kinematics.h"
 #include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_block_derived.h"
+
 
 namespace wolf {
 
@@ -37,7 +39,7 @@ CaptureInertialKinematics::CaptureInertialKinematics(const TimeStamp& _init_ts,
                                                                _sensor_ptr,
                                                                nullptr,
                                                                nullptr,
-                                                               std::make_shared<StateBlock>(_bias_pbc, false)),
+                                                               std::make_shared<StateParams3>(_bias_pbc, false)),
                                                 data_(_data),
                                                 B_I_q_(_B_I_q),
                                                 B_Lc_m_(_B_Lc_m)
@@ -56,7 +58,7 @@ CaptureInertialKinematics::CaptureInertialKinematics(const TimeStamp& _init_ts,
                                                                _sensor_ptr,
                                                                nullptr,
                                                                nullptr,
-                                                               std::make_shared<StateBlock>(3, false)),
+                                                               std::make_shared<StateParams3>(Vector3d::Zero(), false)),
                                                 data_(_data),
                                                 B_I_q_(_B_I_q),
                                                 B_Lc_m_(_B_Lc_m)

src/sensor/sensor_inertial_kinematics.cpp

@@ -21,7 +21,7 @@
 //--------LICENSE_END--------
 #include "bodydynamics/sensor/sensor_inertial_kinematics.h"
 
-#include "core/state_block/state_block.h"
+#include "core/state_block/state_block_derived.h"
 #include "core/state_block/state_quaternion.h"
 
 namespace wolf {
@@ -30,7 +30,7 @@ SensorInertialKinematics::SensorInertialKinematics(const Eigen::VectorXd& _extri
         SensorBase("SensorInertialKinematics",
                    nullptr, // no position
                    nullptr, // no orientation
-                   std::make_shared<StateBlock>(Eigen::Vector3d(0,0,0), false), // bias; false = unfixed
+                   std::make_shared<StateParams3>(Eigen::Vector3d(0,0,0), false), // bias; false = unfixed
                    (Eigen::Vector6d() << _params->std_pbc,_params->std_pbc,_params->std_pbc,
                                          _params->std_vbc,_params->std_vbc,_params->std_vbc).finished(),
                    false, false, true),

test/gtest_factor_force_torque.cpp

@@ -58,6 +58,7 @@ FactorInertialKinematics_2KF_1v_bfix,ZeroMvt:
 #include <core/capture/capture_base.h>
 #include <core/feature/feature_base.h>
 #include <core/factor/factor_block_absolute.h>
+#include <core/state_block/state_block_derived.h>
 
 // Imu
 // #include "imu/internal/config.h"
@@ -169,18 +170,26 @@ void perturbateAllIfUnFixed(const FrameBasePtr& KF)
     }
 }
 
-FrameBasePtr createKFWithCDLI(const ProblemPtr& problem, const TimeStamp& t, VectorComposite x_origin,
-                              Vector3d c, Vector3d cd, Vector3d Lc, Vector6d bias_imu)
+FrameBasePtr createKFWithCDLI(const ProblemPtr& problem,
+                              const TimeStamp&  t,
+                              VectorComposite   x_origin,
+                              Vector3d          c,
+                              Vector3d          cd,
+                              Vector3d          Lc,
+                              Vector6d          bias_imu)
 {
-    FrameBasePtr KF = FrameBase::emplace<FrameBase>(problem->getTrajectory(), t, "POV", x_origin);
-    StateBlockPtr sbc = make_shared<StateBlock>(c);  KF->addStateBlock('C', sbc, problem);
-    StateBlockPtr sbd = make_shared<StateBlock>(cd); KF->addStateBlock('D', sbd, problem);
-    StateBlockPtr sbL = make_shared<StateBlock>(Lc); KF->addStateBlock('L', sbL, problem);
-    StateBlockPtr sbbimu = make_shared<StateBlock>(bias_imu); KF->addStateBlock('I', sbbimu, problem);  // Simulating IMU
+    FrameBasePtr  KF  = FrameBase::emplace<FrameBase>(problem->getTrajectory(), t, "POV", x_origin);
+    StateBlockPtr sbc = make_shared<StatePoint3d>(c);
+    KF->addStateBlock('C', sbc, problem);
+    StateBlockPtr sbd = make_shared<StateVector3d>(cd);
+    KF->addStateBlock('D', sbd, problem);
+    StateBlockPtr sbL = make_shared<StateVector3d>(Lc);
+    KF->addStateBlock('L', sbL, problem);
+    StateBlockPtr sbbimu = make_shared<StateParams6>(bias_imu);
+    KF->addStateBlock('I', sbbimu, problem);  // Simulating IMU
     return KF;
 }
 
-
 class FactorInertialKinematics_2KF : public testing::Test
 {
     public:
@@ -247,10 +256,10 @@ class FactorInertialKinematics_2KF : public testing::Test
         // ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
         bias_p_ = zero3;
         bias_imu_ = zero6;
-        StateBlockPtr sbcA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('C', sbcA, problem_);
-        StateBlockPtr sbdA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('D', sbdA, problem_);
-        StateBlockPtr sbLA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('L', sbLA, problem_);
-        StateBlockPtr sbbimuA = make_shared<StateBlock>(bias_imu_); KFA_->addStateBlock('I', sbbimuA, problem_);
+        StateBlockPtr sbcA = make_shared<StatePoint3d>(zero3); KFA_->addStateBlock('C', sbcA, problem_);
+        StateBlockPtr sbdA = make_shared<StateVector3d>(zero3); KFA_->addStateBlock('D', sbdA, problem_);
+        StateBlockPtr sbLA = make_shared<StateVector3d>(zero3); KFA_->addStateBlock('L', sbLA, problem_);
+        StateBlockPtr sbbimuA = make_shared<StateParams6>(bias_imu_); KFA_->addStateBlock('I', sbbimuA, problem_);
 
         // Fix the one we cannot estimate
         // KFA_->getP()->fix();

test/gtest_factor_inertial_kinematics.cpp

@@ -58,6 +58,7 @@ FactorInertialKinematics_1KF_1v_bfix,ZeroMvt:
 #include <core/capture/capture_base.h>
 #include <core/feature/feature_base.h>
 #include <core/factor/factor_block_absolute.h>
+#include <core/state_block/state_block_derived.h>
 
 // Imu
 //#include "imu/internal/config.h"
@@ -72,6 +73,7 @@ FactorInertialKinematics_1KF_1v_bfix,ZeroMvt:
 #include "bodydynamics/capture/capture_inertial_kinematics.h"
 #include "bodydynamics/feature/feature_inertial_kinematics.h"
 #include "bodydynamics/factor/factor_inertial_kinematics.h"
+#include <core/state_block/state_block_derived.h>
 
 #include <Eigen/Eigenvalues>
 
@@ -126,11 +128,11 @@ class FactorInertialKinematics_1KF : public testing::Test
         // ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
         bias_p_ = ZERO3;
         bias_imu_ = ZERO6;
-        StateBlockPtr sbc = make_shared<StateBlock>(ZERO3);
-        StateBlockPtr sbd = make_shared<StateBlock>(ZERO3);
-        StateBlockPtr sbL = make_shared<StateBlock>(ZERO3);
-        StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
-        sbbimu_ = make_shared<StateBlock>(bias_imu_);
+        StateBlockPtr sbc = make_shared<StatePoint3d>(ZERO3);
+        StateBlockPtr sbL = make_shared<StateVector3d>(ZERO3);
+        StateBlockPtr sbd = make_shared<StateVector3d>(ZERO3);
+        StateBlockPtr sbb = make_shared<StateParams3>(bias_p_);
+        sbbimu_ = make_shared<StateParams6>(bias_imu_);
 
         KF0_->addStateBlock('C', sbc, problem_);
         KF0_->addStateBlock('D', sbd, problem_);
@@ -328,7 +330,7 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
 //        Vector6d ZERO6; ZERO6 << ZERO3, ZERO3;
 //        Vector3d bias_p_ = ZERO3;
 //        Vector6d bias_imu_ = ZERO6;
-//        StateBlockPtr sbc = make_shared<StateBlock>(ZERO3);
+//        StateBlockPtr sbc = make_shared<StatePoint3d>(ZERO3);
 //        StateBlockPtr sbd = make_shared<StateBlock>(ZERO3);
 //        StateBlockPtr sbL = make_shared<StateBlock>(ZERO3);
 //        StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);