diff --git a/include/bodydynamics/factor/factor_force_torque_inertial.h b/include/bodydynamics/factor/factor_force_torque_inertial.h
index 51c60184de4a56d64da4f20992a7ade895baea72..cfea278e30816f355353fbc833d4647c66cf6726 100644
--- a/include/bodydynamics/factor/factor_force_torque_inertial.h
+++ b/include/bodydynamics/factor/factor_force_torque_inertial.h
@@ -35,9 +35,9 @@ WOLF_PTR_TYPEDEFS(FactorForceTorqueInertial);
/**
* @brief Pre-integrated factor taking IMU and force-torque measurements (FTI)
- *
+ *
* This factor evaluates the preintegrated FTI against pose, linear velocity, IMU bias and angular momentum.
- *
+ *
* State blocks considered:
* - Frame 1 (origin)
* 'P' position
@@ -51,7 +51,7 @@ WOLF_PTR_TYPEDEFS(FactorForceTorqueInertial);
* 'V' velocity
* 'O' orientation
* 'L' angular momentum
- *
+ *
* The residual computed has the following components, in this order
* - position delta error according to IMU preintegration
* - velocity delta error according to IMU preintegration
@@ -191,10 +191,11 @@ inline bool FactorForceTorqueInertial::residual(const MatrixBase<D1>& _p1,
fti::betweenStates(_p1, _v1, _L1, _q1, _p2, _v2, _L2, _q2, dt_, dpi, dvi, dpd, dvd, dL, dq);
Matrix<T, 19, 1> delta_preint = delta_preint_.cast<T>();
- Matrix<T, 18, 1> delta_step = J_delta_bias_ * (_b1 - bias_preint_); // canviar _b1 per calib = (I,C,i,m), de mida 13 Matrix<T,13,1>
- Matrix<T, 19, 1> delta_corr = fti::plus(delta_preint, delta_step);
- Matrix<T, 18, 1> delta_err = fti::diff(delta_est, delta_corr);
- _res = sqrt_info_upper_ * delta_err;
+ Matrix<T, 18, 1> delta_step =
+ J_delta_bias_ * (_b1 - bias_preint_); // canviar _b1 per calib = (I,C,i,m), de mida 13 Matrix<T,13,1>
+ Matrix<T, 19, 1> delta_corr = fti::plus(delta_preint, delta_step);
+ Matrix<T, 18, 1> delta_err = fti::diff(delta_est, delta_corr);
+ _res = sqrt_info_upper_ * delta_err;
return true;
}
diff --git a/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h b/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h
index f33b5d10d34d1bd659fbe5aca49e92dfac05a7b8..92fe3ca902450760dcdfdf54594b04f39d4801c0 100644
--- a/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h
+++ b/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h
@@ -173,7 +173,7 @@ inline bool FactorForceTorqueInertialDynamics::residual(const MatrixBase<D1>&
const MatrixBase<D3>& _L2,
const MatrixBase<D6>& _C,
const MatrixBase<D6>& _i,
- const D7& _m,
+ const D7& _m,
MatrixBase<D8>& _res) const
{
MatrixSizeCheck<18, 1>::check(_res);
@@ -228,21 +228,21 @@ inline bool FactorForceTorqueInertialDynamics::residual(const MatrixBase<D1>&
inline VectorXd FactorForceTorqueInertialDynamics::residual() const
{
- VectorXd res(18);
- Vector3d p0 = getFrameOther()->getStateBlock('P')->getState(); // previous frame P
- Quaterniond q0 = Quaterniond(getFrameOther()->getStateBlock('O')->getState().data());
- Vector3d v0 = getFrameOther()->getStateBlock('V')->getState();
- Vector3d L0 = getFrameOther()->getStateBlock('L')->getState();
- Vector6d bias = getCaptureOther()->getSensorIntrinsic()->getState();
- Vector3d p1 = getFrame()->getStateBlock('P')->getState(); // previous frame P
- Quaterniond q1 = Quaterniond(getFrame()->getStateBlock('O')->getState().data());
- Vector3d v1 = getFrame()->getStateBlock('V')->getState();
- Vector3d L1 = getFrame()->getStateBlock('L')->getState();
- Vector3d C = getCapture()->getSensor()->getStateBlock('C')->getState();
- Vector3d i = getCapture()->getSensor()->getStateBlock('i')->getState();
- double m = getCapture()->getSensor()->getStateBlock('m')->getState()(0);
+ VectorXd res(18);
+ Vector3d p0 = getFrameOther()->getStateBlock('P')->getState(); // previous frame P
+ Quaterniond q0 = Quaterniond(getFrameOther()->getStateBlock('O')->getState().data());
+ Vector3d v0 = getFrameOther()->getStateBlock('V')->getState();
+ Vector3d L0 = getFrameOther()->getStateBlock('L')->getState();
+ Vector6d bias = getCaptureOther()->getSensorIntrinsic()->getState();
+ Vector3d p1 = getFrame()->getStateBlock('P')->getState(); // previous frame P
+ Quaterniond q1 = Quaterniond(getFrame()->getStateBlock('O')->getState().data());
+ Vector3d v1 = getFrame()->getStateBlock('V')->getState();
+ Vector3d L1 = getFrame()->getStateBlock('L')->getState();
+ Vector3d C = getCapture()->getSensor()->getStateBlock('C')->getState();
+ Vector3d i = getCapture()->getSensor()->getStateBlock('i')->getState();
+ double m = getCapture()->getSensor()->getStateBlock('m')->getState()(0);
- residual(p0, q0, v0, L0, bias, p1, q1, v1, L1, C, i, m, res);
+ residual(p0, q0, v0, L0, bias, p1, q1, v1, L1, C, i, m, res);
return res;
}
@@ -259,7 +259,7 @@ inline bool FactorForceTorqueInertialDynamics::operator()(const T* const _p1,
const T* const _C,
const T* const _i,
const T* const _m,
- T* _res) const
+ T* _res) const
{
Map<const Matrix<T, 3, 1>> p1(_p1);
Map<const Quaternion<T>> q1(_q1);
diff --git a/src/feature/feature_force_torque.cpp b/src/feature/feature_force_torque.cpp
index 52a9421da7e2df46d9c4b02d2f9f8cafa08666bf..04eacfa7e8c4c02944bb2fd6a151f05551030c47 100644
--- a/src/feature/feature_force_torque.cpp
+++ b/src/feature/feature_force_torque.cpp
@@ -20,17 +20,18 @@
//
//--------LICENSE_END--------
#include "bodydynamics/feature/feature_force_torque.h"
-namespace wolf {
+namespace wolf
+{
-FeatureForceTorque::FeatureForceTorque(const Eigen::VectorXd& _delta_preintegrated,
- const Eigen::MatrixXd& _delta_preintegrated_covariance,
- const Eigen::Vector6d& _biases_preint,
- const Eigen::Matrix<double,12,6>& _J_delta_biases) :
- FeatureBase("FeatureForceTorque", _delta_preintegrated, _delta_preintegrated_covariance),
- pbc_bias_preint_(_biases_preint.head<3>()),
- gyro_bias_preint_(_biases_preint.tail<3>()),
- J_delta_bias_(_J_delta_biases)
+FeatureForceTorque::FeatureForceTorque(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::Vector6d& _biases_preint,
+ const Eigen::Matrix<double, 12, 6>& _J_delta_biases)
+ : FeatureBase("FeatureForceTorque", _delta_preintegrated, _delta_preintegrated_covariance),
+ pbc_bias_preint_(_biases_preint.head<3>()),
+ gyro_bias_preint_(_biases_preint.tail<3>()),
+ J_delta_bias_(_J_delta_biases)
{
}
-} // namespace wolf
+} // namespace wolf
diff --git a/src/processor/processor_force_torque.cpp b/src/processor/processor_force_torque.cpp
index 8afdfe3d49708fe6c9b1c40c2065829527be3af5..d9bd77e5474df95514c60c72c5f3f138e92415b4 100644
--- a/src/processor/processor_force_torque.cpp
+++ b/src/processor/processor_force_torque.cpp
@@ -26,24 +26,32 @@
#include "bodydynamics/processor/processor_force_torque.h"
#include "bodydynamics/factor/factor_force_torque.h"
-namespace wolf {
+namespace wolf
+{
-int compute_data_size(int nbc, int dimc){
+int compute_data_size(int nbc, int dimc)
+{
// compute the size of the data/body vectors used by processorMotion API depending
// on the number of contacts (nbc) and contact dimension (dimc)
- // result: forces (+torques for 6D contacts) + pbc + wb + contact/base positions + contact/base orientations
- return nbc*dimc + 3 + 3 + nbc*3 + nbc*4;
+ // result: forces (+torques for 6D contacts) + pbc + wb + contact/base positions + contact/base orientations
+ return nbc * dimc + 3 + 3 + nbc * 3 + nbc * 4;
}
using namespace Eigen;
-ProcessorForceTorque::ProcessorForceTorque(ParamsProcessorForceTorquePtr _params_motion_force_torque) :
- ProcessorMotion("ProcessorForceTorque", "CDLO", 3, 13, 13, 12,
- compute_data_size(_params_motion_force_torque->nbc, _params_motion_force_torque->dimc),
- 6, _params_motion_force_torque),
- params_motion_force_torque_(std::make_shared<ParamsProcessorForceTorque>(*_params_motion_force_torque)),
- nbc_(_params_motion_force_torque->nbc),
- dimc_(_params_motion_force_torque->dimc)
+ProcessorForceTorque::ProcessorForceTorque(ParamsProcessorForceTorquePtr _params_motion_force_torque)
+ : ProcessorMotion("ProcessorForceTorque",
+ "CDLO",
+ 3,
+ 13,
+ 13,
+ 12,
+ compute_data_size(_params_motion_force_torque->nbc, _params_motion_force_torque->dimc),
+ 6,
+ _params_motion_force_torque),
+ params_motion_force_torque_(std::make_shared<ParamsProcessorForceTorque>(*_params_motion_force_torque)),
+ nbc_(_params_motion_force_torque->nbc),
+ dimc_(_params_motion_force_torque->dimc)
{
//
@@ -59,60 +67,59 @@ bool ProcessorForceTorque::voteForKeyFrame() const
// time span
if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_force_torque_->max_time_span)
{
- WOLF_DEBUG( "PM: vote: time span" );
+ WOLF_DEBUG("PM: vote: time span");
return true;
}
// buffer length
if (getBuffer().size() > params_motion_force_torque_->max_buff_length)
{
- WOLF_DEBUG( "PM: vote: buffer length" );
+ WOLF_DEBUG("PM: vote: buffer length");
return true;
}
-
+
// Some other measure of movement?
- //WOLF_DEBUG( "PM: do not vote" );
+ // WOLF_DEBUG( "PM: do not vote" );
return false;
}
-
-CaptureMotionPtr ProcessorForceTorque::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)
+CaptureMotionPtr ProcessorForceTorque::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)
{
-
// Here we have to retrieve the origin capture no
// !! PROBLEM:
// when doing setOrigin, emplaceCapture is called 2 times
// - first on the first KF (usually created by setPrior), this one contains the biases
// - secondly on the inner frame (last) which does not contains other captures
- auto capture_ikin = _frame_own->getCaptureOf(sensor_ikin_);
+ auto capture_ikin = _frame_own->getCaptureOf(sensor_ikin_);
auto capture_angvel = _frame_own->getCaptureOf(sensor_angvel_);
- if ((capture_ikin == nullptr) || (capture_angvel == nullptr)){
+ if ((capture_ikin == nullptr) || (capture_angvel == nullptr))
+ {
// We have to retrieve the bias from a former Keyframe: origin
- capture_ikin = origin_ptr_->getFrame()->getCaptureOf(sensor_ikin_);
- capture_angvel = origin_ptr_->getFrame()->getCaptureOf(sensor_angvel_);
+ capture_ikin = origin_ptr_->getFrame()->getCaptureOf(sensor_ikin_);
+ capture_angvel = origin_ptr_->getFrame()->getCaptureOf(sensor_angvel_);
}
- auto cap = CaptureBase::emplace<CaptureForceTorque>(
- _frame_own,
- _ts,
- _sensor,
- std::static_pointer_cast<CaptureInertialKinematics>(capture_ikin),
- std::static_pointer_cast<CaptureMotion>(capture_angvel),
- _data,
- _data_cov,
- _capture_origin);
+ auto cap =
+ CaptureBase::emplace<CaptureForceTorque>(_frame_own,
+ _ts,
+ _sensor,
+ std::static_pointer_cast<CaptureInertialKinematics>(capture_ikin),
+ std::static_pointer_cast<CaptureMotion>(capture_angvel),
+ _data,
+ _data_cov,
+ _capture_origin);
// Cannot be recovered from the _calib and _calib_preint which are initialized using calib_size_
// which is zero in our case since the calibration stateblocks are not actually in the FTPreint sensor/captures
- auto cap_ft = std::static_pointer_cast<CaptureForceTorque>(cap);
- Vector6d calib = getCalibration(cap_ft);
+ auto cap_ft = std::static_pointer_cast<CaptureForceTorque>(cap);
+ Vector6d calib = getCalibration(cap_ft);
setCalibration(cap_ft, calib);
cap_ft->setCalibrationPreint(calib);
@@ -122,26 +129,26 @@ CaptureMotionPtr ProcessorForceTorque::emplaceCapture(const FrameBasePtr& _frame
FeatureBasePtr ProcessorForceTorque::emplaceFeature(CaptureMotionPtr _capture_motion)
{
// Retrieving the origin capture and getting the bias from here should be done here?
- auto feature = FeatureBase::emplace<FeatureForceTorque>(_capture_motion,
- _capture_motion->getBuffer().back().delta_integr_,
- _capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
- _capture_motion->getCalibrationPreint(),
- _capture_motion->getBuffer().back().jacobian_calib_);
+ auto feature = FeatureBase::emplace<FeatureForceTorque>(
+ _capture_motion,
+ _capture_motion->getBuffer().back().delta_integr_,
+ _capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
+ _capture_motion->getCalibrationPreint(),
+ _capture_motion->getBuffer().back().jacobian_calib_);
return feature;
}
-Eigen::VectorXd ProcessorForceTorque::correctDelta (const Eigen::VectorXd& delta_preint,
- const Eigen::VectorXd& delta_step) const
+Eigen::VectorXd ProcessorForceTorque::correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const
{
return bodydynamics::plus(delta_preint, delta_step);
}
-VectorXd ProcessorForceTorque::getCalibration (const CaptureBaseConstPtr _capture) const
+VectorXd ProcessorForceTorque::getCalibration(const CaptureBaseConstPtr _capture) const
{
-
VectorXd bias_vec(6);
- if (_capture) // access from capture is quicker
+ if (_capture) // access from capture is quicker
{
auto cap_ft(std::static_pointer_cast<const CaptureForceTorque>(_capture));
@@ -149,20 +156,27 @@ VectorXd ProcessorForceTorque::getCalibration (const CaptureBaseConstPtr _captur
bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState();
// get calib part from IMU capture
- bias_vec.segment<3>(3) = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState().tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
+ bias_vec.segment<3>(3) =
+ cap_ft->getGyroCaptureOther()
+ ->getSensorIntrinsic()
+ ->getState()
+ .tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
}
- else // access from sensor is slower
+ else // access from sensor is slower
{
// get calib part from Ikin capture
bias_vec.segment<3>(0) = sensor_ikin_->getIntrinsic()->getState();
// get calib part from IMU capture
- bias_vec.segment<3>(3) = sensor_angvel_->getIntrinsic()->getState().tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
+ bias_vec.segment<3>(3) =
+ sensor_angvel_->getIntrinsic()
+ ->getState()
+ .tail<3>(); // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
}
return bias_vec;
}
-void ProcessorForceTorque::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+void ProcessorForceTorque::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
{
CaptureForceTorquePtr cap_ft(std::static_pointer_cast<CaptureForceTorque>(_capture));
@@ -179,47 +193,50 @@ void ProcessorForceTorque::setCalibration (const CaptureBasePtr _capture, const
FactorBasePtr ProcessorForceTorque::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
CaptureForceTorquePtr cap_ft_origin = std::static_pointer_cast<CaptureForceTorque>(_capture_origin);
- FeatureForceTorquePtr ftr_ft = std::static_pointer_cast<FeatureForceTorque>(_feature_motion);
- CaptureForceTorquePtr cap_ft_new = std::static_pointer_cast<CaptureForceTorque>(ftr_ft->getCapture());
-
- auto fac_ft = FactorBase::emplace<FactorForceTorque>(
- ftr_ft,
- ftr_ft,
- cap_ft_origin,
- shared_from_this(),
- cap_ft_origin->getIkinCaptureOther(), // to retrieve sb_bp1
- cap_ft_origin->getGyroCaptureOther(), // to retrieve sb_w1
- false);
+ FeatureForceTorquePtr ftr_ft = std::static_pointer_cast<FeatureForceTorque>(_feature_motion);
+ CaptureForceTorquePtr cap_ft_new = std::static_pointer_cast<CaptureForceTorque>(ftr_ft->getCapture());
+
+ auto fac_ft = FactorBase::emplace<FactorForceTorque>(ftr_ft,
+ ftr_ft,
+ cap_ft_origin,
+ shared_from_this(),
+ cap_ft_origin->getIkinCaptureOther(), // to retrieve sb_bp1
+ cap_ft_origin->getGyroCaptureOther(), // to retrieve sb_w1
+ false);
return fac_ft;
}
-void ProcessorForceTorque::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
+void ProcessorForceTorque::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
{
assert(_data.size() == data_size_ && "Wrong data size!");
// create delta
- MatrixXd jac_body_bias(data_size_-nbc_,6);
+ MatrixXd jac_body_bias(data_size_ - nbc_, 6);
VectorXd body(data_size_);
bodydynamics::debiasData(_data, _calib, nbc_, dimc_, body, jac_body_bias);
- MatrixXd jac_delta_body(12,data_size_-nbc_);
- bodydynamics::body2delta(body, _dt, std::static_pointer_cast<const SensorForceTorque>(getSensor())->getMass(),
- nbc_, dimc_,
- _delta, jac_delta_body); // Jacobians tested in bodydynamics_tools
+ MatrixXd jac_delta_body(12, data_size_ - nbc_);
+ bodydynamics::body2delta(body,
+ _dt,
+ std::static_pointer_cast<const SensorForceTorque>(getSensor())->getMass(),
+ nbc_,
+ dimc_,
+ _delta,
+ jac_delta_body); // Jacobians tested in bodydynamics_tools
// [f], [tau], pbc, wb
- MatrixXd jac_delta_body_reduced = jac_delta_body.leftCols(nbc_*dimc_+6);
+ MatrixXd jac_delta_body_reduced = jac_delta_body.leftCols(nbc_ * dimc_ + 6);
// compute delta_cov (using uncertainty on f,tau,pbc)
- _delta_cov = jac_delta_body_reduced * _data_cov * jac_delta_body_reduced.transpose(); // trivially: jac_body_delta = Identity
+ _delta_cov = jac_delta_body_reduced * _data_cov *
+ jac_delta_body_reduced.transpose(); // trivially: jac_body_delta = Identity
// _delta_cov = jac_delta_body * _data_cov * jac_delta_body.transpose(); // trivially: jac_body_delta = Identity
// compute jacobian_calib
@@ -227,17 +244,17 @@ void ProcessorForceTorque::computeCurrentDelta(
}
void ProcessorForceTorque::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
- const Eigen::VectorXd& _delta,
- const double _dt,
- Eigen::VectorXd& _delta_preint_plus_delta) const
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta) const
{
_delta_preint_plus_delta = bodydynamics::compose(_delta_preint, _delta, _dt);
}
void ProcessorForceTorque::statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _dt,
- VectorComposite& _x_plus_delta) const
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const
{
assert(_delta.size() == 13 && "Wrong _delta vector size");
assert(_dt >= 0 && "Time interval _dt is negative!");
@@ -248,20 +265,25 @@ void ProcessorForceTorque::statePlusDelta(const VectorComposite& _x,
VectorXd x_plus_delta = bodydynamics::composeOverState(x, _delta, _dt);
- _x_plus_delta = VectorComposite(x_plus_delta, "CDLO", {3,3,3,4});
+ _x_plus_delta = VectorComposite(x_plus_delta, "CDLO", {3, 3, 3, 4});
}
void ProcessorForceTorque::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 Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta,
+ Eigen::MatrixXd& _jacobian_delta_preint,
+ Eigen::MatrixXd& _jacobian_delta) const
{
- bodydynamics::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in bodydynamics_tools
+ bodydynamics::compose(_delta_preint,
+ _delta,
+ _dt,
+ _delta_preint_plus_delta,
+ _jacobian_delta_preint,
+ _jacobian_delta); // jacobians tested in bodydynamics_tools
}
-} // namespace wolf
+} // namespace wolf
// Register in the FactorySensor
#include "core/processor/factory_processor.h"
diff --git a/src/processor/processor_force_torque_inertial.cpp b/src/processor/processor_force_torque_inertial.cpp
index 0e2720f8f118b80c7592bc73780c4c8f07696c62..8b54a8cb543393a0bd4198d95e214fc9c65f0d0a 100644
--- a/src/processor/processor_force_torque_inertial.cpp
+++ b/src/processor/processor_force_torque_inertial.cpp
@@ -60,16 +60,16 @@ ProcessorForceTorqueInertial::~ProcessorForceTorqueInertial()
}
CaptureMotionPtr ProcessorForceTorqueInertial::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_ptr)
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin_ptr)
{
- CaptureMotionPtr capture = CaptureBase::emplace<CaptureMotion>(_frame_own, "CaptureForceTorqueInertial", _ts,
- _sensor, _data, _data_cov, _capture_origin_ptr);
+ CaptureMotionPtr capture = CaptureBase::emplace<CaptureMotion>(
+ _frame_own, "CaptureForceTorqueInertial", _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
setCalibration(capture, _calib);
capture->setCalibrationPreint(_calib_preint);
return capture;
@@ -82,15 +82,15 @@ FeatureBasePtr ProcessorForceTorqueInertial::emplaceFeature(CaptureMotionPtr _ca
}
FactorBasePtr ProcessorForceTorqueInertial::emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin)
+ CaptureBasePtr _capture_origin)
{
FactorBasePtr returnValue;
return returnValue;
}
-void ProcessorForceTorqueInertial::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
+void ProcessorForceTorqueInertial::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
{
- _capture->getStateBlock('I')->setState(_calibration); // Set IMU bias
+ _capture->getStateBlock('I')->setState(_calibration); // Set IMU bias
}
void ProcessorForceTorqueInertial::configure(SensorBasePtr _sensor)
@@ -99,12 +99,12 @@ void ProcessorForceTorqueInertial::configure(SensorBasePtr _sensor)
}
void ProcessorForceTorqueInertial::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& _jacobian_calib) const
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const
{
// compute tangent by removing IMU bias
Matrix<double, 12, 1> tangent = _data;
@@ -126,27 +126,27 @@ void ProcessorForceTorqueInertial::computeCurrentDelta(const Eigen::VectorXd& _d
}
void ProcessorForceTorqueInertial::deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _dt2,
- Eigen::VectorXd& _delta1_plus_delta2) const
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const
{
bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2);
}
void ProcessorForceTorqueInertial::deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _dt2,
- Eigen::VectorXd& _delta1_plus_delta2,
- Eigen::MatrixXd& _jacobian1,
- Eigen::MatrixXd& _jacobian2) const
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const
{
bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
void ProcessorForceTorqueInertial::statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _dt,
- VectorComposite& _x_plus_delta) const
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const
{
auto x = _x.vector("PVLO");
VectorXd x_plus_delta = bodydynamics::fti::composeOverState(x, _delta, _dt);
@@ -154,7 +154,7 @@ void ProcessorForceTorqueInertial::statePlusDelta(const VectorComposite& _x,
}
Eigen::VectorXd ProcessorForceTorqueInertial::correctDelta(const Eigen::VectorXd& _delta_preint,
- const Eigen::VectorXd& _delta_step) const
+ const Eigen::VectorXd& _delta_step) const
{
return fti::plus(_delta_preint, _delta_step);
}
diff --git a/src/processor/processor_force_torque_inertial_dynamics.cpp b/src/processor/processor_force_torque_inertial_dynamics.cpp
index 31380dcb7f0a2585b814d10f1b20a8f842506dc4..90746a9adca85359bac3178971c66e56b312d907 100644
--- a/src/processor/processor_force_torque_inertial_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_dynamics.cpp
@@ -61,13 +61,13 @@ ProcessorForceTorqueInertialDynamics::~ProcessorForceTorqueInertialDynamics()
}
CaptureMotionPtr ProcessorForceTorqueInertialDynamics::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_ptr)
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin_ptr)
{
CaptureMotionPtr capture = CaptureBase::emplace<CaptureMotion>(
_frame_own, "CaptureMotion", _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
@@ -76,10 +76,8 @@ CaptureMotionPtr ProcessorForceTorqueInertialDynamics::emplaceCapture(const Fram
return capture;
}
-
FeatureBasePtr ProcessorForceTorqueInertialDynamics::emplaceFeature(CaptureMotionPtr _capture_own)
{
-
auto motion = _capture_own->getBuffer().back();
FeatureBasePtr ftr = FeatureBase::emplace<FeatureMotion>(_capture_own,
"FeatureMotion",
@@ -91,7 +89,7 @@ FeatureBasePtr ProcessorForceTorqueInertialDynamics::emplaceFeature(CaptureMotio
}
FactorBasePtr ProcessorForceTorqueInertialDynamics::emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin)
+ CaptureBasePtr _capture_origin)
{
FeatureMotionPtr ftr_ftipd = std::static_pointer_cast<FeatureMotion>(_feature_motion);
@@ -101,17 +99,17 @@ FactorBasePtr ProcessorForceTorqueInertialDynamics::emplaceFactor(FeatureBasePtr
}
void ProcessorForceTorqueInertialDynamics::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin,
- CaptureMotionPtr _capture_own)
+ CaptureMotionPtr _capture_own)
{
// 1. Feature and factor FTI -- force-torque-inertial
//----------------------------------------------------
auto motion = _capture_own->getBuffer().back();
auto ftr_fti = FeatureBase::emplace<FeatureMotion>(_capture_own,
- "FeatureMotion",
- motion.delta_integr_,
- motion.delta_integr_cov_ + unmeasured_perturbation_cov_,
- _capture_own->getCalibrationPreint(),
- motion.jacobian_calib_);
+ "FeatureMotion",
+ motion.delta_integr_,
+ motion.delta_integr_cov_ + unmeasured_perturbation_cov_,
+ _capture_own->getCalibrationPreint(),
+ motion.jacobian_calib_);
FactorBase::emplace<FactorForceTorqueInertialDynamics>(
ftr_fti, ftr_fti, _capture_origin, shared_from_this(), params_->apply_loss_function);
@@ -120,11 +118,10 @@ 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); // ??
-
// 3. Feature and factor bias -- IMU bias drift for acc and gyro
//---------------------------------------------------------------
// - This factor only if IMU biases are Dynamic (check sensor->isStateBlockDynamic('I'))
@@ -160,8 +157,7 @@ void ProcessorForceTorqueInertialDynamics::emplaceFeaturesAndFactors(CaptureBase
}
}
-void ProcessorForceTorqueInertialDynamics::setCalibration(const CaptureBasePtr _capture,
- const VectorXd& _calibration)
+void ProcessorForceTorqueInertialDynamics::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
{
_capture->getStateBlock('I')->setState(_calibration.segment<6>(0)); // Set IMU bias
_capture->getStateBlock('C')->setState(_calibration.segment<3>(6)); // Set C
@@ -184,20 +180,20 @@ void ProcessorForceTorqueInertialDynamics::configure(SensorBasePtr _sensor)
}
void ProcessorForceTorqueInertialDynamics::data2tangent(const Eigen::VectorXd& _data,
- const Eigen::VectorXd& _bias,
- const Eigen::VectorXd& _model,
- Eigen::VectorXd& _tangent,
- Eigen::MatrixXd& _J_tangent_data,
- Eigen::MatrixXd& _J_tangent_bias,
- Eigen::MatrixXd& _J_tangent_model) const
+ const Eigen::VectorXd& _bias,
+ const Eigen::VectorXd& _model,
+ Eigen::VectorXd& _tangent,
+ Eigen::MatrixXd& _J_tangent_data,
+ Eigen::MatrixXd& _J_tangent_bias,
+ Eigen::MatrixXd& _J_tangent_model) const
{
- const Vector6d& awd = _data.segment<6>(0); // this is (a,w) of data in a 6-vector
- const Vector3d& fd = _data.segment<3>(6);
- const Vector3d& td = _data.segment<3>(9);
- const Vector3d& c = _model.segment<3>(0);
+ const Vector6d& awd = _data.segment<6>(0); // this is (a,w) of data in a 6-vector
+ const Vector3d& fd = _data.segment<3>(6);
+ const Vector3d& td = _data.segment<3>(9);
+ const Vector3d& c = _model.segment<3>(0);
const Matrix3d& fd_cross = skew(fd);
- const Matrix3d& c_cross = skew(c);
+ const Matrix3d& c_cross = skew(c);
// tangent(a,w) = data(a,w) - bias(a,w)
// tangent(f) = data(f)
@@ -215,17 +211,17 @@ void ProcessorForceTorqueInertialDynamics::data2tangent(const Eigen::VectorXd& _
_J_tangent_bias.bottomRows<6>() = Matrix6d::Zero();
// J_tangent_model
- _J_tangent_model.setZero(12,7);
+ _J_tangent_model.setZero(12, 7);
_J_tangent_model.block<3, 3>(9, 0) = fd_cross; // J_tt_c = fd_cross
}
void ProcessorForceTorqueInertialDynamics::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& _jacobian_calib) const
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const
{
// compute tangent by removing IMU bias
@@ -425,27 +421,27 @@ void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::Vect
}
void ProcessorForceTorqueInertialDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _dt2,
- Eigen::VectorXd& _delta1_plus_delta2) const
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const
{
bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2);
}
void ProcessorForceTorqueInertialDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _dt2,
- Eigen::VectorXd& _delta1_plus_delta2,
- Eigen::MatrixXd& _jacobian1,
- Eigen::MatrixXd& _jacobian2) const
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const
{
bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
void ProcessorForceTorqueInertialDynamics::statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _dt,
- VectorComposite& _x_plus_delta) const
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const
{
auto x = _x.vector("PVLO");
VectorXd x_plus_delta = bodydynamics::fti::composeOverState(x, _delta, _dt);
@@ -453,7 +449,7 @@ void ProcessorForceTorqueInertialDynamics::statePlusDelta(const VectorComposite&
}
Eigen::VectorXd ProcessorForceTorqueInertialDynamics::correctDelta(const Eigen::VectorXd& _delta_preint,
- const Eigen::VectorXd& _delta_step) const
+ const Eigen::VectorXd& _delta_step) const
{
return fti::plus(_delta_preint, _delta_step);
}
@@ -485,8 +481,7 @@ VectorXd ProcessorForceTorqueInertialDynamics::getCalibration(const CaptureBaseC
bool ProcessorForceTorqueInertialDynamics::voteForKeyFrame() const
{
// time span
- if (getBuffer().back().ts_ - getBuffer().front().ts_ >
- params_force_torque_inertial_dynamics_->max_time_span)
+ if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_force_torque_inertial_dynamics_->max_time_span)
{
WOLF_DEBUG("PM: vote: time span");
return true;
diff --git a/src/sensor/sensor_force_torque_inertial.cpp b/src/sensor/sensor_force_torque_inertial.cpp
index 89fac3f9749e224738807467384d8aa837571638..aa38e8ab62d4bd0bdc0fee48bd15c227ea52fc8d 100644
--- a/src/sensor/sensor_force_torque_inertial.cpp
+++ b/src/sensor/sensor_force_torque_inertial.cpp
@@ -41,14 +41,13 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
false),
params_fti_(_params)
{
- addStateBlock('C', FactoryStateBlock::create("StateParams3", params_fti_->com, true)); // centre of mass
+ addStateBlock('C', FactoryStateBlock::create("StateParams3", params_fti_->com, true)); // centre of mass
addStateBlock('i', FactoryStateBlock::create("StateParams3", params_fti_->inertia, true)); // inertial vector
addStateBlock('m', FactoryStateBlock::create("StateParams1", Vector1d(params_fti_->mass), true)); // total mass
setStateBlockDynamic('I', false);
setStateBlockDynamic('C', false);
setStateBlockDynamic('i', false);
setStateBlockDynamic('m', false);
-
}
// TODO: Adapt this API to that of MR !448
@@ -76,7 +75,8 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorComposite&
} // namespace wolf
#include <core/sensor/factory_sensor.h>
-namespace wolf{
+namespace wolf
+{
WOLF_REGISTER_SENSOR(SensorForceTorqueInertial);
WOLF_REGISTER_SENSOR_AUTO(SensorForceTorqueInertial);
-}
\ No newline at end of file
+} // namespace wolf
\ No newline at end of file