mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2024-12-29 11:06:18 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

h/mercury: Use post-rotation residual that'll handle big rotations

Browse source 
Fixes a long-standing bug
This commit is contained in:
Moshi Turner 2023-03-06 20:05:01 -06:00 committed by Moses Turner
parent cdd0a6df67
commit 3f2b71a9d5
2 changed files with 53 additions and 12 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

40
src/xrt/tracking/hand/mercury/kine_lm/lm_defines.hpp
View file

@ -289,18 +289,42 @@ template <typename Scalar> struct Vec3
return Vec3(0.f, 0.f, 0.f);
}
// Norm, vector length, whatever.
Scalar
norm()
norm_sqrd() const
{
Scalar len = (Scalar)(0);
Scalar len_sqrd = (Scalar)(0);
len += this->x * this->x;
len += this->y * this->y;
len += this->z * this->z;
len_sqrd += this->x * this->x;
len_sqrd += this->y * this->y;
len_sqrd += this->z * this->z;
return len_sqrd;
}
len = sqrt(len);
return len;
// Norm, vector length, whatever.
// WARNING: Can return NaNs in the derivative part of Jets if magnitude is 0, because d/dx(sqrt(x)) at x=0 is
// undefined.
// There's no norm_safe because generally you need to add zero-checks somewhere *before* calling
// this, and it's not possible to produce correct derivatives from here.
Scalar
norm() const
{
Scalar len_sqrd = this->norm_sqrd();
return sqrt(len_sqrd);
}
// WARNING: Will return NaNs if vector magnitude is zero due to zero division.
// Do not call this on vectors with zero norm.
Vec3
normalized() const
{
Scalar norm = this->norm();
Vec3<Scalar> retval;
retval.x = this->x / norm;
retval.y = this->y / norm;
retval.z = this->z / norm;
return retval;
}
};

25
src/xrt/tracking/hand/mercury/kine_lm/lm_main.cpp
View file

@ -328,9 +328,26 @@ computeResidualStability(const OptimizerHand<T> &hand,
helper.AddValue((hand.wrist_post_location.z) * stab.stabilityRootPosition);
helper.AddValue((hand.wrist_post_orientation_aax.x) * (T)(stab.stabilityHandOrientationXY));
helper.AddValue((hand.wrist_post_orientation_aax.y) * (T)(stab.stabilityHandOrientationXY));
helper.AddValue((hand.wrist_post_orientation_aax.z) * (T)(stab.stabilityHandOrientationZ));
// Needed because d/dx(sqrt(x)) at x=0 is undefined, and the first iteration *always* starts at 0.
// x-2sin(0.5x) at x=0.001 is 4.16e-11 - this is a reasonable epsilon to pick.
const float epsilon = 0.001;
if (hand.wrist_post_orientation_aax.x < epsilon && //
hand.wrist_post_orientation_aax.y < epsilon && //
hand.wrist_post_orientation_aax.z < epsilon) {
helper.AddValue((hand.wrist_post_orientation_aax.x) * (T)(stab.stabilityHandOrientationXY));
helper.AddValue((hand.wrist_post_orientation_aax.y) * (T)(stab.stabilityHandOrientationXY));
helper.AddValue((hand.wrist_post_orientation_aax.z) * (T)(stab.stabilityHandOrientationZ));
} else {
T rotation_magnitude = hand.wrist_post_orientation_aax.norm();
T magnitude_sin = T(2) * sin(T(0.5) * rotation_magnitude);
Vec3<T> rotation_axis = hand.wrist_post_orientation_aax.normalized();
helper.AddValue((magnitude_sin * rotation_axis.x) * (T)(stab.stabilityHandOrientationXY));
helper.AddValue((magnitude_sin * rotation_axis.y) * (T)(stab.stabilityHandOrientationXY));
helper.AddValue((magnitude_sin * rotation_axis.z) * (T)(stab.stabilityHandOrientationZ));
}
@ -1065,7 +1082,7 @@ optimizer_run(KinematicHandLM *hand,
state.this_frame_pre_position = state.last_frame.wrist_final_location;
// Repack - brings the curl values back into original domain. Look at ModelToLM/LMToModel, we're
// using sin/asin.
state.last_frame.wrist_post_location.x = 0.0f;
state.last_frame.wrist_post_location.y = 0.0f;
state.last_frame.wrist_post_location.z = 0.0f;