Commit 81366323 authored by Moses Turner's avatar Moses Turner
Browse files

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!
Please register or to comment