From a8666ceaf180a05bf678361df5d01911f2c0520d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Thu, 23 Jun 2022 19:42:01 +0200
Subject: [PATCH] Fix classes to new StateBlock abstract class
---
demos/hello_wolf/sensor_range_bearing.cpp | 3 ++-
src/landmark/landmark_base.cpp | 5 +++--
src/sensor/sensor_diff_drive.cpp | 19 ++++++++++---------
src/sensor/sensor_pose.cpp | 4 ++--
4 files changed, 17 insertions(+), 14 deletions(-)
diff --git a/demos/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
index 7073082e3..affd707e2 100644
--- a/demos/hello_wolf/sensor_range_bearing.cpp
+++ b/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)
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index e7acd4f0d..bbda7dcba 100644
--- a/src/landmark/landmark_base.cpp
+++ b/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"])
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index f5c4e6aa2..256def479 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/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()
diff --git a/src/sensor/sensor_pose.cpp b/src/sensor/sensor_pose.cpp
index 87f2880b8..0d12027c9 100644
--- a/src/sensor/sensor_pose.cpp
+++ b/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)
{
--
GitLab