From ffc612efaa7fc740f0ceb96396fc28319b6daa4e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Mon, 20 Jun 2022 09:46:51 +0200
Subject: [PATCH] Use derived state blocks in sensorImu2d

---
 src/sensor/sensor_imu2d.cpp | 34 +++++++++++++++++++++-------------
 1 file changed, 21 insertions(+), 13 deletions(-)

diff --git a/src/sensor/sensor_imu2d.cpp b/src/sensor/sensor_imu2d.cpp
index f74b3baa2..ca6130465 100644
--- a/src/sensor/sensor_imu2d.cpp
+++ b/src/sensor/sensor_imu2d.cpp
@@ -19,8 +19,9 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-#include <imu/sensor/sensor_imu2d.h>
-#include <core/state_block/state_block.h>
+#include "imu/sensor/sensor_imu2d.h"
+
+#include <core/state_block/state_block_derived.h>
 #include <core/state_block/state_angle.h>
 
 namespace wolf {
@@ -31,20 +32,27 @@ SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, ParamsSensorImu2dPt
     //
 }
 
-SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params) :
-        SensorBase("SensorImu2d", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), std::make_shared<StateBlock>(3, false, nullptr), (Eigen::Vector3d()<<_params.a_noise,_params.a_noise,_params.w_noise).finished(), false, false, true),
-        a_noise(_params.a_noise),
-        w_noise(_params.w_noise),
-        ab_initial_stdev(_params.ab_initial_stdev),
-        wb_initial_stdev(_params.wb_initial_stdev),
-        ab_rate_stdev(_params.ab_rate_stdev),
-        wb_rate_stdev(_params.wb_rate_stdev),
-        orthogonal_gravity(_params.orthogonal_gravity)
+SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params)
+    : SensorBase("SensorImu2d",
+                 std::make_shared<StatePoint2d>(_extrinsics.head(2), true, false),
+                 std::make_shared<StateAngle>(_extrinsics(2), true, false),
+                 std::make_shared<StateParams3>(Vector3d::Zero(3), false),
+                 (Eigen::Vector3d() << _params.a_noise, _params.a_noise, _params.w_noise).finished(),
+                 false,
+                 false,
+                 true),
+      a_noise(_params.a_noise),
+      w_noise(_params.w_noise),
+      ab_initial_stdev(_params.ab_initial_stdev),
+      wb_initial_stdev(_params.wb_initial_stdev),
+      ab_rate_stdev(_params.ab_rate_stdev),
+      wb_rate_stdev(_params.wb_rate_stdev),
+      orthogonal_gravity(_params.orthogonal_gravity)
 {
     assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2d.");
-    if(!orthogonal_gravity)
+    if (!orthogonal_gravity)
     {
-        addStateBlock('G', std::make_shared<StateBlock>(2, false), false);
+        addStateBlock('G', std::make_shared<StateVector2d>(Vector2d::Zero(), false, true), false);
     }
 }
 
-- 
GitLab