Skip to content
Snippets Groups Projects
Commit c1a8daa8 authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

Attempt to include cost computation in factor inertial kin and ft preint.... 

Attempt to include cost computation in factor inertial kin and ft preint. Works only for inertial kin.
parent 8254fdd9
No related branches found
No related tags found
3 merge requests!18Release after RAL,!17After 2nd RAL submission,!5WIP: Resolve "Any contact type/number refactoring"
Hide whitespace changes
Inline Side-by-side
include/bodydynamics/factor/factor_force_torque_preint.h
+ 86
− 173
View file @ c1a8daa8
......@@ -52,12 +52,6 @@ class FactorForceTorquePreint : public FactorAutodiff<FactorForceTorquePreint, 1
const T* const _q2,
T* _res) const;
/** \brief Estimation error given the states in the wolf tree
*
*/
// Eigen::Matrix<double, 12, 1> error();
/** \brief : compute the residual from the state blocks being iterated by the solver. (same as operator())
-> computes the expected measurement
-> corrects actual measurement with new bias
......@@ -77,71 +71,38 @@ class FactorForceTorquePreint : public FactorAutodiff<FactorForceTorquePreint, 1
* _res: Factor residuals (c,cd,Lc,o) O is rotation vector... NOT A QUATERNION
*/
template<typename D1, typename D2, typename D3, typename D4>
bool residual(const Eigen::MatrixBase<D1> & _c1,
const Eigen::MatrixBase<D1> & _cd1,
const Eigen::MatrixBase<D1> & _Lc1,
const Eigen::QuaternionBase<D2> & _q1,
const Eigen::MatrixBase<D1> & _bp1,
const Eigen::MatrixBase<D1> & _bw1,
const Eigen::MatrixBase<D1> & _c2,
const Eigen::MatrixBase<D1> & _cd2,
const Eigen::MatrixBase<D1> & _Lc2,
const Eigen::QuaternionBase<D3> & _q2,
Eigen::MatrixBase<D4> & _res) const;
/** Function expectation(...)
* params :
* _c1 : COM position at time t1 in world frame
* _cd1: COM velocity at time t1 in world frame
* _Lc1: Angular momentum at time t1 in world frame
* _q1 : Base orientation at time t1
* _bp1: COM position measurement at time t1 in body frame
* _bw1: gyroscope bias at time t1 in body frame
* _c1 : COM position at time t1 in world frame
* _cd1: COM velocity at time t1 in world frame
* _Lc1: Angular momentum at time t1 in world frame
* _dce : expected COM position delta
* _dcde: expected COM velocity delta
* _dLce: expected Angular momentum delta
* _dqe : expected orientation delta
*/
template<typename D1, typename D2, typename D3>
void expectation(const Eigen::MatrixBase<D1> & _c1,
const Eigen::MatrixBase<D1> & _cd1,
const Eigen::MatrixBase<D1> & _Lc1,
const Eigen::QuaternionBase<D2> & _q1,
const Eigen::MatrixBase<D1> & _bp1,
const Eigen::MatrixBase<D1> & _bw1,
const Eigen::MatrixBase<D1> & _c2,
const Eigen::MatrixBase<D1> & _cd2,
const Eigen::MatrixBase<D1> & _Lc2,
const Eigen::QuaternionBase<D2> & _q2,
Eigen::MatrixBase<D3> & _dcexp,
Eigen::MatrixBase<D3> & _dcdexp,
Eigen::MatrixBase<D3> & _dLcexp,
Eigen::QuaternionBase<D3> & _dqexp) const;
// /** \brief : return the expected delta given the state blocks in the wolf tree
// */
// Eigen::VectorXd expectation() const;
bool residual(const MatrixBase<D1> & _c1,
const MatrixBase<D1> & _cd1,
const MatrixBase<D1> & _Lc1,
const QuaternionBase<D2> & _q1,
const MatrixBase<D1> & _bp1,
const MatrixBase<D1> & _bw1,
const MatrixBase<D1> & _c2,
const MatrixBase<D1> & _cd2,
const MatrixBase<D1> & _Lc2,
const QuaternionBase<D3> & _q2,
MatrixBase<D4> & _res) const;
// Matrix<double,12,1> residual() const;
// double cost() const;
private:
/// Preintegrated delta
Eigen::Vector3d dc_preint_;
Eigen::Vector3d dcd_preint_;
Eigen::Vector3d dLc_preint_;
Eigen::Quaterniond dq_preint_;
Vector3d dc_preint_;
Vector3d dcd_preint_;
Vector3d dLc_preint_;
Quaterniond dq_preint_;
// Biases used during preintegration
Eigen::Vector3d pbc_bias_preint_;
Eigen::Vector3d gyro_bias_preint_;
Vector3d pbc_bias_preint_;
Vector3d gyro_bias_preint_;
// Jacobians of preintegrated deltas wrt biases
Eigen::Matrix3d J_dLc_pb_;
Eigen::Matrix3d J_dc_wb_;
Eigen::Matrix3d J_dcd_wb_;
Eigen::Matrix3d J_dLc_wb_;
Eigen::Matrix3d J_dq_wb_;
Matrix3d J_dLc_pb_;
Matrix3d J_dc_wb_;
Matrix3d J_dcd_wb_;
Matrix3d J_dLc_wb_;
Matrix3d J_dq_wb_;
/// Metrics
double dt_; ///< delta-time and delta-time-squared between keyframes
......@@ -210,15 +171,12 @@ inline bool FactorForceTorquePreint::operator ()(const T* const _c1,
const T* const _q2,
T* _res) const
{
using namespace Eigen;
// MAPS
Map<const Matrix<T,3,1> > c1(_c1);
Map<const Matrix<T,3,1> > cd1(_cd1);
Map<const Matrix<T,3,1> > Lc1(_Lc1);
Map<const Quaternion<T> > q1(_q1);
Map<const Matrix<T,3,1> > bp1(_bp1);
Map<const Matrix<T,3,1> > bw1(_bw1 + 3);
Map<const Matrix<T,3,1> > bw1(_bw1 + 3); // bw1 = bimu = [ba, bw]
Map<const Matrix<T,3,1> > c2(_c2);
Map<const Matrix<T,3,1> > cd2(_cd2);
Map<const Matrix<T,3,1> > Lc2(_Lc2);
......@@ -230,42 +188,18 @@ inline bool FactorForceTorquePreint::operator ()(const T* const _c1,
return true;
}
// Eigen::Vector12d FactorForceTorquePreint::error()
// {
// Vector6d bias = capture_other_ptr_.lock()->getCalibration();
// Map<const Vector3d > acc_bias(bias.data());
// Map<const Vector3d > gyro_bias(bias.data() + 3);
// Eigen::Vector9d delta_exp = expectation();
// Eigen::Vector9d delta_preint = getMeasurement();
// Eigen::Vector9d delta_step;
// delta_step.head<3>() = dDp_dab_ * (acc_bias - acc_bias_preint_ ) + dDp_dwb_ * (gyro_bias - gyro_bias_preint_);
// delta_step.segment<3>(3) = dDq_dwb_ * (gyro_bias - gyro_bias_preint_);
// delta_step.tail<3>() = dDv_dab_ * (acc_bias - acc_bias_preint_ ) + dDv_dwb_ * (gyro_bias - gyro_bias_preint_);
// Eigen::VectorXd delta_corr = imu::plus(delta_preint, delta_step);
// Eigen::Vector9d res = imu::diff(delta_exp, delta_corr);
// return res;
// }
template<typename D1, typename D2, typename D3, typename D4>
inline bool FactorForceTorquePreint::residual(const Eigen::MatrixBase<D1> & _c1,
const Eigen::MatrixBase<D1> & _cd1,
const Eigen::MatrixBase<D1> & _Lc1,
const Eigen::QuaternionBase<D2> & _q1,
const Eigen::MatrixBase<D1> & _bp1,
const Eigen::MatrixBase<D1> & _bw1,
const Eigen::MatrixBase<D1> & _c2,
const Eigen::MatrixBase<D1> & _cd2,
const Eigen::MatrixBase<D1> & _Lc2,
const Eigen::QuaternionBase<D3> & _q2,
Eigen::MatrixBase<D4> & _res) const
inline bool FactorForceTorquePreint::residual(const MatrixBase<D1> & _c1,
const MatrixBase<D1> & _cd1,
const MatrixBase<D1> & _Lc1,
const QuaternionBase<D2> & _q1,
const MatrixBase<D1> & _bp1,
const MatrixBase<D1> & _bw1,
const MatrixBase<D1> & _c2,
const MatrixBase<D1> & _cd2,
const MatrixBase<D1> & _Lc2,
const QuaternionBase<D3> & _q2,
MatrixBase<D4> & _res) const
{
/* Help for the Imu residual function
*
......@@ -311,32 +245,26 @@ inline bool FactorForceTorquePreint::residual(const Eigen::MatrixBase<D1> &
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D4, 12);
// 1. Expected delta from states
Eigen::Matrix<T, 3, 1 > dc_exp;
Eigen::Matrix<T, 3, 1 > dcd_exp;
Eigen::Matrix<T, 3, 1 > dLc_exp;
Eigen::Quaternion<T> dq_exp;
Matrix<T, 3, 1 > dc_exp;
Matrix<T, 3, 1 > dcd_exp;
Matrix<T, 3, 1 > dLc_exp;
Quaternion<T> dq_exp;
bodydynamics::betweenStates(_c1, _cd1, _Lc1, _q1, _c2, _cd2, _Lc2, _q2, dt_, dc_exp, dcd_exp, dLc_exp, dq_exp);
// 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint)
// 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint)
Eigen::Matrix<T, 3, 1> dc_step = J_dc_wb_ * (_bw1 - gyro_bias_preint_);
Eigen::Matrix<T, 3, 1> dcd_step = J_dcd_wb_ * (_bw1 - gyro_bias_preint_);
Eigen::Matrix<T, 3, 1> dLc_step = J_dLc_pb_ * (_bp1 - pbc_bias_preint_) + J_dLc_wb_ * (_bw1 - gyro_bias_preint_);
Eigen::Matrix<T, 3, 1> do_step = J_dq_wb_ * (_bw1 - gyro_bias_preint_);
// std::cout << "\n\nJ_dLc_pb_" << std::endl;
// std::cout << J_dLc_pb_ << std::endl;
// FullPivLU<Matrix3d> lu_decomp(J_dLc_pb_);
// std::cout << "J_dLc_pb_ rank: " << lu_decomp.rank() << std::endl;
Matrix<T, 3, 1> dc_step = J_dc_wb_ * (_bw1 - gyro_bias_preint_);
Matrix<T, 3, 1> dcd_step = J_dcd_wb_ * (_bw1 - gyro_bias_preint_);
Matrix<T, 3, 1> dLc_step = J_dLc_pb_ * (_bp1 - pbc_bias_preint_) + J_dLc_wb_ * (_bw1 - gyro_bias_preint_);
Matrix<T, 3, 1> do_step = J_dq_wb_ * (_bw1 - gyro_bias_preint_);
// 2.b. Add the correction step to the preintegrated delta: delta_cor = delta_preint (+) step
Eigen::Matrix<T,3,1> dc_correct;
Eigen::Matrix<T,3,1> dcd_correct;
Eigen::Matrix<T,3,1> dLc_correct;
Eigen::Quaternion<T> dq_correct;
Matrix<T,3,1> dc_correct;
Matrix<T,3,1> dcd_correct;
Matrix<T,3,1> dLc_correct;
Quaternion<T> dq_correct;
bodydynamics::plus(dc_preint_.cast<T>(), dcd_preint_.cast<T>(), dLc_preint_.cast<T>(), dq_preint_.cast<T>(),
dc_step, dcd_step, dLc_step, do_step,
......@@ -344,11 +272,11 @@ inline bool FactorForceTorquePreint::residual(const Eigen::MatrixBase<D1> &
// 3. Delta error in minimal form: D_err = diff(D_exp , D_corr)
// Note the Dt here is zero because it's the delta-time between the same time stamps!
Eigen::Matrix<T, 12, 1> d_error;
Eigen::Map<Eigen::Matrix<T, 3, 1> > dc_error (d_error.data() );
Eigen::Map<Eigen::Matrix<T, 3, 1> > dcd_error(d_error.data() + 3);
Eigen::Map<Eigen::Matrix<T, 3, 1> > dLc_error(d_error.data() + 6);
Eigen::Map<Eigen::Matrix<T, 3, 1> > do_error (d_error.data() + 9);
Matrix<T, 12, 1> d_error;
Map<Matrix<T, 3, 1> > dc_error (d_error.data() );
Map<Matrix<T, 3, 1> > dcd_error(d_error.data() + 3);
Map<Matrix<T, 3, 1> > dLc_error(d_error.data() + 6);
Map<Matrix<T, 3, 1> > do_error (d_error.data() + 9);
bodydynamics::diff(dc_exp, dcd_exp, dLc_exp, dq_exp,
......@@ -360,53 +288,38 @@ inline bool FactorForceTorquePreint::residual(const Eigen::MatrixBase<D1> &
return true;
}
//inline Eigen::VectorXd FactorForceTorquePreint::expectation() const
//{
// FrameBasePtr frm_current = getFeature()->getFrame();
// FrameBasePtr frm_previous = getFrameOther();
// //get information on current_frame in the FactorForceTorquePreint
// const Eigen::Vector3d frame_current_pos = (frm_current->getP()->getState());
// const Eigen::Quaterniond frame_current_ori (frm_current->getO()->getState().data());
// const Eigen::Vector3d frame_current_vel = (frm_current->getV()->getState());
// // get info on previous frame in the FactorForceTorquePreint
// const Eigen::Vector3d frame_previous_pos = (frm_previous->getP()->getState());
// const Eigen::Quaterniond frame_previous_ori (frm_previous->getO()->getState().data());
// const Eigen::Vector3d frame_previous_vel = (frm_previous->getV()->getState());
// // Define results vector and Map bits over it
// Eigen::Matrix<double, 10, 1> exp;
// Eigen::Map<Eigen::Matrix<double, 3, 1> > pe(exp.data() );
// Eigen::Map<Eigen::Quaternion<double> > qe(exp.data() + 3);
// Eigen::Map<Eigen::Matrix<double, 3, 1> > ve(exp.data() + 7);
// expectation(frame_previous_pos, frame_previous_ori, frame_previous_vel,
// frame_current_pos, frame_current_ori, frame_current_vel,
// dt_,
// pe, qe, ve);
// return exp;
//}
template<typename D1, typename D2, typename D3>
inline void FactorForceTorquePreint::expectation(const Eigen::MatrixBase<D1> & _c1,
const Eigen::MatrixBase<D1> & _cd1,
const Eigen::MatrixBase<D1> & _Lc1,
const Eigen::QuaternionBase<D2> & _q1,
const Eigen::MatrixBase<D1> & _bp1,
const Eigen::MatrixBase<D1> & _bw1,
const Eigen::MatrixBase<D1> & _c2,
const Eigen::MatrixBase<D1> & _cd2,
const Eigen::MatrixBase<D1> & _Lc2,
const Eigen::QuaternionBase<D2> & _q2,
Eigen::MatrixBase<D3> & _dcexp,
Eigen::MatrixBase<D3> & _dcdexp,
Eigen::MatrixBase<D3> & _dLcexp,
Eigen::QuaternionBase<D3> & _dqexp) const
{
bodydynamics::betweenStates(_c1, _cd1, _Lc1, _q1, _bp1, _bw1, _c2, _cd2, _Lc2, _q2, dt_, _dcexp, _dcdexp, _dLcexp, _dqexp);
}
// Matrix<double,12,1> FactorForceTorquePreint::residual() const
// {
// Matrix<double,12,1> res;
// FrameBasePtr frm_prev = getFrameOther();
// FrameBasePtr frm_curr = getFeature()->getFrame();
// CaptureBaseWPtrList cap_lst = getCaptureOtherList(); // !! not retrieving the rigt captures...
// auto cap_ikin_other = cap_lst.front().lock();
// auto cap_gyro_other = cap_lst.back().lock();
// Map<const Matrix<double,3,1> > c1( frm_prev->getStateBlock('C')->getState().data());
// Map<const Matrix<double,3,1> > cd1(frm_prev->getStateBlock('D')->getState().data());
// Map<const Matrix<double,3,1> > Lc1(frm_prev->getStateBlock('L')->getState().data());
// Map<const Quaternion<double> > q1( frm_prev->getStateBlock('O')->getState().data());
// Map<const Matrix<double,3,1> > bp1(cap_ikin_other->getStateBlock('I')->getState().data());
// Map<const Matrix<double,3,1> > bw1(cap_gyro_other->getStateBlock('I')->getState().data() + 3); // bw1 = bimu = [ba, bw]
// Map<const Matrix<double,3,1> > c2 (frm_curr->getStateBlock('C')->getState().data());
// Map<const Matrix<double,3,1> > cd2(frm_curr->getStateBlock('D')->getState().data());
// Map<const Matrix<double,3,1> > Lc2(frm_curr->getStateBlock('L')->getState().data());
// Map<const Quaternion<double> > q2 (frm_curr->getStateBlock('O')->getState().data());
// residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res);
// return res;
// }
// double FactorForceTorquePreint::cost() const
// {
// Matrix<double,12,1> toto = residual();
// }
} // namespace wolf
......
include/bodydynamics/factor/factor_inertial_kinematics.h
+ 49
− 28
View file @ c1a8daa8
......@@ -32,6 +32,9 @@ class FactorInertialKinematics : public FactorAutodiff<FactorInertialKinematics,
return std::string("GEOM");
}
// to keep track of the imu bias, not the standard way to proceed
StateBlockPtr sb_bias_imu_;
/*
Factor state blocks:
_pb: W_p_WB -> base position in world frame
......@@ -54,6 +57,10 @@ class FactorInertialKinematics : public FactorAutodiff<FactorInertialKinematics,
const T* const _bp,
const T* const _baw,
T* _res) const;
Vector9d residual() const;
double cost() const;
};
} /* namespace wolf */
......@@ -84,10 +91,32 @@ FactorInertialKinematics::FactorInertialKinematics(
_feat->getFrame()->getStateBlock('L'), // Angular momentum
_feat->getCapture()->getStateBlock('I'), // body-to-COM relative position bias
_sb_bias_imu
)
),
sb_bias_imu_(_sb_bias_imu)
{
}
Vector9d FactorInertialKinematics::residual() const{
Vector9d res;
double * pb, * qb, * vb, * c, * cd, * Lc, * bp, * baw;
pb = getFrame()->getP()->getState().data();
qb = getFrame()->getO()->getState().data();
vb = getFrame()->getV()->getState().data();
c = getFrame()->getStateBlock('C')->getState().data();
cd = getFrame()->getStateBlock('D')->getState().data();
Lc = getFrame()->getStateBlock('L')->getState().data();
bp = getCapture()->getStateBlock('I')->getState().data();
baw = sb_bias_imu_->getState().data();
operator() (pb, qb, vb, c, cd, Lc, bp, baw, res.data());
// std::cout << "res: " << res.transpose() << std::endl;
return res;
}
double FactorInertialKinematics::cost() const{
return residual().squaredNorm();
}
template<typename T>
bool FactorInertialKinematics::operator () (
const T* const _pb,
......@@ -100,42 +129,34 @@ bool FactorInertialKinematics::operator () (
const T* const _baw,
T* _res) const
{
FeatureInertialKinematicsPtr feat = std::static_pointer_cast<FeatureInertialKinematics>(getFeature());
// State variables instanciation
Eigen::Map<const Eigen::Matrix<T,3,1> > pb(_pb);
Eigen::Map<const Eigen::Quaternion<T> > qb(_qb);
Eigen::Map<const Eigen::Matrix<T,3,1> > vb(_vb);
Eigen::Map<const Eigen::Matrix<T,3,1> > c(_c);
Eigen::Map<const Eigen::Matrix<T,3,1> > cd(_cd);
Eigen::Map<const Eigen::Matrix<T,3,1> > Lc(_Lc);
Eigen::Map<const Eigen::Matrix<T,3,1> > bp(_bp);
Eigen::Map<const Eigen::Matrix<T,6,1> > baw(_baw);
Eigen::Map<Eigen::Matrix<T,9,1> > res(_res);
Map<const Matrix<T,3,1> > pb(_pb);
Map<const Quaternion<T> > qb(_qb);
Map<const Matrix<T,3,1> > vb(_vb);
Map<const Matrix<T,3,1> > c(_c);
Map<const Matrix<T,3,1> > cd(_cd);
Map<const Matrix<T,3,1> > Lc(_Lc);
Map<const Matrix<T,3,1> > bp(_bp);
Map<const Matrix<T,6,1> > baw(_baw);
Map<Matrix<T,9,1> > res(_res);
FeatureInertialKinematicsPtr feat = std::static_pointer_cast<FeatureInertialKinematics>(getFeature());
// Measurements retrieval
Eigen::Map<const Eigen::Vector3d> pBC_m(getMeasurement().data()); // B_p_BC
Eigen::Map<const Eigen::Vector3d> vBC_m(getMeasurement().data() + 3); // B_v_BC
Eigen::Map<const Eigen::Vector3d> w_m(getMeasurement().data() + 6); // B_wB
Map<const Vector3d> pBC_m(getMeasurement().data()); // B_p_BC
Map<const Vector3d> vBC_m(getMeasurement().data() + 3); // B_v_BC
Map<const Vector3d> w_m(getMeasurement().data() + 6); // B_wB
// Error variable instanciation
Eigen::Matrix<T, 9, 1> err;
Eigen::Map<Eigen::Matrix<T, 3, 1> > err_c(err.data() );
Eigen::Map<Eigen::Matrix<T, 3, 1> > err_cd(err.data() + 3);
Eigen::Map<Eigen::Matrix<T, 3, 1> > err_Lc(err.data() + 6);
Matrix<T, 9, 1> err;
Map<Matrix<T, 3, 1> > err_c(err.data() );
Map<Matrix<T, 3, 1> > err_cd(err.data() + 3);
Map<Matrix<T, 3, 1> > err_Lc(err.data() + 6);
err_c = pBC_m - (qb.conjugate()*(c - pb) + bp);
err_cd = vBC_m + (w_m - baw.tail(3)).cross(pBC_m - bp) - qb.conjugate()*(cd - vb);
err_Lc = feat->getBIq() * (w_m - baw.tail(3)) + feat->getBLcm() - qb.conjugate()*Lc;
// err_Lc = feat->getBLcm() - feat->getBIq()*baw.tail(3) - qb.conjugate()*Lc; // w_m is actually used to compute BLcm beforehand
// TEST: pretend as if getBLcm was a direct measurement of bLc
// err_Lc = feat->getBLcm() - qb.conjugate()*Lc;
// std::cout << "\nFACTOR INERTIAL KINEMATICS" << std::endl;
// std::cout << "w_m" << w_m.transpose() << std::endl;
// std::cout << "feat->getBLcm()" << feat->getBLcm().transpose() << std::endl;
res = getMeasurementSquareRootInformationUpper() * err;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment