diff --git a/include/bodydynamics/processor/processor_force_torque.h b/include/bodydynamics/processor/processor_force_torque.h
index 8c9a8d4ab0a8bf944a86f88c8d7ad44bfb330d33..1b8197fc27350412ec330feb802d0a5751bcb916 100644
--- a/include/bodydynamics/processor/processor_force_torque.h
+++ b/include/bodydynamics/processor/processor_force_torque.h
@@ -104,9 +104,8 @@ class ProcessorForceTorque : public ProcessorMotion{
const VectorXd& _calib,
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin) override;
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override;
+ void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+
private:
ParamsProcessorForceTorquePtr params_motion_force_torque_;
diff --git a/include/bodydynamics/processor/processor_force_torque_inertial.h b/include/bodydynamics/processor/processor_force_torque_inertial.h
index 616411f510f74ff6fa12d26d5270ce0252bb5dee..c51dff8a183d77f33856abd55fc665e44b703024 100644
--- a/include/bodydynamics/processor/processor_force_torque_inertial.h
+++ b/include/bodydynamics/processor/processor_force_torque_inertial.h
@@ -70,6 +70,7 @@ class ProcessorForceTorqueInertial : public ProcessorMotion
protected:
ParamsProcessorForceTorqueInertialPtr params_force_torque_inertial_;
SensorForceTorqueInertialPtr sensor_fti_;
+ Matrix6d imu_drift_cov_;
public:
/** \brief convert raw CaptureMotion data to the delta-state format.
@@ -225,16 +226,8 @@ class ProcessorForceTorqueInertial : public ProcessorMotion
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin_ptr) override;
- /** \brief emplace a feature corresponding to given capture and add the feature to this capture
- * \param _capture_motion: the parent capture
- */
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
+ void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
- /** \brief emplace a factor and link it in the wolf tree
- * \param _feature_motion: the parent feature
- * \param _frame_origin: the frame constrained by this motion factor
- */
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
diff --git a/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h b/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
index c05887e9614252f80afd8861e4b7fc16fe45243c..dd9eef9ee86e6fd6daab00cdd142fc6713217d59 100644
--- a/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
+++ b/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
@@ -231,17 +231,6 @@ class ProcessorForceTorqueInertialDynamics : public ProcessorMotion
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin_ptr) override;
- /** \brief emplace a feature corresponding to given capture and add the feature to this capture
- * \param _capture_motion: the parent capture
- */
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
-
- /** \brief emplace a factor and link it in the wolf tree
- * \param _feature_motion: the parent feature
- * \param _capture_origin: the capture constrained by this motion factor
- */
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) override;
-
/** \brief Emplace features and factors
*
* This processor emplaces tree possible factors (with its features):
diff --git a/src/processor/processor_force_torque.cpp b/src/processor/processor_force_torque.cpp
index d9bd77e5474df95514c60c72c5f3f138e92415b4..dd7f5806463b14bbe56ee458d542048675879b7a 100644
--- a/src/processor/processor_force_torque.cpp
+++ b/src/processor/processor_force_torque.cpp
@@ -126,17 +126,17 @@ CaptureMotionPtr ProcessorForceTorque::emplaceCapture(const FrameBasePtr& _fra
return cap;
}
-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_);
- return feature;
-}
+// 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_);
+// return feature;
+// }
Eigen::VectorXd ProcessorForceTorque::correctDelta(const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const
@@ -190,21 +190,27 @@ void ProcessorForceTorque::setCalibration(const CaptureBasePtr _capture, const V
cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->setState(calib_imu);
}
-FactorBasePtr ProcessorForceTorque::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+void ProcessorForceTorque::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
{
+ // Retrieving the origin capture and getting the bias from here should be done here?
+ auto feature = FeatureBase::emplace<FeatureForceTorque>(
+ _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_);
+
CaptureForceTorquePtr cap_ft_origin = std::static_pointer_cast<CaptureForceTorque>(_capture_origin);
- FeatureForceTorquePtr ftr_ft = std::static_pointer_cast<FeatureForceTorque>(_feature_motion);
+ FeatureForceTorquePtr ftr_ft = std::static_pointer_cast<FeatureForceTorque>(feature);
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;
+ 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);
}
void ProcessorForceTorque::computeCurrentDelta(const Eigen::VectorXd& _data,
diff --git a/src/processor/processor_force_torque_inertial.cpp b/src/processor/processor_force_torque_inertial.cpp
index 8b54a8cb543393a0bd4198d95e214fc9c65f0d0a..dc3256c2674eb1205a59c98ece711c9fb2f734c8 100644
--- a/src/processor/processor_force_torque_inertial.cpp
+++ b/src/processor/processor_force_torque_inertial.cpp
@@ -34,6 +34,7 @@
// wolf
#include <core/state_block/state_composite.h>
#include <core/capture/capture_motion.h>
+#include <core/factor/factor_block_difference.h>
namespace wolf
{
@@ -75,18 +76,58 @@ CaptureMotionPtr ProcessorForceTorqueInertial::emplaceCapture(const FrameBasePtr
return capture;
}
-FeatureBasePtr ProcessorForceTorqueInertial::emplaceFeature(CaptureMotionPtr _capture_own)
+void ProcessorForceTorqueInertial::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin,
+ CaptureMotionPtr _capture_own)
{
- FeatureBasePtr returnValue;
- return returnValue;
+ // 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_);
+
+ FactorBase::emplace<FactorForceTorqueInertial>(
+ ftr_fti, ftr_fti, _capture_origin, shared_from_this(), params_->apply_loss_function);
+
+
+ // 2. Feature and factor bias -- IMU bias drift for acc and gyro
+ //---------------------------------------------------------------
+ // - This factor only if IMU biases are Dynamic (check sensor->isStateBlockDynamic('I'))
+ // - feature has zero measurement size 6, with the cov of the bias drift size 6x6
+ // - factor relates bias(last capture) and bias(origin capture)
+ 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
+ {
+ auto dt = _capture_own->getTimeStamp() - _capture_origin->getTimeStamp();
+ auto ftr_bias =
+ FeatureBase::emplace<FeatureBase>(_capture_own,
+ "FeatureBase",
+ Vector6d::Zero(), // mean IMU drift is zero
+ imu_drift_cov_ * dt); // IMU drift cov specified in continuous time
+ 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
+ false); // loss function
+ }
+ }
}
-FactorBasePtr ProcessorForceTorqueInertial::emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin)
-{
- FactorBasePtr returnValue;
- return returnValue;
-}
void ProcessorForceTorqueInertial::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
{
@@ -96,6 +137,14 @@ void ProcessorForceTorqueInertial::setCalibration(const CaptureBasePtr _capture,
void ProcessorForceTorqueInertial::configure(SensorBasePtr _sensor)
{
sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor);
+
+
+ auto acc_drift_std = sensor_fti_->getParamsSensorForceTorqueInertial()->acc_drift_std;
+ auto gyro_drift_std = sensor_fti_->getParamsSensorForceTorqueInertial()->gyro_drift_std;
+
+ ArrayXd imu_drift_sigmas(6);
+ imu_drift_sigmas << acc_drift_std, acc_drift_std, acc_drift_std, gyro_drift_std, gyro_drift_std, gyro_drift_std;
+ imu_drift_cov_ = imu_drift_sigmas.square().matrix().asDiagonal();
}
void ProcessorForceTorqueInertial::computeCurrentDelta(const Eigen::VectorXd& _data,
diff --git a/src/processor/processor_force_torque_inertial_dynamics.cpp b/src/processor/processor_force_torque_inertial_dynamics.cpp
index 8becfc0a198f2c4169067d9ceae7b81ecc0ed4bd..678074d6f632f70b47712da7ffcf056fa8a64146 100644
--- a/src/processor/processor_force_torque_inertial_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_dynamics.cpp
@@ -77,28 +77,6 @@ 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",
- motion.delta_integr_,
- motion.delta_integr_cov_ + unmeasured_perturbation_cov_,
- _capture_own->getCalibrationPreint(),
- motion.jacobian_calib_);
- return ftr;
-}
-
-FactorBasePtr ProcessorForceTorqueInertialDynamics::emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin)
-{
- FeatureMotionPtr ftr_ftipd = std::static_pointer_cast<FeatureMotion>(_feature_motion);
-
- auto fac_ftipd = FactorBase::emplace<FactorForceTorqueInertialDynamics>(
- _feature_motion, ftr_ftipd, _capture_origin, shared_from_this(), params_->apply_loss_function);
- return fac_ftipd;
-}
-
void ProcessorForceTorqueInertialDynamics::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin,
CaptureMotionPtr _capture_own)
{