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"); } }