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()
}
// -- OrientationFilterComplementaryOpticalARG --
void OrientationFilterComplementaryOpticalARG::update(const float delta_time, const PoseFilterPacket &packet)
{
if (packet.tracking_projection_area_px_sqr <= k_real_epsilon)
{
OrientationFilterMadgwickARG::update(delta_time, packet);
return;
}
const Eigen::Vector3f ¤t_omega= packet.imu_gyroscope_rad_per_sec;
Eigen::Vector3f current_g= packet.imu_accelerometer_g_units;
eigen_vector3f_normalize_with_default(current_g, Eigen::Vector3f::Zero());
// Current orientation from earth frame to sensor frame
const Eigen::Quaternionf SEq = m_state->orientation;
Eigen::Quaternionf SEq_new = SEq;
// Compute the quaternion derivative measured by gyroscopes
// Eqn 12) q_dot = 0.5*q*omega
Eigen::Quaternionf omega = Eigen::Quaternionf(0.f, current_omega.x(), current_omega.y(), current_omega.z());
Eigen::Quaternionf SEqDot_omega = Eigen::Quaternionf(SEq.coeffs() * 0.5f) *omega;
if (!current_g.isApprox(Eigen::Vector3f::Zero(), k_normal_epsilon))
{
// Get the direction of the gravitational fields in the identity pose
Eigen::Vector3f k_identity_g_direction = m_constants.gravity_calibration_direction;
// Eqn 15) Applied to the gravity vector
// Fill in the 3x1 objective function matrix f(SEq, Sa) =|f_g|
Eigen::Matrix<float, 3, 1> f_g;
eigen_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]
Eigen::Matrix<float, 4, 3> J_g;
eigen_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
Eigen::Matrix<float, 4, 1> gradient_f = J_g * f_g;
Eigen::Quaternionf SEqHatDot =
Eigen::Quaternionf(gradient_f(0, 0), gradient_f(1, 0), gradient_f(2, 0), gradient_f(3, 0));
// normalize the gradient
eigen_quaternion_normalize_with_default(SEqHatDot, *k_eigen_quaternion_zero);
// Compute the estimated quaternion rate of change
// Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot
const float beta= sqrtf(3.0f / 4.0f) * fmaxf(fmaxf(m_constants.gyro_variance.x(), m_constants.gyro_variance.y()), m_constants.gyro_variance.z());
Eigen::Quaternionf SEqDot_est = Eigen::Quaternionf(SEqDot_omega.coeffs() - SEqHatDot.coeffs()*beta);
// Compute then integrate the estimated quaternion rate
// Eqn 42) SEq_new = SEq + SEqDot_est*delta_t
SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_est.coeffs()*delta_time);
}
else
{
SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_omega.coeffs()*delta_time);
}
// Make sure the net quaternion is a pure rotation quaternion
SEq_new.normalize();
// Save the new quaternion and first derivative back into the orientation state
// Derive the second derivative
{
// The final rotation is a blend between the integrated orientation and absolute optical orientation
const float max_variance_fraction =
safe_divide_with_default(
m_constants.orientation_variance_curve.evaluate(packet.tracking_projection_area_px_sqr),
m_constants.orientation_variance_curve.MaxValue,
1.f);
float optical_weight=
lerp_clampf(1.f - max_variance_fraction, 0, k_max_optical_orientation_weight);
static float g_weight_override= -1.f;
if (g_weight_override >= 0.f)
{
optical_weight= g_weight_override;
}
const Eigen::EulerAnglesf optical_euler_angles = eigen_quaternionf_to_euler_angles(packet.optical_orientation);
const Eigen::EulerAnglesf SEeuler_new= eigen_quaternionf_to_euler_angles(packet.optical_orientation);
// Blend in the yaw from the optical orientation
const float blended_heading_radians=
wrap_lerpf(
SEeuler_new.get_heading_radians(),
optical_euler_angles.get_heading_radians(),
optical_weight,
-k_real_pi, k_real_pi);
const Eigen::EulerAnglesf new_euler_angles(
SEeuler_new.get_bank_radians(), blended_heading_radians, SEeuler_new.get_attitude_radians());
const Eigen::Quaternionf new_orientation =
eigen_euler_angles_to_quaternionf(new_euler_angles);
const Eigen::Vector3f &new_angular_velocity= current_omega;
const Eigen::Vector3f new_angular_acceleration = (current_omega - m_state->angular_velocity) / delta_time;
m_state->apply_state(new_orientation, new_angular_velocity, new_angular_acceleration);
}
}
void AppStage_GyroscopeCalibration::renderUI()
{
const float k_panel_width = 500;
const char *k_window_title = "Gyroscope Calibration";
const ImGuiWindowFlags window_flags =
ImGuiWindowFlags_ShowBorders |
ImGuiWindowFlags_NoResize |
ImGuiWindowFlags_NoMove |
ImGuiWindowFlags_NoScrollbar |
ImGuiWindowFlags_NoCollapse;
switch (m_menuState)
{
case eCalibrationMenuState::pendingTrackingSpaceSettings:
case eCalibrationMenuState::waitingForStreamStartResponse:
{
ImGui::SetNextWindowPosCenter();
ImGui::SetNextWindowSize(ImVec2(k_panel_width, 130));
ImGui::Begin(k_window_title, nullptr, window_flags);
ImGui::Text("Waiting for server response...");
ImGui::End();
} break;
case eCalibrationMenuState::failedStreamStart:
case eCalibrationMenuState::failedTrackingSpaceSettings:
{
ImGui::SetNextWindowPosCenter();
ImGui::SetNextWindowSize(ImVec2(k_panel_width, 130));
ImGui::Begin(k_window_title, nullptr, window_flags);
ImGui::Text("Failed server request!");
if (ImGui::Button("Ok"))
{
request_exit_to_app_stage(AppStage_ControllerSettings::APP_STAGE_NAME);
}
ImGui::SameLine();
if (ImGui::Button("Return to Main Menu"))
{
request_exit_to_app_stage(AppStage_MainMenu::APP_STAGE_NAME);
}
ImGui::End();
} break;
case eCalibrationMenuState::waitForStable:
{
ImGui::SetNextWindowPos(ImVec2(ImGui::GetIO().DisplaySize.x / 2.f - k_panel_width / 2.f, 20.f));
ImGui::SetNextWindowSize(ImVec2(k_panel_width, 140));
ImGui::Begin(k_window_title, nullptr, window_flags);
ImGui::TextWrapped(
"[Step 1 of 2: Measuring gyroscope drift and bias]\n" \
"Set the controller down on a level surface.\n" \
"Measurement will start once the controller is aligned with gravity and stable.");
if (m_bIsStable || m_bForceControllerStable)
{
std::chrono::time_point<std::chrono::high_resolution_clock> now= std::chrono::high_resolution_clock::now();
std::chrono::duration<double, std::milli> stableDuration = now - m_stableStartTime;
float fraction = static_cast<float>(stableDuration.count() / k_stabilize_wait_time_ms);
ImGui::ProgressBar(fraction, ImVec2(250, 20));
ImGui::Spacing();
}
else
{
ImGui::Text("Controller Destabilized! Waiting for stabilization..");
}
if (ImGui::Button("Trust me, it's stable"))
{
m_bForceControllerStable= true;
}
ImGui::SameLine();
if (ImGui::Button("Cancel"))
{
request_exit_to_app_stage(AppStage_ControllerSettings::APP_STAGE_NAME);
}
ImGui::End();
} break;
case eCalibrationMenuState::measureBiasAndDrift:
{
ImGui::SetNextWindowPos(ImVec2(ImGui::GetIO().DisplaySize.x / 2.f - k_panel_width / 2.f, 20.f));
ImGui::SetNextWindowSize(ImVec2(k_panel_width, 130));
ImGui::Begin(k_window_title, nullptr, window_flags);
std::chrono::time_point<std::chrono::high_resolution_clock> now= std::chrono::high_resolution_clock::now();
std::chrono::duration<double, std::milli> stableDuration = now - m_gyroNoiseSamples->sampleStartTime;
float timeFraction = static_cast<float>(stableDuration.count() / k_desired_drift_sampling_time);
const float sampleFraction =
static_cast<float>(m_gyroNoiseSamples->sample_count)
/ static_cast<float>(k_desired_noise_sample_count);
ImGui::TextWrapped(
"[Step 1 of 2: Measuring gyroscope drift and bias]\n" \
"Sampling Gyroscope...");
ImGui::ProgressBar(fminf(sampleFraction, timeFraction), ImVec2(250, 20));
if (ImGui::Button("Cancel"))
{
request_exit_to_app_stage(AppStage_ControllerSettings::APP_STAGE_NAME);
}
ImGui::End();
} break;
case eCalibrationMenuState::measureComplete:
{
ImGui::SetNextWindowPos(ImVec2(ImGui::GetIO().DisplaySize.x / 2.f - k_panel_width / 2.f, 20.f));
ImGui::SetNextWindowSize(ImVec2(k_panel_width, 130));
ImGui::Begin(k_window_title, nullptr, window_flags);
ImGui::TextWrapped(
"Sampling complete.\n" \
"Press OK to continue or Redo to recalibration.");
if (ImGui::Button("Ok"))
{
PSM_SetControllerLEDOverrideColor(m_controllerView->ControllerID, 0, 0, 0);
setState(eCalibrationMenuState::test);
}
ImGui::SameLine();
if (ImGui::Button("Redo"))
{
m_gyroNoiseSamples->clear();
setState(eCalibrationMenuState::waitForStable);
}
ImGui::SameLine();
if (ImGui::Button("Cancel"))
{
request_exit_to_app_stage(AppStage_ControllerSettings::APP_STAGE_NAME);
}
ImGui::End();
} break;
case eCalibrationMenuState::test:
{
ImGui::SetNextWindowPos(ImVec2(ImGui::GetIO().DisplaySize.x / 2.f - k_panel_width / 2.f, 20.f));
ImGui::SetNextWindowSize(ImVec2(k_panel_width, 140));
ImGui::Begin("Test Orientation", nullptr, window_flags);
if (m_bBypassCalibration)
{
ImGui::Text("Testing Calibration of Controller ID #%d", m_controllerView->ControllerID);
}
else
{
ImGui::Text("Calibration of Controller ID #%d complete!", m_controllerView->ControllerID);
}
PSMQuatf controllerQuat;
if (PSM_GetControllerOrientation(m_controllerView->ControllerID, &controllerQuat) == PSMResult_Success)
{
const Eigen::Quaternionf eigen_quat = psm_quatf_to_eigen_quaternionf(controllerQuat);
const Eigen::EulerAnglesf euler_angles = eigen_quaternionf_to_euler_angles(eigen_quat);
ImGui::Text("Pitch(x): %.2f, Yaw(y): %.2f, Roll(z): %.2f",
m_lastCalibratedGyroscope.x * k_radians_to_degreees,
m_lastCalibratedGyroscope.y * k_radians_to_degreees,
m_lastCalibratedGyroscope.z * k_radians_to_degreees);
ImGui::Text("Attitude: %.2f, Heading: %.2f, Bank: %.2f",
euler_angles.get_attitude_degrees(), euler_angles.get_heading_degrees(), euler_angles.get_bank_degrees());
}
if (m_controllerView->ControllerType == PSMController_DualShock4)
{
ImGui::TextWrapped(
"[Press the Options button with controller pointed straight forward\n" \
"to recenter the controller]");
}
else if (m_controllerView->ControllerType == PSMController_Move)
{
ImGui::TextWrapped(
"[Hold the Select button with controller pointed forward\n" \
"to recenter the controller]");
}
if (ImGui::Button("Ok"))
{
request_exit_to_app_stage(AppStage_ControllerSettings::APP_STAGE_NAME);
}
ImGui::SameLine();
if (ImGui::Button("Return to Main Menu"))
{
request_exit_to_app_stage(AppStage_MainMenu::APP_STAGE_NAME);
}
ImGui::End();
} break;
default:
assert(0 && "unreachable");
}
}
