diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1def785eba87f87d2ce061b93927064b906240d9..1c86e754c1565eeb626442ea535c4b000aa40069 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -142,7 +142,6 @@ SET(SRCS_PROCESSOR
)
SET(SRCS_SENSOR
src/sensor/sensor_compass.cpp
- src/sensor/sensor_imu.cpp
)
# create the shared library
diff --git a/include/imu/capture/capture_compass.h b/include/imu/capture/capture_compass.h
index 22e6179ead557c4d43878ab44bd92d4214df1cd1..0cfb7d49f7f834be453b101d9bbef9bc74632f99 100644
--- a/include/imu/capture/capture_compass.h
+++ b/include/imu/capture/capture_compass.h
@@ -19,12 +19,12 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#ifndef CAPTURE_COMPASS_H_
-#define CAPTURE_COMPASS_H_
+#pragma once
//Wolf includes
#include "imu/sensor/sensor_compass.h"
#include <core/capture/capture_base.h>
+#include <core/state_block/state_block_derived.h>
//std includes
//
@@ -51,7 +51,7 @@ class CaptureCompass : public CaptureBase
nullptr,
nullptr,
_sensor_ptr->isStateBlockDynamic('I') ?
- std::make_shared<StateBlock>(_sensor_ptr->getProblem()->getDim(), false) :
+ std::make_shared<StateParams3>(Eigen::Vector3d::Zero(), false) :
nullptr),
data_(_data)
{
@@ -61,7 +61,7 @@ class CaptureCompass : public CaptureBase
// FIXME: make this generic in SensorBase
if (_sensor_ptr->isStateBlockDynamic('F'))
- addStateBlock('F',std::make_shared<StateBlock>(_sensor_ptr->getProblem()->getDim(), false), _sensor_ptr->getProblem());
+ addStateBlock('F',std::make_shared<StateParams3>(Eigen::Vector3d::Zero(), false), _sensor_ptr->getProblem());
}
CaptureCompass(const TimeStamp& _ts,
@@ -74,7 +74,7 @@ class CaptureCompass : public CaptureBase
nullptr,
nullptr,
_sensor_ptr->isStateBlockDynamic('I') ?
- std::make_shared<StateBlock>(_sensor_ptr->getProblem()->getDim(), false) :
+ std::make_shared<StateParams3>(Eigen::Vector3d::Zero(), false) :
nullptr),
data_(_data),
data_covariance_(_data_covariance)
@@ -83,7 +83,7 @@ class CaptureCompass : public CaptureBase
// FIXME: make this generic in SensorBase
if (_sensor_ptr->isStateBlockDynamic('F'))
- addStateBlock('F',std::make_shared<StateBlock>(_sensor_ptr->getProblem()->getDim(), false), _sensor_ptr->getProblem());
+ addStateBlock('F',std::make_shared<StateParams3>(Eigen::Vector3d::Zero(), false), _sensor_ptr->getProblem());
}
~CaptureCompass() override
@@ -102,5 +102,4 @@ class CaptureCompass : public CaptureBase
}
};
-} //namespace wolf
-#endif
+} //namespace wolf
\ No newline at end of file
diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h
index 71dd3de29f713b439e82d8a9634cfb125fb6ef29..7ea62c9608a60e2a17ff07a9c7c93a8ceb914f58 100644
--- a/include/imu/factor/factor_imu.h
+++ b/include/imu/factor/factor_imu.h
@@ -25,7 +25,6 @@
//Wolf includes
#include "imu/feature/feature_imu.h"
#include "imu/sensor/sensor_imu.h"
-#include "imu/processor/processor_imu.h"
#include <core/factor/factor_autodiff.h>
#include <core/math/rotations.h>
@@ -174,7 +173,6 @@ inline FactorImu::FactorImu(const FeatureImuPtr& _ftr_ptr,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_ftr_ptr->getFrame()->getV()),
- processor_imu_(std::static_pointer_cast<ProcessorImu>(_processor_ptr)),
dp_preint_(_ftr_ptr->getMeasurement().head<3>()), // dp, dv, dq at preintegration time
dq_preint_(_ftr_ptr->getMeasurement().data()+3),
dv_preint_(_ftr_ptr->getMeasurement().tail<3>()),
diff --git a/include/imu/math/imu2d_tools.h b/include/imu/math/imu2d_tools.h
index 2311caf0c35f19ad6721998b41eaac08523632ca..672646233f580b1c925904528f1eff442bcbf058 100644
--- a/include/imu/math/imu2d_tools.h
+++ b/include/imu/math/imu2d_tools.h
@@ -197,7 +197,7 @@ inline void compose(const VectorComposite& d1, const VectorComposite& d2, double
inline VectorComposite compose(const VectorComposite& d1, const VectorComposite& d2, double dt)
{
- VectorComposite dc("POV", {2,1,2});
+ VectorComposite dc("POV", Vector5d::Zero(), {2,1,2});
compose(d1.at('P'), d1.at('O')(0), d1.at('V'), d2.at('P'), d2.at('O')(0), d2.at('V'), dt, dc['P'], dc['O'](0), dc['V']);
return dc;
}
@@ -490,7 +490,7 @@ inline VectorComposite composeOverState(const VectorComposite& x,
const VectorComposite& d,
T dt)
{
- VectorComposite ret("POV", {2,1,2});
+ VectorComposite ret("POV", Vector5d::Zero(), {2,1,2});
composeOverState(x, d, dt, ret);
return ret;
@@ -502,7 +502,7 @@ inline VectorComposite composeOverStateWithGravity(const VectorComposite& x,
T dt,
const VectorComposite& g)
{
- VectorComposite ret("POV", {2,1,2});
+ VectorComposite ret("POV", Vector5d::Zero(), {2,1,2});
composeOverStateWithGravity(x, d, dt, g, ret);
return ret;
diff --git a/include/imu/math/imu_tools.h b/include/imu/math/imu_tools.h
index 68b49dc327204409ab35cbe24003e8ce60ed653a..60a0a00f30715a5601ffb8822e475fb1e4553a0e 100644
--- a/include/imu/math/imu_tools.h
+++ b/include/imu/math/imu_tools.h
@@ -221,7 +221,7 @@ inline void compose(const VectorComposite& d1, const VectorComposite& d2, double
inline VectorComposite compose(const VectorComposite& d1, const VectorComposite& d2, double dt)
{
- VectorComposite dc("POV", {3,4,3});
+ VectorComposite dc("POV", Vector10d::Zero(), {3,4,3});
compose(d1.at('P'), d1.at('O'), d1.at('V'), d2.at('P'), d2.at('O'), d2.at('V'), dt, dc['P'], dc['O'], dc['V']);
return dc;
}
@@ -465,7 +465,7 @@ inline VectorComposite composeOverState(const VectorComposite& x,
const VectorComposite& d,
T dt)
{
- VectorComposite ret("POV", {3,4,3});
+ VectorComposite ret("POV", Vector10d::Zero(), {3,4,3});
composeOverState(x, d, dt, ret);
return ret;
@@ -619,7 +619,7 @@ inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorCom
inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
{
- VectorComposite res("POV", {3,4,3});
+ VectorComposite res("POV", Vector10d::Zero(), {3,4,3});
plus(x, tau, res);
diff --git a/include/imu/processor/processor_compass.h b/include/imu/processor/processor_compass.h
index ca1cd6e725b63848706fb2f9a25c342fed2277ce..e24f1aeefaf5f5d11a3eb5cfe81b762abd688b49 100644
--- a/include/imu/processor/processor_compass.h
+++ b/include/imu/processor/processor_compass.h
@@ -31,8 +31,8 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorCompass);
struct ParamsProcessorCompass : public ParamsProcessorBase
{
ParamsProcessorCompass() = default;
- ParamsProcessorCompass(std::string _unique_name, const ParamsServer& _server):
- ParamsProcessorBase(_unique_name, _server)
+ ParamsProcessorCompass(const YAML::Node& _n):
+ ParamsProcessorBase(_n)
{
//
}
diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index 0fb04fd3fe13363407da78b03d3b9bb7d05a081e..766cf7bf5f6460355ef723dc50125d5f37ac2f51 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -45,25 +45,23 @@ struct ParamsProcessorImu : public ParamsProcessorMotion
double bootstrap_averaging_length;
ParamsProcessorImu() = default;
- ParamsProcessorImu(std::string _unique_name, const ParamsServer& _server):
- ParamsProcessorMotion(_unique_name, _server)
+ ParamsProcessorImu(const YAML::Node& _n):
+ ParamsProcessorMotion(_n)
{
- bootstrap_enable = _server.getParam<bool>(prefix + _unique_name + "/bootstrap/enable");
- if (_server.hasParam(prefix + _unique_name + "/bootstrap/method"))
+ bootstrap_enable = _n["bootstrap"]["enable"].as<bool>();
+ if (_n["bootstrap"]["method"])
{
- string str = _server.getParam<string>(prefix + _unique_name + "/bootstrap/method");
+ string str = _n["bootstrap"]["method"].as<string>();
std::transform(str.begin(), str.end(), str.begin(), ::toupper);
if (str == "STATIC" /**/)
{
bootstrap_method = BOOTSTRAP_STATIC;
- bootstrap_averaging_length =
- _server.getParam<double>(prefix + _unique_name + "/bootstrap/averaging_length");
+ bootstrap_averaging_length = _n["bootstrap"]["averaging_length"].as<double>();
}
if (str == "G" /* */)
{
bootstrap_method = BOOTSTRAP_G;
- bootstrap_averaging_length =
- _server.getParam<double>(prefix + _unique_name + "/bootstrap/averaging_length");
+ bootstrap_averaging_length = _n["bootstrap"]["averaging_length"].as<double>();
}
if (str == "V0_G" /* */)
{
@@ -152,7 +150,7 @@ class ProcessorImu : public ProcessorMotion{
protected:
ParamsProcessorImuPtr params_motion_Imu_;
std::list<FactorBasePtr> bootstrap_factor_list_; ///< List of all IMU factors created while IMU is bootstrapping
- SensorImuPtr sensor_imu_;
+ SensorImu3dPtr sensor_imu_;
Matrix6d imu_drift_cov_;
};
}
diff --git a/include/imu/processor/processor_imu2d.h b/include/imu/processor/processor_imu2d.h
index f8f4a547838bc7e13db224f915cb8d8934ea7975..8780c32e6a3b24512d1ff8f85f4dbba9a00bd97c 100644
--- a/include/imu/processor/processor_imu2d.h
+++ b/include/imu/processor/processor_imu2d.h
@@ -23,7 +23,7 @@
#define PROCESSOR_IMU2D_H
// Wolf
-#include "imu/sensor/sensor_imu2d.h"
+#include "imu/sensor/sensor_imu.h"
#include "imu/capture/capture_imu.h"
#include "imu/feature/feature_imu.h"
#include <core/processor/processor_motion.h>
@@ -34,8 +34,8 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorImu2d);
struct ParamsProcessorImu2d : public ParamsProcessorMotion
{
ParamsProcessorImu2d() = default;
- ParamsProcessorImu2d(std::string _unique_name, const ParamsServer& _server):
- ParamsProcessorMotion(_unique_name, _server)
+ ParamsProcessorImu2d(const YAML::Node& _n) :
+ ParamsProcessorMotion(_n)
{
//
}
diff --git a/include/imu/sensor/sensor_compass.h b/include/imu/sensor/sensor_compass.h
index 52e9bd66eefb46b8dce8372a24e7daedb9cb87ba..8f961524bc937486ded115ac80598b1bb8c00723 100644
--- a/include/imu/sensor/sensor_compass.h
+++ b/include/imu/sensor/sensor_compass.h
@@ -24,7 +24,6 @@
//wolf includes
#include <core/sensor/sensor_base.h>
-#include <core/utils/params_server.h>
#include <iostream>
@@ -37,10 +36,10 @@ struct ParamsSensorCompass : public ParamsSensorBase
double stdev_noise;
ParamsSensorCompass() = default;
- ParamsSensorCompass(std::string _unique_name, const wolf::ParamsServer& _server)
- : ParamsSensorBase(_unique_name, _server)
+ ParamsSensorCompass(const YAML::Node& _input_node)
+ : ParamsSensorBase(_input_node)
{
- stdev_noise = _server.getParam<double>(prefix + _unique_name + "/stdev_noise");
+ stdev_noise = _input_node["stdev_noise"].as<double>();
}
~ParamsSensorCompass() override = default;
std::string print() const override
@@ -55,20 +54,12 @@ WOLF_PTR_TYPEDEFS(SensorCompass);
class SensorCompass : public SensorBase
{
protected:
- SizeEigen dim_;
ParamsSensorCompassPtr params_compass_;
public:
- SensorCompass(const std::string& _unique_name,
- const SizeEigen& _dim,
- ParamsSensorCompassPtr _params,
+ SensorCompass(ParamsSensorCompassPtr _params,
const SpecSensorComposite& _priors);
- SensorCompass(const std::string& _unique_name,
- const SizeEigen& _dim,
- ParamsSensorCompassPtr _params,
- const ParamsServer& _server);
-
WOLF_SENSOR_CREATE(SensorCompass, ParamsSensorCompass);
~SensorCompass() override;
diff --git a/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h
index f3f87e5387daca511c975ba979aa1eed6a3fa47a..87865841c71d6494756c9ffc27ca0b7ace9607fa 100644
--- a/include/imu/sensor/sensor_imu.h
+++ b/include/imu/sensor/sensor_imu.h
@@ -19,14 +19,10 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#ifndef SENSOR_IMU_H
-#define SENSOR_IMU_H
+#pragma once
//wolf includes
#include <core/sensor/sensor_base.h>
-#include <core/utils/params_server.h>
-
-#include <iostream>
namespace wolf {
@@ -43,15 +39,14 @@ struct ParamsSensorImu : public ParamsSensorBase
bool orthogonal_gravity = true;
ParamsSensorImu() = default;
- ParamsSensorImu(std::string _unique_name, const ParamsServer& _server):
- ParamsSensorBase(_unique_name, _server)
+ ParamsSensorImu(const YAML::Node& _input_node):
+ ParamsSensorBase(_input_node)
{
- w_noise = _server.getParam<double>(prefix + _unique_name + "/w_noise");
- a_noise = _server.getParam<double>(prefix + _unique_name + "/a_noise");
+ w_noise = _input_node["w_noise"].as<double>();
+ a_noise = _input_node["a_noise"].as<double>();
- if (_server.hasParam(prefix + _unique_name + "/orthogonal_gravity"))
- orthogonal_gravity = _server.getParam<bool>(prefix + _unique_name + "/orthogonal_gravity");
-
+ if (_input_node["orthogonal_gravity"])
+ orthogonal_gravity = _input_node["orthogonal_gravity"].as<bool>();
}
~ParamsSensorImu() override = default;
std::string print() const override
@@ -63,27 +58,25 @@ struct ParamsSensorImu : public ParamsSensorBase
}
};
-WOLF_PTR_TYPEDEFS(SensorImu);
-
+template <unsigned int DIM>
class SensorImu : public SensorBase
{
protected:
- SizeEigen dim_;
ParamsSensorImuPtr params_imu_;
public:
- SensorImu(const std::string& _unique_name,
- const SizeEigen& _dim,
- ParamsSensorImuPtr _params,
- const SpecSensorComposite& _priors);
+ SensorImu(ParamsSensorImuPtr _params,
+ const SpecSensorComposite& _priors) :
+ SensorBase("SensorImu",
+ DIM,
+ _params,
+ _priors("POI")),
+ params_imu_(_params)
+ {
+ };
- SensorImu(const std::string& _unique_name,
- const SizeEigen& _dim,
- ParamsSensorImuPtr _params,
- const ParamsServer& _server);
-
- WOLF_SENSOR_CREATE(SensorImu, ParamsSensorImu);
+ WOLF_SENSOR_CREATE(SensorImu<DIM>, ParamsSensorImu);
double getGyroNoise() const;
double getAccelNoise() const;
@@ -95,25 +88,59 @@ class SensorImu : public SensorBase
Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override;
- ~SensorImu() override;
+ ~SensorImu() override
+ {
+ };
};
-inline double SensorImu::getGyroNoise() const
+template <unsigned int DIM>
+inline double SensorImu<DIM>::getGyroNoise() const
{
return params_imu_->w_noise;
}
-inline double SensorImu::getAccelNoise() const
+template <unsigned int DIM>
+inline double SensorImu<DIM>::getAccelNoise() const
{
return params_imu_->a_noise;
}
-inline bool SensorImu::isGravityOrthogonal() const
+template <unsigned int DIM>
+inline bool SensorImu<DIM>::isGravityOrthogonal() const
{
return params_imu_->orthogonal_gravity;
}
+template <>
+inline Eigen::MatrixXd SensorImu<2>::computeNoiseCov(const Eigen::VectorXd & _data) const
+{
+ return (Eigen::Vector3d() << params_imu_->a_noise,
+ params_imu_->a_noise,
+ params_imu_->w_noise).finished().cwiseAbs2().asDiagonal();
+}
+
+template <>
+inline Eigen::MatrixXd SensorImu<3>::computeNoiseCov(const Eigen::VectorXd & _data) const
+{
+ return (Eigen::Vector6d() << params_imu_->a_noise,
+ params_imu_->a_noise,
+ params_imu_->a_noise,
+ params_imu_->w_noise,
+ params_imu_->w_noise,
+ params_imu_->w_noise).finished().cwiseAbs2().asDiagonal();
+}
+
+typedef SensorImu<2> SensorImu2d;
+typedef SensorImu<3> SensorImu3d;
+WOLF_DECLARED_PTR_TYPEDEFS(SensorImu2d);
+WOLF_DECLARED_PTR_TYPEDEFS(SensorImu3d);
+
} // namespace wolf
-#endif // SENSOR_IMU_H
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorImu2d);
+WOLF_REGISTER_SENSOR(SensorImu3d);
+}
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index 371e97b60b7137a35b8c7b440dffa319620ba19a..d5809731a5e530177267647f312ea0ff0f977852 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -31,7 +31,9 @@
namespace wolf {
ProcessorImu::ProcessorImu(ParamsProcessorImuPtr _params_motion_imu) :
- ProcessorMotion("ProcessorImu", "POV", 3, 10, 10, 9, 6, 6, _params_motion_imu),
+ ProcessorMotion("ProcessorImu",
+ {{'P',"StatePoint3d"},{'O',"StateQuaternion"},{'V',"StateVector3d"}},
+ 3, 10, 10, 9, 6, 6, _params_motion_imu),
params_motion_Imu_(_params_motion_imu)
{
bootstrapping_ = params_motion_Imu_->bootstrap_enable;
@@ -109,7 +111,7 @@ void ProcessorImu::setCalibration (const CaptureBasePtr _capture, const VectorXd
void ProcessorImu::configure(SensorBasePtr _sensor)
{
- sensor_imu_ = std::static_pointer_cast<SensorImu>(_sensor);
+ sensor_imu_ = std::static_pointer_cast<SensorImu3d>(_sensor);
auto acc_drift_std = sensor_imu_->getAbRateStdev();
auto gyro_drift_std = sensor_imu_->getAbRateStdev();
@@ -245,7 +247,7 @@ void ProcessorImu::statePlusDelta(const VectorComposite& _x,
assert(_delta.size() == 10 && "Wrong _delta vector size");
assert(_dt >= 0 && "Time interval _dt is negative!");
- const auto& delta = VectorComposite(_delta, "POV", {3,4,3});
+ const auto& delta = VectorComposite("POV", _delta, {3,4,3});
/* MATH according to Sola-16
*
@@ -307,7 +309,7 @@ void ProcessorImu::bootstrap()
CaptureBasePtr first_capture = bootstrapOrigin();
TimeStamp t_current = last_ptr_->getBuffer().back().ts_;
- VectorComposite transfo_w_l("PO");
+ VectorComposite transfo_w_l;
switch (params_motion_Imu_->bootstrap_method)
{
case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_STATIC:
@@ -476,7 +478,7 @@ bool ProcessorImu::recomputeStates() const
const auto& frm = cap->getFrame();
const auto& cap_origin = cap->getOriginCapture();
const auto& frm_origin = cap_origin->getFrame();
- const auto& delta = VectorComposite(ftr->getMeasurement(), "POV", {3, 4, 3});
+ const auto& delta = VectorComposite("POV", ftr->getMeasurement(), {3, 4, 3});
const auto& x_origin = frm_origin->getState();
auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp();
auto x = imu::composeOverState(x_origin, delta, dt);
diff --git a/src/processor/processor_imu2d.cpp b/src/processor/processor_imu2d.cpp
index 8e2d532adf16605a1a0f67c1d439d8dcbe47e8e9..8db279c69c55283a3fcf37904ccc04593690067b 100644
--- a/src/processor/processor_imu2d.cpp
+++ b/src/processor/processor_imu2d.cpp
@@ -19,15 +19,10 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/*
- * processor_imu2d.cpp
- *
- * Created on: Nov 16, 2020
- * Author: igeer
- */
+
// imu
#include "imu/processor/processor_imu2d.h"
-#include "imu/sensor/sensor_imu2d.h"
+#include "imu/sensor/sensor_imu.h"
#include "imu/factor/factor_imu2d.h"
#include "imu/factor/factor_imu2d_with_gravity.h"
#include "imu/math/imu2d_tools.h"
@@ -39,7 +34,10 @@
namespace wolf {
ProcessorImu2d::ProcessorImu2d(ParamsProcessorImu2dPtr _params_motion_imu) :
- ProcessorMotion("ProcessorImu2d", "POV", 2, 5, 5, 5, 6, 3, _params_motion_imu),
+ ProcessorMotion("ProcessorImu2d",
+ {{'P',"StatePoint2d"},{'O',"StateAngle"},{'V',"StateVector2d"}},
+ 2, 5, 5, 5, 6, 3,
+ _params_motion_imu),
params_motion_Imu_(std::make_shared<ParamsProcessorImu2d>(*_params_motion_imu))
{
//
@@ -170,10 +168,18 @@ void ProcessorImu2d::configure(SensorBasePtr _sensor)
shared_from_this(), // this processor
params_->apply_loss_function); // loss function
- if( std::static_pointer_cast<SensorImu>(getSensor())->isGravityOrthogonal() )
- return FactorBase::emplace<FactorImu2d>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+ if( std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal() )
+ FactorBase::emplace<FactorImu2d>(ftr_imu,
+ ftr_imu,
+ cap_imu,
+ shared_from_this(),
+ params_->apply_loss_function);
else
- return FactorBase::emplace<FactorImu2dWithGravity>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+ FactorBase::emplace<FactorImu2dWithGravity>(ftr_imu,
+ ftr_imu,
+ cap_imu,
+ shared_from_this(),
+ params_->apply_loss_function);
}
}
}
@@ -247,17 +253,17 @@ void ProcessorImu2d::configure(SensorBasePtr _sensor)
const double _dt,
VectorComposite& _x_plus_delta) const
{
- assert(_x.includesStructure("POV") && "Any key missing in _x");
+ assert(_x.has("POV") && "Any key missing in _x");
assert(_delta.size() == 5 && "Wrong _delta vector size");
assert(_dt >= 0 && "Time interval _dt is negative!");
- const auto& delta = VectorComposite(_delta, "POV", {2,1,2});
+ const auto& delta = VectorComposite("POV", _delta, {2,1,2});
/*
* MATHS:
*
* In the absence of gravity (2D case) it's the same as deltaPlusDelta(), so the same compose() function is used
*/
- if( std::static_pointer_cast<const SensorImu>(getSensor())->isGravityOrthogonal() )
+ if( std::static_pointer_cast<const SensorImu2d>(getSensor())->isGravityOrthogonal() )
_x_plus_delta = imu2d::composeOverState(_x, delta, _dt);
else
_x_plus_delta = imu2d::composeOverStateWithGravity(_x, delta, _dt, getSensor()->getState("G"));
diff --git a/src/sensor/sensor_compass.cpp b/src/sensor/sensor_compass.cpp
index 7454777137ccaaec749d478cc363a030284387e4..d0416bf6a3e5d920b923b0be0ee1a23e8fa464a1 100644
--- a/src/sensor/sensor_compass.cpp
+++ b/src/sensor/sensor_compass.cpp
@@ -26,41 +26,12 @@
namespace wolf
{
-SpecStates specs_states_compass_2d{{'I',SpecState("StateBlock", 2,
- "Vector containing the compass biases (x, y)")},
- {'F',SpecState("StateBlock", 2,
- "Vector containing the magnetic field (x, y)")}};
-SpecStates specs_states_compass_3d{{'I',SpecState("StateBlock", 3,
- "Vector containing the compass biases (x, y, z)")},
- {'F',SpecState("StateBlock", 3,
- "Vector containing the magnetic field (x, y, z)")}};
-
-SensorCompass::SensorCompass(const std::string& _unique_name,
- const SizeEigen& _dim,
- ParamsSensorCompassPtr _params,
+SensorCompass::SensorCompass(ParamsSensorCompassPtr _params,
const SpecSensorComposite& _priors) :
SensorBase("SensorCompass",
- _unique_name,
- _dim,
- _params,
- _priors,
- _dim == 2 ? specs_states_compass_2d : specs_states_compass_3d),
- dim_(_dim),
- params_compass_(_params)
-{
-}
-
-SensorCompass::SensorCompass(const std::string& _unique_name,
- const SizeEigen& _dim,
- ParamsSensorCompassPtr _params,
- const ParamsServer& _server) :
- SensorBase("SensorCompass",
- _unique_name,
- _dim,
+ 3,
_params,
- _server,
- _dim == 2 ? specs_states_compass_2d : specs_states_compass_3d),
- dim_(_dim),
+ _priors("PO")),
params_compass_(_params)
{
}
@@ -71,7 +42,7 @@ SensorCompass::~SensorCompass()
Eigen::MatrixXd SensorCompass::computeNoiseCov(const Eigen::VectorXd& _data) const
{
- return Eigen::VectorXd::Constant(dim_, pow(params_compass_->stdev_noise,2)).asDiagonal();
+ return Eigen::Vector3d::Constant(pow(params_compass_->stdev_noise,2)).asDiagonal();
}
} /* namespace wolf */
diff --git a/src/sensor/sensor_imu.cpp b/src/sensor/sensor_imu.cpp
deleted file mode 100644
index 954416b69db81b36b4a7bc28e72d9a8e526e1023..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_imu.cpp
+++ /dev/null
@@ -1,91 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#include "imu/sensor/sensor_imu.h"
-
-namespace wolf {
-
-SpecStates specs_states_imu_2d_g{{'I',SpecState("StateBlock", 3,
- "Vector containing accel. biases (x,y) and gyro bias (z)")},
- {'G',SpecState("StateBlock", 2,
- "(ONLY IF orthogonal_gravity = false) Vector containing the gravity projected to axes x and y")}};
-SpecStates specs_states_imu_2d{{'I',SpecState("StateBlock", 3,
- "Vector containing accel. biases (x,y) and gyro bias (z)")}};
-SpecStates specs_states_imu_3d{{'I',SpecState("StateBlock", 6,
- "Vector containing accel. biases (x,y,z) and gyro biases (x, y, z)")}};
-
-SensorImu::SensorImu(const std::string& _unique_name,
- const SizeEigen& _dim,
- ParamsSensorImuPtr _params,
- const SpecSensorComposite& _priors) :
- SensorBase("SensorImu",
- _unique_name,
- _dim,
- _params,
- _priors,
- _dim == 2 ? (_params->orthogonal_gravity ? specs_states_imu_2d : specs_states_imu_2d_g) : specs_states_imu_3d),
- dim_(_dim),
- params_imu_(_params)
-{
-}
-
-SensorImu::SensorImu(const std::string& _unique_name,
- const SizeEigen& _dim,
- ParamsSensorImuPtr _params,
- const ParamsServer& _server) :
- SensorBase("SensorImu",
- _unique_name,
- _dim,
- _params,
- _server,
- _dim == 2 ? (_params->orthogonal_gravity ? specs_states_imu_2d : specs_states_imu_2d_g) : specs_states_imu_3d),
- dim_(_dim),
- params_imu_(_params)
-{
-}
-
-SensorImu::~SensorImu()
-{
- //
-}
-
-Eigen::MatrixXd SensorImu::computeNoiseCov(const Eigen::VectorXd & _data) const
-{
- if (dim_ == 2)
- return (Eigen::Vector3d() << params_imu_->a_noise,
- params_imu_->a_noise,
- params_imu_->w_noise).finished().cwiseAbs2().asDiagonal();
- else
- return (Eigen::Vector6d() << params_imu_->a_noise,
- params_imu_->a_noise,
- params_imu_->a_noise,
- params_imu_->w_noise,
- params_imu_->w_noise,
- params_imu_->w_noise).finished().cwiseAbs2().asDiagonal();
-}
-
-} // namespace wolf
-
-// Register in the FactorySensor
-#include "core/sensor/factory_sensor.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR(SensorImu)
-} // namespace wolf