Skip to content

Commit

Permalink
Implement Math based v2
Browse files Browse the repository at this point in the history 
This could use further work to look more natural, but
it's good enough for the time being.
  • Loading branch information
@hyblocker
hyblocker committed Oct 9, 2022
1 parent 1e2f481 commit 9776649
Showing 1 changed file with 101 additions and 70 deletions.
171 changes: 101 additions & 70 deletions Amethyst/K2DeviceMath.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,16 @@

namespace TrackingDevices::Math
{

// The upper bound of the fog volume along the y-axis (height)
// This defines how far up the fog extends from the floor plane
// This is hard coded because all V2 sensors are the same (afaik, I don't know if the fog height changes depending on room conditions though)
constexpr double SOFTWARE_CALCULATED_ROTATION_FOG_THRESHOLD = 0.4;

inline double Lerp(double from, double to, const double delta) {
return from * (1 - delta) + to * delta;
}

// Calculate the math-based orientation
// Take the device pointer as the input
// Output to K2Settings.K2TrackersVector[L:0 | R:1]
Expand Down Expand Up @@ -327,52 +337,106 @@ namespace TrackingDevices::Math
// Check if the tracking is valid
if (_kinect->isSkeletonTracked())
{
/* Here the actual result is being applied, either to only one or both trackers */
// @TODO: Add biasing from head / waist

// 0 is always going to be the waist
bool isWaistOverriden = TrackingDevices::IsJointOverriden(0).second;

Eigen::Vector3d waistRotation = (Eigen::Vector3d)(
// a unit forward vector multiplied with rotation will give us the waist's forward direction in vector form
k2app::K2Settings.K2TrackersVector[0].getFullOrientation(k2_NoOrientationTrackingFilter) * Eigen::Vector3d(0, 0, 1));

// @HACK: Computes the inverse of the calibration matrix for the waist's device
// Ideally we should only compute the inverse during initialisation or calibration time
auto inverseCalibrationMatrix = K2Settings.deviceCalibrationRotationMatrices[K2Settings.K2TrackersVector[0].overrideGUID].inverse();

// The improved approach for fixing foot rotation on the Xbox One Kinect
//
// Thigh rotation is copied onto the foot, this is due to the fact that more often than not, the thigh
// is facing the same direction as your foot. Given the foot is an unreliable mess due to the fog on
// the Xbox One Kinect,
//
// The ankle position is stable though, so we can use it.
{
Eigen::Vector3d legsDir =
_joints[base_flip ? ktvr::ITrackedJointType::Joint_KneeRight : ktvr::ITrackedJointType::Joint_KneeLeft].getJointPosition()
- _joints[base_flip ? ktvr::ITrackedJointType::Joint_AnkleRight : ktvr::ITrackedJointType::Joint_AnkleLeft].getJointPosition();

// Normalize the direction to have a length of 1
legsDir.normalize();

// @TODO: Apply cross product

// Eigen::Vector3f left_ori_vector = EigenUtils::QuatToEulers();

// tend towards 0 below the fog threshold
// Remove the pitch entirely if within the fog area
legsDir.y() =
// smoothly interpolate between 0 and y^2 if below fog threshold
Lerp(
legsDir.y(),
// from 0 to y^2 (where 1 is fog threshold)
Lerp(0.0,
legsDir.y() * legsDir.y(),
// such that we remap the y direction relative to the threshold between 0 and 1
std::max(std::min(SOFTWARE_CALCULATED_ROTATION_FOG_THRESHOLD, legsDir.y()), 0.0) / SOFTWARE_CALCULATED_ROTATION_FOG_THRESHOLD),

// from 0.8 * fog threshold to 1.2 * fog threshold
std::min(1.0, std::max(0.0, 0.4 * SOFTWARE_CALCULATED_ROTATION_FOG_THRESHOLD * legsDir.y() - 0.8 * SOFTWARE_CALCULATED_ROTATION_FOG_THRESHOLD)));

// Calculate the Left Foot?
if (K2Settings.K2TrackersVector[1].orientationTrackingOption == k2_SoftwareCalculatedRotation_V2)
// Normalize the direction to have a length of 1
legsDir.normalize();

Eigen::Vector3d from = Eigen::Vector3d::UnitX();
Eigen::Vector3d base = Eigen::Vector3d::UnitX();

calculatedLeftFootOrientation = EigenUtils::DirectionQuat(from, legsDir, base);
}
{
// Flip the orientation based on the host flip
K2Settings.K2TrackersVector[1].pose_orientation =
base_flip
// If flip
? ktvr::quaternion_normal(
calculatedRightFootOrientation).
inverse()
Eigen::Vector3d legsDir =
_joints[base_flip ? ktvr::ITrackedJointType::Joint_KneeLeft : ktvr::ITrackedJointType::Joint_KneeRight].getJointPosition()
- _joints[base_flip ? ktvr::ITrackedJointType::Joint_AnkleLeft : ktvr::ITrackedJointType::Joint_AnkleRight].getJointPosition();

// If no flip
: ktvr::quaternion_normal(
calculatedLeftFootOrientation);
// Normalize the direction to have a length of 1
legsDir.normalize();

// Standard math-based pushes some fixes here
// Feel free to uncomment if you even need them
// (Or just fix the fixes and remove em in both...)
// Eigen::Vector3f left_ori_vector = EigenUtils::QuatToEulers();

/*
// Apply fixes
// tend towards 0 below the fog threshold
// Remove the pitch entirely if within the fog area
legsDir.y() =
// smoothly interpolate between 0 and y^2 if below fog threshold
Lerp(
legsDir.y(),
// from 0 to y^2 (where 1 is fog threshold)
Lerp(0.0,
legsDir.y() * legsDir.y(),
// such that we remap the y direction relative to the threshold between 0 and 1
std::max(std::min(SOFTWARE_CALCULATED_ROTATION_FOG_THRESHOLD, legsDir.y()), 0.0) / SOFTWARE_CALCULATED_ROTATION_FOG_THRESHOLD),

// Grab original orientations and make them euler angles
Eigen::Vector3d left_ori_vector = EigenUtils::QuatToEulers(
K2Settings.K2TrackersVector[1].pose_orientation);
// from 0.8 * fog threshold to 1.2 * fog threshold
std::min(1.0, std::max(0.0, 0.4 * SOFTWARE_CALCULATED_ROTATION_FOG_THRESHOLD * legsDir.y() - 0.8 * SOFTWARE_CALCULATED_ROTATION_FOG_THRESHOLD)));

// Kind of a solution for flipping at too big X.
// Found out during testing,
// no other known mathematical reason (maybe except gimbal lock)
// ------------------------------------------
if (left_ori_vector.y() <= 0.f
&& left_ori_vector.y() >= -1.f
// Normalize the direction to have a length of 1
legsDir.normalize();

&& left_ori_vector.z() <= -1.f
&& left_ori_vector.z() >= -_PI)
Eigen::Vector3d from = Eigen::Vector3d::UnitX();
Eigen::Vector3d base = Eigen::Vector3d::UnitX();

left_ori_vector.y() += -_PI;
// ------------------------------------------
calculatedRightFootOrientation = EigenUtils::DirectionQuat(from, legsDir, base);
}

// Apply to the base
// Calculate the Left Foot?
if (K2Settings.K2TrackersVector[1].orientationTrackingOption == k2_SoftwareCalculatedRotation_V2)
{
// Flip the orientation based on the host flip
K2Settings.K2TrackersVector[1].pose_orientation =
EigenUtils::EulersToQuat(left_ori_vector);
*/
base_flip
// If flip
? ktvr::quaternion_normal(calculatedRightFootOrientation).inverse()

// If no flip
: ktvr::quaternion_normal(calculatedLeftFootOrientation);
}

// Calculate the Right Foot?
Expand All @@ -382,43 +446,10 @@ namespace TrackingDevices::Math
K2Settings.K2TrackersVector[2].pose_orientation =
base_flip
// If flip
? ktvr::quaternion_normal(
calculatedLeftFootOrientation).
inverse()
? ktvr::quaternion_normal(calculatedLeftFootOrientation).inverse()

// If no flip
: ktvr::quaternion_normal(
calculatedRightFootOrientation);

// Standard math-based pushes some fixes here
// Feel free to uncomment if you even need them
// (Or just fix the fixes and remove em in both...)

/*
// Apply fixes
// Grab original orientations and make them euler angles
Eigen::Vector3d right_ori_vector = EigenUtils::QuatToEulers(
K2Settings.K2TrackersVector[2].pose_orientation);
// Kind of a solution for flipping at too big X.
// Found out during testing,
// no other known mathematical reason (maybe except gimbal lock)
// ------------------------------------------
if (right_ori_vector.y() <= 0.f
&& right_ori_vector.y() >= -1.f
&& right_ori_vector.z() <= -1.f
&& right_ori_vector.z() >= -_PI)
right_ori_vector.y() += -_PI;
// ------------------------------------------
// Apply to the base
K2Settings.K2TrackersVector[2].pose_orientation =
EigenUtils::EulersToQuat(right_ori_vector);
*/
: ktvr::quaternion_normal(calculatedRightFootOrientation);
}
}
}
Expand Down

0 comments on commit 9776649

Please sign in to comment.