static bool math_eigen_test_rotation_method_consistency(const Eigen::Quaternionf &q, const Eigen::Vector3f &v)
{
bool success = true;
assert_eigen_quaternion_is_normalized(q);
// This is the same as computing the rotation by computing: q^-1*[0,v]*q, but cheaper
Eigen::Vector3f v_rotated = eigen_vector3f_clockwise_rotate(q, v);
// Make sure doing the matrix based rotation performs the same result
{
Eigen::Matrix3f m = eigen_quaternion_to_clockwise_matrix3f(q);
Eigen::Vector3f v_test = m * v;
success &= v_test.isApprox(v_rotated, k_normal_epsilon);
assert(success);
}
// Make sure the Hamilton product style rotation matches
{
Eigen::Quaternionf v_as_quaternion = Eigen::Quaternionf(0.f, v.x(), v.y(), v.z());
Eigen::Quaternionf q_inv = q.conjugate();
Eigen::Quaternionf qinv_v = q_inv * v_as_quaternion;
Eigen::Quaternionf qinv_v_q = qinv_v * q;
success &=
is_nearly_equal(qinv_v_q.w(), 0.f, k_normal_epsilon) &&
is_nearly_equal(qinv_v_q.x(), v_rotated.x(), k_normal_epsilon) &&
is_nearly_equal(qinv_v_q.y(), v_rotated.y(), k_normal_epsilon) &&
is_nearly_equal(qinv_v_q.z(), v_rotated.z(), k_normal_epsilon);
assert(success);
}
return success;
}
// This algorithm comes from Sebastian O.H. Madgwick's 2010 paper:
// "An efficient orientation filter for inertial and inertial/magnetic sensor arrays"
// https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf
static void _psmove_orientation_fusion_imu_update(
PSMoveOrientation *orientation_state,
float delta_t,
const glm::vec3 ¤t_omega,
const glm::vec3 ¤t_g)
{
// Current orientation from earth frame to sensor frame
glm::quat SEq = orientation_state->quaternion;
glm::quat SEq_new = SEq;
// Compute the quaternion derivative measured by gyroscopes
// Eqn 12) q_dot = 0.5*q*omega
glm::quat omega = glm::quat(0.f, current_omega.x, current_omega.y, current_omega.z);
glm::quat SEqDot_omega = (SEq * 0.5f) *omega;
if (is_nearly_equal(glm::dot(current_g, current_g), 0.f, k_normal_epsilon*k_normal_epsilon))
{
// Get the direction of the gravitational fields in the identity pose
PSMove_3AxisVector identity_g= psmove_orientation_get_gravity_calibration_direction(orientation_state);
glm::vec3 k_identity_g_direction = glm::vec3(identity_g.x, identity_g.y, identity_g.z);
// Eqn 15) Applied to the gravity vector
// Fill in the 3x1 objective function matrix f(SEq, Sa) =|f_g|
glm::vec3 f_g;
psmove_alignment_compute_objective_vector(SEq, k_identity_g_direction, current_g, f_g, NULL);
// Eqn 21) Applied to the gravity vector
// Fill in the 4x3 objective function Jacobian matrix: J_gb(SEq)= [J_g]
glm::mat3x4 J_g;
psmove_alignment_compute_objective_jacobian(SEq, k_identity_g_direction, J_g);
// Eqn 34) gradient_F= J_g(SEq)*f(SEq, Sa)
// Compute the gradient of the objective function
glm::vec4 gradient_f = J_g * f_g;
glm::quat SEqHatDot =
glm::quat(gradient_f[0], gradient_f[1], gradient_f[2], gradient_f[3]);
// normalize the gradient
psmove_quaternion_normalize_with_default(SEqHatDot, *k_psmove_quaternion_zero);
// Compute the estimated quaternion rate of change
// Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot
glm::quat SEqDot_est = SEqDot_omega + SEqHatDot*(-beta);
// Compute then integrate the estimated quaternion rate
// Eqn 42) SEq_new = SEq + SEqDot_est*delta_t
SEq_new = SEq + SEqDot_est*delta_t;
}
else
{
SEq_new = SEq + SEqDot_omega*delta_t;
}
// Make sure the net quaternion is a pure rotation quaternion
SEq_new= glm::normalize(SEq_new);
// Save the new quaternion back into the orientation state
orientation_state->quaternion= SEq_new;
}
bool ClientMorpheusView::GetIsStableAndAlignedWithGravity() const
{
const float k_cosine_20_degrees = 0.9396926f;
// Get the direction the gravity vector should be pointing
// while the controller is in cradle pose.
PSMoveFloatVector3 acceleration_direction = CalibratedSensorData.Accelerometer;
const float acceleration_magnitude = acceleration_direction.normalize_with_default(*k_psmove_float_vector3_zero);
const bool isOk =
is_nearly_equal(1.f, acceleration_magnitude, 0.1f) &&
PSMoveFloatVector3::dot(k_identity_gravity_calibration_direction, acceleration_direction) >= k_cosine_20_degrees;
return isOk;
}
bool math_eigen_test_euler_angles()
{
UNIT_TEST_BEGIN("euler angles")
Eigen::Quaternionf q_pitch90 = eigen_quaternion_angle_axis(k_real_half_pi, Eigen::Vector3f::UnitX());
Eigen::Quaternionf q_yaw90 = eigen_quaternion_angle_axis(k_real_half_pi, Eigen::Vector3f::UnitY());
Eigen::Quaternionf q_roll90 = eigen_quaternion_angle_axis(k_real_half_pi, Eigen::Vector3f::UnitZ());
Eigen::Quaternionf q_euler_roll90 = eigen_euler_angles_to_quaternionf(Eigen::EulerAnglesf(k_real_half_pi, 0.f, 0.f));
Eigen::Quaternionf q_euler_yaw90 = eigen_euler_angles_to_quaternionf(Eigen::EulerAnglesf(0.f, k_real_half_pi, 0.f));
Eigen::Quaternionf q_euler_pitch90 = eigen_euler_angles_to_quaternionf(Eigen::EulerAnglesf(0.f, 0.f, k_real_half_pi));
success &= q_pitch90.isApprox(q_euler_pitch90, k_normal_epsilon);
assert(success);
success &= q_yaw90.isApprox(q_euler_yaw90, k_normal_epsilon);
assert(success);
success &= q_roll90.isApprox(q_euler_roll90, k_normal_epsilon);
assert(success);
Eigen::EulerAnglesf euler_angles_pitch90= eigen_quaternionf_to_euler_angles(q_euler_pitch90);
success &= is_nearly_equal(euler_angles_pitch90.get_heading_radians(), 0.f, k_normal_epsilon);
success &= is_nearly_equal(euler_angles_pitch90.get_attitude_radians(), k_real_half_pi, k_normal_epsilon);
success &= is_nearly_equal(euler_angles_pitch90.get_bank_radians(), 0.f, k_normal_epsilon);
assert(success);
Eigen::EulerAnglesf euler_angles_yaw90 = eigen_quaternionf_to_euler_angles(q_euler_yaw90);
success &= is_nearly_equal(euler_angles_yaw90.get_heading_radians(), k_real_half_pi, k_normal_epsilon);
success &= is_nearly_equal(euler_angles_yaw90.get_attitude_radians(), 0.f, k_normal_epsilon);
success &= is_nearly_equal(euler_angles_yaw90.get_bank_radians(), 0.f, k_normal_epsilon);
assert(success);
Eigen::EulerAnglesf euler_angles_roll90 = eigen_quaternionf_to_euler_angles(q_euler_roll90);
success &= is_nearly_equal(euler_angles_roll90.get_heading_radians(), 0.f, k_normal_epsilon);
success &= is_nearly_equal(euler_angles_roll90.get_attitude_radians(), 0.f, k_normal_epsilon);
success &= is_nearly_equal(euler_angles_roll90.get_bank_radians(), k_real_half_pi, k_normal_epsilon);
assert(success);
UNIT_TEST_COMPLETE()
}
//-- private methods -----
static bool
is_move_stable_and_aligned_with_gravity(PSMove *move)
{
const float k_cosine_10_degrees = 0.984808f;
PSMove_3AxisVector k_identity_gravity_vector;
PSMove_3AxisVector acceleration_direction;
float acceleration_magnitude;
bool isOk;
// Get the direction the gravity vector should be pointing
// while the controller is in cradle pose.
psmove_get_identity_gravity_calibration_direction(move, &k_identity_gravity_vector);
psmove_get_accelerometer_frame(move, Frame_SecondHalf,
&acceleration_direction.x, &acceleration_direction.y, &acceleration_direction.z);
acceleration_magnitude = psmove_3axisvector_normalize_with_default(&acceleration_direction, k_psmove_vector_zero);
isOk =
is_nearly_equal(1.f, acceleration_magnitude, 0.1f) &&
psmove_3axisvector_dot(&k_identity_gravity_vector, &acceleration_direction) >= k_cosine_10_degrees;
return isOk;
}
// This algorithm comes from Sebastian O.H. Madgwick's 2010 paper:
// "An efficient orientation filter for inertial and inertial/magnetic sensor arrays"
// https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf
static void
_psmove_orientation_fusion_madgwick_marg_update(
PSMoveOrientation *orientation_state,
float delta_t,
const glm::vec3 ¤t_omega,
const glm::vec3 ¤t_g,
const glm::vec3 ¤t_m)
{
// If there isn't a valid magnetometer or accelerometer vector, fall back to the IMU style update
if (is_nearly_equal(glm::dot(current_g, current_g), 0.f, k_normal_epsilon*k_normal_epsilon) ||
is_nearly_equal(glm::dot(current_m, current_m), 0.f, k_normal_epsilon*k_normal_epsilon))
{
_psmove_orientation_fusion_imu_update(
orientation_state,
delta_t,
current_omega,
current_g);
return;
}
PSMoveMadgwickMARGState *marg_state = &orientation_state->fusion_state.madgwick_marg_state;
// Current orientation from earth frame to sensor frame
glm::quat SEq = orientation_state->quaternion;
// Get the direction of the magnetic fields in the identity pose.
// NOTE: In the original paper we converge on this vector over time automatically (See Eqn 45 & 46)
// but since we've already done the work in calibration to get this vector, let's just use it.
// This also removes the last assumption in this function about what
// the orientation of the identity-pose is (handled by the sensor transform).
PSMove_3AxisVector identity_m= psmove_orientation_get_magnetometer_calibration_direction(orientation_state);
glm::vec3 k_identity_m_direction = glm::vec3(identity_m.x, identity_m.y, identity_m.z);
// Get the direction of the gravitational fields in the identity pose
PSMove_3AxisVector identity_g= psmove_orientation_get_gravity_calibration_direction(orientation_state);
glm::vec3 k_identity_g_direction = glm::vec3(identity_g.x, identity_g.y, identity_g.z);
// Eqn 15) Applied to the gravity and magnetometer vectors
// Fill in the 6x1 objective function matrix f(SEq, Sa, Eb, Sm) =|f_g|
// |f_b|
glm::vec3 f_g;
psmove_alignment_compute_objective_vector(SEq, k_identity_g_direction, current_g, f_g, NULL);
glm::vec3 f_m;
psmove_alignment_compute_objective_vector(SEq, k_identity_m_direction, current_m, f_m, NULL);
// Eqn 21) Applied to the gravity and magnetometer vectors
// Fill in the 4x6 objective function Jacobian matrix: J_gb(SEq, Eb)= [J_g|J_b]
glm::mat3x4 J_g;
psmove_alignment_compute_objective_jacobian(SEq, k_identity_g_direction, J_g);
glm::mat3x4 J_m;
psmove_alignment_compute_objective_jacobian(SEq, k_identity_m_direction, J_m);
// Eqn 34) gradient_F= J_gb(SEq, Eb)*f(SEq, Sa, Eb, Sm)
// Compute the gradient of the objective function
glm::vec4 gradient_f = J_g*f_g + J_m*f_m;
glm::quat SEqHatDot =
glm::quat(gradient_f[0], gradient_f[1], gradient_f[2], gradient_f[3]);
// normalize the gradient to estimate direction of the gyroscope error
psmove_quaternion_normalize_with_default(SEqHatDot, *k_psmove_quaternion_zero);
// Eqn 47) omega_err= 2*SEq*SEqHatDot
// compute angular estimated direction of the gyroscope error
glm::quat omega_err = (SEq*2.f) * SEqHatDot;
// Eqn 48) net_omega_bias+= zeta*omega_err
// Compute the net accumulated gyroscope bias
glm::quat omega_bias= marg_state->omega_bias;
omega_bias = omega_bias + omega_err*zeta*delta_t;
omega_bias.w = 0.f; // no bias should accumulate on the w-component
marg_state->omega_bias= omega_bias;
// Eqn 49) omega_corrected = omega - net_omega_bias
glm::quat omega = glm::quat(0.f, current_omega.x, current_omega.y, current_omega.z);
glm::quat corrected_omega = omega + (-omega_bias);
// Compute the rate of change of the orientation purely from the gyroscope
// Eqn 12) q_dot = 0.5*q*omega
glm::quat SEqDot_omega = (SEq * 0.5f) * corrected_omega;
// Compute the estimated quaternion rate of change
// Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot
glm::quat SEqDot_est = SEqDot_omega + SEqHatDot*(-beta);
// Compute then integrate the estimated quaternion rate
// Eqn 42) SEq_new = SEq + SEqDot_est*delta_t
glm::quat SEq_new = SEq + SEqDot_est*delta_t;
// Make sure the net quaternion is a pure rotation quaternion
SEq_new= glm::normalize(SEq_new);
// Save the new quaternion back into the orientation state
orientation_state->quaternion= SEq_new;
}
