//============================================================================== // 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(); }