From 4f10166a0fdcb7f1082047e3d8013c785f7ccb7e 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:58:51 +0200
Subject: [PATCH] use derived state blocks
---
src/capture/capture_inertial_kinematics.cpp | 6 ++--
src/sensor/sensor_inertial_kinematics.cpp | 4 +--
test/gtest_factor_force_torque.cpp | 33 +++++++++++++--------
test/gtest_factor_inertial_kinematics.cpp | 14 +++++----
4 files changed, 35 insertions(+), 22 deletions(-)
diff --git a/src/capture/capture_inertial_kinematics.cpp b/src/capture/capture_inertial_kinematics.cpp
index 3640f6b..3f3c387 100644
--- a/src/capture/capture_inertial_kinematics.cpp
+++ b/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)
diff --git a/src/sensor/sensor_inertial_kinematics.cpp b/src/sensor/sensor_inertial_kinematics.cpp
index 33369c5..5cfaa76 100644
--- a/src/sensor/sensor_inertial_kinematics.cpp
+++ b/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),
diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
index 16bd8d8..0ccf513 100644
--- a/test/gtest_factor_force_torque.cpp
+++ b/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();
diff --git a/test/gtest_factor_inertial_kinematics.cpp b/test/gtest_factor_inertial_kinematics.cpp
index 45034d6..0f63fd9 100644
--- a/test/gtest_factor_inertial_kinematics.cpp
+++ b/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_);
--
GitLab