From 613b1cf558027139a0a2b5d7063ac1223f6ec2d4 Mon Sep 17 00:00:00 2001
From: asanjuan <asanjuan@iri.upc.edu>
Date: Tue, 9 Aug 2022 15:27:59 +0200
Subject: [PATCH] New factor angular momentum and new emplace at the processor
---
.../factor/factor_angular_momentum.h | 81 +++++++++----------
...ocessor_force_torque_inertial_dynamics.cpp | 10 ++-
2 files changed, 48 insertions(+), 43 deletions(-)
diff --git a/include/bodydynamics/factor/factor_angular_momentum.h b/include/bodydynamics/factor/factor_angular_momentum.h
index 5c120ae..7a28b12 100644
--- a/include/bodydynamics/factor/factor_angular_momentum.h
+++ b/include/bodydynamics/factor/factor_angular_momentum.h
@@ -48,12 +48,12 @@ WOLF_PTR_TYPEDEFS(FactorAngularMomentum);
* 'i' inertia matrix components (we are assuming that it is a diagonal matrix)
*
* The residual computed has the following components, in this order
- * - angular velocity error according to FT preintegration
+ * - angular velocity error
*/
-class FactorAngularMomentum : public FactorAutodiff<FactorAngularMomentum, 3, 3, 3, 6, 3> // State Blocks: L, I, i
+class FactorAngularMomentum : public FactorAutodiff<FactorAngularMomentum, 3, 3, 6, 3> // State Blocks: L, I, i
{
public:
- FactorAngularMomentum(const FeatureMotionPtr& _ftr, // gets measurement and access to parents
+ FactorAngularMomentum(const FeatureBasePtr& _ftr, // gets measurement and access to parents
const ProcessorBasePtr& _processor, // gets access to processor and sensor
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
@@ -75,39 +75,39 @@ class FactorAngularMomentum : public FactorAutodiff<FactorAngularMomentum, 3, 3,
Vector3d residual() const;
private:
- Matrix<double, 12, 1> data;
- Matrix<double, 3, 3> sqrt_info_upper_;
+ Vector3d measurement_ang_vel_; // gyroscope measurement data
+ Matrix3d sqrt_info_upper_;
};
-inline FactorAngularMomentum::FactorAngularMomentum(const FeatureBasePtr& _ftr,
+inline FactorAngularMomentum::FactorAngularMomentum(const FeatureBasePtr& _ftr,
const ProcessorBasePtr& _processor,
- bool _apply_loss_function,
- FactorStatus _status)
- : FactorAutodiff<FactorAngularMomentum, 3, 3, 3, 6, 3>(
+ bool _apply_loss_function,
+ FactorStatus _status)
+ : FactorAutodiff<FactorAngularMomentum, 3, 3, 6, 3>(
"FactorAngularMomentum",
TOP_MOTION,
- _ftr, // parent feature
- nullptr, // frame other
- nullptr, // capture other
- nullptr, // feature other
- nullptr, // landmark other
- _processor, // processor
+ _ftr, // parent feature
+ nullptr, // frame other
+ nullptr, // capture other
+ nullptr, // feature other
+ nullptr, // landmark other
+ _processor, // processor
_apply_loss_function,
_status,
- _ftr->getFrame()->getStateBlock('L'), // previous frame L
- _capture_origin->getStateBlock('I'), // previous frame IMU bias
- _processor->getSensor()->getStateBlock('i'), // sensor i
- data(ftr->getMeasurement()),
- sqrt_info_upper_(_processor->getSensor()->getNoiseCov().block<3,3>(3,3)))
+ _ftr->getFrame()->getStateBlock('L'), // current linear momentum
+ _ftr->getCapture()->getStateBlock('I'), // IMU bias
+ _processor->getSensor()->getStateBlock('i')), // moment of inertia
+ measurement_ang_vel_(_ftr->getMeasurement()),
+ sqrt_info_upper_(_ftr->getMeasurementSquareRootInformationUpper()) // Buscar funcio correcte
{
//
}
template <typename D1, typename D2, typename D3, typename D4>
-inline bool FactorAngularMomentum::residual(const MatrixBase<D1>& _L,
- const MatrixBase<D2>& _I,
- const MatrixBase<D3>& _i,
- MatrixBase<D4>& _res) const
+inline bool FactorAngularMomentum::residual(const MatrixBase<D1>& _L,
+ const MatrixBase<D2>& _I,
+ const MatrixBase<D3>& _i,
+ MatrixBase<D4>& _res) const
{
MatrixSizeCheck<3, 1>::check(_res);
@@ -126,16 +126,18 @@ inline bool FactorAngularMomentum::residual(const MatrixBase<D1>& _L,
* res = W * err , with W the sqrt of the info matrix.
*/
- Matrix<T, 3, 1> w_real = data.segment<3>(3) - _I.segment<3>(3);
- double Lx = _L(0);
- double Ly = _L(1);
- double Lz = _L(2);
- double ixx = _i(0);
- double iyy = _i(1);
- double izz = _i(2);
- Matrix<T, 3, 1> w_est = [Lx/ixx, Ly/iyy, Lz/izz];
- Matrix<T, 3, 1> err = w_real - w_est;
- _res = sqrt_info_upper_ * err;
+ typedef typename D4::Scalar T;
+
+ auto w_real = measurement_ang_vel_ - _I.segment<3>(3);
+ const auto& Lx = _L(0);
+ const auto& Ly = _L(1);
+ const auto& Lz = _L(2);
+ const auto& ixx = _i(0);
+ const auto& iyy = _i(1);
+ const auto& izz = _i(2);
+ Matrix<T, 3, 1> w_est(Lx / ixx, Ly / iyy, Lz / izz);
+ Matrix<T, 3, 1> err = w_real - w_est;
+ _res = sqrt_info_upper_ * err;
return true;
}
@@ -144,23 +146,20 @@ inline Vector3d FactorAngularMomentum::residual() const
{
Vector3d res(3);
Vector3d L = getFrame()->getStateBlock('L')->getState();
- Vector6d I = getFeature()->getCapture()->getSensor()->getState();
- Vector3d i = getFeature()->getSensor()->getStateBlock('i')->getState();
+ Vector6d I = getCapture()->getStateBlock('I')->getState();
+ Vector3d i = getSensor()->getStateBlock('i')->getState();
residual(L, I, i, res);
return res;
}
template <typename T>
-inline bool FactorAngularMomentum::operator()(const T* const _L,
- const T* const _I,
- const T* const _i,
- T* _res) const
+inline bool FactorAngularMomentum::operator()(const T* const _L, const T* const _I, const T* const _i, T* _res) const
{
Map<const Matrix<T, 3, 1>> L(_L);
Map<const Matrix<T, 6, 1>> I(_I);
Map<const Matrix<T, 3, 1>> i(_i);
- Map<Matrix<T, 3, 1>> res(_res);
+ Map<Matrix<T, 3, 1>> res(_res);
residual(L, I, i, res);
diff --git a/src/processor/processor_force_torque_inertial_dynamics.cpp b/src/processor/processor_force_torque_inertial_dynamics.cpp
index 678074d..994246f 100644
--- a/src/processor/processor_force_torque_inertial_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_dynamics.cpp
@@ -28,6 +28,7 @@
// bodydynamics
#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
+#include "bodydynamics/factor/factor_angular_momentum.h"
#include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
#include "bodydynamics/capture/capture_force_torque_inertial.h"
@@ -98,8 +99,13 @@ void ProcessorForceTorqueInertialDynamics::emplaceFeaturesAndFactors(CaptureBase
// - feature has the current gyro measurement
// - factor relates w_measured, IMU_bias(last capture), L(last capture) and i(sensor)
- // auto measurement_gyro = motion.data_.segment<3>(3);
- // auto meas_cov = sensor_fti_->getNoiseCov().block<3,3>(3,3); // ??
+ auto measurement_gyro = motion.data_.segment<3>(3);
+ auto gyro_cov = sensor_fti_->getNoiseCov().block<3, 3>(3, 3);
+
+ auto ftr_base = FeatureBase::emplace<FeatureBase>(_capture_own, "FeatureBase", measurement_gyro, gyro_cov);
+
+ FactorBase::emplace<FactorAngularMomentum>(
+ ftr_base, ftr_base, _capture_own->getSensor()->getProcessorList().back(), params_->apply_loss_function);
// 3. Feature and factor bias -- IMU bias drift for acc and gyro
//---------------------------------------------------------------
--
GitLab