From ec353d364d02173f9ab3d5b9ee97b5640ad866f2 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Mon, 19 Jun 2023 11:41:54 +0200
Subject: [PATCH] wip
---
include/imu/capture/capture_compass.h | 8 -
include/imu/factor/factor_imu.h | 117 ++++----
include/imu/sensor/sensor_compass.h | 6 +
include/imu/sensor/sensor_imu.h | 103 ++++---
schema/sensor/SensorCompass.schema | 9 +-
schema/sensor/SensorImu2d.schema | 11 +-
schema/sensor/SensorImu3d.schema | 11 +-
src/processor/processor_imu.cpp | 2 +-
src/processor/processor_imu2d.cpp | 389 +++++++++++++-------------
src/sensor/sensor_compass.cpp | 21 +-
10 files changed, 335 insertions(+), 342 deletions(-)
diff --git a/include/imu/capture/capture_compass.h b/include/imu/capture/capture_compass.h
index fa0904605..6c2b21f2d 100644
--- a/include/imu/capture/capture_compass.h
+++ b/include/imu/capture/capture_compass.h
@@ -58,10 +58,6 @@ class CaptureCompass : public CaptureBase
auto sensor_compass = std::dynamic_pointer_cast<SensorCompass>(_sensor_ptr);
assert(sensor_compass != nullptr && "CaptureCompass: Sensor should be of type SensorCompass");
data_covariance_ = sensor_compass->computeNoiseCov(data_);
-
- // FIXME: make this generic in SensorBase
- if (_sensor_ptr->isStateBlockDynamic('F'))
- addStateBlock('F',std::make_shared<StateParams3>(Eigen::Vector3d::Zero(), false), _sensor_ptr->getProblem());
}
CaptureCompass(const TimeStamp& _ts,
@@ -80,10 +76,6 @@ class CaptureCompass : public CaptureBase
data_covariance_(_data_covariance)
{
assert((_sensor_ptr == nullptr or std::dynamic_pointer_cast<SensorCompass>(_sensor_ptr) != nullptr) && "CaptureCompass: Sensor should be of type SensorCompass");
-
- // FIXME: make this generic in SensorBase
- if (_sensor_ptr->isStateBlockDynamic('F'))
- addStateBlock('F',std::make_shared<StateParams3>(Eigen::Vector3d::Zero(), false), _sensor_ptr->getProblem());
}
~CaptureCompass() override
diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h
index 20881ad1e..9f4cd187d 100644
--- a/include/imu/factor/factor_imu.h
+++ b/include/imu/factor/factor_imu.h
@@ -36,8 +36,8 @@ WOLF_PTR_TYPEDEFS(FactorImu);
class FactorImu : public FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>
{
public:
- FactorImu(const FeatureImuPtr & _ftr_ptr,
- const CaptureImuPtr & _capture_origin_ptr,
+ FactorImu(const FeatureImuPtr &_ftr_ptr,
+ const CaptureImuPtr &_capture_origin_ptr,
const ProcessorBasePtr &_processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
@@ -60,7 +60,7 @@ class FactorImu : public FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>
const T *const _p2,
const T *const _o2,
const T *const _v2,
- T * _res) const;
+ T *_res) const;
/** \brief : compute the residual from the state blocks being iterated by the
solver. (same as operator())
@@ -82,15 +82,15 @@ class FactorImu : public FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>
vector... NOT A QUATERNION
*/
template <typename D1, typename D2, typename D3>
- bool residual(const Eigen::MatrixBase<D1> & _p1,
+ bool residual(const Eigen::MatrixBase<D1> &_p1,
const Eigen::QuaternionBase<D2> &_q1,
- const Eigen::MatrixBase<D1> & _v1,
- const Eigen::MatrixBase<D1> & _ab1,
- const Eigen::MatrixBase<D1> & _wb1,
- const Eigen::MatrixBase<D1> & _p2,
+ const Eigen::MatrixBase<D1> &_v1,
+ const Eigen::MatrixBase<D1> &_ab1,
+ const Eigen::MatrixBase<D1> &_wb1,
+ const Eigen::MatrixBase<D1> &_p2,
const Eigen::QuaternionBase<D2> &_q2,
- const Eigen::MatrixBase<D1> & _v2,
- Eigen::MatrixBase<D3> & _res) const;
+ const Eigen::MatrixBase<D1> &_v2,
+ Eigen::MatrixBase<D3> &_res) const;
/** Function expectation(...)
* params :
@@ -106,16 +106,16 @@ class FactorImu : public FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>
* _ve : expected velocity delta
*/
template <typename D1, typename D2, typename D3, typename D4>
- void expectation(const Eigen::MatrixBase<D1> & _p1,
+ void expectation(const Eigen::MatrixBase<D1> &_p1,
const Eigen::QuaternionBase<D2> &_q1,
- const Eigen::MatrixBase<D1> & _v1,
- const Eigen::MatrixBase<D1> & _p2,
+ const Eigen::MatrixBase<D1> &_v1,
+ const Eigen::MatrixBase<D1> &_p2,
const Eigen::QuaternionBase<D2> &_q2,
- const Eigen::MatrixBase<D1> & _v2,
+ const Eigen::MatrixBase<D1> &_v2,
typename D1::Scalar _dt,
- Eigen::MatrixBase<D3> & _pe,
- Eigen::QuaternionBase<D4> & _qe,
- Eigen::MatrixBase<D3> & _ve) const;
+ Eigen::MatrixBase<D3> &_pe,
+ Eigen::QuaternionBase<D4> &_qe,
+ Eigen::MatrixBase<D3> &_ve) const;
/** \brief : return the expected delta given the state blocks in the wolf tree
*/
@@ -147,8 +147,8 @@ class FactorImu : public FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>
///////////////////// IMPLEMENTAITON ////////////////////////////
-inline FactorImu::FactorImu(const FeatureImuPtr & _ftr_ptr,
- const CaptureImuPtr & _cap_origin_ptr,
+inline FactorImu::FactorImu(const FeatureImuPtr &_ftr_ptr,
+ const CaptureImuPtr &_cap_origin_ptr,
const ProcessorBasePtr &_processor_ptr,
bool _apply_loss_function,
FactorStatus _status)
@@ -175,7 +175,7 @@ inline FactorImu::FactorImu(const FeatureImuPtr & _ftr_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>()),
- acc_bias_preint_(_ftr_ptr->getAccBiasPreint()), // state biases at preintegration time
+ acc_bias_preint_(_ftr_ptr->getAccBiasPreint()), // state biases at preintegration time
gyro_bias_preint_(_ftr_ptr->getGyroBiasPreint()),
dDp_dab_(_ftr_ptr->getJacobianBias().block(0, 0, 3, 3)), // Jacs of dp dv dq wrt biases
dDv_dab_(_ftr_ptr->getJacobianBias().block(6, 0, 3, 3)),
@@ -195,7 +195,7 @@ inline bool FactorImu::operator()(const T *const _p1,
const T *const _p2,
const T *const _q2,
const T *const _v2,
- T * _res) const
+ T *_res) const
{
using namespace Eigen;
@@ -218,15 +218,15 @@ inline bool FactorImu::operator()(const T *const _p1,
}
template <typename D1, typename D2, typename D3>
-inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
+inline bool FactorImu::residual(const Eigen::MatrixBase<D1> &_p1,
const Eigen::QuaternionBase<D2> &_q1,
- const Eigen::MatrixBase<D1> & _v1,
- const Eigen::MatrixBase<D1> & _ab1,
- const Eigen::MatrixBase<D1> & _wb1,
- const Eigen::MatrixBase<D1> & _p2,
+ const Eigen::MatrixBase<D1> &_v1,
+ const Eigen::MatrixBase<D1> &_ab1,
+ const Eigen::MatrixBase<D1> &_wb1,
+ const Eigen::MatrixBase<D1> &_p2,
const Eigen::QuaternionBase<D2> &_q2,
- const Eigen::MatrixBase<D1> & _v2,
- Eigen::MatrixBase<D3> & _res) const
+ const Eigen::MatrixBase<D1> &_v2,
+ Eigen::MatrixBase<D3> &_res) const
{
/* Help for the Imu residual function
*
@@ -286,15 +286,15 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
// 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias *
// (bias_current - bias_preint)
-#ifdef METHOD_1 // method 1
+#ifdef METHOD_1 // method 1
// 2.a. Compute the delta step in tangent space: step = J_bias * (bias -
// bias_preint)
Eigen::Matrix<T, 3, 1> dp_step, do_step, dv_step;
- d_step.block(0, 0, 3, 1) = dDp_dab_ * (_ab1 - acc_bias_preint_) + dDp_dwb_ * (_wb1 - gyro_bias_preint_);
- d_step.block(3, 0, 3, 1) = /* it happens that dDq_dab = 0 ! */ dDq_dwb_ * (_wb1 - gyro_bias_preint_);
- d_step.block(6, 0, 3, 1) = dDv_dab_ * (_ab1 - acc_bias_preint_) + dDv_dwb_ * (_wb1 - gyro_bias_preint_);
+ dp_step = dDp_dab_ * (_ab1 - acc_bias_preint_) + dDp_dwb_ * (_wb1 - gyro_bias_preint_);
+ do_step = /* it happens that dDq_dab = 0 ! */ dDq_dwb_ * (_wb1 - gyro_bias_preint_);
+ dv_step = dDv_dab_ * (_ab1 - acc_bias_preint_) + dDv_dwb_ * (_wb1 - gyro_bias_preint_);
// 2.b. Add the correction step to the preintegrated delta: delta_cor =
// delta_preint (+) step
@@ -340,26 +340,26 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
#else // method 2
-Eigen::Matrix<T, 9, 1> d_diff;
-Eigen::Map<Eigen::Matrix<T, 3, 1>> dp_diff(d_diff.data());
-Eigen::Map<Eigen::Matrix<T, 3, 1>> do_diff(d_diff.data() + 3);
-Eigen::Map<Eigen::Matrix<T, 3, 1>> dv_diff(d_diff.data() + 6);
+ Eigen::Matrix<T, 9, 1> d_diff;
+ Eigen::Map<Eigen::Matrix<T, 3, 1>> dp_diff(d_diff.data());
+ Eigen::Map<Eigen::Matrix<T, 3, 1>> do_diff(d_diff.data() + 3);
+ Eigen::Map<Eigen::Matrix<T, 3, 1>> dv_diff(d_diff.data() + 6);
-imu::diff(dp_preint_.cast<T>(),
- dq_preint_.cast<T>(),
- dv_preint_.cast<T>(),
- dp_exp,
- dq_exp,
- dv_exp,
- dp_diff,
- do_diff,
- dv_diff);
+ imu::diff(dp_preint_.cast<T>(),
+ dq_preint_.cast<T>(),
+ dv_preint_.cast<T>(),
+ dp_exp,
+ dq_exp,
+ dv_exp,
+ dp_diff,
+ do_diff,
+ dv_diff);
-Eigen::Matrix<T, 9, 1> d_error;
-d_error << d_diff - d_step;
+ Eigen::Matrix<T, 9, 1> d_error;
+ d_error << d_diff - d_step;
-// 4. Residuals are the weighted errors
-_res = getMeasurementSquareRootInformationUpper() * d_error;
+ // 4. Residuals are the weighted errors
+ _res = getMeasurementSquareRootInformationUpper() * d_error;
#endif
@@ -429,8 +429,9 @@ _res = getMeasurementSquareRootInformationUpper() * d_error;
inline Eigen::VectorXd FactorImu::expectation() const
{
- auto frm_current = getFeature()->getFrame();
- auto frm_previous = getFrameOther();
+ // There are only 2 factored frames: origin and current
+ auto frm_previous = getFramesFactored().front().lock();
+ auto frm_current = getFramesFactored().back().lock();
// get information on current_frame in the FactorImu
const Eigen::Vector3d frame_current_pos = (frm_current->getP()->getState());
@@ -463,16 +464,16 @@ inline Eigen::VectorXd FactorImu::expectation() const
}
template <typename D1, typename D2, typename D3, typename D4>
-inline void FactorImu::expectation(const Eigen::MatrixBase<D1> & _p1,
+inline void FactorImu::expectation(const Eigen::MatrixBase<D1> &_p1,
const Eigen::QuaternionBase<D2> &_q1,
- const Eigen::MatrixBase<D1> & _v1,
- const Eigen::MatrixBase<D1> & _p2,
+ const Eigen::MatrixBase<D1> &_v1,
+ const Eigen::MatrixBase<D1> &_p2,
const Eigen::QuaternionBase<D2> &_q2,
- const Eigen::MatrixBase<D1> & _v2,
+ const Eigen::MatrixBase<D1> &_v2,
typename D1::Scalar _dt,
- Eigen::MatrixBase<D3> & _pe,
- Eigen::QuaternionBase<D4> & _qe,
- Eigen::MatrixBase<D3> & _ve) const
+ Eigen::MatrixBase<D3> &_pe,
+ Eigen::QuaternionBase<D4> &_qe,
+ Eigen::MatrixBase<D3> &_ve) const
{
imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, _dt, _pe, _qe, _ve);
}
diff --git a/include/imu/sensor/sensor_compass.h b/include/imu/sensor/sensor_compass.h
index 4ca89da55..ac8b90f13 100644
--- a/include/imu/sensor/sensor_compass.h
+++ b/include/imu/sensor/sensor_compass.h
@@ -40,6 +40,12 @@ struct ParamsSensorCompass : public ParamsSensorBase
stdev_noise = _input_node["stdev_noise"].as<double>();
}
~ParamsSensorCompass() override = default;
+
+ std::string getStatesKeys() const override
+ {
+ return "POIF";
+ }
+
std::string print() const override
{
return ParamsSensorBase::print() + "\n" + "stdev_noise: " + toString(stdev_noise) + "\n";
diff --git a/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h
index 0c8ce27ed..b738d16cb 100644
--- a/include/imu/sensor/sensor_imu.h
+++ b/include/imu/sensor/sensor_imu.h
@@ -20,74 +20,66 @@
#pragma once
-//wolf includes
+// wolf includes
#include "imu/common/imu.h"
#include <core/sensor/sensor_base.h>
-namespace wolf {
+namespace wolf
+{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorImu);
-
struct ParamsSensorImu : public ParamsSensorBase
{
- //noise std dev
- double w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
- double a_noise = 0.04; //standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
-
+ // noise std dev
+ double w_noise = 0.001; // standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
+ double a_noise = 0.04; // standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
+
// OPTIONAL Is the 2D plane orthogonal to gravity? (only for 2D cases)
bool orthogonal_gravity = true;
ParamsSensorImu() = default;
- ParamsSensorImu(const YAML::Node& _input_node):
- ParamsSensorBase(_input_node)
+ ParamsSensorImu(const YAML::Node& _input_node) : ParamsSensorBase(_input_node)
{
w_noise = _input_node["w_noise"].as<double>();
a_noise = _input_node["a_noise"].as<double>();
-
- if (_input_node["orthogonal_gravity"])
- orthogonal_gravity = _input_node["orthogonal_gravity"].as<bool>();
+
+ if (_input_node["orthogonal_gravity"]) orthogonal_gravity = _input_node["orthogonal_gravity"].as<bool>();
}
~ParamsSensorImu() override = default;
+
+ std::string getStatesKeys() const override
+ {
+ return "POI";
+ }
+
std::string print() const override
{
- return ParamsSensorBase::print() + "\n"
- + "w_noise: " + std::to_string(w_noise) + "\n"
- + "a_noise: " + std::to_string(a_noise) + "\n"
- + "orthogonal_gravity: " + std::to_string(orthogonal_gravity)+ "\n";
+ return ParamsSensorBase::print() + "\n" + "w_noise: " + std::to_string(w_noise) + "\n" +
+ "a_noise: " + std::to_string(a_noise) + "\n" +
+ "orthogonal_gravity: " + std::to_string(orthogonal_gravity) + "\n";
}
};
template <unsigned int DIM>
class SensorImu : public SensorBase
{
- protected:
- ParamsSensorImuPtr params_imu_;
-
- public:
+ protected:
+ ParamsSensorImuPtr params_imu_;
- SensorImu(ParamsSensorImuPtr _params,
- const SpecStateSensorComposite& _priors) :
- SensorBase("SensorImu",
- DIM,
- _params,
- _priors("POI")),
- params_imu_(_params)
- {
- };
+ public:
+ SensorImu(ParamsSensorImuPtr _params, const SpecStateSensorComposite& _priors)
+ : SensorBase("SensorImu", DIM, _params, _priors("POI")), params_imu_(_params){};
- WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorImu, DIM, ParamsSensorImu);
+ WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorImu, DIM, ParamsSensorImu);
- double getGyroNoise() const;
- double getAccelNoise() const;
- bool isGravityOrthogonal() const;
+ double getGyroNoise() const;
+ double getAccelNoise() const;
+ bool isGravityOrthogonal() const;
- Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override;
-
- ~SensorImu() override
- {
- };
+ Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override;
+ ~SensorImu() override{};
};
template <unsigned int DIM>
@@ -105,26 +97,30 @@ inline double SensorImu<DIM>::getAccelNoise() const
template <unsigned int DIM>
inline bool SensorImu<DIM>::isGravityOrthogonal() const
{
- return DIM==2 and params_imu_->orthogonal_gravity;
+ return DIM == 2 and params_imu_->orthogonal_gravity;
}
template <>
-inline Eigen::MatrixXd SensorImu<2>::computeNoiseCov(const Eigen::VectorXd & _data) const
+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();
+ 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
+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();
+ 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;
@@ -132,11 +128,12 @@ typedef SensorImu<3> SensorImu3d;
WOLF_DECLARED_PTR_TYPEDEFS(SensorImu2d);
WOLF_DECLARED_PTR_TYPEDEFS(SensorImu3d);
-} // namespace wolf
+} // namespace wolf
// Register in the FactorySensor
#include "core/sensor/factory_sensor.h"
-namespace wolf {
+namespace wolf
+{
WOLF_REGISTER_SENSOR(SensorImu2d);
WOLF_REGISTER_SENSOR(SensorImu3d);
-}
+} // namespace wolf
diff --git a/schema/sensor/SensorCompass.schema b/schema/sensor/SensorCompass.schema
index 84aeb6058..3b9f8916b 100644
--- a/schema/sensor/SensorCompass.schema
+++ b/schema/sensor/SensorCompass.schema
@@ -15,8 +15,7 @@ states:
type:
_type: string
_mandatory: false
- _default: StateParams3
- _options: [StateParams3]
+ _value: StateParams3
_doc: The derived type of the StateBlock for compass biases
state:
_type: Vector3d
@@ -39,8 +38,7 @@ states:
type:
_type: string
_mandatory: false
- _default: StateParams3
- _options: [StateParams3]
+ _value: StateParams3
_doc: The derived type of the StateBlock for magnetic field
state:
_type: Vector3d
@@ -49,8 +47,7 @@ states:
dynamic:
_type: bool
_mandatory: false
- _default: false
- _options: [false]
+ _value: false
_doc: The magnetic field is not dynamic.
noise_std:
_type: Vector3d
diff --git a/schema/sensor/SensorImu2d.schema b/schema/sensor/SensorImu2d.schema
index 0be0668eb..5687b1694 100644
--- a/schema/sensor/SensorImu2d.schema
+++ b/schema/sensor/SensorImu2d.schema
@@ -13,15 +13,22 @@ a_noise:
states:
P:
follow: SpecStateSensorP2d.schema
+ state:
+ _type: Vector2d
+ _value: [0,0]
+ _doc: For the moment, IMU only implemented in the origin.
O:
follow: SpecStateSensorO2d.schema
+ state:
+ _type: Vector1d
+ _value: [0]
+ _doc: For the moment, IMU only implemented in the origin.
I:
follow: SpecStateSensor.schema
type:
_type: string
_mandatory: false
- _default: StateParams3
- _options: [StateParams3]
+ _value: StateParams3
_doc: The derived type of the StateBlock for IMU biases
state:
_type: Vector3d
diff --git a/schema/sensor/SensorImu3d.schema b/schema/sensor/SensorImu3d.schema
index e484cdf39..bc3bbd6d9 100644
--- a/schema/sensor/SensorImu3d.schema
+++ b/schema/sensor/SensorImu3d.schema
@@ -13,15 +13,22 @@ a_noise:
states:
P:
follow: SpecStateSensorP3d.schema
+ state:
+ _type: Vector3d
+ _value: [0,0,0]
+ _doc: For the moment, IMU only implemented in the origin.
O:
follow: SpecStateSensorO3d.schema
+ state:
+ _type: Vector4d
+ _value: [0,0,0,1]
+ _doc: For the moment, IMU only implemented in the origin.
I:
follow: SpecStateSensor.schema
type:
_type: string
_mandatory: false
- _default: StateParams6
- _options: [StateParams6]
+ _value: StateParams6
_doc: The derived type of the StateBlock for IMU biases
state:
_type: Vector6d
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index 80ad04c00..b0546defe 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -440,7 +440,7 @@ VectorXd ProcessorImu::bootstrapDelta() const
{
if (std::dynamic_pointer_cast<FactorImu>(fac) != nullptr)
{
- dt = fac->getCapture()->getTimeStamp() - fac->getCaptureOther()->getTimeStamp();
+ dt = fac->getCapture()->getTimeStamp() - fac->getCapturesFactored().front().lock()->getTimeStamp();
const auto& delta = fac->getFeature()->getMeasurement(); // In FeatImu, delta = measurement
delta_int = imu::compose(delta_int, delta, dt);
}
diff --git a/src/processor/processor_imu2d.cpp b/src/processor/processor_imu2d.cpp
index 255992896..0fbba76f1 100644
--- a/src/processor/processor_imu2d.cpp
+++ b/src/processor/processor_imu2d.cpp
@@ -29,77 +29,80 @@
#include <core/composite/vector_composite.h>
#include <core/factor/factor_block_difference.h>
-namespace wolf {
+namespace wolf
+{
-ProcessorImu2d::ProcessorImu2d(ParamsProcessorImu2dPtr _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))
+ProcessorImu2d::ProcessorImu2d(ParamsProcessorImu2dPtr _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))
{
//
}
ProcessorImu2d::~ProcessorImu2d()
{
- //
+ //
}
void ProcessorImu2d::preProcess()
{
- auto cap_ptr = std::dynamic_pointer_cast<CaptureImu>(incoming_ptr_);
- assert(cap_ptr != nullptr && ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureImu").c_str());
+ auto cap_ptr = std::dynamic_pointer_cast<CaptureImu>(incoming_ptr_);
+ assert(
+ cap_ptr != nullptr &&
+ ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureImu").c_str());
}
bool ProcessorImu2d::voteForKeyFrame() const
{
- // time span
- if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_Imu_->max_time_span)
- {
- WOLF_DEBUG( "PM: vote: time span" );
- return true;
- }
- // buffer length
- if (getBuffer().size() > params_motion_Imu_->max_buff_length)
- {
- WOLF_DEBUG( "PM: vote: buffer length" );
- return true;
- }
- // angle turned
- double angle = std::abs(delta_integrated_(2));
- if (angle > params_motion_Imu_->angle_turned)
- {
- WOLF_DEBUG( "PM: vote: angle turned" );
- return true;
- }
- //WOLF_DEBUG( "PM: do not vote" );
- return false;
+ // time span
+ if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_Imu_->max_time_span)
+ {
+ WOLF_DEBUG("PM: vote: time span");
+ return true;
+ }
+ // buffer length
+ if (getBuffer().size() > params_motion_Imu_->max_buff_length)
+ {
+ WOLF_DEBUG("PM: vote: buffer length");
+ return true;
+ }
+ // angle turned
+ double angle = std::abs(delta_integrated_(2));
+ if (angle > params_motion_Imu_->angle_turned)
+ {
+ WOLF_DEBUG("PM: vote: angle turned");
+ return true;
+ }
+ // WOLF_DEBUG( "PM: do not vote" );
+ return false;
}
-
-CaptureMotionPtr ProcessorImu2d::emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
+CaptureMotionPtr ProcessorImu2d::emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin)
{
- auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureImu>(_frame_own,
- _ts,
- _sensor,
- _data,
- _data_cov,
- _capture_origin));
- setCalibration(cap_motion, _calib);
- cap_motion->setCalibrationPreint(_calib_preint);
+ auto cap_motion = std::static_pointer_cast<CaptureMotion>(
+ CaptureBase::emplace<CaptureImu>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin));
+ setCalibration(cap_motion, _calib);
+ cap_motion->setCalibrationPreint(_calib_preint);
- return cap_motion;
+ return cap_motion;
}
-VectorXd ProcessorImu2d::getCalibration (const CaptureBaseConstPtr _capture) const
+VectorXd ProcessorImu2d::getCalibration(const CaptureBaseConstPtr _capture) const
{
if (_capture)
return _capture->getStateBlock('I')->getState();
@@ -107,207 +110,197 @@ VectorXd ProcessorImu2d::getCalibration (const CaptureBaseConstPtr _capture) con
return getSensor()->getStateBlockDynamic('I')->getState();
}
-void ProcessorImu2d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+void ProcessorImu2d::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
{
_capture->getSensorIntrinsic()->setState(_calibration);
}
-void ProcessorImu2d::configure(SensorBasePtr _sensor)
+void ProcessorImu2d::configure(SensorBasePtr _sensor)
{
- imu_drift_cov_ = _sensor->getDriftCov('I');
+ imu_drift_cov_ = _sensor->getDriftCov('I');
}
void ProcessorImu2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
{
auto feature = FeatureBase::emplace<FeatureImu2d>(
- _capture_own, _capture_own->getBuffer().back().delta_integr_,
+ _capture_own,
+ _capture_own->getBuffer().back().delta_integr_,
_capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
- _capture_own->getCalibrationPreint(), _capture_own->getBuffer().back().jacobian_calib_);
+ _capture_own->getCalibrationPreint(),
+ _capture_own->getBuffer().back().jacobian_calib_);
CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
FeatureImu2dPtr ftr_imu = std::static_pointer_cast<FeatureImu2d>(feature);
if (std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal())
- FactorBase::emplace<FactorImu2d>(ftr_imu, ftr_imu, cap_imu, shared_from_this(),
- params_->apply_loss_function);
+ FactorBase::emplace<FactorImu2d>(ftr_imu, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
else
- FactorBase::emplace<FactorImu2dWithGravity>(ftr_imu, 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);
if (getSensor()->isStateBlockDynamic('I'))
{
- const auto& sb_imubias_own = _capture_own->getStateBlock('I');
- const auto& sb_imubias_origin = _capture_origin->getStateBlock('I');
- if (sb_imubias_own != sb_imubias_origin) // make sure it's two different state blocks! -- just in case
+ if (_capture_own->getStateBlock('I') != _capture_origin->getStateBlock('I')) // make sure it's two different state blocks! -- just in case
{
- auto dt = _capture_own->getTimeStamp() - _capture_origin->getTimeStamp();
- auto ftr_bias = FeatureBase::emplace<FeatureBase>(
- _capture_own, "FeatureBase",
- Vector3d::Zero(), // mean IMU drift is zero
- imu_drift_cov_ * dt); // IMU drift cov specified in continuous time
+ auto dt = _capture_own->getTimeStamp() - _capture_origin->getTimeStamp();
+ auto ftr_bias =
+ FeatureBase::emplace<FeatureBase>(_capture_own,
+ "FeatureBase",
+ Vector3d::Zero(), // mean IMU drift is zero
+ imu_drift_cov_ * dt); // IMU drift cov specified in continuous time
auto fac_bias =
- FactorBase::emplace<FactorBlockDifference>(ftr_bias, ftr_bias,
- sb_imubias_own, // IMU bias block at t=own
- sb_imubias_origin, // IMU bias block at t=origin
- nullptr, // frame other
- _capture_origin, // origin capture
- nullptr, // feature other
- nullptr, // landmark other
- 0, // take all of first state block
- -1, // take all of first state block
- 0, // take all of first second block
- -1, // take all of first second block
- shared_from_this(), // this processor
- params_->apply_loss_function); // 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
- FactorBase::emplace<FactorImu2dWithGravity>(ftr_imu,
- ftr_imu,
- cap_imu,
- shared_from_this(),
- params_->apply_loss_function);
+ FactorBase::emplace<FactorBlockDifference>(ftr_bias,
+ ftr_bias->getMeasurement(),
+ ftr_bias->getMeasurementSquareRootInformationUpper(),
+ _capture_own,
+ _capture_origin,
+ 'I',
+ 'I',
+ shared_from_this(), // this processor
+ params_->apply_loss_function); // 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
+ FactorBase::emplace<FactorImu2dWithGravity>(
+ ftr_imu, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
}
}
}
void ProcessorImu2d::computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jac_delta_calib) const
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jac_delta_calib) const
{
- using namespace Eigen;
- assert(_data.size() == data_size_ && "Wrong data size!");
- Vector3d data_2d;
- data_2d << _data(0), _data(1), _data(5);
- Matrix3d data_cov_2d;
- data_cov_2d << _data_cov(0,0), _data_cov(0,1), _data_cov(0,5),
- _data_cov(1,0), _data_cov(1,1), _data_cov(1,5),
- _data_cov(5,0), _data_cov(5,1), _data_cov(5,5);
-
-
-
- Matrix<double, 5, 3> jac_delta_data;
- /*
- * We have the following computing pipeline:
- * Input: data, calib, dt
- * Output: delta, delta_cov, jac_calib
- *
- * We do:
- * body = data - calib (measurement - bias)
- * delta = body2delta(body, dt) --> jac_body
- * jac_data = jac_body
- * jac_calib = - jac_body
- * delta_cov <-- propagate data_cov using jac_data
- *
- * where body2delta(.) produces a delta from body=(a,w) as follows:
- * dp = P * a * dt^2
- * dth = exp(w * dt) = w * dt
- * dv = Q * a * dt
- * where P and Q are defined in imu2d::exp(), and also a jacobian J^delta_data
- */
-
- //create delta
- imu2d::body2delta(data_2d - _calib, _dt, _delta, jac_delta_data);
-
- // compute delta_cov
- _delta_cov = jac_delta_data * data_cov_2d * jac_delta_data.transpose();
-
- // compute jacobian_calib
- _jac_delta_calib = - jac_delta_data;
+ using namespace Eigen;
+ assert(_data.size() == data_size_ && "Wrong data size!");
+ Vector3d data_2d;
+ data_2d << _data(0), _data(1), _data(5);
+ Matrix3d data_cov_2d;
+ data_cov_2d << _data_cov(0, 0), _data_cov(0, 1), _data_cov(0, 5), _data_cov(1, 0), _data_cov(1, 1),
+ _data_cov(1, 5), _data_cov(5, 0), _data_cov(5, 1), _data_cov(5, 5);
+
+ Matrix<double, 5, 3> jac_delta_data;
+ /*
+ * We have the following computing pipeline:
+ * Input: data, calib, dt
+ * Output: delta, delta_cov, jac_calib
+ *
+ * We do:
+ * body = data - calib (measurement - bias)
+ * delta = body2delta(body, dt) --> jac_body
+ * jac_data = jac_body
+ * jac_calib = - jac_body
+ * delta_cov <-- propagate data_cov using jac_data
+ *
+ * where body2delta(.) produces a delta from body=(a,w) as follows:
+ * dp = P * a * dt^2
+ * dth = exp(w * dt) = w * dt
+ * dv = Q * a * dt
+ * where P and Q are defined in imu2d::exp(), and also a jacobian J^delta_data
+ */
+
+ // create delta
+ imu2d::body2delta(data_2d - _calib, _dt, _delta, jac_delta_data);
+
+ // compute delta_cov
+ _delta_cov = jac_delta_data * data_cov_2d * jac_delta_data.transpose();
+
+ // compute jacobian_calib
+ _jac_delta_calib = -jac_delta_data;
}
void ProcessorImu2d::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _delta_preint_plus_delta) const
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta) const
{
- /* MATHS:
- * Simple composition,
- * (D o d)p = Dp + Dth*dp + Dv*dt
- * (D o d)th = Dth+dth
- * (D o d)v = Dv + Dth*dv
- * where Dth*dp and Dth*dv are rotations and not scalar products. The version with jacobians is not used here.
- */
- _delta_preint_plus_delta = imu2d::compose(_delta_preint, _delta, _dt);
+ /* MATHS:
+ * Simple composition,
+ * (D o d)p = Dp + Dth*dp + Dv*dt
+ * (D o d)th = Dth+dth
+ * (D o d)v = Dv + Dth*dv
+ * where Dth*dp and Dth*dv are rotations and not scalar products. The version with jacobians is not used here.
+ */
+ _delta_preint_plus_delta = imu2d::compose(_delta_preint, _delta, _dt);
}
void ProcessorImu2d::statePlusDelta(const VectorComposite& _x,
const Eigen::VectorXd& _delta,
- const double _dt,
- VectorComposite& _x_plus_delta) const
+ const double _dt,
+ VectorComposite& _x_plus_delta) const
{
- 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("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 SensorImu2d>(getSensor())->isGravityOrthogonal() )
- _x_plus_delta = imu2d::composeOverState(_x, delta, _dt);
- else
- _x_plus_delta = imu2d::composeOverStateWithGravity(_x, delta, _dt, getSensor()->getState("G"));
-
+ 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("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 SensorImu2d>(getSensor())->isGravityOrthogonal())
+ _x_plus_delta = imu2d::composeOverState(_x, delta, _dt);
+ else
+ _x_plus_delta = imu2d::composeOverStateWithGravity(_x, delta, _dt, getSensor()->getState("G"));
}
void ProcessorImu2d::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _delta_preint_plus_delta,
- Eigen::MatrixXd& _jacobian_delta_preint,
- Eigen::MatrixXd& _jacobian_delta) const
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta,
+ Eigen::MatrixXd& _jacobian_delta_preint,
+ Eigen::MatrixXd& _jacobian_delta) const
{
- /*
- * Expression of the delta integration step, D' = D (+) d:
- *
- * Dp' = Dp + Dv*dt + Dq*dp
- * Dv' = Dv + Dq*dv
- * Dq' = Dq * dq
- *
- * Jacobians for covariance propagation.
- *
- * a. With respect to Delta, gives _jacobian_delta_preint = D'_D as:
- *
- * D'_D = [ I DR*skew(1)*dp I*dt
- * 0 1 0
- * 0 DR*skew(1)*dv I ]
- *
- * b. With respect to delta, gives _jacobian_delta = D'_d as:
- *
- * D'_d = [ DR 0 0
- * 0 1 0
- * 0 0 DR ]
- *
- * Note: covariance propagation, i.e., P+ = D_D * P * D_D' + D_d * M * D_d', is done in ProcessorMotion.
- */
- imu2d::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in imu2d_tools
+ /*
+ * Expression of the delta integration step, D' = D (+) d:
+ *
+ * Dp' = Dp + Dv*dt + Dq*dp
+ * Dv' = Dv + Dq*dv
+ * Dq' = Dq * dq
+ *
+ * Jacobians for covariance propagation.
+ *
+ * a. With respect to Delta, gives _jacobian_delta_preint = D'_D as:
+ *
+ * D'_D = [ I DR*skew(1)*dp I*dt
+ * 0 1 0
+ * 0 DR*skew(1)*dv I ]
+ *
+ * b. With respect to delta, gives _jacobian_delta = D'_d as:
+ *
+ * D'_d = [ DR 0 0
+ * 0 1 0
+ * 0 0 DR ]
+ *
+ * Note: covariance propagation, i.e., P+ = D_D * P * D_D' + D_d * M * D_d', is done in ProcessorMotion.
+ */
+ imu2d::compose(_delta_preint,
+ _delta,
+ _dt,
+ _delta_preint_plus_delta,
+ _jacobian_delta_preint,
+ _jacobian_delta); // jacobians tested in imu2d_tools
}
-Eigen::VectorXd ProcessorImu2d::correctDelta (const Eigen::VectorXd& delta_preint,
- const Eigen::VectorXd& delta_step) const
+Eigen::VectorXd ProcessorImu2d::correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const
{
- return imu2d::plus(delta_preint, delta_step);
+ return imu2d::plus(delta_preint, delta_step);
}
-
-} // namespace wolf
+} // namespace wolf
// Register in the FactorySensor
#include "core/processor/factory_processor.h"
namespace wolf
{
- WOLF_REGISTER_PROCESSOR(ProcessorImu2d)
+WOLF_REGISTER_PROCESSOR(ProcessorImu2d)
}
diff --git a/src/sensor/sensor_compass.cpp b/src/sensor/sensor_compass.cpp
index c8d77b6a1..dd30a3c1b 100644
--- a/src/sensor/sensor_compass.cpp
+++ b/src/sensor/sensor_compass.cpp
@@ -25,30 +25,23 @@
namespace wolf
{
-SensorCompass::SensorCompass(ParamsSensorCompassPtr _params,
- const SpecStateSensorComposite& _priors) :
- SensorBase("SensorCompass",
- 3,
- _params,
- _priors("PO")),
- params_compass_(_params)
+SensorCompass::SensorCompass(ParamsSensorCompassPtr _params, const SpecStateSensorComposite& _priors)
+ : SensorBase("SensorCompass", 3, _params, _priors), params_compass_(_params)
{
}
-SensorCompass::~SensorCompass()
-{
-}
+SensorCompass::~SensorCompass() {}
Eigen::MatrixXd SensorCompass::computeNoiseCov(const Eigen::VectorXd& _data) const
{
- return Eigen::Vector3d::Constant(pow(params_compass_->stdev_noise,2)).asDiagonal();
+ return Eigen::Vector3d::Constant(pow(params_compass_->stdev_noise, 2)).asDiagonal();
}
} /* namespace wolf */
// Register in the FactorySensor
#include "core/sensor/factory_sensor.h"
-namespace wolf {
+namespace wolf
+{
WOLF_REGISTER_SENSOR(SensorCompass);
-} // namespace wolf
-
+} // namespace wolf
--
GitLab