Commit 81366323 by Moses Turner

### BLARG

parent bc9a1544
Pipeline #732875 failed with stages
in 4 minutes and 33 seconds
 ... ... @@ -350,26 +350,26 @@ math_quat_finite_difference(const struct xrt_quat *quat0, float dt, struct xrt_vec3 *out_ang_vel); /*! * Converts a rotation vector in axis-angle form to its corresponding unit quaternion. * * @relates xrt_quat * @see xrt_vec3 * @ingroup aux_math */ void math_quat_exp(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat); /*! * Converts a unit quaternion into its corresponding axis-angle vector representation. * * @relates xrt_quat * @see xrt_vec3 * @ingroup aux_math */ void math_quat_ln(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle); // /*! // * Converts a rotation vector in axis-angle form to its corresponding unit quaternion. // * // * @relates xrt_quat // * @see xrt_vec3 // * @ingroup aux_math // */ // void // math_quat_from_rodrigues(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat); // /*! // * Converts a unit quaternion into its corresponding axis-angle vector representation. // * // * @relates xrt_quat // * @see xrt_vec3 // * @ingroup aux_math // */ // void // math_quat_to_rodrigues(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle); /*! * Converts a rotation vector in axis-angle form to its corresponding unit quaternion. ... ... @@ -379,7 +379,7 @@ math_quat_ln(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle); * @ingroup aux_math */ void math_quat_to_axis_angle(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle); math_quat_to_rodrigues(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle); /*! ... ... @@ -390,7 +390,7 @@ math_quat_to_axis_angle(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_a * @ingroup aux_math */ void math_quat_from_axis_angle(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat); math_quat_from_rodrigues(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat); /*! ... ...
 ... ... @@ -391,7 +391,7 @@ math_quat_slerp(const struct xrt_quat *left, const struct xrt_quat *right, float * @ingroup aux_math */ extern "C" void math_quat_from_axis_angle(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat) math_quat_from_rodrigues(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat) { float a0 = axis_angle->x; float a1 = axis_angle->y; ... ... @@ -429,7 +429,7 @@ math_quat_from_axis_angle(const struct xrt_vec3 *axis_angle, struct xrt_quat *ou * @ingroup aux_math */ extern "C" void math_quat_to_axis_angle(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle) { math_quat_to_rodrigues(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle) { float q1 = quat->x; float q2 = quat->y; float q3 = quat->z; ... ... @@ -520,7 +520,7 @@ math_quat_from_swing_twist(const struct xrt_vec2 *swing, const float twist, stru math_quat_from_swing(swing, &swing_quat); math_quat_from_axis_angle(&aax_twist, &twist_quat); math_quat_from_rodrigues(&aax_twist, &twist_quat); math_quat_rotate(&swing_quat, &twist_quat, result); } ... ... @@ -550,15 +550,15 @@ math_quat_to_swing_twist(const struct xrt_quat *in, struct xrt_vec2 *out_swing, out_swing->y = swing_aax.axis().y() * swing_aax.angle(); assert(swing_aax.axis().z() < 0.001); std::cout << "axis:" << swing_aax.axis()*swing_aax.angle() << "\n" ; std::cout << "axis:" << twist_aax.axis() *twist_aax.angle()<< "\n" ; // std::cout << "axis:" << swing_aax.axis()*swing_aax.angle() << "\n" ; // std::cout << "axis:" << twist_aax.axis() *twist_aax.angle()<< "\n" ; Eigen::Quaternionf result0 = swing_aax * twist_aax; // This one is right // Eigen::Quaternionf result1 = twist_aax * swing_aax; // Eigen::Quaternionf result0 = swing_aax * twist_aax; // This one is right // // Eigen::Quaternionf result1 = twist_aax * swing_aax; std::cout << result0 << std::endl << std::endl; // std::cout << result1 << std::endl<< std::endl; std::cout << map_quat(*in) << std::endl << std::endl; // std::cout << result0 << std::endl << std::endl; // // std::cout << result1 << std::endl<< std::endl; // std::cout << map_quat(*in) << std::endl << std::endl; *out_twist = twist_aax.axis().z() * twist_aax.angle(); ... ...
 ... ... @@ -276,15 +276,15 @@ m_filter_euro_quat_run(struct m_filter_euro_quat *f, uint64_t ts, const struct x // Scale dy with dt through a conversion to angle_axis struct xrt_vec3 dy_aa; math_quat_ln(&dy, &dy_aa); math_quat_to_rodrigues(&dy, &dy_aa); dy_aa = m_vec3_div_scalar(dy_aa, dt); math_quat_exp(&dy_aa, &dy); math_quat_from_rodrigues(&dy_aa, &dy); f->prev_dy = exp_smooth_quat(alpha_d, dy, f->prev_dy); // The magnitud of the smoothed dy (f->prev_dy) is its rotation angle in radians struct xrt_vec3 smooth_dy_aa; math_quat_ln(&f->prev_dy, &smooth_dy_aa); math_quat_to_rodrigues(&f->prev_dy, &smooth_dy_aa); double smooth_dy_mag = m_vec3_len(smooth_dy_aa); double alpha = filter_one_euro_compute_alpha(&f->base, dt, smooth_dy_mag); ... ...
 ... ... @@ -168,15 +168,15 @@ math_quat_finite_difference(const struct xrt_quat *quat0, map_vec3(*out_ang_vel) = 2.f * quat_ln(inc_quat) / dt; } extern "C" void math_quat_exp(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat) { map_quat(*out_quat) = quat_exp(map_vec3(*axis_angle)); } extern "C" void math_quat_ln(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle) { Eigen::Quaternionf eigen_quat = map_quat(*quat); map_vec3(*out_axis_angle) = quat_ln(eigen_quat); } // extern "C" void // math_quat_from_rodrigues(const struct xrt_vec3 *axis_angle, struct xrt_quat *out_quat) // { // map_quat(*out_quat) = quat_exp(map_vec3(*axis_angle)); // } // extern "C" void // math_quat_to_rodrigues(const struct xrt_quat *quat, struct xrt_vec3 *out_axis_angle) // { // Eigen::Quaternionf eigen_quat = map_quat(*quat); // map_vec3(*out_axis_angle) = quat_ln(eigen_quat); // }
 ... ... @@ -67,10 +67,10 @@ TEST_CASE("m_quatexpmap") for (xrt_vec3 aa : aas) { xrt_quat quat{}; math_quat_from_axis_angle(&aa, &quat); math_quat_from_rodrigues(&aa, &quat); xrt_vec3 expected_aa{}; math_quat_to_axis_angle(&quat, &expected_aa); math_quat_to_rodrigues(&quat, &expected_aa); CHECK(m_vec3_len(expected_aa - aa) <= 0.001); } ... ... @@ -84,7 +84,7 @@ TEST_CASE("m_quatexpmap") xrt_vec3 axis = axis4; xrt_vec3 aa = axis * angle; xrt_quat q{}; math_quat_from_axis_angle(&aa, &q); math_quat_from_rodrigues(&aa, &q); CHECK(q.x == Approx(axis.x * sin(angle / 2))); CHECK(q.y == Approx(axis.y * sin(angle / 2))); ... ...
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!