//==============================================================================
// CALC REVERSE MOBILIZER HDOT_FM
//==============================================================================
// This is the default implementation for turning HDot_MF into HDot_FM.
// A mobilizer can override this to do it faster.
// We depend on H_FM having already been calculated.
//
// From the Simbody theory manual,
// HDot_FM_w = -R_FM * HDot_MF_w + w_FM_x H_FM_w
//
// HDot_FM_v = -R_FM * HDot_MF_v + w_FM_x H_FM_v
// - (p_FM_x HDot_FM_w
// + (v_FM_x - w_FM_x p_FM_x)H_FM_w)
//
// where "a_x" indicates the cross product matrix of vector a.
//
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcReverseMobilizerHDot_FM(
const SBStateDigest& sbs,
HType& HDot_FM) const
{
const SBTreePositionCache& pc = sbs.getTreePositionCache();
// Must use "upd" here rather than "get" because this is
// called during realize(Velocity).
const SBTreeVelocityCache& vc = sbs.updTreeVelocityCache();
HType HDot_MF;
calcAcrossJointVelocityJacobianDot(sbs, HDot_MF);
const Rotation& R_FM = getX_FM(pc).R();
const Vec3& p_FM = getX_FM(pc).p();
const HType& H_FM = getH_FM(pc);
const Vec3& w_FM = getV_FM(vc)[0];
const Vec3& v_FM = getV_FM(vc)[1];
// Make cross product matrices.
const Mat33 p_FM_x = crossMat(p_FM); // 3 flops
const Mat33 w_FM_x = crossMat(w_FM); // 3 flops
const Mat33 v_FM_x = crossMat(v_FM); // 3 flops
const Mat33 vwp = v_FM_x - w_FM_x*p_FM_x; // 54 flops
// Initialize both rows with the first two terms above.
HDot_FM = w_FM_x*H_FM - (noR_FM ? HDot_MF : R_FM*HDot_MF); // 66*dof flops
// Add in the additional terms in the second row.
HDot_FM[1] -= p_FM_x * HDot_FM[0] + vwp * H_FM[0]; // 36*dof flops
}
void
EstimatorFull::UpdateKeyframe(void)
{
Matrix3d C_q_i_w = q_i_w.toQuat().toRotationMatrix();
Matrix3d C_q_c_i = q_c_i.toQuat().toRotationMatrix();
Matrix<double,3,22> J_p;
Matrix<double,3,22> J_q;
Matrix<double,6,22> J;
// Build jacobians
J_p <<
-C_q_c_i*C_q_i_w,
Matrix3d::Zero(),
C_q_c_i*crossMat( C_q_i_w*p_i_w ),
Matrix3d::Zero(),
Matrix3d::Zero(),
Vector3d::Zero(),
-C_q_c_i,
crossMat( C_q_c_i*(p_c_i + C_q_i_w*p_i_w) );
J_q <<
Matrix3d::Zero(),
Matrix3d::Zero(),
-C_q_i_w.transpose(),
Matrix3d::Zero(),
Matrix3d::Zero(),
Vector3d::Zero(),
Matrix3d::Zero(),
-C_q_c_i.transpose()*C_q_i_w.transpose();
J << J_p, J_q;
// Calculate new keyframe pose
p_w_v = -C_q_c_i*( p_c_i + C_q_i_w*p_i_w );
q_w_v = Eigen::QuaternionAd( q_i_w.toQuat().conjugate() * q_c_i.toQuat().conjugate() );
// Calculate new P matrix
P.block<22,6>(0,22) = P.block<22,22>(0,0)*J.transpose();
P.block<6,6>(22,22) = J*P.block<22,22>(0,0)*J.transpose();
P.block<6,22>(22,0) = P.block<22,6>(0,22).transpose();
}
void
EstimatorFull::UpdateCamera(const Eigen::Vector3d &p_c_v, const Eigen::Quaterniond &q_c_v,
const Eigen::Matrix<double,6,6> &R,
bool absolute, bool isKeyframe, double dLambda )
{
Eigen::QuaternionAd q_c_kf;
Eigen::Vector3d p_c_kf;
if (absolute)
{
q_c_kf = Eigen::QuaternionAd(q_c_v.conjugate() * q_kf_v.toQuat().conjugate());
p_c_kf = q_kf_v.toQuat().toRotationMatrix() * (p_c_v - p_kf_v);
}
else
{
q_c_kf.q = q_c_v;
p_c_kf = p_c_v;
}
Matrix3d C_q_w_v = q_w_v.toQuat().toRotationMatrix();
Matrix3d C_q_i_w = q_i_w.toQuat().toRotationMatrix();
Matrix3d C_q_c_i = q_c_i.toQuat().toRotationMatrix();
// Build Jacobian
Matrix<double,3,28> H_p;
H_p <<
C_q_w_v.transpose() * exp(lambda),
Matrix3d::Zero(),
-C_q_w_v.transpose()*C_q_i_w.transpose()*crossMat(p_c_i)*exp(lambda),
Matrix3d::Zero(),
Matrix3d::Zero(),
(C_q_w_v.transpose()*(C_q_i_w.transpose()*p_c_i + p_i_w) + p_w_v) * exp(lambda),
C_q_w_v.transpose()*C_q_i_w.transpose()*exp(lambda),
Matrix3d::Zero(),
Matrix3d::Identity()*exp(lambda),
-C_q_w_v.transpose()*crossMat( p_i_w + C_q_i_w.transpose()*p_c_i )*exp(lambda);
Matrix<double,3,28> H_q;
H_q <<
Matrix3d::Zero(),
Matrix3d::Zero(),
C_q_c_i,
Matrix3d::Zero(),
Matrix3d::Zero(),
Vector3d::Zero(),
Matrix3d::Zero(),
Matrix3d::Identity(),
Matrix3d::Zero(),
C_q_c_i*C_q_i_w;
Matrix<double,6,28> H;
H << H_p, H_q;
// Calculate residual
Vector3d r_p = p_c_kf - ( C_q_w_v.transpose()*(p_i_w + C_q_i_w.transpose()*p_c_i) + p_w_v ) * exp(lambda);
Quaterniond r_q = (q_c_kf.toQuat()*( q_c_i.toQuat()*q_i_w.toQuat()*q_w_v.toQuat() ).conjugate()).conjugate();
Matrix<double,6,1> r;
r << r_p, 2*r_q.x(), 2*r_q.y(), 2*r_q.z();
// Calculate kalmn gain
Matrix<double,6,6> S = H*P*H.transpose() + R;
//K = P*H.transpose()*S^-1;
//TODO: use cholsky to solve K*S = P*H.transposed()?
// (K*S)' = (P*H')'
// S'*K' = H*P'
// K' = S.transpose.ldlt().solve(H*P.transpose)
Matrix<double,28,6> K = (S.ldlt().solve(H*P)).transpose(); // P and S are symmetric
//Matrix<double,28,6> K = P*H.transpose()*S.inverse();//S.lu().solve(H*P).transpose();
Matrix<double,28,1> x_error = K*r;
// Apply Kalman gain
ApplyCorrection( x_error );
Matrix<double,28,28> T = Matrix<double,28,28>::Identity() - K*H;
P = T*P*T.transpose() + K*R*K.transpose();
// Symmetrize
P = (P + P.transpose())/2.0;
if (isKeyframe)
{
// Add new keyframe from current position/camera pose
UpdateKeyframe( );
// Add noise to lambda
P(15,15) += dLambda;
// Update keyframe reference, if given path is absolute
if (absolute)
{
p_kf_v = p_c_v;
q_kf_v.q = q_c_v;
}
}
}
void
EstimatorFull::PropagateCovariance(const Eigen::Vector3d &omega_m, const Eigen::Vector3d &a_m)
{
Vector3d omega_hat = omega_m - b_omega;
Vector3d a_hat = a_m - b_a;
Matrix3d C_q_w_v = q_w_v.toQuat().toRotationMatrix();
Matrix3d C_q_i_w = q_i_w.toQuat().toRotationMatrix();
Matrix3d C_q_c_i = q_c_i.toQuat().toRotationMatrix();
double Delta_t2 = Delta_t*Delta_t/2.0;
double Delta_t3 = Delta_t*Delta_t*Delta_t/6.0;
double Delta_t4 = Delta_t*Delta_t*Delta_t*Delta_t/24.0;
double Delta_t5 = Delta_t*Delta_t*Delta_t*Delta_t*Delta_t/120.0;
// Build Jacobian
Matrix3d A = -C_q_i_w.transpose()*crossMat(a_hat)
*(Delta_t2*Matrix3d::Identity() - Delta_t3*crossMat(omega_hat) + Delta_t4*crossMat(omega_hat)*crossMat(omega_hat));
Matrix3d B = -C_q_i_w.transpose()*crossMat(a_hat)
*(-Delta_t3*Matrix3d::Identity() + Delta_t4*crossMat(omega_hat) - Delta_t5*crossMat(omega_hat)*crossMat(omega_hat));
Matrix3d C = -C_q_i_w.transpose()*crossMat(a_hat)
*(Delta_t*Matrix3d::Identity() - Delta_t2*crossMat(omega_hat) + Delta_t3*crossMat(omega_hat)*crossMat(omega_hat));
Matrix3d D = -A;
Matrix3d E = Matrix3d::Identity() - Delta_t*crossMat(omega_hat) + Delta_t2*crossMat(omega_hat)*crossMat(omega_hat);
Matrix3d F = -(Delta_t*Matrix3d::Identity() - Delta_t2*crossMat(omega_hat) + Delta_t3*crossMat(omega_hat)*crossMat(omega_hat));
Matrix<double,9,15> F_d_nonzero;
// Optimized:
Matrix<double,9,9> F_d_A;
Matrix<double,9,6> F_d_B;
Matrix<double,9,9> N_c_E = Matrix<double,9,9>::Zero();
Matrix<double,6,6> N_c_H = Matrix<double,6,6>::Zero();
Matrix<double,9,9> Q_d_A;
Matrix<double,9,6> Q_d_B;
Matrix<double,6,6> Q_d_C;
#define P_C P.block<9,9>(0,0)
#define P_D P.block<9,6>(0,9)
#define P_E P.block<9,13>(0,9+6)
#define P_D_ P.block<6,9>(9,0)
#define P_E_ P.block<13,9>(9+6,0)
#define P_G P.block<6,6>(9,9)
#define P_H P.block<6,13>(9,9+6)
F_d_A << Matrix3d::Identity(), Delta_t*Matrix3d::Identity(), A,
Matrix3d::Zero(), Matrix3d::Identity(), C,
Matrix3d::Zero(), Matrix3d::Zero(), E;
F_d_B << B, -C_q_i_w.transpose()*Delta_t2,
D, -C_q_i_w.transpose()*Delta_t,
F, Matrix3d::Zero();
for (int i = 0; i<3; i++)
{
N_c_E(3+i,3+i) = sq_sigma_a;
N_c_E(6+i,6+i) = sq_sigma_omega;
N_c_H(i,i) = sq_sigma_b_omega;
N_c_H(3+i,3+i) = sq_sigma_b_a;
}
Q_d_A = Delta_t/2.0 * ( F_d_A*N_c_E*F_d_A.transpose() + F_d_B*N_c_H*F_d_B.transpose() + N_c_E );
Q_d_B = Delta_t/2.0 * ( F_d_B*N_c_H );
Q_d_C = Delta_t * N_c_H;
Matrix<double,15,15> Q_d;
Q_d << Q_d_A, Q_d_B, Q_d_B.transpose(), Q_d_C;
P_C = F_d_A*P_C*F_d_A.transpose() + F_d_B*P_D.transpose()*F_d_A.transpose() + F_d_A*P_D*F_d_B.transpose() + F_d_B*P_G*F_d_B.transpose() + Q_d_A;
P_D = F_d_A*P_D + F_d_B*P_G + Q_d_B;
P_E = F_d_A*P_E + F_d_B*P_H;
P_G += Q_d_C;
P_D_ = P_D.transpose();
P_E_ = P_E.transpose();
}
