From 9ceeffb20f74bc786091dffe517f7936d846d47a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 24 Jun 2022 19:53:51 +0200
Subject: [PATCH] Use derived state blocks
---
src/capture/capture_gnss.cpp | 10 +++++-----
src/sensor/sensor_gnss.cpp | 14 +++++++-------
2 files changed, 12 insertions(+), 12 deletions(-)
diff --git a/src/capture/capture_gnss.cpp b/src/capture/capture_gnss.cpp
index 299f515a9..751561dfb 100644
--- a/src/capture/capture_gnss.cpp
+++ b/src/capture/capture_gnss.cpp
@@ -20,7 +20,7 @@
//
//--------LICENSE_END--------
#include "gnss/capture/capture_gnss.h"
-
+#include <core/state_block/state_block_derived.h>
namespace wolf {
@@ -30,18 +30,18 @@ CaptureGnss::CaptureGnss(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, GnssUt
{
// Clock bias
assert(_sensor_ptr->getStateBlock('T') != nullptr and _sensor_ptr->isStateBlockDynamic('T'));
- addStateBlock('T', std::make_shared<StateBlock>(1,true), nullptr);
+ addStateBlock('T', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
// interconstellation clock bias
assert(_sensor_ptr->getStateBlock('G') != nullptr);
if(_sensor_ptr->isStateBlockDynamic('G'))
- addStateBlock('G', std::make_shared<StateBlock>(1,true), nullptr);
+ addStateBlock('G', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
assert(_sensor_ptr->getStateBlock('E') != nullptr);
if(_sensor_ptr->isStateBlockDynamic('E'))
- addStateBlock('E', std::make_shared<StateBlock>(1,true), nullptr);
+ addStateBlock('E', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
assert(_sensor_ptr->getStateBlock('M') != nullptr);
if(_sensor_ptr->isStateBlockDynamic('M'))
- addStateBlock('M', std::make_shared<StateBlock>(1,true), nullptr);
+ addStateBlock('M', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
}
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index 6e8d73c39..b049301a7 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -20,7 +20,7 @@
//
//--------LICENSE_END--------
#include "gnss/sensor/sensor_gnss.h"
-#include "core/state_block/state_block.h"
+#include <core/state_block/state_block_derived.h>
#include "core/state_block/state_angle.h"
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/utils/transformations.h"
@@ -31,7 +31,7 @@ SensorGnss::SensorGnss(const Eigen::VectorXd& _extrinsics,
const ParamsSensorGnssPtr& _params)
:
SensorBase("SensorGnss",
- std::make_shared<StateBlock>(_extrinsics,_params->extrinsics_fixed),
+ std::make_shared<StatePoint3d>(_extrinsics,_params->extrinsics_fixed),
nullptr, // antena orientation has no sense in GNSS
nullptr, // SensorParams are set afterwards
0),
@@ -46,15 +46,15 @@ SensorGnss::SensorGnss(const Eigen::VectorXd& _extrinsics,
// STATE BLOCKS
// ENU-MAP
- addStateBlock('t', std::make_shared<StateBlock>(3, params_->translation_fixed), false);
+ addStateBlock('t', std::make_shared<StatePoint3d>(Vector3d::Zero(), params_->translation_fixed), false);
addStateBlock('r', std::make_shared<StateAngle>(0.0, params_->roll_fixed), false);
addStateBlock('p', std::make_shared<StateAngle>(0.0, params_->pitch_fixed), false);
addStateBlock('y', std::make_shared<StateAngle>(0.0, params_->yaw_fixed), false);
// clock bias
- addStateBlock(CLOCK_BIAS_KEY, std::make_shared<StateBlock>(1,false), true); // receiver clock bias
- addStateBlock(CLOCK_BIAS_GPS_GLO_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_GLO_dynamic); // GPS-GLO clock bias
- addStateBlock(CLOCK_BIAS_GPS_GAL_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_GAL_dynamic); // GPS-GAL clock bias
- addStateBlock(CLOCK_BIAS_GPS_CMP_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_CMP_dynamic); // GPS-CMP clock bias
+ addStateBlock(CLOCK_BIAS_KEY, std::make_shared<StateParams1>(Vector1d::Zero(),false), true); // receiver clock bias
+ addStateBlock(CLOCK_BIAS_GPS_GLO_KEY, std::make_shared<StateParams1>(Vector1d::Zero(),true), params_->clock_bias_GPS_GLO_dynamic); // GPS-GLO clock bias
+ addStateBlock(CLOCK_BIAS_GPS_GAL_KEY, std::make_shared<StateParams1>(Vector1d::Zero(),true), params_->clock_bias_GPS_GAL_dynamic); // GPS-GAL clock bias
+ addStateBlock(CLOCK_BIAS_GPS_CMP_KEY, std::make_shared<StateParams1>(Vector1d::Zero(),true), params_->clock_bias_GPS_CMP_dynamic); // GPS-CMP clock bias
// ENU-ECEF
// Mode "manual": ENU provided via params
--
GitLab